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 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:
# 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 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 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 main():
# Import the utilities helper module
sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
# Example core
success = True
success &= example_move_to_home_position(base)
success &= example_cartesian_action_movement(base, base_cyclic)
success &= example_angular_action_movement(base)
success &= example_move_to_home_position(base)
success &= example_cartesian_trajectory_movement(base, base_cyclic)
success &= example_angular_trajectory_movement(base)
return 0 if success else 1
def __init__(self, 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 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
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',
'--labels',
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()
class KinovaRobotEnv:
metadata = {'render_modes':['human', 'rbg_array']}
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 seed(self, seed):
pass
def close(self):
self.connection.__exit__(None, None, None)
def render(self, mode='human', width=84, height=84):
# return np.zeros((width, height, 3))
ret, frame = self.webcam.read()
frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
if mode == 'human':
cv2.imshow('', frame)
elif mode == 'rgb_array':
frame = cv2.resize(frame, (width, height))
return frame
def sample_goal(self, target_range=None):
if target_range is None:
target_range = self.target_range
self.goal = self.init_xyz + (-1 + 2*np.random.rand(*self.init_xyz.shape))*self.target_range
return self.goal
def reset(self):
self.sample_goal()
self._set_to_home()
return self._get_obs_dict()
def step(self, act):
print(f'act: {act}')
act = act.copy()*self.action_scale
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
action.reach_pose.constraint.speed.translation = self.speed
feedback = self.base_cyclic.RefreshFeedback()
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = feedback.base.tool_pose_x + act[0] # (meters)
cartesian_pose.y = feedback.base.tool_pose_y + act[1] # (meters)
cartesian_pose.z = feedback.base.tool_pose_z + act[2] # (meters)
cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees)
cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees)
cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees)
self.base.ExecuteAction(action)
duration = 1. + (np.linalg.norm(act)/self.speed)
print(f'step w duration {duration}')
time.sleep(duration)
obs_dict = self._get_obs_dict()
info = dict()
reward = self.compute_reward(
obs_dict['achieved_goal'],
obs_dict['desired_goal'],
info,
)
done = True if np.abs(reward) < self.distance_threshold else False
info['is_success'] = done
return obs_dict, reward, done, info
def _set_to(self, xyz):
action = Base_pb2.Action()
action.name = "Example Cartesian action movement"
action.application_data = ""
action.reach_pose.constraint.speed.translation = self.speed
feedback = self.base_cyclic.RefreshFeedback()
current_pos = np.asarray([feedback.base.tool_pose_x, feedback.base.tool_pose_y, feedback.base.tool_pose_z])
distance_to_xyz = np.linalg.norm(current_pos - xyz)
duration = 1. + (distance_to_xyz/self.speed)
print(f'_set_to w duration {duration}')
cartesian_pose = action.reach_pose.target_pose
cartesian_pose.x = xyz[0]
cartesian_pose.y = xyz[1]
cartesian_pose.z = xyz[2]
cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees)
cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees)
cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees)
self.base.ExecuteAction(action)
time.sleep(duration)
def _set_to_home(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:
raise Run("Can't reach safe position. Exiting")
self.base.ExecuteActionFromReference(action_handle)
print('Resetting for duration 6')
time.sleep(6)
def compute_reward(self, achieved_goal, desired_goal, info):
d = np.linalg.norm(achieved_goal - desired_goal, axis=-1)
return 0. if d < self.distance_threshold else -1.
def _is_success(self, achieved_goal, desired_goal):
d = np.linalg.norm(achieved_goal - desired_goal, axis=-1)
return True if d < self.distance_threshold else False
def _get_obs(self):
feedback = self.base_cyclic.RefreshFeedback()
observation = np.asarray([
feedback.base.tool_pose_x,
feedback.base.tool_pose_y,
feedback.base.tool_pose_z,
])
return observation
def _get_obs_dict(self):
observation = self._get_obs()
achieved_goal = observation[:3]
desired_goal = self.goal
return dict(
observation=observation,
achieved_goal=achieved_goal,
desired_goal=desired_goal,
)
def wave():
elapsed_time.value = 0
global e
global args
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
device_manager = DeviceManagerClient(router)
#Activate the continuous auto-focus of the camera
vision_config = VisionConfigClient(router)
vision_device_id = vision_action.example_vision_get_device_id(
device_manager)
# if vision_device_id != 0:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=2)
# check if robot is not in transport position, if so, move safely
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == True: #Robot in transport position?
highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
#Wave Sequence
wave_speed = 70 #degrees/sec
# wave_start_pose=[180, 0, 0, 0, 90, 90, 270] #joint4
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,30,0,0,-30])) #joint4
# wave_start_pose=[180, 0, 0, 0, 0, 0, 0] #joint5
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,0,0,40,0])) #joint4
# wave_start_pose=[182.49, 36.45, 0.78, 269.14, 2.33, 56.02, 0.11] #joint5
# wave_left_pose=list(map(add,wave_start_pose,[0,0,0,0,0,40,0])) #joint4
wave_start_pose = [182.9, 320.18, 177.58, 260.17, 1.06, 65.48,
180.64] #joint5
wave_left_pose = list(map(add, wave_start_pose,
[0, 0, 0, 0, 0, 40, 0])) #joint4
# highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=wave_start_pose,speed=speedj)
# sys.exit()
highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=wave_left_pose,
speed=speedj) #okay
#time.sleep(1)
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=-1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=1,
base=base,
base_cyclic=base_cyclic)
time.sleep(0.4) #must be because else meassurements are wrong
highlevel_movements.sinus_move(e,
v_max=wave_speed,
degrees_move=60,
joint_id=5,
direction=-1,
base=base,
base_cyclic=base_cyclic)
# time.sleep(0.4)
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=-1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=1,base=base,base_cyclic=base_cyclic)
# time.sleep(0.4) #must be because else meassurements are wrong
# highlevel_movements.sinus_move(e,v_max=wave_speed,degrees_move=60,joint_id=5,direction=-1,base=base,base_cyclic=base_cyclic)
elapsed_time.value = -2
time.sleep(0.4)
highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
highlevel_movements.angular_action_movement(
e, base, base_cyclic, joint_positions=transport_pos,
speed=speedj) #transport pose
def __init__(self, router, router_real_time, proportional_gain=2.0):
"""
GripperLowLevelExample class constructor.
Inputs:
kortex_api.RouterClient router: TCP router
kortex_api.RouterClient router_real_time: Real-time UDP router
float proportional_gain: Proportional gain used in control loop (default value is 2.0)
Outputs:
None
Notes:
- Actuators and gripper initial position are retrieved to set initial positions
- Actuator and gripper cyclic command objects are created in constructor. Their
references are used to update position and speed.
"""
self.proportional_gain = proportional_gain
###########################################################################################
# UDP and TCP sessions are used in this example.
# TCP is used to perform the change of servoing mode
# UDP is used for cyclic commands.
#
# 2 sessions have to be created: 1 for TCP and 1 for UDP
###########################################################################################
self.router = router
self.router_real_time = router_real_time
# Create base client using TCP router
self.base = BaseClient(self.router)
# Create base cyclic client using UDP router.
self.base_cyclic = BaseCyclicClient(self.router_real_time)
# Create base cyclic command object.
self.base_command = BaseCyclic_pb2.Command()
self.base_command.frame_id = 0
self.base_command.interconnect.command_id.identifier = 0
self.base_command.interconnect.gripper_command.command_id.identifier = 0
# Add motor command to interconnect's cyclic
self.motorcmd = self.base_command.interconnect.gripper_command.motor_cmd.add(
)
# Set gripper's initial position velocity and force
base_feedback = self.base_cyclic.RefreshFeedback()
self.motorcmd.position = base_feedback.interconnect.gripper_feedback.motor[
0].position
self.motorcmd.velocity = 0
self.motorcmd.force = 100
for actuator in base_feedback.actuators:
self.actuator_command = self.base_command.actuators.add()
self.actuator_command.position = actuator.position
self.actuator_command.velocity = 0.0
self.actuator_command.torque_joint = 0.0
self.actuator_command.command_id = 0
print("Position = ", actuator.position)
# Save servoing mode before changing it
self.previous_servoing_mode = self.base.GetServoingMode()
# Set base in low level servoing mode
servoing_mode_info = Base_pb2.ServoingModeInformation()
servoing_mode_info.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING
self.base.SetServoingMode(servoing_mode_info)
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
