def __init__(self, action_scale=0.05, target_range=0.15, distance_threshold=0.05, speed=0.10): self.connection = utilities.DeviceConnection.createTcpConnection() self.router = self.connection.__enter__() self.base = BaseClient(self.router) self.base_cyclic = BaseCyclicClient(self.router) self.webcam = cv2.VideoCapture(0) self.action_scale = action_scale self.target_range = target_range self.distance_threshold = distance_threshold self.speed = speed self.observation_space = gym.spaces.Dict( observation=gym.spaces.Box(-np.inf, np.inf, self._get_obs().shape), achieved_goal=gym.spaces.Box(-np.inf, np.inf, (3,)), desired_goal=gym.spaces.Box(-np.inf, np.inf, (3,)), ) self.action_space = gym.spaces.Box(-1., 1., (3,)) self.reward_range = (-np.inf, 0) self.unwrapped = self self.spec = None self._set_to_home() self.init_xyz = self._get_obs()[:3] self.goal = None self.sample_goal() assert self.goal is not None
def look_into_crowd(): global args global e if not args: args = utilities.parseConnectionArguments() with utilities.DeviceConnection.createTcpConnection( args) as router: #kann das "with" weg? Nein # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) device_manager = DeviceManagerClient(router) #Activate the continuous auto-focus of the camera vision_config = VisionConfigClient(router) vision_device_id = vision_action.example_vision_get_device_id( device_manager) # check if robot is not in transport position, if so, move safely if functions_.pose_comparison( transport_pos, base_cyclic, "joint") == True: #Robot in transport position? highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=pre_transport_pos, speed=speedj) #pre transport safety pose #INSERT WAYPOINTS HERE return
def main(): # Import the utilities helper module sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) import utilities # Parse arguments args = utilities.parseConnectionArguments() # Create connection to the device and get the router with utilities.DeviceConnection.createTcpConnection(args) as router: # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) # Example core success = True success &= example_move_to_home_position(base) success &= example_cartesian_action_movement(base, base_cyclic) success &= example_angular_action_movement(base) # You can also refer to the 110-Waypoints examples if you want to execute # a trajectory defined by a series of waypoints in joint space or in Cartesian space return 0 if success else 1
def read_base_cyclic(): global args global e if not args: args = utilities.parseConnectionArguments() with utilities.DeviceConnection.createTcpConnection( args) as router: #kann das "with" weg? Nein # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) tlog = time.time() now = datetime.datetime.now() #print (now.strftime("%Y-%m-%d %H:%M:%S")) timestamp = now.strftime("%Y-%m-%d-%H-%M-%S") f = open( "./Subfunctions/log_recordings/rec_logging_%s.txt" % timestamp, "w") while e.is_set() == False: if time.time() - tlog > 0.1: log_string = "" log_string += "tool pose " log_string += str( highlevel_movements.get_tcp_pose(base_cyclic)) log_string += " at joints " log_string += str( highlevel_movements.get_joint_angles(base_cyclic)) log_string += "\n" f.write(log_string) print(log_string) tlog = time.time()
def transport_position(): global args global e if not args: args = utilities.parseConnectionArguments() with utilities.DeviceConnection.createTcpConnection( args) as router: #kann das "with" weg? Nein # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) #Moving back to init Pose if functions_.pose_comparison( transport_pos, base_cyclic, "joint") == False: #Robot already in position? success = True success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=pre_transport_pos, speed=speedj) #pre transport safety pose success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=transport_pos, speed=speedj) #transport pose
def idle_upright_pos(): #move into idle upright joint_position global args global e if not args: args = utilities.parseConnectionArguments() with utilities.DeviceConnection.createTcpConnection( args) as router: #kann das "with" weg? Nein # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) if functions_.pose_comparison( transport_pos, base_cyclic, "joint") == True: #Robot in Transport position? success = True success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=pre_transport_pos, speed=speedj) #pre transport safety pose # success &= highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=transport_pos ,speed=speedj) #transport pose success = True success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=idle_pos, speed=speedj)
def main(): # Import the utilities helper module sys.path.insert(0, os.path.join(os.path.dirname(__file__), "..")) import utilities # Parse arguments args = utilities.parseConnectionArguments() # Create connection to the device and get the router with utilities.DeviceConnection.createTcpConnection(args) as router: # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) # Example core success = True success &= example_move_to_home_position(base) success &= example_cartesian_action_movement(base, base_cyclic) success &= example_angular_action_movement(base) success &= example_move_to_home_position(base) success &= example_cartesian_trajectory_movement(base, base_cyclic) success &= example_angular_trajectory_movement(base) return 0 if success else 1
def __init__(self, router, router_real_time): self.expected_number_of_actuators = 7 # example works for 7dof Gen3 # self.torque_amplification = 2.0 # Torque measure on 7th actuator is sent as a command to first actuator self.torque_amplification = 20.0 # Torque measure on 7th actuator is sent as a command to first actuator # Create required services device_manager = DeviceManagerClient(router) self.actuator_config = ActuatorConfigClient(router) self.base = BaseClient(router) self.base_cyclic = BaseCyclicClient(router_real_time) self.base_command = BaseCyclic_pb2.Command() self.base_feedback = BaseCyclic_pb2.Feedback() self.base_custom_data = BaseCyclic_pb2.CustomData() # Detect all devices device_handles = device_manager.ReadAllDevices() self.actuator_count = 0 # Only actuators are relevant for this example for handle in device_handles.device_handle: if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR: self.base_command.actuators.add() self.base_feedback.actuators.add() self.actuator_count += 1 # Change send option to reduce max timeout at 3ms self.sendOption = RouterClientSendOptions() self.sendOption.andForget = False self.sendOption.delay_ms = 0 self.sendOption.timeout_ms = 3 self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite. self.cyclic_thread = {} self.kill_the_thread = False self.already_stopped = False self.cyclic_running = False
def __init__(self, router, router_real_time): # Maximum allowed waiting time during actions (in seconds) self.ACTION_TIMEOUT_DURATION = 20 self.torque_amplification = 2.0 # Torque measure on last actuator is sent as a command to first actuator # Create required services device_manager = DeviceManagerClient(router) self.actuator_config = ActuatorConfigClient(router) self.base = BaseClient(router) self.base_cyclic = BaseCyclicClient(router_real_time) self.base_command = BaseCyclic_pb2.Command() self.base_feedback = BaseCyclic_pb2.Feedback() self.base_custom_data = BaseCyclic_pb2.CustomData() # Detect all devices device_handles = device_manager.ReadAllDevices() self.actuator_count = self.base.GetActuatorCount().count # Only actuators are relevant for this example for handle in device_handles.device_handle: if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR: self.base_command.actuators.add() self.base_feedback.actuators.add() # Change send option to reduce max timeout at 3ms self.sendOption = RouterClientSendOptions() self.sendOption.andForget = False self.sendOption.delay_ms = 0 self.sendOption.timeout_ms = 3 self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite. self.cyclic_thread = {} self.kill_the_thread = False self.already_stopped = False self.cyclic_running = False
def __init__(self, router, router_real_time): self.camera_mtr = None self.world3D_list = [] self.camera3D_list = [] self.camera3D = None self.camera3D_count = -100 self.world3D_count = 0 self.joint_angle_list = None device_manager = DeviceManagerClient(router) self.actuator_config = ActuatorConfigClient(router) self.base = BaseClient(router) self.base_cyclic = BaseCyclicClient(router_real_time) self.base_feedback = BaseCyclic_pb2.Feedback() self.gripper = GripperController(router, router_real_time)
def high_five(base_angle=93.54): global args global e if not args: args = utilities.parseConnectionArguments() with utilities.DeviceConnection.createTcpConnection( args) as router: #kann das "with" weg? Nein # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) temp_speed = 5 high_five_pos[0] = base_angle if functions_.pose_comparison( transport_pos, base_cyclic, "joint") == True: #Robot in Transport position? success = True success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=pre_transport_pos, speed=speedj) #pre transport safety pose # success &= highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=transport_pos ,speed=speedj) #transport pose temp_speed = 10 success = True success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=high_five_pos, speed=speedj) time.sleep(1) elapsed_time.value = -1 torque_tool = functions_.TorqueTool() torque_tool.tap_toggle(base_cyclic, torque_threshold=8) time.sleep(0.1) success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=[ base_angle, 352.44, 355.16, 329.09, 1.48, 38.79, 96.13 ], speed=speedj) success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=high_five_pos, speed=speedj) elapsed_time.value = 0
if __name__ == '__main__': # Parse arguments args = utilities.parseConnectionArguments() # Create connection to the device and get the router # e = multiprocessing.Event() # p = multiprocessing.Process(target=cam, args=('rtsp://192.168.1.10/color',e)) # p.start() with utilities.DeviceConnection.createTcpConnection(args) as router: #open camera #cap = cv2.VideoCapture('rtsp://192.168.1.10/color') #start a video stream process # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) #Start EdgeTPU default_model_dir = './models' #default_model = 'mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite' default_model = 'mobilenet_ssd_v2_face_quant_postprocess_edgetpu.tflite' default_labels = 'coco_labels.txt' parser = argparse.ArgumentParser( formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('-m', '--model', help='File path of .tflite file.', default=os.path.join(default_model_dir, default_model)) parser.add_argument('-l', '--labels',
def main(): target_middle_shared=multiprocessing.Array("i",[0,0]) delta_shared=multiprocessing.Array("i",[0,0]) success_flag_shared=multiprocessing.Value("i",0) vision_middle_shared=multiprocessing.Array("i",[0,0]) h_shared=multiprocessing.Value("i",0) w_shared=multiprocessing.Value("i",0) elapsed_time=multiprocessing.Value("d",0.0) args = utilities.parseConnectionArguments() # Create connection to the device and get the router e = multiprocessing.Event() p = multiprocessing.Process(target=cam, args=('rtsp://192.168.1.10/color',e,target_middle_shared,delta_shared,success_flag_shared,vision_middle_shared,h_shared,w_shared,elapsed_time,beer_time)) p.start() with utilities.DeviceConnection.createTcpConnection(args) as router: # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) device_manager = DeviceManagerClient(router) #Activate the continuous auto-focus of the camera vision_config = VisionConfigClient(router) vision_device_id = vision_action.example_vision_get_device_id(device_manager) # if vision_device_id != 0: # vision_action.autofocus_action(vision_config, vision_device_id,action_id=2) # Initial Movements success = True #Move into transport position (home) home_pose=[184.19,291.7,171.7,213.3,181.8,45.8,266.6] #success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=5) #success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[-0.06,-0.096,0.8,-90.823,171.8,113.73],speed=speedl) # Grab the beer #Desk Sequence # highlevel_movements.send_gripper_command(base,value=0.0) # #success &= highlevel_movements.angular_action_movement(base,joint_positions=[89.695, 336.743, 176.642, 232.288, 180.629, 70.981, 272.165],speed=speedj) # success &= highlevel_movements.angular_action_movement(base,joint_positions=[91.583, 23.663, 174.547, 265.846, 180.949, 35.446, 272.106],speed=speedj) # success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.01796681061387062, -0.6095998287200928, 0.2607220709323883, -89.98168182373047, -176.41896057128906, 178.88327026367188],speed=speedl) # success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.015759950503706932, -0.6483935713768005, 0.2543827295303345, -91.34257507324219, 178.12986755371094, 178.2921905517578],speed=speedl) # # GRIPPER_ACTION # success &= highlevel_movements.send_gripper_command(base,value=0.7) # time.sleep(1) # success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.016505524516105652, -0.65036940574646, 0.38649141788482666, -92.8912353515625, 178.2748565673828, 179.34559631347656],speed=speedl) # success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.07478067278862, -0.1246325895190239, 0.8614432215690613, 90.16726684570312, 9.310687065124512, 5.854083061218262],speed=speedl) # # move to surveillance position #success &= highlevel_movements.angular_action_movement(base,joint_positions=[179.62, 307.879, 172.652, 289.174, 180.408, 69.243, 272.359],speed=speedj) #move to surveillance position #success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[-0.06,-0.096,0.8,-90.823,171.8,113.73],speed=0.1) #Dog Sequence Desired Pose : [249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95] #TODO Startposition [249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95] check #TODO Schwenk ins Publikum nach vorne (extra funktion, muss aufgerufen werden) Tkinter #TODO Winken nur mit oberen zwei Gelenken Tkinter #TODO Theta_y korrigieren #TODO Sicht korrigieren (Fokus und ggf. heller/Dunkler) # success &= highlevel_movements.angular_action_movement(base,joint_positions=[249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95],speed=10) #Dog Pose # sys.exit() success &= highlevel_movements.send_gripper_command(base,value=0) #open gripper success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose success &= highlevel_movements.angular_action_movement(base,joint_positions=[257.64, 246.34, 31.6, 218.63, 214.63, 98.69, 129.78],speed=3) #desired dog pose #success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose success &= highlevel_movements.angular_action_movement(base,joint_positions=[238.78, 264.98, 70.21, 217.99, 349.19, 120.32, 8.3],speed=3) #snake success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.006, -0.536, 0.191, 88.99, 1.239, 7.97],speed=speedl) #GRIPPER_ACTION success &= highlevel_movements.send_gripper_command(base,value=0.4) time.sleep(1.5) success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.008, -0.535, 0.275, 88.988, 1.245, 7.968],speed=speedl) #move a little up #success &= highlevel_movements.angular_action_movement(base,joint_positions=[235.7, 344.71, 64.58, 219.35, 49.72, 80.94, 54.84],speed=speedj) # upright1 #success &= highlevel_movements.angular_action_movement(base,joint_positions=[231.32, 353.1, 32.7, 304.08, 37.1, 321.32, 58.52],speed=speedj) #upright 2 success &= highlevel_movements.angular_action_movement(base,joint_positions=[270.94, 42.99, 348.31, 305.15, 0.84, 286.28, 91.49],speed=speedj) #final #success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.117, -0.058, 0.914, 86.046, 9.61, 6.732],speed=speedl) #final #sys.exit() #search for a face and interrupt when found #vision_action.autofocus_action(vision_config, vision_device_id,action_id=4) success &= highlevel_movements.example_send_joint_speeds(base,speeds=[18, 0, 0, 0, 0, 0, 0],timer=20,success_flag_shared=success_flag_shared) #sys.exit() #Find Face t0=time.time() token=False target_reached_flag=False beer_mode=False print("searching for person") while True: if time.time()-t0>action_time_interval and success_flag_shared.value==True and beer_mode==False and p.is_alive(): #calculate the pose increment and move robot into target target_reached_flag=functions_.rotate_to_target( delta_shared[:],target_middle_shared[:],vision_middle_shared[:],w_shared.value,h_shared.value,max_speed,min_speed, target_circle_size,base,base_cyclic) t0=time.time() elif time.time()-t0>action_time_interval and success_flag_shared.value==False and beer_mode==False: base.Stop() # if time.time()-t0>2: # vision_action.autofocus_action(vision_config, vision_device_id,action_id=6,focus_point=target_middle_shared[:]) elif p.is_alive()==False: base.Stop() print("Process aborded") sys.exit() #after robot is Xs in target, hand over beer if (target_reached_flag==True and token==True and success_flag_shared.value==True) or beer_mode==True: elapsed_time.value=time.time()-beer_timer if elapsed_time.value>beer_time: if beer_mode==False: reach_beer_twist_timer=time.time() beer_mode=True #the target was long enough in scope tool_twist_duration=6 if time.time()-reach_beer_twist_timer<tool_twist_duration: highlevel_movements.tool_twist_time(base,tool_twist_duration,pose_distance=[0,-0.35,0.4,0,0,0]) else: base.Stop() time.sleep(2) #swing out #wait until force applied to robot torque_tool=functions_.TorqueTool() torque_tool.tap_toggle(base_cyclic,torque_threshold=5) #open gripper highlevel_movements.send_gripper_command(base,value=0.0) time.sleep(2) home_pose=[184.19,291.7,171.7,213.3,181.8,45.8,266.6] #Desk #success &= highlevel_movements.angular_action_movement(base,joint_positions=[267.038, 239.612, 177.453, 212.171, 185.765, 56.095, 269.943],speed=speedj) highlevel_movements.tool_twist_time(base,tool_twist_duration,pose_distance=[0,-0.35,0.4,0,0,0]) #Wave Sequence wave_start_pose=[180, 0, 0, 0, 82, 90, 270] wave_left_pose=list(map(add,wave_start_pose,[0,0,0,30,0,0,-30])) wave_time=2 #seconds #wave_left_pose=list(map(add,wave_start_pose,[0,-30,0,-30,0,0,0])) highlevel_movements.angular_action_movement(base,joint_positions=wave_start_pose,speed=4) #while True: highlevel_movements.angular_action_movement(base,joint_positions=wave_left_pose,speed=2.5) #okay highlevel_movements.angular_action_movement(base,joint_positions=[180, 0, 0, 330, 82, 90, 300],speed=4.5) highlevel_movements.angular_action_movement(base,joint_positions=wave_left_pose,speed=4.5) highlevel_movements.angular_action_movement(base,joint_positions=[180, 0, 0, 330, 82, 90, 300],speed=4.5) highlevel_movements.angular_action_movement(base,joint_positions=wave_start_pose,speed=2.5) #sys.exit() #Moving back to init Pose success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose success &= highlevel_movements.angular_action_movement(base,joint_positions=[257.64, 246.34, 31.6, 218.63, 214.63, 98.69, 129.78],speed=3) #desired dog pose #success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=speedj) e.set() #kill the camera process break elif target_reached_flag==True and success_flag_shared.value==True: beer_timer=time.time() token=True else: token=False elapsed_time.value=0
class TorqueExample: def __init__(self, router, router_real_time): self.expected_number_of_actuators = 7 # example works for 7dof Gen3 # self.torque_amplification = 2.0 # Torque measure on 7th actuator is sent as a command to first actuator self.torque_amplification = 20.0 # Torque measure on 7th actuator is sent as a command to first actuator # Create required services device_manager = DeviceManagerClient(router) self.actuator_config = ActuatorConfigClient(router) self.base = BaseClient(router) self.base_cyclic = BaseCyclicClient(router_real_time) self.base_command = BaseCyclic_pb2.Command() self.base_feedback = BaseCyclic_pb2.Feedback() self.base_custom_data = BaseCyclic_pb2.CustomData() # Detect all devices device_handles = device_manager.ReadAllDevices() self.actuator_count = 0 # Only actuators are relevant for this example for handle in device_handles.device_handle: if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR: self.base_command.actuators.add() self.base_feedback.actuators.add() self.actuator_count += 1 # Change send option to reduce max timeout at 3ms self.sendOption = RouterClientSendOptions() self.sendOption.andForget = False self.sendOption.delay_ms = 0 self.sendOption.timeout_ms = 3 self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite. self.cyclic_thread = {} self.kill_the_thread = False self.already_stopped = False self.cyclic_running = False def MoveToHomePosition(self): # Make sure the arm is in Single Level Servoing mode base_servo_mode = Base_pb2.ServoingModeInformation() base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING self.base.SetServoingMode(base_servo_mode) # Move arm to ready position print("Moving the arm to a safe position") action_type = Base_pb2.RequestedActionType() action_type.action_type = Base_pb2.REACH_JOINT_ANGLES action_list = self.base.ReadAllActions(action_type) action_handle = None for action in action_list.action_list: if action.name == "Home": action_handle = action.handle if action_handle == None: print("Can't reach safe position. Exiting") return False self.base.ExecuteActionFromReference(action_handle) time.sleep(20) # Leave time to action to complete return True def InitCyclic(self, sampling_time_cyclic, t_end, print_stats): if self.cyclic_running: return True # Move to Home position first if not self.MoveToHomePosition(): return False print("Init Cyclic") sys.stdout.flush() base_feedback = self.SendCallWithRetry(self.base_cyclic.RefreshFeedback, 3) if base_feedback: self.base_feedback = base_feedback if len(self.base_feedback.actuators) == self.expected_number_of_actuators: # Init command frame for x in range(self.actuator_count): self.base_command.actuators[x].flags = 1 # servoing self.base_command.actuators[x].position = self.base_feedback.actuators[x].position # First actuator is going to be controlled in torque # To ensure continuity, torque command is set to measure self.base_command.actuators[0].torque_joint = self.base_feedback.actuators[0].torque # Set arm in LOW_LEVEL_SERVOING base_servo_mode = Base_pb2.ServoingModeInformation() base_servo_mode.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING self.base.SetServoingMode(base_servo_mode) # Send first frame self.base_feedback = self.base_cyclic.Refresh(self.base_command, 0, self.sendOption) # Set first actuator in torque mode now that the command is equal to measure control_mode_message = ActuatorConfig_pb2.ControlModeInformation() control_mode_message.control_mode = ActuatorConfig_pb2.ControlMode.Value('TORQUE') device_id = 1 # first actuator as id = 1 self.SendCallWithRetry(self.actuator_config.SetControlMode, 3, control_mode_message, device_id) # Init cyclic thread self.cyclic_t_end = t_end self.cyclic_thread = threading.Thread(target=self.RunCyclic, args=(sampling_time_cyclic, print_stats)) self.cyclic_thread.daemon = True self.cyclic_thread.start() return True else: print("InitCyclic: number of actuators in base_feedback does not match expected number") return False else: print("InitCyclic: failed to communicate") return False def RunCyclic(self, t_sample, print_stats): self.cyclic_running = True print("Run Cyclic") sys.stdout.flush() cyclic_count = 0 # Counts refresh stats_count = 0 # Counts stats prints failed_cyclic_count = 0 # Count communication timeouts # Initial delta between first and last actuator init_delta_position = self.base_feedback.actuators[0].position - self.base_feedback.actuators[6].position # Initial first and last actuator torques; avoids unexpected movement due to torque offsets init_last_torque = self.base_feedback.actuators[6].torque init_first_torque = -self.base_feedback.actuators[0].torque # Torque measure is reversed compared to actuator direction t_now = time.time() t_cyclic = t_now # cyclic time t_stats = t_now # print time t_init = t_now # init time print("Running torque control example for {} seconds".format(self.cyclic_t_end)) while not self.kill_the_thread: t_now = time.time() # Cyclic Refresh if (t_now - t_cyclic) >= t_sample: t_cyclic = t_now # Position command to first actuator is set to measured one to avoid following error to trigger # Bonus: When doing this instead of disabling the following error, if communication is lost and first # actuator continue to move under torque command, resulting position error with command will # trigger a following error and switch back the actuator in position command to hold its position self.base_command.actuators[0].position = self.base_feedback.actuators[0].position # First actuator torque command is set to last actuator torque measure times an amplification # self.base_command.actuators[0].torque_joint = init_first_torque + \ # self.torque_amplification * (self.base_feedback.actuators[6].torque - init_last_torque) self.base_command.actuators[0].torque_joint = 5.0 print(self.base_command.actuators[0].torque_joint) # First actuator position is sent as a command to last actuator self.base_command.actuators[6].position = self.base_feedback.actuators[0].position - init_delta_position # Incrementing identifier ensure actuators can reject out of time frames self.base_command.frame_id += 1 if self.base_command.frame_id > 65535: self.base_command.frame_id = 0 for i in range(self.expected_number_of_actuators): self.base_command.actuators[i].command_id = self.base_command.frame_id # Frame is sent try: self.base_feedback = self.base_cyclic.Refresh(self.base_command, 0, self.sendOption) except: failed_cyclic_count = failed_cyclic_count + 1 print("failed") cyclic_count = cyclic_count + 1 # Stats Print if print_stats and ((t_now - t_stats) > 1): t_stats = t_now stats_count = stats_count + 1 cyclic_count = 0 failed_cyclic_count = 0 sys.stdout.flush() if self.cyclic_t_end != 0 and (t_now - t_init > self.cyclic_t_end): print("Cyclic Finished") sys.stdout.flush() break self.cyclic_running = False return True def StopCyclic(self): print ("Stopping the cyclic and putting the arm back in position mode...") if self.already_stopped: return # Kill the thread first if self.cyclic_running: self.kill_the_thread = True self.cyclic_thread.join() # Set first actuator back in position mode control_mode_message = ActuatorConfig_pb2.ControlModeInformation() control_mode_message.control_mode = ActuatorConfig_pb2.ControlMode.Value('POSITION') device_id = 1 # first actuator has id = 1 self.SendCallWithRetry(self.actuator_config.SetControlMode, 3, control_mode_message, device_id) base_servo_mode = Base_pb2.ServoingModeInformation() base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING self.base.SetServoingMode(base_servo_mode) self.cyclic_t_end = 0.1 self.already_stopped = True print('Clean Exit') @staticmethod def SendCallWithRetry(call, retry, *args): i = 0 arg_out = [] while i < retry: try: arg_out = call(*args) break except: i = i + 1 continue if i == retry: print("Failed to communicate") return arg_out
def main(): # Create connection to the device and get the router global args global e if not args: args = utilities.parseConnectionArguments() with utilities.DeviceConnection.createTcpConnection( args) as router: #kann das "with" weg? Nein # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) device_manager = DeviceManagerClient(router) #Activate the continuous auto-focus of the camera vision_config = VisionConfigClient(router) vision_device_id = vision_action.example_vision_get_device_id( device_manager) # if vision_device_id != 0: # vision_action.autofocus_action(vision_config, vision_device_id,action_id=2) # Initial Movements success = True success &= highlevel_movements.send_gripper_command( base, value=0) #open gripper if functions_.pose_comparison( transport_pos, base_cyclic, "joint") == True: #Robot already in position? success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=pre_transport_pos, speed=speedj) #pre transport pose temp_speed = 3 else: temp_speed = 10 #robot further away so give more time success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=[ 231.4, 266.21, 67.14, 216.41, 346.65, 114.31, 10.59 ], speed=speedj) #snake time.sleep(0.4) success &= highlevel_movements.move_to_waypoint_linear( e, base, base_cyclic, [0.02, -0.511, 0.184, 90.689, 3.832, 4.054], speed=3) #pre bottle success &= highlevel_movements.move_to_waypoint_linear( e, base, base_cyclic, [0.023, -0.539, 0.184, 90.687, 3.828, 4.049], speed=3) #bottle success &= highlevel_movements.send_gripper_command(base, value=0.8) #time.sleep(2) success &= highlevel_movements.move_to_waypoint_linear( e, base, base_cyclic, [0.032, -0.553, 0.335, 90.691, 3.831, 4.048], speed=3) #move bottle up success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=[ 270.94, 42.99, 348.31, 305.15, 0.84, 286.28, 91.49 ], speed=speedj) #final time.sleep(0.5) #INSERT Focus on target_pose #search for a face and interrupt when found success &= highlevel_movements.example_send_joint_speeds( e, base, speeds=[12, 0, 0, 0, 0, 0, 0], timer=0) #robot rotates until face found while success_flag_shared.value == False and e.is_set( ) == False: #success_flag_shared gets True when face was detected time.sleep(0.05) base.Stop() #Follow Face t0 = time.time() token = False # plays a role when beer has to be handed over after target was in lock for some time target_reached_flag = False beer_mode = False #hand over beer print("searching for person") while e.is_set() == False: if time.time( ) - t0 > action_time_interval and success_flag_shared.value == True and beer_mode == False: #calculate the pose increment and rotate robot into target target_reached_flag = functions_.rotate_to_target( delta_shared[:], w_shared.value, h_shared.value, max_speed, min_speed, target_circle_size, base, base_cyclic) t0 = time.time() elif time.time( ) - t0 > action_time_interval and success_flag_shared.value == False and beer_mode == False: #no target in sight base.Stop() # if no target was detected, search with base rotation if time.time() - t0 > 3: #search for a face and interrupt when found highlevel_movements.example_send_joint_speeds( e, base, speeds=[12, 0, 0, 0, 0, 0, 0], timer=0) #robot rotates until face found while success_flag_shared.value == False and e.is_set( ) == False: #success_flag_shared gets True when face was detected time.sleep(0.05) base.Stop() t0 = time.time() #after robot is Xs in target, hand over beer if (target_reached_flag == True and token == True and success_flag_shared.value == True) or beer_mode == True: elapsed_time.value = time.time() - beer_timer if elapsed_time.value > beer_time: if beer_mode == False: reach_beer_twist_timer = time.time() beer_mode = True #the target was long enough in scope tool_twist_duration = 6 if time.time( ) - reach_beer_twist_timer < tool_twist_duration: highlevel_movements.tool_twist_time( e, base, tool_twist_duration, pose_distance=beer_pass_dist) else: base.Stop() time.sleep(2) #swing out #wait until force applied to robot torque_tool = functions_.TorqueTool() torque_tool.tap_toggle(base_cyclic, torque_threshold=5) #open gripper highlevel_movements.send_gripper_command(base, value=0.0) time.sleep(2) #move into idle upright joint_position #success &= highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=[87.58, 78.68, 353.68, 216.49, 4.0, 336.91, 89.14],speed=speedj) print("beer was passed") break elif target_reached_flag == True and success_flag_shared.value == True: beer_timer = time.time() token = True else: token = False elapsed_time.value = 0 if e.is_set() == True or no_high_five == True: return else: # move robot up so he is on face eight again rev_beer_pass_dist = [i * (-1) for i in beer_pass_dist] tool_twist_duration = 6 # highlevel_movements.tool_twist_time(e,base,tool_twist_duration,pose_distance=beer_pass_dist) highlevel_movements.tool_twist_time( e, base, tool_twist_duration, pose_distance=rev_beer_pass_dist) # print("Done") #sys.exit() # follow a little then high five to person t0 = time.time() t0_wave = time.time() time_to_wave = False while e.is_set() == False: if time.time( ) - t0 > action_time_interval and success_flag_shared.value == True and time_to_wave == False: target_reached_flag = functions_.rotate_to_target( delta_shared[:], target_middle_shared[:], vision_middle_shared[:], w_shared.value, h_shared.value, max_speed, min_speed, target_circle_size, base, base_cyclic) t0 = time.time() elif time.time( ) - t0 > action_time_interval and success_flag_shared.value == False and time_to_wave == False: base.Stop() if time.time() - t0_wave > 5: time_to_wave = True base.Stop() time.sleep(0.5) break if time.time() - t0_wave > 5: elapsed_time.value = -1 print("time to high five!") #read the joint angles to get the base angle pose = highlevel_movements.get_joint_angles(base_cyclic) base_angle = pose[0] #enter high five position with base rotation # high_five(base_angle=base_angle) high_five_pos[0] = base_angle success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=high_five_pos, speed=speedj) torque_tool = functions_.TorqueTool() torque_tool.tap_toggle(base_cyclic, torque_threshold=12) time.sleep(0.1) success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=[ base_angle, 352.44, 355.16, 329.09, 1.48, 38.79, 96.13 ], speed=speedj) success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=high_five_pos, speed=speedj) elapsed_time.value = -2 # transport_position() success = True success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=pre_transport_pos, speed=speedj) #pre transport safety pose success &= highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=transport_pos, speed=speedj) #transport pose #process finished base.Stop()
class KinovaRobotEnv: metadata = {'render_modes':['human', 'rbg_array']} def __init__(self, action_scale=0.05, target_range=0.15, distance_threshold=0.05, speed=0.10): self.connection = utilities.DeviceConnection.createTcpConnection() self.router = self.connection.__enter__() self.base = BaseClient(self.router) self.base_cyclic = BaseCyclicClient(self.router) self.webcam = cv2.VideoCapture(0) self.action_scale = action_scale self.target_range = target_range self.distance_threshold = distance_threshold self.speed = speed self.observation_space = gym.spaces.Dict( observation=gym.spaces.Box(-np.inf, np.inf, self._get_obs().shape), achieved_goal=gym.spaces.Box(-np.inf, np.inf, (3,)), desired_goal=gym.spaces.Box(-np.inf, np.inf, (3,)), ) self.action_space = gym.spaces.Box(-1., 1., (3,)) self.reward_range = (-np.inf, 0) self.unwrapped = self self.spec = None self._set_to_home() self.init_xyz = self._get_obs()[:3] self.goal = None self.sample_goal() assert self.goal is not None def seed(self, seed): pass def close(self): self.connection.__exit__(None, None, None) def render(self, mode='human', width=84, height=84): # return np.zeros((width, height, 3)) ret, frame = self.webcam.read() frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) if mode == 'human': cv2.imshow('', frame) elif mode == 'rgb_array': frame = cv2.resize(frame, (width, height)) return frame def sample_goal(self, target_range=None): if target_range is None: target_range = self.target_range self.goal = self.init_xyz + (-1 + 2*np.random.rand(*self.init_xyz.shape))*self.target_range return self.goal def reset(self): self.sample_goal() self._set_to_home() return self._get_obs_dict() def step(self, act): print(f'act: {act}') act = act.copy()*self.action_scale action = Base_pb2.Action() action.name = "Example Cartesian action movement" action.application_data = "" action.reach_pose.constraint.speed.translation = self.speed feedback = self.base_cyclic.RefreshFeedback() cartesian_pose = action.reach_pose.target_pose cartesian_pose.x = feedback.base.tool_pose_x + act[0] # (meters) cartesian_pose.y = feedback.base.tool_pose_y + act[1] # (meters) cartesian_pose.z = feedback.base.tool_pose_z + act[2] # (meters) cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees) cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees) cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees) self.base.ExecuteAction(action) duration = 1. + (np.linalg.norm(act)/self.speed) print(f'step w duration {duration}') time.sleep(duration) obs_dict = self._get_obs_dict() info = dict() reward = self.compute_reward( obs_dict['achieved_goal'], obs_dict['desired_goal'], info, ) done = True if np.abs(reward) < self.distance_threshold else False info['is_success'] = done return obs_dict, reward, done, info def _set_to(self, xyz): action = Base_pb2.Action() action.name = "Example Cartesian action movement" action.application_data = "" action.reach_pose.constraint.speed.translation = self.speed feedback = self.base_cyclic.RefreshFeedback() current_pos = np.asarray([feedback.base.tool_pose_x, feedback.base.tool_pose_y, feedback.base.tool_pose_z]) distance_to_xyz = np.linalg.norm(current_pos - xyz) duration = 1. + (distance_to_xyz/self.speed) print(f'_set_to w duration {duration}') cartesian_pose = action.reach_pose.target_pose cartesian_pose.x = xyz[0] cartesian_pose.y = xyz[1] cartesian_pose.z = xyz[2] cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees) cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees) cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees) self.base.ExecuteAction(action) time.sleep(duration) def _set_to_home(self): # Make sure the arm is in Single Level Servoing mode base_servo_mode = Base_pb2.ServoingModeInformation() base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING self.base.SetServoingMode(base_servo_mode) # Move arm to ready position print("Moving the arm to a safe position") action_type = Base_pb2.RequestedActionType() action_type.action_type = Base_pb2.REACH_JOINT_ANGLES action_list = self.base.ReadAllActions(action_type) action_handle = None for action in action_list.action_list: if action.name == "Home": action_handle = action.handle if action_handle == None: raise Run("Can't reach safe position. Exiting") self.base.ExecuteActionFromReference(action_handle) print('Resetting for duration 6') time.sleep(6) def compute_reward(self, achieved_goal, desired_goal, info): d = np.linalg.norm(achieved_goal - desired_goal, axis=-1) return 0. if d < self.distance_threshold else -1. def _is_success(self, achieved_goal, desired_goal): d = np.linalg.norm(achieved_goal - desired_goal, axis=-1) return True if d < self.distance_threshold else False def _get_obs(self): feedback = self.base_cyclic.RefreshFeedback() observation = np.asarray([ feedback.base.tool_pose_x, feedback.base.tool_pose_y, feedback.base.tool_pose_z, ]) return observation def _get_obs_dict(self): observation = self._get_obs() achieved_goal = observation[:3] desired_goal = self.goal return dict( observation=observation, achieved_goal=achieved_goal, desired_goal=desired_goal, )
def wave(): elapsed_time.value = 0 global e global args if not args: args = utilities.parseConnectionArguments() with utilities.DeviceConnection.createTcpConnection( args) as router: #kann das "with" weg? Nein # Create required services base = BaseClient(router) base_cyclic = BaseCyclicClient(router) device_manager = DeviceManagerClient(router) #Activate the continuous auto-focus of the camera vision_config = VisionConfigClient(router) vision_device_id = vision_action.example_vision_get_device_id( device_manager) # if vision_device_id != 0: # vision_action.autofocus_action(vision_config, vision_device_id,action_id=2) # check if robot is not in transport position, if so, move safely if functions_.pose_comparison( transport_pos, base_cyclic, "joint") == True: #Robot in transport position? highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=pre_transport_pos, speed=speedj) #pre transport safety pose #Wave Sequence wave_speed = 70 #degrees/sec # wave_start_pose=[180, 0, 0, 0, 90, 90, 270] #joint4 # wave_left_pose=list(map(add,wave_start_pose,[0,0,0,30,0,0,-30])) #joint4 # wave_start_pose=[180, 0, 0, 0, 0, 0, 0] #joint5 # wave_left_pose=list(map(add,wave_start_pose,[0,0,0,0,0,40,0])) #joint4 # wave_start_pose=[182.49, 36.45, 0.78, 269.14, 2.33, 56.02, 0.11] #joint5 # wave_left_pose=list(map(add,wave_start_pose,[0,0,0,0,0,40,0])) #joint4 wave_start_pose = [182.9, 320.18, 177.58, 260.17, 1.06, 65.48, 180.64] #joint5 wave_left_pose = list(map(add, wave_start_pose, [0, 0, 0, 0, 0, 40, 0])) #joint4 # highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=wave_start_pose,speed=speedj) # sys.exit() highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=wave_left_pose, speed=speedj) #okay #time.sleep(1) highlevel_movements.sinus_move(e, v_max=wave_speed, degrees_move=60, joint_id=5, direction=1, base=base, base_cyclic=base_cyclic) time.sleep(0.4) #must be because else meassurements are wrong highlevel_movements.sinus_move(e, v_max=wave_speed, degrees_move=60, joint_id=5, direction=-1, base=base, base_cyclic=base_cyclic) time.sleep(0.4) #must be because else meassurements are wrong highlevel_movements.sinus_move(e, v_max=wave_speed, degrees_move=60, joint_id=5, direction=1, base=base, base_cyclic=base_cyclic) time.sleep(0.4) #must be because else meassurements are wrong highlevel_movements.sinus_move(e, v_max=wave_speed, degrees_move=60, joint_id=5, direction=-1, base=base, base_cyclic=base_cyclic) # time.sleep(0.4) # highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=1,base=base,base_cyclic=base_cyclic) # time.sleep(0.4) #must be because else meassurements are wrong # highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=-1,base=base,base_cyclic=base_cyclic) # time.sleep(0.4) #must be because else meassurements are wrong # highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=1,base=base,base_cyclic=base_cyclic) # time.sleep(0.4) #must be because else meassurements are wrong # highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=-1,base=base,base_cyclic=base_cyclic) elapsed_time.value = -2 time.sleep(0.4) highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=pre_transport_pos, speed=speedj) #pre transport safety pose highlevel_movements.angular_action_movement( e, base, base_cyclic, joint_positions=transport_pos, speed=speedj) #transport pose
def __init__(self, router, router_real_time, proportional_gain=2.0): """ GripperLowLevelExample class constructor. Inputs: kortex_api.RouterClient router: TCP router kortex_api.RouterClient router_real_time: Real-time UDP router float proportional_gain: Proportional gain used in control loop (default value is 2.0) Outputs: None Notes: - Actuators and gripper initial position are retrieved to set initial positions - Actuator and gripper cyclic command objects are created in constructor. Their references are used to update position and speed. """ self.proportional_gain = proportional_gain ########################################################################################### # UDP and TCP sessions are used in this example. # TCP is used to perform the change of servoing mode # UDP is used for cyclic commands. # # 2 sessions have to be created: 1 for TCP and 1 for UDP ########################################################################################### self.router = router self.router_real_time = router_real_time # Create base client using TCP router self.base = BaseClient(self.router) # Create base cyclic client using UDP router. self.base_cyclic = BaseCyclicClient(self.router_real_time) # Create base cyclic command object. self.base_command = BaseCyclic_pb2.Command() self.base_command.frame_id = 0 self.base_command.interconnect.command_id.identifier = 0 self.base_command.interconnect.gripper_command.command_id.identifier = 0 # Add motor command to interconnect's cyclic self.motorcmd = self.base_command.interconnect.gripper_command.motor_cmd.add( ) # Set gripper's initial position velocity and force base_feedback = self.base_cyclic.RefreshFeedback() self.motorcmd.position = base_feedback.interconnect.gripper_feedback.motor[ 0].position self.motorcmd.velocity = 0 self.motorcmd.force = 100 for actuator in base_feedback.actuators: self.actuator_command = self.base_command.actuators.add() self.actuator_command.position = actuator.position self.actuator_command.velocity = 0.0 self.actuator_command.torque_joint = 0.0 self.actuator_command.command_id = 0 print("Position = ", actuator.position) # Save servoing mode before changing it self.previous_servoing_mode = self.base.GetServoingMode() # Set base in low level servoing mode servoing_mode_info = Base_pb2.ServoingModeInformation() servoing_mode_info.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING self.base.SetServoingMode(servoing_mode_info)
class GripperLowLevelExample: def __init__(self, router, router_real_time, proportional_gain=2.0): """ GripperLowLevelExample class constructor. Inputs: kortex_api.RouterClient router: TCP router kortex_api.RouterClient router_real_time: Real-time UDP router float proportional_gain: Proportional gain used in control loop (default value is 2.0) Outputs: None Notes: - Actuators and gripper initial position are retrieved to set initial positions - Actuator and gripper cyclic command objects are created in constructor. Their references are used to update position and speed. """ self.proportional_gain = proportional_gain ########################################################################################### # UDP and TCP sessions are used in this example. # TCP is used to perform the change of servoing mode # UDP is used for cyclic commands. # # 2 sessions have to be created: 1 for TCP and 1 for UDP ########################################################################################### self.router = router self.router_real_time = router_real_time # Create base client using TCP router self.base = BaseClient(self.router) # Create base cyclic client using UDP router. self.base_cyclic = BaseCyclicClient(self.router_real_time) # Create base cyclic command object. self.base_command = BaseCyclic_pb2.Command() self.base_command.frame_id = 0 self.base_command.interconnect.command_id.identifier = 0 self.base_command.interconnect.gripper_command.command_id.identifier = 0 # Add motor command to interconnect's cyclic self.motorcmd = self.base_command.interconnect.gripper_command.motor_cmd.add( ) # Set gripper's initial position velocity and force base_feedback = self.base_cyclic.RefreshFeedback() self.motorcmd.position = base_feedback.interconnect.gripper_feedback.motor[ 0].position self.motorcmd.velocity = 0 self.motorcmd.force = 100 for actuator in base_feedback.actuators: self.actuator_command = self.base_command.actuators.add() self.actuator_command.position = actuator.position self.actuator_command.velocity = 0.0 self.actuator_command.torque_joint = 0.0 self.actuator_command.command_id = 0 print("Position = ", actuator.position) # Save servoing mode before changing it self.previous_servoing_mode = self.base.GetServoingMode() # Set base in low level servoing mode servoing_mode_info = Base_pb2.ServoingModeInformation() servoing_mode_info.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING self.base.SetServoingMode(servoing_mode_info) def Cleanup(self): """ Restore arm's servoing mode to the one that was effective before running the example. Inputs: None Outputs: None Notes: None """ # Restore servoing mode to the one that was in use before running the example self.base.SetServoingMode(self.previous_servoing_mode) def Goto(self, target_position): """ Position gripper to a requested target position using a simple proportional feedback loop which changes speed according to error between target position and current gripper position Inputs: float target_position: position (0% - 100%) to send gripper to. Outputs: Returns True if gripper was positionned successfully, returns False otherwise. Notes: - This function blocks until position is reached. - If target position exceeds 100.0, its value is changed to 100.0. - If target position is below 0.0, its value is set to 0.0. """ if target_position > 100.0: target_position = 100.0 if target_position < 0.0: target_position = 0.0 while True: try: base_feedback = self.base_cyclic.Refresh(self.base_command) # Calculate speed according to position error (target position VS current position) position_error = target_position - base_feedback.interconnect.gripper_feedback.motor[ 0].position # If positional error is small, stop gripper if abs(position_error) < 1.5: position_error = 0 self.motorcmd.velocity = 0 self.base_cyclic.Refresh(self.base_command) return True else: self.motorcmd.velocity = self.proportional_gain * abs( position_error) if self.motorcmd.velocity > 100.0: self.motorcmd.velocity = 100.0 self.motorcmd.position = 100.0 if position_error < 0.0: self.motorcmd.position = 0.0 except Exception as e: print("Error in refresh: " + str(e)) return False time.sleep(0.001) return True