def __init__(self, from_pixels=True, height=100, width=100, camera_id=0, control="torque"): side = "right" ip = "127.0.0.1" self.blue = BlueInterface(side, ip) self.blue.calibrate_gripper() self._from_pixels = from_pixels self.control = control self._frame_skip = frame_skip self.channels_first=True self.camera = StreamVideo(height=height, width=width, camera_id=camera_id) self.camera.start() # true and normalized action spaces self._true_action_space = spaces.Box( low=[-3.3999, -2.2944, -2.6761, -2.2944, -2.6761, -2.2944, -2.6761, 0.0], high=[2.3412, 0, 2.6761, 0, 2.6761, 0, 2.676, 0.0], shape=np.zeros(8), dtype=np.float32 ) self._norm_action_space = spaces.Box( low=-1.0, high=1.0, shape=self._true_action_space.shape, dtype=np.float32 ) # create observation space if from_pixels: shape = [3, height, width] if self.channels_first else [height, width, 3] self._observation_space = spaces.Box( low=0, high=255, shape=shape, dtype=np.uint8 ) else: self._observation_space = _spec_to_box( self._env.observation_spec().values() ) self._state_space = _spec_to_box( self._env.observation_spec().values() ) self.current_state = None # set seed self.seed(seed=task_kwargs.get('random', 1))
def test_no_rosbridge(): """Make sure that importing BlueInterface and instantiating it results in the correct error when no rosbridge server is running. (( assumes that no rosbridge server is running at localhost:444 )) """ from blue_interface import BlueInterface try: error = ConnectionRefusedError except NameError: # Python 2.7 error = Exception with pytest.raises(error): BlueInterface(side="left", ip="127.0.0.1", port=444)
import pickle # will use to save data between program launches import sys sys.path.append('blue_interface') from blue_interface import BlueInterface # this is the API for the robot sys.path.append('../../blue_vive_tracking/scripts') from vive_tracking import ViveTracker import random import os import time import numpy as np kk = BlueInterface( "right", "hekate.cs.berkeley.edu" ) # creates object of class KokoInterface at the IP in quotes with the name 'kk' kk.disable_control( ) # this turns off any other control currently on the robot (leaves it in gravtiy comp mode) vv = ViveTracker() kk.command_gripper(2.05, .1) #This isn't working if not os.path.isfile('../results/benchmark_joint_angle_list.p'): print("You should first run 'record_positions.py") #TODO make this an actual error joint_angle_list = pickle.load( open("../results/benchmark_joint_angle_list.p", "rb") ) #uses the pickle function to read the binary file created in record_poses.py pose_list = pickle.load(open("../results/benchmark_pose_list.p", "rb")) location_list = [
#!/usr/bin/env python3 # A basic example of using BlueInterface for joint positions control. # It allows a user to record four sets of joint positions by manually moving the arm to each # position and pressing enter. It then plays back a trajectory comprised of the four sets of # joint positions in an infinite loop. import sys from blue_interface import BlueInterface import numpy as np import time if __name__ == '__main__': arm = "right" address = "127.0.0.1" blue = BlueInterface(arm, address) blue.set_joint_positions(np.zeros(7), duration=3.0) while True: pass # blue.shutdown()
#!/usr/bin/env python3 # A basic example of using BlueInterface for gripper control. # It allows a user to open and close the gripper. import sys from blue_interface import BlueInterface import numpy as np import consts if __name__ == '__main__': blue = BlueInterface( consts.default_arm, consts.default_address ) #creates object of class KokoInterface at the IP in quotes with the name 'blue' opened = True try: while True: input( "Press enter to open/close the gripper. To exit, press <ctrl+c>." ) if opened: blue.command_gripper(-1.5, 20.0) print("Closing...") else: blue.command_gripper(0.0, 10.0) print("Opening...") opened = not opened except: pass
#!/usr/bin/env python3 from blue_interface import BlueInterface import numpy as np import time import transformations as t import spacemouse import sys if __name__ == '__main__': arm_side = "right" assert len(sys.argv) == 2 address = sys.argv[1] # Python interface blue = BlueInterface(arm_side, address) home = [ -0.433836, -1.16384, 0.75438465, -1.58150699, -0.05635529, -1.67967716, -0.13010218 ] home = [0.0, -1.571, 0.0, -1.571, 0.0, -1.571, 0.0] blue.set_joint_positions( # TODO: this sometimes fights with the IK solver, and makes the robot really jittery home, duration=1.0, # soft_position_control=True ) position_control_mode = blue._control_mode # super sketchy # Set up a publisher for our cartesian pose command (a little sketchy) RBC = blue._RBC pose_target_publisher = RBC.publisher(
help='Port that the ros web host was started on') parser.add_argument('--frequency', default=0, type=float, help='Replay the recording at a custom frequency (Hz)') args = parser.parse_args() filename = args.record_file arm = consts.default_arm address = args.address port = args.port #blue = BlueInterface("left","10.42.0.1") # creates object of class KokoInterface at the IP in quotes with the name 'blue' blue = BlueInterface( arm, address, port ) #creates object of class KokoInterface at the IP in quotes with the name 'blue' # This turns off any other control currently on the robot (leaves it in gravtiy comp mode) blue.disable_control() data = pickle.load( open(filename, "rb") ) #uses the pickle function to read the binary file created in record_poses.py joint_angle_list, _, gripper_list, record_frequency = data # If no argument is passed for replay frequency, play the recording at the rate it was recorded. if args.frequency == 0: frequency = record_frequency # In Hertz else: frequency = args.frequency
#!/usr/bin/env python3 # A basic example of using BlueInterface for gripper control. # It allows a user to open and close the gripper. from blue_interface import BlueInterface import numpy as np side = "right" ip = "127.0.0.1" blue = BlueInterface(side, ip) # After calibration, the gripper will be closed blue.calibrate_gripper() opened = False while True: input("Press enter to open/close the gripper. To exit, press <ctrl+c>.") if opened: print("Closing...") blue.command_gripper(-1.5, 20.0, wait=True) else: print("Opening...") blue.command_gripper(0.0, 10.0, wait=True) opened = not opened
import sys sys.path.append('../../../../../blue_interface') from blue_interface import BlueInterface from control_benchmarking import ControllerVis import time import matplotlib.pyplot as plt from sensor_msgs.msg import JointState from std_msgs.msg import Float64MultiArray from time import sleep control_info = ControllerVis() blu = BlueInterface("right","hekate.cs.berkeley.edu") blu.disable_control() sampling_delay = 1.0/125 # seconds, samples at about the same rate as the response is published (use rostopic hz /joint_states) sample_duration = 2 # Helper functions def record_rxn(): time_end = time.time() + sample_duration # 3 seconds from now # Initialize cmd = [] response = [] while time.time()< time_end: cmd.append(control_info.get_reference_data()) response.append(control_info.get_response_data()) sleep(sampling_delay) print "Samples collected: %d" % len(cmd)
import numpy as np #will use to create arrays import pickle #will use to save data between program launches import sys sys.path.append('blue_interface') from blue_interface import BlueInterface # this is the API for the robot blu = BlueInterface("right","hekate.cs.berkeley.edu") #creates object of class KokoInterface at the IP in quotes with the name 'blu' # THIS IS THE LINE THAT GETS MY PGORAM STUCK FROM EXITING blu.disable_control() #this turns off any other control currently on the robot (leaves it in gravtiy comp mode) joint_angle_list = [] #Initialize the list to hold our joint positions pose_list = [] done_collecting_positions = False #create a variable to track when we've collected all the desired poses while not done_collecting_positions: s = raw_input("Move the robot to a new position and press enter to record or enter d for done and press enter") if s != "d": #if the user is not done position = blu.get_joint_positions() #record the pose, this function returns a dictionary object joint_angle_list.append(position) pose = blu.get_cartesian_pose() pose_list.append(pose) print("Position recorded!") else: done_collecting_positions = True #set the variable such that the loop doesn't run again if len(joint_angle_list)==0: print('You did not save any positions') else: pickle.dump(joint_angle_list, open("../results/benchmark_joint_angle_list.p", "wb")) #uses the pickle function to write a binary file pickle.dump(pose_list, open("../results/benchmark_pose_list.p", "wb")) print('Your position list has been saved in the results directory')