# 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 blue.disable_control() blue.shutdown()
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 = [ 0 for x in range(len(joint_angle_list)) ] #initialize a list of locations where each entry is of the form [joint_angle_position, pose] for k in range(len(joint_angle_list)): location_ID = k
# 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 input("Press enter to start replay. To exit, press <ctrl+c>.") while True: 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: blue.command_gripper(2, 15.0) else: blue.command_gripper(-0.8, 10.0) sleep_time = 1.0 / frequency - (time.time() - last_time) if sleep_time > 0: time.sleep(sleep_time) last_time = time.time() except: print(sys.exc_info()[0]) print("Something went wrong... exiting") break
input_rot, current_orientation) ## Publishing target pose pose_target_msg = { "header": { "frame_id": "base_link" }, "pose": { "position": dict(zip(("x", "y", "z"), target_position)), "orientation": dict(zip(("x", "y", "z", "w"), target_orientation)) } } pose_target_publisher.publish(pose_target_msg) ## Gripper stuff if gripper_closed != mouse.input_button1: if gripper_closed: # open gripper blue.command_gripper(-1.0, 10.0) else: # close gripper blue.command_gripper(2.00, 30.0) gripper_closed = mouse.input_button1 prev_input_button0 = mouse.input_button0 ## Sleep time.sleep(0.01)
#!/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
# 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) else: blue.command_gripper(0, 2.0) sleep_time = 1.0/frequency - (time.time() - last_time) if sleep_time > 0: time.sleep(sleep_time) last_time = time.time() except: print (sys.exc_info()[0]) print ("Something went wrong... exiting") pass
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, ])
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: for desired_position in recorded_positions: current_position = (blue.get_gripper_position(), blue.get_joint_positions()) blue.set_joint_positions(np.array(desired_position[1])) blue.command_gripper(desired_position[0], 2.0, True) while np.linalg.norm(desired_position[1] - current_position[1]) > error: current_position = (blue.get_gripper_position(), blue.get_joint_positions()) time.sleep(0.1)
target_position[0] -= 0.4 target_position[2] += 0.3 target_orientation = list(euler2quat(ori)) # w, x, y, z # target_orientation = target_orientation[1:] + target_orientation[:1] # Compute IK solution goal_curr = blue.inverse_kinematics(target_position, target_orientation) # Send command to robot if goal_curr != []: goal = goal_curr print("goal: ", goal) blue.set_joint_positions(goal, duration=3, soft_position_control=False) blue.command_gripper(grab_strength, 10.0, wait=False) # Wait for system to settle i += 1 time.sleep(3) # Direct motor angle mapping approach else: if "Right hand" in hands_data.keys(): hand_data = hands_data["Right hand"] pos = hand_data['palm_position'] ori = [ hand_data['palm_pitch'], hand_data['palm_roll'], hand_data['palm_yaw'] ] grab_strength = hand_data['grab_strength']