def __init__(self, router): self.router = 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 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: device_manager = DeviceManagerClient(router) device_config = DeviceConfigClient(router) vision_config = VisionConfigClient(router) # example core vision_device_id = example_vision_get_device_id(device_manager) if vision_device_id != 0: example_routed_vision_get_option_information( vision_config, vision_device_id) example_routed_vision_get_sensor_options_values( vision_config, vision_device_id) example_routed_vision_set_sensor_options_values( vision_config, vision_device_id) example_routed_vision_confirm_saved_sensor_options_values( vision_config, device_config, vision_device_id)
def __init__(self, router): ''' Implements methods for establishing and operating I2C bridge through the base ''' self.router = router # Create device manager client. Device manager is used get a list of devices present in the arm. In this example # we use device manager to determine the device ID associated with the interconnect. self.device_manager = DeviceManagerClient(self.router) # Create interconnect configuration client. This client is used to perform I2C bus configuration and I2C bus actions. 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 __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 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 device_manager = DeviceManagerClient(router) device_config = DeviceConfigClient(router) # Example core example_routed_device_config(device_manager, device_config)
class EthernetBridgeConfigurationExample: def __init__(self, router): # Create required services self.interconnect_config = InterconnectConfigClient(router) self.device_manager = DeviceManagerClient(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 EnableEthernetBridge(self): # Configure the Interconnect to enable the bridge ethernet_configuration = InterconnectConfig_pb2.EthernetConfiguration() ethernet_configuration.device = InterconnectConfig_pb2.ETHERNET_DEVICE_EXPANSION ethernet_configuration.enabled = True ethernet_configuration.speed = InterconnectConfig_pb2.ETHERNET_SPEED_100M ethernet_configuration.duplex = InterconnectConfig_pb2.ETHERNET_DUPLEX_FULL try: self.interconnect_config.SetEthernetConfiguration( ethernet_configuration, self.interconnect_device_id) except Exception as e: print("An unexpected error occured : {}".format(e))
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 I2CBridge: def __init__(self, router): ''' Implements methods for establishing and operating I2C bridge through the base ''' self.router = router # Create device manager client. Device manager is used get a list of devices present in the arm. In this example # we use device manager to determine the device ID associated with the interconnect. self.device_manager = DeviceManagerClient(self.router) # Create interconnect configuration client. This client is used to perform I2C bus configuration and I2C bus actions. 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) """ GetDeviceIdFromDevType(devType, devIndex) Get device ID according to a given device type (Actuator or interconnect). Inputs: devType : Device type Index argument correspond to the position of the device (i.e.: 0 being the first,1 the second, etc.) """ 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 """ WriteValue(device_address, data, timeout_ms) Writes a data array to I2C bus to a given device. inputs: device_address: device's I2C address. data: list containing data to write to device timeout_ms: write operation timeout in milliseconds """ def WriteValue(self, device_address, data, timeout_ms): i2c_write_parameter = InterconnectConfig_pb2.I2CWriteParameter() i2c_write_parameter.device = InterconnectConfig_pb2.I2C_DEVICE_EXPANSION i2c_write_parameter.device_address = device_address bytesData = bytes(data) i2c_write_parameter.data.data = bytesData i2c_write_parameter.data.size = len(bytesData) i2c_write_parameter.timeout = timeout_ms return self.interconnect_config.I2CWrite( i2c_write_parameter, deviceId=self.interconnect_device_id) """ ReadValue(device_address, bytes_to_read, timeout_ms) Reads a data array from I2C bus from a given device. inputs: device_address: device's I2C address. bytes_to_read: number of bytes to read from device timeout_ms: read operation timeout in milliseconds """ def ReadValue(self, device_address, bytes_to_read, timeout_ms): # Create the I2C read request i2c_read_request = InterconnectConfig_pb2.I2CReadParameter() i2c_read_request.device = InterconnectConfig_pb2.I2C_DEVICE_EXPANSION i2c_read_request.device_address = device_address i2c_read_request.size = bytes_to_read i2c_read_request.timeout = timeout_ms # Read the data and print it read_result = self.interconnect_config.I2CRead( i2c_read_request, deviceId=self.interconnect_device_id) data = read_result.data print("We were supposed to read {} bytes and we read {} bytes.".format( bytes_to_read, read_result.size)) print("The data is : {0:b}".format(ord(data))) """ Configure(is_enabled, mode, addressing) Configure expansion bus I2C bus on interconnect. Inputs: is_enabled: Enables i2cbus on interconnect's expansion bus if true, disable it otherwise. mode: I2C mode in which the bus is set ( InterconnectConfig_pb2.I2C_MODE_STANDARD, InterconnectConfig_pb2.I2C_MODE_FAST or InterconnectConfig_pb2.I2C_MODE_FAST_PLUS) addressing: Addressing size used on I2C bus (I2C_DEVICE_ADDRESSING_7_BITS or I2C_DEVICE_ADDRESSING_10_BITS). """ def Configure(self, is_enabled, mode, addressing): # Create the configuration I2CConfiguration = InterconnectConfig_pb2.I2CConfiguration() I2CConfiguration.device = InterconnectConfig_pb2.I2C_DEVICE_EXPANSION I2CConfiguration.enabled = is_enabled I2CConfiguration.mode = mode I2CConfiguration.addressing = addressing # Set the configuration self.interconnect_config.SetI2CConfiguration( I2CConfiguration, deviceId=self.interconnect_device_id)
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)
# 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 # print("Actions:")
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)
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 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)
def cam(cap_id, e, target_middle_shared, delta_shared, success_flag_shared, vision_middle_shared, h_shared, w_shared, elapsed_time, beer_time): global args if not args: args = utilities.parseConnectionArguments() # Create connection to the device and get the router with utilities.DeviceConnection.createTcpConnection(args) as 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' default_threshold = 0.1 labels = load_labels(os.path.join(default_model_dir, default_labels)) interpreter = make_interpreter( os.path.join(default_model_dir, default_model)) interpreter.allocate_tensors() cap = WebcamVideoStream(cap_id).start() #multithreading Video Capture #Window Settings window_name = "Robot_Camera" size_wh = (1920 - 300, 1080) location_xy = (0, 0) cv2.namedWindow(window_name, cv2.WINDOW_KEEPRATIO | cv2.WINDOW_GUI_NORMAL) cv2.resizeWindow(window_name, *size_wh) # cv2.namedWindow(window_name, cv2.WINDOW_AUTOSIZE) cv2.moveWindow(window_name, *location_xy) # cv2.imshow(window_name, image) success_flag = False t0 = time.time() auto_focus_time = 7 device_manager = DeviceManagerClient(router) vision_config = VisionConfigClient(router) vision_device_id = vision_action.example_vision_get_device_id( device_manager) #Disable Auto-Focus vision_action.autofocus_action(vision_config, vision_device_id, action_id=1) while True: frame = cap.read() (h, w) = frame.shape[:2] vision_middle = (int(w / 2), int(h / 2) - 150) cv2.circle(frame, vision_middle, target_circle_size * 2, (255, 0, 255), 2) #Detect the object and get the target middle cv2_im = frame cv2_im_rgb = cv2.cvtColor(cv2_im, cv2.COLOR_BGR2RGB) pil_im = Image.fromarray(cv2_im_rgb) scale = detect.set_input( interpreter, pil_im.size, lambda size: pil_im.resize(size, Image.ANTIALIAS)) interpreter.invoke() objs = detect.get_output(interpreter, default_threshold, scale) cv2_im = append_objs_to_img(cv2_im, objs, labels) if objs: #find the biggest bounding box area = 0 for i0 in range(len(objs)): x0, y0, x1, y1 = list(objs[i0].bbox) area_temp = (x1 - x0) * (y1 - y0) if area_temp > area: area = area_temp biggest_area_index = i0 x0, y0, x1, y1 = list(objs[biggest_area_index].bbox) target_middle = (int(x0 + (x1 - x0) / 2), int(y0 + (y1 - y0) / 2)) success_flag = True cv2.circle(cv2_im, target_middle, int(target_circle_size / 2), (0, 255, 0), 2) else: success_flag = False if success_flag == False: #human/face was not detected target_middle = vision_middle if time.time() - t0 > auto_focus_time: try: #sometimes when stream corrupted because of Schleifring, auotfocus leads to error vision_action.autofocus_action( vision_config, vision_device_id, action_id=6, focus_point=target_middle_shared[:]) except: None t0 = time.time() #draw the delta delta = [0, 0] delta[0] = target_middle[0] - vision_middle[0] delta[1] = target_middle[1] - vision_middle[1] cv2.line( cv2_im, vision_middle, (vision_middle[0] + delta[0], vision_middle[1] + delta[1]), (0, 255, 0), 1) #what needs to be given to the process: target_middle_shared[0] = target_middle[0] target_middle_shared[1] = target_middle[1] delta_shared[0] = delta[0] delta_shared[1] = delta[1] success_flag_shared.value = success_flag vision_middle_shared[0] = vision_middle[0] vision_middle_shared[1] = vision_middle[1] h_shared.value = h w_shared.value = w if elapsed_time.value > 1: cv2_im = functions_.draw_loading_circle( img=cv2_im, radius=target_circle_size, center=vision_middle, elapsed_time=elapsed_time.value - 1, end_time=beer_time - 1) if elapsed_time.value > beer_time: #write text font = cv2.FONT_HERSHEY_SIMPLEX org = (vision_middle[0] - 300, vision_middle[1] - 100) fontScale = 2 color = (255, 255, 255) thickness = 3 cv2_im = cv2.putText(cv2_im, 'Have a beer, buddy', org, font, fontScale, color, thickness, cv2.LINE_AA) #Call to high five if elapsed_time.value == -1: #write text font = cv2.FONT_HERSHEY_SIMPLEX org = (vision_middle[0] - 550, vision_middle[1]) fontScale = 4 color = (255, 255, 255) thickness = 4 cv2_im = cv2.putText(cv2_im, ' High Five', org, font, fontScale, color, thickness, cv2.LINE_AA) if elapsed_time.value == -2: #write text font = cv2.FONT_HERSHEY_SIMPLEX org = (vision_middle[0] - 550, vision_middle[1]) fontScale = 4 color = (255, 255, 255) thickness = 4 cv2_im = cv2.putText(cv2_im, ' Good Night', org, font, fontScale, color, thickness, cv2.LINE_AA) cv2.imshow(window_name, frame) if (cv2.waitKey(1) & 0xFF == ord('q')) or e.is_set(): break cap.stop() cv2.destroyWindow(str(cap_id)) return
class GpioBridge: ''' Implements methods for establishing and operating GPIO bridge through the base ''' GpioEnum = ( InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_IDENTIFIER_3, InterconnectConfig_pb2.GPIO_IDENTIFIER_4 ) def __init__(self, router): self.router = 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 InitGpioInputsAndOutputs(self): gpio_config = InterconnectConfig_pb2.GPIOConfiguration() # Pins 1 and 2 as output gpio_config.mode = InterconnectConfig_pb2.GPIO_MODE_OUTPUT_PUSH_PULL gpio_config.pull = InterconnectConfig_pb2.GPIO_PULL_NONE gpio_config.default_value = InterconnectConfig_pb2.GPIO_VALUE_LOW gpio_config.identifier = InterconnectConfig_pb2.GPIO_IDENTIFIER_1 print ("Setting pin #1 as output...") self.interconnect_config.SetGPIOConfiguration(gpio_config, deviceId=self.interconnect_device_id) time.sleep(1) gpio_config.identifier = InterconnectConfig_pb2.GPIO_IDENTIFIER_2 print ("Setting pin #2 as output...") self.interconnect_config.SetGPIOConfiguration(gpio_config, deviceId=self.interconnect_device_id) time.sleep(1) # Pins 3 and 4 as input pullup gpio_config.mode = InterconnectConfig_pb2.GPIO_MODE_INPUT_FLOATING gpio_config.pull = InterconnectConfig_pb2.GPIO_PULL_UP gpio_config.identifier = InterconnectConfig_pb2.GPIO_IDENTIFIER_3 print ("Setting pin #3 as input pullup...") self.interconnect_config.SetGPIOConfiguration(gpio_config, deviceId=self.interconnect_device_id) time.sleep(1) gpio_config.identifier = InterconnectConfig_pb2.GPIO_IDENTIFIER_4 print ("Setting pin #4 as input pullup...") self.interconnect_config.SetGPIOConfiguration(gpio_config, deviceId=self.interconnect_device_id) time.sleep(1) def SetOutputPinValue(self, identifier, value): gpio_state = InterconnectConfig_pb2.GPIOState() gpio_state.identifier = identifier gpio_state.value = value print ("GPIO pin {} will be put at value {}".format(InterconnectConfig_pb2.GPIOIdentifier.Name(identifier), InterconnectConfig_pb2.GPIOValue.Name(value))) self.interconnect_config.SetGPIOState(gpio_state,deviceId=self.interconnect_device_id) def ReadInputPinValue(self, identifier): gpio_identification = InterconnectConfig_pb2.GPIOIdentification() gpio_identification.identifier = identifier state = self.interconnect_config.GetGPIOState(gpio_identification,deviceId=self.interconnect_device_id) if (state.value == InterconnectConfig_pb2.GPIO_VALUE_HIGH): return 1 elif (state.value == InterconnectConfig_pb2.GPIO_VALUE_LOW): return 0 else: print ("Error : the value read is unspecified") return -1 def ExampleSetAndReadValues(self): # We sleep a bit between the reads and the writes # Technically the InterconnectConfig service runs at 25ms but we sleep 100ms to make sure we let enough time sleep_time_sec = 0.1 # The Arduino reads pin 1 and sets pin 3 the same # The Arduino reads pin 2 and sets pin 4 the same self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_VALUE_HIGH) self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_VALUE_LOW) time.sleep(sleep_time_sec) pin3_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_3) # should be HIGH pin4_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_4) # should be LOW print ("Value read for pin #3 is : {}".format(pin3_in)) print ("Value read for pin #4 is : {}".format(pin4_in)) self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_VALUE_LOW) self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_VALUE_HIGH) time.sleep(sleep_time_sec) pin3_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_3) # should be LOW pin4_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_4) # should be HIGH print ("Value read for pin #3 is : {}".format(pin3_in)) print ("Value read for pin #4 is : {}".format(pin4_in)) self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_VALUE_HIGH) self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_VALUE_HIGH) time.sleep(sleep_time_sec) pin3_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_3) # should be HIGH pin4_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_4) # should be HIGH print ("Value read for pin #3 is : {}".format(pin3_in)) print ("Value read for pin #4 is : {}".format(pin4_in)) self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_1, InterconnectConfig_pb2.GPIO_VALUE_LOW) self.SetOutputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_2, InterconnectConfig_pb2.GPIO_VALUE_LOW) time.sleep(sleep_time_sec) pin3_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_3) # should be LOW pin4_in = self.ReadInputPinValue(InterconnectConfig_pb2.GPIO_IDENTIFIER_4) # should be LOW print ("Value read for pin #3 is : {}".format(pin3_in)) print ("Value read for pin #4 is : {}".format(pin4_in))
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