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)
#!/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 time import argparse from blue_interface import BlueInterface if __name__ == '__main__': # load file name if len(sys.argv) == 2: filename = sys.argv[1] else: filename = "motion.pkl" # Blue Python interface arm_side = "right" address = "127.0.0.1" blue = BlueInterface(arm_side, address) 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, frequency = data # If no argument is passed for replay frequency, play the recording at the rate it was recorded. input("Press enter to start replay. To exit, press <ctrl+c>.") try: last_time = 0.0 for i in range (len(joint_angle_list)): #if len(joint_angle_list[i]) == 7: blue.set_joint_positions(np.array(joint_angle_list[i])) # tell the robot to go to a set of joint angles #blue.command_gripper(gripper_list[i], 30.0) if gripper_list[i] < -0.2: blue.command_gripper(-1.3, 15.0)
class BlueRobotEnv(object): 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 __delete__(self): self.camera.stop() @property def observation_space(self): return self._observation_space @property def state_space(self): return self._state_space @property def action_space(self): return self._norm_action_space def step(self,a): for _ in range(self._frame_skip): # time_step = self._env.step(action) # reward += time_step.reward or 0 if control=="torque": self.blue.set_joint_torques(a[:-1]) self.blue.command_gripper(a[-1], 20.0, wait=True) else: self.blue.set_joint_positions(a[:-1], duration=0.1) self.blue.command_gripper(a[-1], 20.0, wait=True) obs = self._get_obs() reward = 0 done = False return obs, reward, done, None def reset(self): init_pos = np.array([0, -2.31, 1.57, -0.75, -1.57, 0, 0]) self.blue.set_joint_positions(init_pos, duration=0.0) self.blue.calibrate_gripper() self.blue.command_gripper(0.0, 20.0, wait=False) obs = self._get_obs() return obs def _get_obs(self): if self._from_pixels: obs = self.render() if self._channels_first: obs = obs.transpose(2, 0, 1).copy() else: obs = get_robot_joints() return obs def render(self): return self.camera() @property def get_qpos(self): joint = self.blue.get_joint_positions() gripper = self.blue.get_gripper_position() return np.concatenate((joint,gripper),axis=0) @property def get_qvel(self): joint = self.blue.get_joint_velocities() gripper = 0 return np.concatenate((joint,gripper),axis=0) def get_robot_joints(self): return np.concatenate([ self.get_qpos, self.get_qvel, ])
import pickle # will use to save data between program launches import sys import numpy as np 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 'kk' blu.disable_control( ) # this turns off any other control currently on the robot (leaves it in gravtiy comp mode) import numpy as np 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 done = False while not done: s = raw_input( "Type the pose number to which you want to return. Type 'd' for done. " ) if s != 'd': if int(s) in range(len(joint_angle_list)): blu.set_joint_positions(np.array(joint_angle_list[int( s)])) # tell the robot to go to a set of joint angles print('position set') else: print "Please enter a valid pose number between 0 and %d " % ( len(joint_angle_list) - 1) else: done = True
#!/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 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(
#!/usr/bin/env python3 # A basic example of using BlueInterface for joint positions control. import numpy as np from blue_interface import BlueInterface side = "right" ip = "127.0.0.1" blue = BlueInterface(side, ip) blue.set_joint_positions(np.zeros(7), duration=3.0) while True: pass # When this script terminates (eg via Ctrl+C), the robot will automatically # stop all controllers and go back to gravity compensation mode
import Leap from utils.rotations import quat2euler, euler2quat, mat2euler from utils.leap_listener import SampleListener import matplotlib.pyplot as plt parser = argparse.ArgumentParser(description='switch the control mode') parser.add_argument('--IK', default=False, action='store_true', help='switch to IK-control') args = parser.parse_args() side = "right" ip = "127.0.0.1" blue = BlueInterface(side, ip) # Initialize the blue gripper blue.calibrate_gripper() # Leap Motion listener = SampleListener() controller = Leap.Controller() target_angles_init = np.array([0.0, -0.85, 1.571, 0, -1.571, -0.2, 0.0]) target_angles_hist = target_angles_init.copy() i = 0 while True: hands_data = listener.get_hand(controller) ## IK approach
#!/usr/bin/env python3 # A basic example of using BlueInterface for joint positions control. from blue_interface import BlueInterface import numpy as np import sys assert len(sys.argv) == 2 side = "right" ip = sys.argv[1] blue = BlueInterface(side, ip) current_joints = blue.get_joint_positions() blue.set_joint_positions(current_joints, duration=3.0) while True: pass # When this script terminates (eg via Ctrl+C), the robot will automatically # stop all controllers and go back to gravity compensation mode
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')
#!/usr/bin/env python3 # A basic example of sending Blue a command in cartesian space. from blue_interface import BlueInterface import numpy as np import time side = "right" ip = "127.0.0.1" blue = BlueInterface(side, ip) # Compute IK solution target_position = [0.4, 0, 0] # x, y, z target_orientation = [0.6847088, -0.17378805, -0.69229771, -0.1472938] # x, y, z, w target_joint_positions = blue.inverse_kinematics(target_position, target_orientation) # Send command to robot blue.set_joint_positions(target_joint_positions, duration=5) # Wait for system to settle time.sleep(5) # Print results joint_positions = blue.get_joint_positions() pose = blue.get_cartesian_pose() print_aligned = lambda left, right: print("{:30} {}".format( left, np.round(right, 4))) print_aligned("Target joint positions: ", target_joint_positions)
#!/usr/bin/python3 import pickle import sys import numpy as np import time import argparse from blue_interface import BlueInterface if __name__ == '__main__': # Blue Python interface arm_side = "right" address = "127.0.0.1" blue = BlueInterface(arm_side, address) blue.disable_control()
parser.add_argument('--frequency', default=consts.default_frequency, type=int, help='Record at a custom frequency (Hz)') args = parser.parse_args() filename = args.record_file arm = consts.default_arm address = args.address port = args.port frequency = args.frequency # In Hertz #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' blue.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 = [] gripper_list = [] input( "Press enter to start recording. To finish recording press <ctrl+c>.") try: last_time = 0.0 while True: position = blue.get_joint_positions(
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 reading information about Blue's current state. import time import numpy as np from blue_interface import BlueInterface side = "right" ip = "127.0.0.1" blue = BlueInterface(side, ip) def print_aligned(left, right): print("{:30} {}".format(left, np.round(right, 4))) while True: print_aligned("End Effector Position:", blue.get_cartesian_pose()["position"]) print_aligned("End Effector Orientation:", blue.get_cartesian_pose()["orientation"]) print_aligned("Joint Positions:", blue.get_joint_positions()) print_aligned("Joint Velocities:", blue.get_joint_velocities()) print_aligned("Joint Torques:", blue.get_joint_torques()) print_aligned("Gripper Position:", blue.get_gripper_position()) print_aligned("Gripper Effort:", blue.get_gripper_effort()) print("=" * 30) time.sleep(0.5)
#!/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
import argparse import consts if __name__ == '__main__': parser = argparse.ArgumentParser(description='Replay a blue arm through a recorded set of positions.') parser.add_argument('--tape_file', default = 'tape_trajectory.pickle', type=str, help='Saves a set of positions to this file') parser.add_argument('--address', default=consts.default_address, type=str, help='Address of the host computer') parser.add_argument('--port', default=consts.default_port, type=int, help='Port that the ros web host was started on') args = parser.parse_args() arm = consts.default_arm address = args.address port = args.port filename = args.tape_file blue = BlueInterface(arm, address, port) #creates object of class KokoInterface at the IP in quotes with the name 'blue' print("..") recorded_positions = [] error = 0.5 blue.disable_control() blue.disable_gripper() #TODO: make number of positions a command line arg with open(filename, 'rb') as handle: recorded_positions = pickle.load(handle) input("Press enter to start trajectory.") while True:
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 = [
import time import sys from blue_interface import BlueInterface # this is the API for the robot if __name__ == '__main__': # motion file name if len(sys.argv) == 2: filename = sys.argv[1] else: filename = "motion.pkl" frequency = 100 # In Hertz # Blue Python interface arm_side = "right" address = "127.0.0.1" blue = BlueInterface(arm_side, address) blue.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 = [] gripper_list = [] input( "Press enter to start recording. To finish recording press <ctrl+c>.") try: last_time = 0.0 while True: position = blue.get_joint_positions( ) #record the pose, this function returns a dictionary object joint_angle_list.append(position)