def __init__(self, action_scale=0.05, target_range=0.15, distance_threshold=0.05, speed=0.10):
self.connection = utilities.DeviceConnection.createTcpConnection()
self.router = self.connection.__enter__()
self.base = BaseClient(self.router)
self.base_cyclic = BaseCyclicClient(self.router)
self.webcam = cv2.VideoCapture(0)
self.action_scale = action_scale
self.target_range = target_range
self.distance_threshold = distance_threshold
self.speed = speed
self.observation_space = gym.spaces.Dict(
observation=gym.spaces.Box(-np.inf, np.inf, self._get_obs().shape),
achieved_goal=gym.spaces.Box(-np.inf, np.inf, (3,)),
desired_goal=gym.spaces.Box(-np.inf, np.inf, (3,)),
)
self.action_space = gym.spaces.Box(-1., 1., (3,))
self.reward_range = (-np.inf, 0)
self.unwrapped = self
self.spec = None
self._set_to_home()
self.init_xyz = self._get_obs()[:3]
self.goal = None
self.sample_goal()
assert self.goal is not None
def __init__(self, router, proportional_gain=2.0):
self.proportional_gain = proportional_gain
self.router = router
# Create base client using TCP router
self.base = BaseClient(self.router)
class KinovaController:
def __init__(self, router, router_real_time):
self.camera_mtr = None
self.world3D_list = []
self.camera3D_list = []
self.camera3D = None
self.camera3D_count = -100
self.world3D_count = 0
self.joint_angle_list = None
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_feedback = BaseCyclic_pb2.Feedback()
self.gripper = GripperController(router, router_real_time)
def set_single_level_servoing_mode(self):
# Save servoing mode before changing it
self.previous_servoing_mode = self.base.GetServoingMode()
# Set base in single level servoing mode
servoing_mode_info = Base_pb2.ServoingModeInformation()
# print("Setting single level servoi
print("Current Mode:")
print(servoing_mode_info)
servoing_mode_info.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
self.base.SetServoingMode(servoing_mode_info)
print("Waiting 5 second for changing to single level servoing mode...")
time.sleep(5)
print("After setting single level")
print(servoing_mode_info)
def move_arm(self, pos_list):
self.set_single_level_servoing_mode()
# for count, joint_angle in enumerate(self.joint_angle_list):
for count, pos in enumerate(pos_list):
print("Moving to ", pos)
cartesian_action_movement(self.base, pos)
self.base_feedback = self.SendCallWithRetry(
self.base_cyclic.RefreshFeedback, 3)
joint_angles = [
self.base_feedback.actuators[i].position for i in range(7)
]
@staticmethod
def SendCallWithRetry(call, retry, *args):
i = 0
arg_out = []
while i < retry:
try:
arg_out = call(*args)
break
except:
i = i + 1
continue
if i == retry:
print("Failed to communicate")
return arg_out
def stop_movements():
e.set() #kill movement processes
time.sleep(0.2)
global args
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base.Stop()
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
# Example core
success = True
success &= example_move_to_home_position(base)
success &= example_cartesian_action_movement(base, base_cyclic)
success &= example_angular_action_movement(base)
# You can also refer to the 110-Waypoints examples if you want to execute
# a trajectory defined by a series of waypoints in joint space or in Cartesian space
return 0 if success else 1
def read_base_cyclic():
global args
global e
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
tlog = time.time()
now = datetime.datetime.now()
#print (now.strftime("%Y-%m-%d %H:%M:%S"))
timestamp = now.strftime("%Y-%m-%d-%H-%M-%S")
f = open(
"./Subfunctions/log_recordings/rec_logging_%s.txt" % timestamp,
"w")
while e.is_set() == False:
if time.time() - tlog > 0.1:
log_string = ""
log_string += "tool pose "
log_string += str(
highlevel_movements.get_tcp_pose(base_cyclic))
log_string += " at joints "
log_string += str(
highlevel_movements.get_joint_angles(base_cyclic))
log_string += "\n"
f.write(log_string)
print(log_string)
tlog = time.time()
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
# Example core
success = True
success &= example_move_to_home_position(base)
success &= example_cartesian_action_movement(base, base_cyclic)
success &= example_angular_action_movement(base)
success &= example_move_to_home_position(base)
success &= example_cartesian_trajectory_movement(base, base_cyclic)
success &= example_angular_trajectory_movement(base)
return 0 if success else 1
def idle_upright_pos():
#move into idle upright joint_position
global args
global e
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == True: #Robot in Transport position?
success = True
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
# success &= highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=transport_pos ,speed=speedj) #transport pose
success = True
success &= highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=idle_pos, speed=speedj)
def __init__(self, router, router_real_time):
self.camera_mtr = None
self.world3D_list = []
self.camera3D_list = []
self.camera3D = None
self.camera3D_count = -100
self.world3D_count = 0
self.joint_angle_list = None
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_feedback = BaseCyclic_pb2.Feedback()
self.gripper = GripperController(router, router_real_time)
def look_into_crowd():
global args
global e
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
device_manager = DeviceManagerClient(router)
#Activate the continuous auto-focus of the camera
vision_config = VisionConfigClient(router)
vision_device_id = vision_action.example_vision_get_device_id(
device_manager)
# check if robot is not in transport position, if so, move safely
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == True: #Robot in transport position?
highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
#INSERT WAYPOINTS HERE
return
def transport_position():
global args
global e
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
#Moving back to init Pose
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == False: #Robot already in position?
success = True
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=transport_pos,
speed=speedj) #transport pose
def __init__(self, router, ip_address):
self.router = router
self.base_ip_address = ip_address
# Create services
self.base = BaseClient(self.router)
self.device_manager = DeviceManagerClient(self.router)
self.interconnect_config = InterconnectConfigClient(self.router)
self.interconnect_device_id = self.GetDeviceIdFromDevType(
Common_pb2.INTERCONNECT, 0)
if (self.interconnect_device_id is None):
print(
"Could not find the Interconnect in the device list, exiting..."
)
sys.exit(0)
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
# Example core
# Move to initial position
move_to_home_position(base)
# Move without the protection zone
print_protection_zones(base)
move_in_front_of_protection_zone(base)
move_to_protection_zone(base)
move_to_home_position(base)
# Move with the protection zone
print_protection_zones(base)
move_in_front_of_protection_zone(base)
# Add the protection zone
handle = create_protection_zone(base)
move_to_protection_zone(base)
move_to_home_position(base)
# Delete the protection zone
base.DeleteProtectionZone(handle)
# Print final protection zones
# The example protection zone should be removed
print_protection_zones(base)
def example_api_creation(args):
'''
This function creates all required objects and connections to use the arm's services.
It is easier to use the DeviceConnection utility class to create the router and then
create the services you need (as done in the other examples).
However, if you want to create all objects yourself, this function tells you how to do it.
'''
PORT = 10000
# Setup API
error_callback = lambda kException: print(
"_________ callback error _________ {}".format(kException))
transport = TCPTransport()
router = RouterClient(transport, error_callback)
transport.connect(args.ip, PORT)
# Create session
session_info = Session_pb2.CreateSessionInfo()
session_info.username = args.username
session_info.password = args.password
session_info.session_inactivity_timeout = 60000 # (milliseconds)
session_info.connection_inactivity_timeout = 2000 # (milliseconds)
print("Creating session for communication")
session_manager = SessionManager(router)
session_manager.CreateSession(session_info)
print("Session created")
# Create required services
device_config = DeviceConfigClient(router)
base = BaseClient(router)
print(device_config.GetDeviceType())
print(base.GetArmState())
# Close API session
session_manager.CloseSession()
# Disconnect from the transport object
transport.disconnect()
def __init__(self, router, router_real_time):
self.expected_number_of_actuators = 7 # example works for 7dof Gen3
# self.torque_amplification = 2.0 # Torque measure on 7th actuator is sent as a command to first actuator
self.torque_amplification = 20.0 # Torque measure on 7th actuator is sent as a command to first actuator
# Create required services
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_command = BaseCyclic_pb2.Command()
self.base_feedback = BaseCyclic_pb2.Feedback()
self.base_custom_data = BaseCyclic_pb2.CustomData()
# Detect all devices
device_handles = device_manager.ReadAllDevices()
self.actuator_count = 0
# Only actuators are relevant for this example
for handle in device_handles.device_handle:
if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR:
self.base_command.actuators.add()
self.base_feedback.actuators.add()
self.actuator_count += 1
# Change send option to reduce max timeout at 3ms
self.sendOption = RouterClientSendOptions()
self.sendOption.andForget = False
self.sendOption.delay_ms = 0
self.sendOption.timeout_ms = 3
self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite.
self.cyclic_thread = {}
self.kill_the_thread = False
self.already_stopped = False
self.cyclic_running = False
def __init__(self, router, router_real_time):
# Maximum allowed waiting time during actions (in seconds)
self.ACTION_TIMEOUT_DURATION = 20
self.torque_amplification = 2.0 # Torque measure on last actuator is sent as a command to first actuator
# Create required services
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_command = BaseCyclic_pb2.Command()
self.base_feedback = BaseCyclic_pb2.Feedback()
self.base_custom_data = BaseCyclic_pb2.CustomData()
# Detect all devices
device_handles = device_manager.ReadAllDevices()
self.actuator_count = self.base.GetActuatorCount().count
# Only actuators are relevant for this example
for handle in device_handles.device_handle:
if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR:
self.base_command.actuators.add()
self.base_feedback.actuators.add()
# Change send option to reduce max timeout at 3ms
self.sendOption = RouterClientSendOptions()
self.sendOption.andForget = False
self.sendOption.delay_ms = 0
self.sendOption.timeout_ms = 3
self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite.
self.cyclic_thread = {}
self.kill_the_thread = False
self.already_stopped = False
self.cyclic_running = False
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
# Example core
example_call_rpc_using_options(base)
def high_five(base_angle=93.54):
global args
global e
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
temp_speed = 5
high_five_pos[0] = base_angle
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == True: #Robot in Transport position?
success = True
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
# success &= highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=transport_pos ,speed=speedj) #transport pose
temp_speed = 10
success = True
success &= highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=high_five_pos, speed=speedj)
time.sleep(1)
elapsed_time.value = -1
torque_tool = functions_.TorqueTool()
torque_tool.tap_toggle(base_cyclic, torque_threshold=8)
time.sleep(0.1)
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=[
base_angle, 352.44, 355.16, 329.09, 1.48, 38.79, 96.13
],
speed=speedj)
success &= highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=high_five_pos, speed=speedj)
elapsed_time.value = 0
def main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
# Example core
success = True
success &= example_forward_kinematics(base)
success &= example_inverse_kinematics(base)
return 0 if success else 1
if __name__ == '__main__':
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
# e = multiprocessing.Event()
# p = multiprocessing.Process(target=cam, args=('rtsp://192.168.1.10/color',e))
# p.start()
with utilities.DeviceConnection.createTcpConnection(args) as router:
#open camera
#cap = cv2.VideoCapture('rtsp://192.168.1.10/color')
#start a video stream process
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
#Start EdgeTPU
default_model_dir = './models'
#default_model = 'mobilenet_ssd_v2_coco_quant_postprocess_edgetpu.tflite'
default_model = 'mobilenet_ssd_v2_face_quant_postprocess_edgetpu.tflite'
default_labels = 'coco_labels.txt'
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('-m',
'--model',
help='File path of .tflite file.',
default=os.path.join(default_model_dir,
default_model))
parser.add_argument('-l',
def wave():
elapsed_time.value = 0
global e
global args
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
device_manager = DeviceManagerClient(router)
#Activate the continuous auto-focus of the camera
vision_config = VisionConfigClient(router)
vision_device_id = vision_action.example_vision_get_device_id(
device_manager)
# if vision_device_id != 0:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=2)
# check if robot is not in transport position, if so, move safely
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == True: #Robot in transport position?
highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
#Wave Sequence
wave_speed = 70 #degrees/sec
# wave_start_pose=[180, 0, 0, 0, 90, 90, 270] #joint4
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,30,0,0,-30])) #joint4
# wave_start_pose=[180, 0, 0, 0, 0, 0, 0] #joint5
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,0,0,40,0])) #joint4
# wave_start_pose=[182.49, 36.45, 0.78, 269.14, 2.33, 56.02, 0.11] #joint5
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,0,0,40,0])) #joint4
wave_start_pose = [182.9, 320.18, 177.58, 260.17, 1.06, 65.48,
180.64] #joint5
wave_left_pose = list(map(add, wave_start_pose,
[0, 0, 0, 0, 0, 40, 0])) #joint4
# highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=wave_start_pose,speed=speedj)
# sys.exit()
highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=wave_left_pose,
speed=speedj) #okay
#time.sleep(1)
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=-1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=-1,
base=base,
base_cyclic=base_cyclic)
# time.sleep(0.4)
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=-1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=-1,base=base,base_cyclic=base_cyclic)
elapsed_time.value = -2
time.sleep(0.4)
highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=transport_pos,
speed=speedj) #transport pose
transport.connect(DEVICE_IP, DEVICE_PORT)
# Create session
print("\nCreating session for communication")
session_info = Session_pb2.CreateSessionInfo()
session_info.username = '******'
session_info.password = '******'
session_info.session_inactivity_timeout = 60000 # (milliseconds)
session_info.connection_inactivity_timeout = 2000 # (milliseconds)
print("Session created")
session_manager = SessionManager(router)
session_manager.CreateSession(session_info)
# Create required services
base_client_service = BaseClient(router)
device_manager_service = DeviceManagerClient(router)
# Find the number of actuator in angular action and trajectory
sub_device_info = device_manager_service.ReadAllDevices()
act_count = 0
for dev in sub_device_info.device_handle:
if dev.device_type is Common_pb2.BIG_ACTUATOR or dev.device_type is Common_pb2.SMALL_ACTUATOR:
act_count += 1
# Move arm to ready position
print("\nMoving the arm to a safe position before executing example")
action_type = Base_pb2.RequestedActionType()
action_type.action_type = Base_pb2.REACH_JOINT_ANGLES
action_list = base_client_service.ReadAllActions(action_type)
action_handle = None
class GripperCommandExample:
def __init__(self, router, proportional_gain=2.0):
self.proportional_gain = proportional_gain
self.router = router
# Create base client using TCP router
self.base = BaseClient(self.router)
def ExampleSendGripperCommands(self):
# Create the GripperCommand we will send
gripper_command = Base_pb2.GripperCommand()
finger = gripper_command.gripper.finger.add()
# Close the gripper with position increments
print("Performing gripper test in position...")
gripper_command.mode = Base_pb2.GRIPPER_POSITION
position = 0.00
finger.finger_identifier = 1
while position < 1.0:
finger.value = position
self.base.SendGripperCommand(gripper_command)
position += 0.1
time.sleep(1)
# Set speed to open gripper
print("Opening gripper using speed command...")
gripper_command.mode = Base_pb2.GRIPPER_SPEED
finger.value = 0.1
self.base.SendGripperCommand(gripper_command)
gripper_request = Base_pb2.GripperRequest()
# Wait for reported position to be opened
gripper_request.mode = Base_pb2.GRIPPER_POSITION
while True:
gripper_measure = self.base.GetMeasuredGripperMovement(
gripper_request)
if len(gripper_measure.finger):
print("Current position is : {0}".format(
gripper_measure.finger[0].value))
if gripper_measure.finger[0].value < 0.01:
break
else: # Else, no finger present in answer, end loop
break
# Set speed to close gripper
print("Closing gripper using speed command...")
gripper_command.mode = Base_pb2.GRIPPER_SPEED
finger.value = -0.1
self.base.SendGripperCommand(gripper_command)
# Wait for reported speed to be 0
gripper_request.mode = Base_pb2.GRIPPER_SPEED
while True:
gripper_measure = self.base.GetMeasuredGripperMovement(
gripper_request)
if len(gripper_measure.finger):
print("Current speed is : {0}".format(
gripper_measure.finger[0].value))
if gripper_measure.finger[0].value == 0.0:
break
else: # Else, no finger present in answer, end loop
break
class GripperLowLevelExample:
def __init__(self, router, router_real_time, proportional_gain=2.0):
"""
GripperLowLevelExample class constructor.
Inputs:
kortex_api.RouterClient router: TCP router
kortex_api.RouterClient router_real_time: Real-time UDP router
float proportional_gain: Proportional gain used in control loop (default value is 2.0)
Outputs:
None
Notes:
- Actuators and gripper initial position are retrieved to set initial positions
- Actuator and gripper cyclic command objects are created in constructor. Their
references are used to update position and speed.
"""
self.proportional_gain = proportional_gain
###########################################################################################
# UDP and TCP sessions are used in this example.
# TCP is used to perform the change of servoing mode
# UDP is used for cyclic commands.
#
# 2 sessions have to be created: 1 for TCP and 1 for UDP
###########################################################################################
self.router = router
self.router_real_time = router_real_time
# Create base client using TCP router
self.base = BaseClient(self.router)
# Create base cyclic client using UDP router.
self.base_cyclic = BaseCyclicClient(self.router_real_time)
# Create base cyclic command object.
self.base_command = BaseCyclic_pb2.Command()
self.base_command.frame_id = 0
self.base_command.interconnect.command_id.identifier = 0
self.base_command.interconnect.gripper_command.command_id.identifier = 0
# Add motor command to interconnect's cyclic
self.motorcmd = self.base_command.interconnect.gripper_command.motor_cmd.add(
)
# Set gripper's initial position velocity and force
base_feedback = self.base_cyclic.RefreshFeedback()
self.motorcmd.position = base_feedback.interconnect.gripper_feedback.motor[
0].position
self.motorcmd.velocity = 0
self.motorcmd.force = 100
for actuator in base_feedback.actuators:
self.actuator_command = self.base_command.actuators.add()
self.actuator_command.position = actuator.position
self.actuator_command.velocity = 0.0
self.actuator_command.torque_joint = 0.0
self.actuator_command.command_id = 0
print("Position = ", actuator.position)
# Save servoing mode before changing it
self.previous_servoing_mode = self.base.GetServoingMode()
# Set base in low level servoing mode
servoing_mode_info = Base_pb2.ServoingModeInformation()
servoing_mode_info.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING
self.base.SetServoingMode(servoing_mode_info)
def Cleanup(self):
"""
Restore arm's servoing mode to the one that
was effective before running the example.
Inputs:
None
Outputs:
None
Notes:
None
"""
# Restore servoing mode to the one that was in use before running the example
self.base.SetServoingMode(self.previous_servoing_mode)
def Goto(self, target_position):
"""
Position gripper to a requested target position using a simple
proportional feedback loop which changes speed according to error
between target position and current gripper position
Inputs:
float target_position: position (0% - 100%) to send gripper to.
Outputs:
Returns True if gripper was positionned successfully, returns False
otherwise.
Notes:
- This function blocks until position is reached.
- If target position exceeds 100.0, its value is changed to 100.0.
- If target position is below 0.0, its value is set to 0.0.
"""
if target_position > 100.0:
target_position = 100.0
if target_position < 0.0:
target_position = 0.0
while True:
try:
base_feedback = self.base_cyclic.Refresh(self.base_command)
# Calculate speed according to position error (target position VS current position)
position_error = target_position - base_feedback.interconnect.gripper_feedback.motor[
0].position
# If positional error is small, stop gripper
if abs(position_error) < 1.5:
position_error = 0
self.motorcmd.velocity = 0
self.base_cyclic.Refresh(self.base_command)
return True
else:
self.motorcmd.velocity = self.proportional_gain * abs(
position_error)
if self.motorcmd.velocity > 100.0:
self.motorcmd.velocity = 100.0
self.motorcmd.position = 100.0
if position_error < 0.0:
self.motorcmd.position = 0.0
except Exception as e:
print("Error in refresh: " + str(e))
return False
time.sleep(0.001)
return True
def main():
target_middle_shared=multiprocessing.Array("i",[0,0])
delta_shared=multiprocessing.Array("i",[0,0])
success_flag_shared=multiprocessing.Value("i",0)
vision_middle_shared=multiprocessing.Array("i",[0,0])
h_shared=multiprocessing.Value("i",0)
w_shared=multiprocessing.Value("i",0)
elapsed_time=multiprocessing.Value("d",0.0)
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
e = multiprocessing.Event()
p = multiprocessing.Process(target=cam, args=('rtsp://192.168.1.10/color',e,target_middle_shared,delta_shared,success_flag_shared,vision_middle_shared,h_shared,w_shared,elapsed_time,beer_time))
p.start()
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
device_manager = DeviceManagerClient(router)
#Activate the continuous auto-focus of the camera
vision_config = VisionConfigClient(router)
vision_device_id = vision_action.example_vision_get_device_id(device_manager)
# if vision_device_id != 0:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=2)
# Initial Movements
success = True
#Move into transport position (home)
home_pose=[184.19,291.7,171.7,213.3,181.8,45.8,266.6]
#success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=5)
#success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[-0.06,-0.096,0.8,-90.823,171.8,113.73],speed=speedl)
# Grab the beer
#Desk Sequence
# highlevel_movements.send_gripper_command(base,value=0.0)
# #success &= highlevel_movements.angular_action_movement(base,joint_positions=[89.695, 336.743, 176.642, 232.288, 180.629, 70.981, 272.165],speed=speedj)
# success &= highlevel_movements.angular_action_movement(base,joint_positions=[91.583, 23.663, 174.547, 265.846, 180.949, 35.446, 272.106],speed=speedj)
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.01796681061387062, -0.6095998287200928, 0.2607220709323883, -89.98168182373047, -176.41896057128906, 178.88327026367188],speed=speedl)
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.015759950503706932, -0.6483935713768005, 0.2543827295303345, -91.34257507324219, 178.12986755371094, 178.2921905517578],speed=speedl)
# # GRIPPER_ACTION
# success &= highlevel_movements.send_gripper_command(base,value=0.7)
# time.sleep(1)
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.016505524516105652, -0.65036940574646, 0.38649141788482666, -92.8912353515625, 178.2748565673828, 179.34559631347656],speed=speedl)
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.07478067278862, -0.1246325895190239, 0.8614432215690613, 90.16726684570312, 9.310687065124512, 5.854083061218262],speed=speedl)
# # move to surveillance position
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[179.62, 307.879, 172.652, 289.174, 180.408, 69.243, 272.359],speed=speedj)
#move to surveillance position
#success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[-0.06,-0.096,0.8,-90.823,171.8,113.73],speed=0.1)
#Dog Sequence Desired Pose : [249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95]
#TODO Startposition [249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95] check
#TODO Schwenk ins Publikum nach vorne (extra funktion, muss aufgerufen werden) Tkinter
#TODO Winken nur mit oberen zwei Gelenken Tkinter
#TODO Theta_y korrigieren
#TODO Sicht korrigieren (Fokus und ggf. heller/Dunkler)
# success &= highlevel_movements.angular_action_movement(base,joint_positions=[249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95],speed=10) #Dog Pose
# sys.exit()
success &= highlevel_movements.send_gripper_command(base,value=0) #open gripper
success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose
success &= highlevel_movements.angular_action_movement(base,joint_positions=[257.64, 246.34, 31.6, 218.63, 214.63, 98.69, 129.78],speed=3) #desired dog pose
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose
success &= highlevel_movements.angular_action_movement(base,joint_positions=[238.78, 264.98, 70.21, 217.99, 349.19, 120.32, 8.3],speed=3) #snake
success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.006, -0.536, 0.191, 88.99, 1.239, 7.97],speed=speedl)
#GRIPPER_ACTION
success &= highlevel_movements.send_gripper_command(base,value=0.4)
time.sleep(1.5)
success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.008, -0.535, 0.275, 88.988, 1.245, 7.968],speed=speedl) #move a little up
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[235.7, 344.71, 64.58, 219.35, 49.72, 80.94, 54.84],speed=speedj) # upright1
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[231.32, 353.1, 32.7, 304.08, 37.1, 321.32, 58.52],speed=speedj) #upright 2
success &= highlevel_movements.angular_action_movement(base,joint_positions=[270.94, 42.99, 348.31, 305.15, 0.84, 286.28, 91.49],speed=speedj) #final
#success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.117, -0.058, 0.914, 86.046, 9.61, 6.732],speed=speedl) #final
#sys.exit()
#search for a face and interrupt when found
#vision_action.autofocus_action(vision_config, vision_device_id,action_id=4)
success &= highlevel_movements.example_send_joint_speeds(base,speeds=[18, 0, 0, 0, 0, 0, 0],timer=20,success_flag_shared=success_flag_shared)
#sys.exit()
#Find Face
t0=time.time()
token=False
target_reached_flag=False
beer_mode=False
print("searching for person")
while True:
if time.time()-t0>action_time_interval and success_flag_shared.value==True and beer_mode==False and p.is_alive():
#calculate the pose increment and move robot into target
target_reached_flag=functions_.rotate_to_target(
delta_shared[:],target_middle_shared[:],vision_middle_shared[:],w_shared.value,h_shared.value,max_speed,min_speed,
target_circle_size,base,base_cyclic)
t0=time.time()
elif time.time()-t0>action_time_interval and success_flag_shared.value==False and beer_mode==False:
base.Stop()
# if time.time()-t0>2:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=6,focus_point=target_middle_shared[:])
elif p.is_alive()==False:
base.Stop()
print("Process aborded")
sys.exit()
#after robot is Xs in target, hand over beer
if (target_reached_flag==True and token==True and success_flag_shared.value==True) or beer_mode==True:
elapsed_time.value=time.time()-beer_timer
if elapsed_time.value>beer_time:
if beer_mode==False:
reach_beer_twist_timer=time.time()
beer_mode=True
#the target was long enough in scope
tool_twist_duration=6
if time.time()-reach_beer_twist_timer<tool_twist_duration:
highlevel_movements.tool_twist_time(base,tool_twist_duration,pose_distance=[0,-0.35,0.4,0,0,0])
else:
base.Stop()
time.sleep(2) #swing out
#wait until force applied to robot
torque_tool=functions_.TorqueTool()
torque_tool.tap_toggle(base_cyclic,torque_threshold=5)
#open gripper
highlevel_movements.send_gripper_command(base,value=0.0)
time.sleep(2)
home_pose=[184.19,291.7,171.7,213.3,181.8,45.8,266.6] #Desk
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[267.038, 239.612, 177.453, 212.171, 185.765, 56.095, 269.943],speed=speedj)
highlevel_movements.tool_twist_time(base,tool_twist_duration,pose_distance=[0,-0.35,0.4,0,0,0])
#Wave Sequence
wave_start_pose=[180, 0, 0, 0, 82, 90, 270]
wave_left_pose=list(map(add,wave_start_pose,[0,0,0,30,0,0,-30]))
wave_time=2 #seconds
#wave_left_pose=list(map(add,wave_start_pose,[0,-30,0,-30,0,0,0]))
highlevel_movements.angular_action_movement(base,joint_positions=wave_start_pose,speed=4)
#while True:
highlevel_movements.angular_action_movement(base,joint_positions=wave_left_pose,speed=2.5) #okay
highlevel_movements.angular_action_movement(base,joint_positions=[180, 0, 0, 330, 82, 90, 300],speed=4.5)
highlevel_movements.angular_action_movement(base,joint_positions=wave_left_pose,speed=4.5)
highlevel_movements.angular_action_movement(base,joint_positions=[180, 0, 0, 330, 82, 90, 300],speed=4.5)
highlevel_movements.angular_action_movement(base,joint_positions=wave_start_pose,speed=2.5)
#sys.exit()
#Moving back to init Pose
success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose
success &= highlevel_movements.angular_action_movement(base,joint_positions=[257.64, 246.34, 31.6, 218.63, 214.63, 98.69, 129.78],speed=3) #desired dog pose
#success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=speedj)
e.set() #kill the camera process
break
elif target_reached_flag==True and success_flag_shared.value==True:
beer_timer=time.time()
token=True
else:
token=False
elapsed_time.value=0
class TorqueExample:
def __init__(self, router, router_real_time):
self.expected_number_of_actuators = 7 # example works for 7dof Gen3
# self.torque_amplification = 2.0 # Torque measure on 7th actuator is sent as a command to first actuator
self.torque_amplification = 20.0 # Torque measure on 7th actuator is sent as a command to first actuator
# Create required services
device_manager = DeviceManagerClient(router)
self.actuator_config = ActuatorConfigClient(router)
self.base = BaseClient(router)
self.base_cyclic = BaseCyclicClient(router_real_time)
self.base_command = BaseCyclic_pb2.Command()
self.base_feedback = BaseCyclic_pb2.Feedback()
self.base_custom_data = BaseCyclic_pb2.CustomData()
# Detect all devices
device_handles = device_manager.ReadAllDevices()
self.actuator_count = 0
# Only actuators are relevant for this example
for handle in device_handles.device_handle:
if handle.device_type == Common_pb2.BIG_ACTUATOR or handle.device_type == Common_pb2.SMALL_ACTUATOR:
self.base_command.actuators.add()
self.base_feedback.actuators.add()
self.actuator_count += 1
# Change send option to reduce max timeout at 3ms
self.sendOption = RouterClientSendOptions()
self.sendOption.andForget = False
self.sendOption.delay_ms = 0
self.sendOption.timeout_ms = 3
self.cyclic_t_end = 30 #Total duration of the thread in seconds. 0 means infinite.
self.cyclic_thread = {}
self.kill_the_thread = False
self.already_stopped = False
self.cyclic_running = False
def MoveToHomePosition(self):
# Make sure the arm is in Single Level Servoing mode
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
self.base.SetServoingMode(base_servo_mode)
# Move arm to ready position
print("Moving the arm to a safe position")
action_type = Base_pb2.RequestedActionType()
action_type.action_type = Base_pb2.REACH_JOINT_ANGLES
action_list = self.base.ReadAllActions(action_type)
action_handle = None
for action in action_list.action_list:
if action.name == "Home":
action_handle = action.handle
if action_handle == None:
print("Can't reach safe position. Exiting")
return False
self.base.ExecuteActionFromReference(action_handle)
time.sleep(20) # Leave time to action to complete
return True
def InitCyclic(self, sampling_time_cyclic, t_end, print_stats):
if self.cyclic_running:
return True
# Move to Home position first
if not self.MoveToHomePosition():
return False
print("Init Cyclic")
sys.stdout.flush()
base_feedback = self.SendCallWithRetry(self.base_cyclic.RefreshFeedback, 3)
if base_feedback:
self.base_feedback = base_feedback
if len(self.base_feedback.actuators) == self.expected_number_of_actuators:
# Init command frame
for x in range(self.actuator_count):
self.base_command.actuators[x].flags = 1 # servoing
self.base_command.actuators[x].position = self.base_feedback.actuators[x].position
# First actuator is going to be controlled in torque
# To ensure continuity, torque command is set to measure
self.base_command.actuators[0].torque_joint = self.base_feedback.actuators[0].torque
# Set arm in LOW_LEVEL_SERVOING
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING
self.base.SetServoingMode(base_servo_mode)
# Send first frame
self.base_feedback = self.base_cyclic.Refresh(self.base_command, 0, self.sendOption)
# Set first actuator in torque mode now that the command is equal to measure
control_mode_message = ActuatorConfig_pb2.ControlModeInformation()
control_mode_message.control_mode = ActuatorConfig_pb2.ControlMode.Value('TORQUE')
device_id = 1 # first actuator as id = 1
self.SendCallWithRetry(self.actuator_config.SetControlMode, 3, control_mode_message, device_id)
# Init cyclic thread
self.cyclic_t_end = t_end
self.cyclic_thread = threading.Thread(target=self.RunCyclic, args=(sampling_time_cyclic, print_stats))
self.cyclic_thread.daemon = True
self.cyclic_thread.start()
return True
else:
print("InitCyclic: number of actuators in base_feedback does not match expected number")
return False
else:
print("InitCyclic: failed to communicate")
return False
def RunCyclic(self, t_sample, print_stats):
self.cyclic_running = True
print("Run Cyclic")
sys.stdout.flush()
cyclic_count = 0 # Counts refresh
stats_count = 0 # Counts stats prints
failed_cyclic_count = 0 # Count communication timeouts
# Initial delta between first and last actuator
init_delta_position = self.base_feedback.actuators[0].position - self.base_feedback.actuators[6].position
# Initial first and last actuator torques; avoids unexpected movement due to torque offsets
init_last_torque = self.base_feedback.actuators[6].torque
init_first_torque = -self.base_feedback.actuators[0].torque # Torque measure is reversed compared to actuator direction
t_now = time.time()
t_cyclic = t_now # cyclic time
t_stats = t_now # print time
t_init = t_now # init time
print("Running torque control example for {} seconds".format(self.cyclic_t_end))
while not self.kill_the_thread:
t_now = time.time()
# Cyclic Refresh
if (t_now - t_cyclic) >= t_sample:
t_cyclic = t_now
# Position command to first actuator is set to measured one to avoid following error to trigger
# Bonus: When doing this instead of disabling the following error, if communication is lost and first
# actuator continue to move under torque command, resulting position error with command will
# trigger a following error and switch back the actuator in position command to hold its position
self.base_command.actuators[0].position = self.base_feedback.actuators[0].position
# First actuator torque command is set to last actuator torque measure times an amplification
# self.base_command.actuators[0].torque_joint = init_first_torque + \
# self.torque_amplification * (self.base_feedback.actuators[6].torque - init_last_torque)
self.base_command.actuators[0].torque_joint = 5.0
print(self.base_command.actuators[0].torque_joint)
# First actuator position is sent as a command to last actuator
self.base_command.actuators[6].position = self.base_feedback.actuators[0].position - init_delta_position
# Incrementing identifier ensure actuators can reject out of time frames
self.base_command.frame_id += 1
if self.base_command.frame_id > 65535:
self.base_command.frame_id = 0
for i in range(self.expected_number_of_actuators):
self.base_command.actuators[i].command_id = self.base_command.frame_id
# Frame is sent
try:
self.base_feedback = self.base_cyclic.Refresh(self.base_command, 0, self.sendOption)
except:
failed_cyclic_count = failed_cyclic_count + 1
print("failed")
cyclic_count = cyclic_count + 1
# Stats Print
if print_stats and ((t_now - t_stats) > 1):
t_stats = t_now
stats_count = stats_count + 1
cyclic_count = 0
failed_cyclic_count = 0
sys.stdout.flush()
if self.cyclic_t_end != 0 and (t_now - t_init > self.cyclic_t_end):
print("Cyclic Finished")
sys.stdout.flush()
break
self.cyclic_running = False
return True
def StopCyclic(self):
print ("Stopping the cyclic and putting the arm back in position mode...")
if self.already_stopped:
return
# Kill the thread first
if self.cyclic_running:
self.kill_the_thread = True
self.cyclic_thread.join()
# Set first actuator back in position mode
control_mode_message = ActuatorConfig_pb2.ControlModeInformation()
control_mode_message.control_mode = ActuatorConfig_pb2.ControlMode.Value('POSITION')
device_id = 1 # first actuator has id = 1
self.SendCallWithRetry(self.actuator_config.SetControlMode, 3, control_mode_message, device_id)
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
self.base.SetServoingMode(base_servo_mode)
self.cyclic_t_end = 0.1
self.already_stopped = True
print('Clean Exit')
@staticmethod
def SendCallWithRetry(call, retry, *args):
i = 0
arg_out = []
while i < retry:
try:
arg_out = call(*args)
break
except:
i = i + 1
continue
if i == retry:
print("Failed to communicate")
return arg_out
def main():
# Create connection to the device and get the router
global args
global e
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
device_manager = DeviceManagerClient(router)
#Activate the continuous auto-focus of the camera
vision_config = VisionConfigClient(router)
vision_device_id = vision_action.example_vision_get_device_id(
device_manager)
# if vision_device_id != 0:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=2)
# Initial Movements
success = True
success &= highlevel_movements.send_gripper_command(
base, value=0) #open gripper
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == True: #Robot already in position?
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport pose
temp_speed = 3
else:
temp_speed = 10 #robot further away so give more time
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=[
231.4, 266.21, 67.14, 216.41, 346.65, 114.31, 10.59
],
speed=speedj) #snake
time.sleep(0.4)
success &= highlevel_movements.move_to_waypoint_linear(
e,
base,
base_cyclic, [0.02, -0.511, 0.184, 90.689, 3.832, 4.054],
speed=3) #pre bottle
success &= highlevel_movements.move_to_waypoint_linear(
e,
base,
base_cyclic, [0.023, -0.539, 0.184, 90.687, 3.828, 4.049],
speed=3) #bottle
success &= highlevel_movements.send_gripper_command(base, value=0.8)
#time.sleep(2)
success &= highlevel_movements.move_to_waypoint_linear(
e,
base,
base_cyclic, [0.032, -0.553, 0.335, 90.691, 3.831, 4.048],
speed=3) #move bottle up
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=[
270.94, 42.99, 348.31, 305.15, 0.84, 286.28, 91.49
],
speed=speedj) #final
time.sleep(0.5)
#INSERT Focus on target_pose
#search for a face and interrupt when found
success &= highlevel_movements.example_send_joint_speeds(
e, base, speeds=[12, 0, 0, 0, 0, 0,
0], timer=0) #robot rotates until face found
while success_flag_shared.value == False and e.is_set(
) == False: #success_flag_shared gets True when face was detected
time.sleep(0.05)
base.Stop()
#Follow Face
t0 = time.time()
token = False # plays a role when beer has to be handed over after target was in lock for some time
target_reached_flag = False
beer_mode = False #hand over beer
print("searching for person")
while e.is_set() == False:
if time.time(
) - t0 > action_time_interval and success_flag_shared.value == True and beer_mode == False:
#calculate the pose increment and rotate robot into target
target_reached_flag = functions_.rotate_to_target(
delta_shared[:], w_shared.value, h_shared.value, max_speed,
min_speed, target_circle_size, base, base_cyclic)
t0 = time.time()
elif time.time(
) - t0 > action_time_interval and success_flag_shared.value == False and beer_mode == False:
#no target in sight
base.Stop()
# if no target was detected, search with base rotation
if time.time() - t0 > 3:
#search for a face and interrupt when found
highlevel_movements.example_send_joint_speeds(
e, base, speeds=[12, 0, 0, 0, 0, 0, 0],
timer=0) #robot rotates until face found
while success_flag_shared.value == False and e.is_set(
) == False: #success_flag_shared gets True when face was detected
time.sleep(0.05)
base.Stop()
t0 = time.time()
#after robot is Xs in target, hand over beer
if (target_reached_flag == True and token == True and
success_flag_shared.value == True) or beer_mode == True:
elapsed_time.value = time.time() - beer_timer
if elapsed_time.value > beer_time:
if beer_mode == False:
reach_beer_twist_timer = time.time()
beer_mode = True
#the target was long enough in scope
tool_twist_duration = 6
if time.time(
) - reach_beer_twist_timer < tool_twist_duration:
highlevel_movements.tool_twist_time(
e,
base,
tool_twist_duration,
pose_distance=beer_pass_dist)
else:
base.Stop()
time.sleep(2) #swing out
#wait until force applied to robot
torque_tool = functions_.TorqueTool()
torque_tool.tap_toggle(base_cyclic, torque_threshold=5)
#open gripper
highlevel_movements.send_gripper_command(base,
value=0.0)
time.sleep(2)
#move into idle upright joint_position
#success &= highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=[87.58, 78.68, 353.68, 216.49, 4.0, 336.91, 89.14],speed=speedj)
print("beer was passed")
break
elif target_reached_flag == True and success_flag_shared.value == True:
beer_timer = time.time()
token = True
else:
token = False
elapsed_time.value = 0
if e.is_set() == True or no_high_five == True:
return
else:
# move robot up so he is on face eight again
rev_beer_pass_dist = [i * (-1) for i in beer_pass_dist]
tool_twist_duration = 6
# highlevel_movements.tool_twist_time(e,base,tool_twist_duration,pose_distance=beer_pass_dist)
highlevel_movements.tool_twist_time(
e, base, tool_twist_duration, pose_distance=rev_beer_pass_dist)
# print("Done")
#sys.exit()
# follow a little then high five to person
t0 = time.time()
t0_wave = time.time()
time_to_wave = False
while e.is_set() == False:
if time.time(
) - t0 > action_time_interval and success_flag_shared.value == True and time_to_wave == False:
target_reached_flag = functions_.rotate_to_target(
delta_shared[:], target_middle_shared[:],
vision_middle_shared[:], w_shared.value,
h_shared.value, max_speed, min_speed,
target_circle_size, base, base_cyclic)
t0 = time.time()
elif time.time(
) - t0 > action_time_interval and success_flag_shared.value == False and time_to_wave == False:
base.Stop()
if time.time() - t0_wave > 5:
time_to_wave = True
base.Stop()
time.sleep(0.5)
break
if time.time() - t0_wave > 5:
elapsed_time.value = -1
print("time to high five!")
#read the joint angles to get the base angle
pose = highlevel_movements.get_joint_angles(base_cyclic)
base_angle = pose[0]
#enter high five position with base rotation
# high_five(base_angle=base_angle)
high_five_pos[0] = base_angle
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=high_five_pos,
speed=speedj)
torque_tool = functions_.TorqueTool()
torque_tool.tap_toggle(base_cyclic, torque_threshold=12)
time.sleep(0.1)
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=[
base_angle, 352.44, 355.16, 329.09, 1.48, 38.79, 96.13
],
speed=speedj)
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=high_five_pos,
speed=speedj)
elapsed_time.value = -2
# transport_position()
success = True
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=transport_pos,
speed=speedj) #transport pose
#process finished
base.Stop()
def __init__(self, router, router_real_time, proportional_gain=2.0):
"""
GripperLowLevelExample class constructor.
Inputs:
kortex_api.RouterClient router: TCP router
kortex_api.RouterClient router_real_time: Real-time UDP router
float proportional_gain: Proportional gain used in control loop (default value is 2.0)
Outputs:
None
Notes:
- Actuators and gripper initial position are retrieved to set initial positions
- Actuator and gripper cyclic command objects are created in constructor. Their
references are used to update position and speed.
"""
self.proportional_gain = proportional_gain
###########################################################################################
# UDP and TCP sessions are used in this example.
# TCP is used to perform the change of servoing mode
# UDP is used for cyclic commands.
#
# 2 sessions have to be created: 1 for TCP and 1 for UDP
###########################################################################################
self.router = router
self.router_real_time = router_real_time
# Create base client using TCP router
self.base = BaseClient(self.router)
# Create base cyclic client using UDP router.
self.base_cyclic = BaseCyclicClient(self.router_real_time)
# Create base cyclic command object.
self.base_command = BaseCyclic_pb2.Command()
self.base_command.frame_id = 0
self.base_command.interconnect.command_id.identifier = 0
self.base_command.interconnect.gripper_command.command_id.identifier = 0
# Add motor command to interconnect's cyclic
self.motorcmd = self.base_command.interconnect.gripper_command.motor_cmd.add(
)
# Set gripper's initial position velocity and force
base_feedback = self.base_cyclic.RefreshFeedback()
self.motorcmd.position = base_feedback.interconnect.gripper_feedback.motor[
0].position
self.motorcmd.velocity = 0
self.motorcmd.force = 100
for actuator in base_feedback.actuators:
self.actuator_command = self.base_command.actuators.add()
self.actuator_command.position = actuator.position
self.actuator_command.velocity = 0.0
self.actuator_command.torque_joint = 0.0
self.actuator_command.command_id = 0
print("Position = ", actuator.position)
# Save servoing mode before changing it
self.previous_servoing_mode = self.base.GetServoingMode()
# Set base in low level servoing mode
servoing_mode_info = Base_pb2.ServoingModeInformation()
servoing_mode_info.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING
self.base.SetServoingMode(servoing_mode_info)
router.SetActivationStatus(False)
transport.disconnect()
DEVICE_IP = "192.168.1.10"
DEVICE_PORT = 10000
# Setup API
errorCallback = lambda kException: print(
"_________ callback error _________ {}".format(kException))
transport = UDPTransport()
router = RouterClient(transport, errorCallback)
transport.connect(DEVICE_IP, DEVICE_PORT)
# Create session
session_info = Session_pb2.CreateSessionInfo()
session_info.username = '******'
session_info.password = '******'
session_info.session_inactivity_timeout = 60000 # (milliseconds)
session_info.connection_inactivity_timeout = 2000 # (milliseconds)
session_manager = SessionManager(router)
session_manager.CreateSession(session_info)
# Create required services
# Create required services
device_manager_service = DeviceManagerClient(router)
vision_config_service = VisionConfigClient(router)
base_client_service = BaseClient(router)
class UARTBridge:
def __init__(self, router, ip_address):
self.router = router
self.base_ip_address = ip_address
# Create services
self.base = BaseClient(self.router)
self.device_manager = DeviceManagerClient(self.router)
self.interconnect_config = InterconnectConfigClient(self.router)
self.interconnect_device_id = self.GetDeviceIdFromDevType(
Common_pb2.INTERCONNECT, 0)
if (self.interconnect_device_id is None):
print(
"Could not find the Interconnect in the device list, exiting..."
)
sys.exit(0)
def GetDeviceIdFromDevType(self, device_type, device_index=0):
devices = self.device_manager.ReadAllDevices()
current_index = 0
for device in devices.device_handle:
if device.device_type == device_type:
if current_index == device_index:
print("Found the Interconnect on device identifier {}".
format(device.device_identifier))
return device.device_identifier
current_index += 1
return None
def Configure(self, port_id, enabled, speed, word_length, stop_bits,
parity):
'''
Enable and configure UART on interconnect. This will open a TCP port on the interconnect. This
port allows bridging TCP socket to UART.
'''
uart_config = Common_pb2.UARTConfiguration()
uart_config.port_id = port_id
uart_config.enabled = enabled # Setting enabled to true opens the TCP port dedicated to UART bridging. Setting this
# field to false disables designated uart and closes the TCP port.
uart_config.speed = speed
uart_config.word_length = word_length
uart_config.stop_bits = stop_bits
uart_config.parity = parity
self.interconnect_config.SetUARTConfiguration(
uart_config, deviceId=self.interconnect_device_id)
def EnableBridge(self, bridge_type, target=0, output=0):
# Create bridge configuration
bridge_config = Base_pb2.BridgeConfig()
bridge_config.device_identifier = self.interconnect_device_id
bridge_config.bridgetype = bridge_type
# If either target or ouput port has valid port value, add port config to bridge configuration
if target or output:
bridge_config.port_config.target_port = 0
bridge_config.port_config.out_port = 0
if target:
bridge_config.port_config.target_port = target
if output:
bridge_config.port_config.out_port = output
# Send the configuration and return the result
bridge_result = self.base.EnableBridge(bridge_config)
return bridge_result
def DisableBridge(self, bridge_id):
return self.base.DisableBridge(bridge_id)
def ExampleSendDataAndReadItBack(self):
# Enable port bridging on base.
bridge_result = self.EnableBridge(Base_pb2.BRIDGE_TYPE_UART)
bridge_id = bridge_result.bridge_id
if bridge_result.status != Base_pb2.BRIDGE_STATUS_OK:
print("Error creating bridge on base, exiting...")
return
# Get created bridge's configuration.
bridge_config = self.base.GetBridgeConfig(bridge_id)
base_port = bridge_config.port_config.out_port
interconnect_port = bridge_config.port_config.target_port
print(
"UARTBridge ID # %i created between Interconnect (dev# %i)'s port #%i and external port #%i"
% (bridge_id.bridge_id, self.interconnect_device_id,
interconnect_port, base_port))
# Open a socket to base's forwarded port.
client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client_socket.connect((self.base_ip_address, base_port))
client_socket.setblocking(0)
print("UART bridge object initialized")
# Send data to be written on UART using TCP connection.
client_socket.send(
b"This data is being written on Interconnect's expansion port UART\n"
)
# Wait for data to be received from UART
print("Waiting 10 seconds for data from uart...")
sys.stdout.write("Received data : ")
sys.stdout.flush()
startTime = time.time()
while time.time() - startTime < 10:
readready, _, _ = select.select([client_socket], [], [], 1)
if readready:
data = client_socket.recv(1)
if len(data):
sys.stdout.write(data.decode("utf-8"))
sys.stdout.flush()
# Disconnect client socket.
client_socket.close()
# Disable bridge on base.
self.DisableBridge(bridge_id)