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