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 __init__(self, router, proportional_gain=2.0): self.proportional_gain = proportional_gain self.router = router # Create base client using TCP router self.base = BaseClient(self.router)
class KinovaController: 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 set_single_level_servoing_mode(self): # Save servoing mode before changing it self.previous_servoing_mode = self.base.GetServoingMode() # Set base in single level servoing mode servoing_mode_info = Base_pb2.ServoingModeInformation() # print("Setting single level servoi print("Current Mode:") print(servoing_mode_info) servoing_mode_info.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING self.base.SetServoingMode(servoing_mode_info) print("Waiting 5 second for changing to single level servoing mode...") time.sleep(5) print("After setting single level") print(servoing_mode_info) def move_arm(self, pos_list): self.set_single_level_servoing_mode() # for count, joint_angle in enumerate(self.joint_angle_list): for count, pos in enumerate(pos_list): print("Moving to ", pos) cartesian_action_movement(self.base, pos) self.base_feedback = self.SendCallWithRetry( self.base_cyclic.RefreshFeedback, 3) joint_angles = [ self.base_feedback.actuators[i].position for i in range(7) ] @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 stop_movements(): e.set() #kill movement processes time.sleep(0.2) 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.Stop()
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 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 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 __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 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 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 __init__(self, router, ip_address): self.router = router self.base_ip_address = ip_address # Create services self.base = BaseClient(self.router) self.device_manager = DeviceManagerClient(self.router) self.interconnect_config = InterconnectConfigClient(self.router) self.interconnect_device_id = self.GetDeviceIdFromDevType( Common_pb2.INTERCONNECT, 0) if (self.interconnect_device_id is None): print( "Could not find the Interconnect in the device list, exiting..." ) sys.exit(0)
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) # Example core # Move to initial position move_to_home_position(base) # Move without the protection zone print_protection_zones(base) move_in_front_of_protection_zone(base) move_to_protection_zone(base) move_to_home_position(base) # Move with the protection zone print_protection_zones(base) move_in_front_of_protection_zone(base) # Add the protection zone handle = create_protection_zone(base) move_to_protection_zone(base) move_to_home_position(base) # Delete the protection zone base.DeleteProtectionZone(handle) # Print final protection zones # The example protection zone should be removed print_protection_zones(base)
def example_api_creation(args): ''' This function creates all required objects and connections to use the arm's services. It is easier to use the DeviceConnection utility class to create the router and then create the services you need (as done in the other examples). However, if you want to create all objects yourself, this function tells you how to do it. ''' PORT = 10000 # Setup API error_callback = lambda kException: print( "_________ callback error _________ {}".format(kException)) transport = TCPTransport() router = RouterClient(transport, error_callback) transport.connect(args.ip, PORT) # Create session session_info = Session_pb2.CreateSessionInfo() session_info.username = args.username session_info.password = args.password session_info.session_inactivity_timeout = 60000 # (milliseconds) session_info.connection_inactivity_timeout = 2000 # (milliseconds) print("Creating session for communication") session_manager = SessionManager(router) session_manager.CreateSession(session_info) print("Session created") # Create required services device_config = DeviceConfigClient(router) base = BaseClient(router) print(device_config.GetDeviceType()) print(base.GetArmState()) # Close API session session_manager.CloseSession() # Disconnect from the transport object transport.disconnect()
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 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) # Example core example_call_rpc_using_options(base)
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
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) # Example core success = True success &= example_forward_kinematics(base) success &= example_inverse_kinematics(base) return 0 if success else 1
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',
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
transport.connect(DEVICE_IP, DEVICE_PORT) # Create session print("\nCreating session for communication") session_info = Session_pb2.CreateSessionInfo() session_info.username = '******' session_info.password = '******' session_info.session_inactivity_timeout = 60000 # (milliseconds) session_info.connection_inactivity_timeout = 2000 # (milliseconds) print("Session created") session_manager = SessionManager(router) session_manager.CreateSession(session_info) # Create required services base_client_service = BaseClient(router) device_manager_service = DeviceManagerClient(router) # Find the number of actuator in angular action and trajectory sub_device_info = device_manager_service.ReadAllDevices() act_count = 0 for dev in sub_device_info.device_handle: if dev.device_type is Common_pb2.BIG_ACTUATOR or dev.device_type is Common_pb2.SMALL_ACTUATOR: act_count += 1 # Move arm to ready position print("\nMoving the arm to a safe position before executing example") action_type = Base_pb2.RequestedActionType() action_type.action_type = Base_pb2.REACH_JOINT_ANGLES action_list = base_client_service.ReadAllActions(action_type) action_handle = None
class GripperCommandExample: def __init__(self, router, proportional_gain=2.0): self.proportional_gain = proportional_gain self.router = router # Create base client using TCP router self.base = BaseClient(self.router) def ExampleSendGripperCommands(self): # Create the GripperCommand we will send gripper_command = Base_pb2.GripperCommand() finger = gripper_command.gripper.finger.add() # Close the gripper with position increments print("Performing gripper test in position...") gripper_command.mode = Base_pb2.GRIPPER_POSITION position = 0.00 finger.finger_identifier = 1 while position < 1.0: finger.value = position self.base.SendGripperCommand(gripper_command) position += 0.1 time.sleep(1) # Set speed to open gripper print("Opening gripper using speed command...") gripper_command.mode = Base_pb2.GRIPPER_SPEED finger.value = 0.1 self.base.SendGripperCommand(gripper_command) gripper_request = Base_pb2.GripperRequest() # Wait for reported position to be opened gripper_request.mode = Base_pb2.GRIPPER_POSITION while True: gripper_measure = self.base.GetMeasuredGripperMovement( gripper_request) if len(gripper_measure.finger): print("Current position is : {0}".format( gripper_measure.finger[0].value)) if gripper_measure.finger[0].value < 0.01: break else: # Else, no finger present in answer, end loop break # Set speed to close gripper print("Closing gripper using speed command...") gripper_command.mode = Base_pb2.GRIPPER_SPEED finger.value = -0.1 self.base.SendGripperCommand(gripper_command) # Wait for reported speed to be 0 gripper_request.mode = Base_pb2.GRIPPER_SPEED while True: gripper_measure = self.base.GetMeasuredGripperMovement( gripper_request) if len(gripper_measure.finger): print("Current speed is : {0}".format( gripper_measure.finger[0].value)) if gripper_measure.finger[0].value == 0.0: break else: # Else, no finger present in answer, end loop break
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
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()
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)
router.SetActivationStatus(False) transport.disconnect() DEVICE_IP = "192.168.1.10" DEVICE_PORT = 10000 # Setup API errorCallback = lambda kException: print( "_________ callback error _________ {}".format(kException)) transport = UDPTransport() router = RouterClient(transport, errorCallback) transport.connect(DEVICE_IP, DEVICE_PORT) # Create session session_info = Session_pb2.CreateSessionInfo() session_info.username = '******' session_info.password = '******' session_info.session_inactivity_timeout = 60000 # (milliseconds) session_info.connection_inactivity_timeout = 2000 # (milliseconds) session_manager = SessionManager(router) session_manager.CreateSession(session_info) # Create required services # Create required services device_manager_service = DeviceManagerClient(router) vision_config_service = VisionConfigClient(router) base_client_service = BaseClient(router)
class UARTBridge: def __init__(self, router, ip_address): self.router = router self.base_ip_address = ip_address # Create services self.base = BaseClient(self.router) self.device_manager = DeviceManagerClient(self.router) self.interconnect_config = InterconnectConfigClient(self.router) self.interconnect_device_id = self.GetDeviceIdFromDevType( Common_pb2.INTERCONNECT, 0) if (self.interconnect_device_id is None): print( "Could not find the Interconnect in the device list, exiting..." ) sys.exit(0) def GetDeviceIdFromDevType(self, device_type, device_index=0): devices = self.device_manager.ReadAllDevices() current_index = 0 for device in devices.device_handle: if device.device_type == device_type: if current_index == device_index: print("Found the Interconnect on device identifier {}". format(device.device_identifier)) return device.device_identifier current_index += 1 return None def Configure(self, port_id, enabled, speed, word_length, stop_bits, parity): ''' Enable and configure UART on interconnect. This will open a TCP port on the interconnect. This port allows bridging TCP socket to UART. ''' uart_config = Common_pb2.UARTConfiguration() uart_config.port_id = port_id uart_config.enabled = enabled # Setting enabled to true opens the TCP port dedicated to UART bridging. Setting this # field to false disables designated uart and closes the TCP port. uart_config.speed = speed uart_config.word_length = word_length uart_config.stop_bits = stop_bits uart_config.parity = parity self.interconnect_config.SetUARTConfiguration( uart_config, deviceId=self.interconnect_device_id) def EnableBridge(self, bridge_type, target=0, output=0): # Create bridge configuration bridge_config = Base_pb2.BridgeConfig() bridge_config.device_identifier = self.interconnect_device_id bridge_config.bridgetype = bridge_type # If either target or ouput port has valid port value, add port config to bridge configuration if target or output: bridge_config.port_config.target_port = 0 bridge_config.port_config.out_port = 0 if target: bridge_config.port_config.target_port = target if output: bridge_config.port_config.out_port = output # Send the configuration and return the result bridge_result = self.base.EnableBridge(bridge_config) return bridge_result def DisableBridge(self, bridge_id): return self.base.DisableBridge(bridge_id) def ExampleSendDataAndReadItBack(self): # Enable port bridging on base. bridge_result = self.EnableBridge(Base_pb2.BRIDGE_TYPE_UART) bridge_id = bridge_result.bridge_id if bridge_result.status != Base_pb2.BRIDGE_STATUS_OK: print("Error creating bridge on base, exiting...") return # Get created bridge's configuration. bridge_config = self.base.GetBridgeConfig(bridge_id) base_port = bridge_config.port_config.out_port interconnect_port = bridge_config.port_config.target_port print( "UARTBridge ID # %i created between Interconnect (dev# %i)'s port #%i and external port #%i" % (bridge_id.bridge_id, self.interconnect_device_id, interconnect_port, base_port)) # Open a socket to base's forwarded port. client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) client_socket.connect((self.base_ip_address, base_port)) client_socket.setblocking(0) print("UART bridge object initialized") # Send data to be written on UART using TCP connection. client_socket.send( b"This data is being written on Interconnect's expansion port UART\n" ) # Wait for data to be received from UART print("Waiting 10 seconds for data from uart...") sys.stdout.write("Received data : ") sys.stdout.flush() startTime = time.time() while time.time() - startTime < 10: readready, _, _ = select.select([client_socket], [], [], 1) if readready: data = client_socket.recv(1) if len(data): sys.stdout.write(data.decode("utf-8")) sys.stdout.flush() # Disconnect client socket. client_socket.close() # Disable bridge on base. self.DisableBridge(bridge_id)