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 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, 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 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 __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 __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 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
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 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)