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
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
