def example_move_to_home_position(base):
# 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
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 = 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
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e),
Base_pb2.NotificationOptions()
)
base.ExecuteActionFromReference(action_handle)
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Safe position reached")
else:
print("Timeout on action notification wait")
return finished
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')
def example_move_to_start_position(base):
# 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
base.SetServoingMode(base_servo_mode)
# Move arm to ready position
constrained_joint_angles = Base_pb2.ConstrainedJointAngles()
# We suppose the arm is a 7DOF
angles = [0.0, 0.0, 0.0, 90.0, 0.0, 0.0, 0.0]
# Actuator 4 at 90 degrees
for joint_id in range(len(angles)):
joint_angle = constrained_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
joint_angle.value = angles[joint_id]
print("Reaching joint angles...")
base.PlayJointTrajectory(constrained_joint_angles)
print("Waiting 20 seconds for movement to finish ...")
time.sleep(20)
print("Joint angles reached")
def example_move_to_start_position(base):
# 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
base.SetServoingMode(base_servo_mode)
# Move arm to ready position
constrained_joint_angles = Base_pb2.ConstrainedJointAngles()
actuator_count = base.GetActuatorCount().count
angles = [0.0] * actuator_count
# Actuator 4 at 90 degrees
for joint_id in range(len(angles)):
joint_angle = constrained_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
joint_angle.value = angles[joint_id]
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Reaching joint angles...")
base.PlayJointTrajectory(constrained_joint_angles)
print("Waiting for movement to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Joint angles reached")
else:
print("Timeout on action notification wait")
return finished
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
# 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: failed to communicate")
return False
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 set_low_level_servoing_mode(self):
# 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()
print("Current Mode:")
print(servoing_mode_info)
servoing_mode_info.servoing_mode = Base_pb2.LOW_LEVEL_SERVOING
self.base.SetServoingMode(servoing_mode_info)
print("Waiting 5 second for changing to low level servoing mode...")
time.sleep(5)
print("After setting low level")
print(servoing_mode_info)
print("waiting 5s to terminate setting low level")
time.sleep(5)
def example_move_to_home_position(base):
# 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
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 = 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")
sys.exit(0)
base.ExecuteActionFromReference(action_handle)
time.sleep(20) # Leave time to action to complete
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 __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 example_trajectory(base, base_cyclic):
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
base.SetServoingMode(base_servo_mode)
jointPoses = tuple(tuple())
product = base.GetProductConfiguration()
if (product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L53 or
product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
if (product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
jointPoses = ((0.0, 344.0, 75.0, 360.0, 300.0,
0.0), (0.0, 21.0, 145.0, 272.0, 32.0, 273.0),
(42.0, 334.0, 79.0, 241.0, 305.0, 56.0))
else:
# Binded to degrees of movement and each degrees correspond to one degree of liberty
degreesOfFreedom = base.GetActuatorCount()
if (degreesOfFreedom.count == 6):
jointPoses = ((360.0, 35.6, 281.8, 0.8, 23.8,
88.9), (359.6, 49.1, 272.1, 0.3, 47.0, 89.1),
(320.5, 76.5, 335.5, 293.4, 46.1, 165.6),
(335.6, 38.8, 266.1, 323.9, 49.7,
117.3), (320.4, 76.5, 335.5, 293.4, 46.1,
165.6), (28.8, 36.7, 273.2, 40.8, 39.5,
59.8), (360.0, 45.6, 251.9,
352.2, 54.3, 101.0))
else:
jointPoses = ((360.0, 35.6, 180.7, 281.8, 0.8, 23.8, 88.9),
(359.6, 49.1, 181.0, 272.1, 0.3, 47.0,
89.1), (320.5, 76.5, 166.5, 335.5, 293.4, 46.1,
165.6), (335.6, 38.8, 177.0, 266.1,
323.9, 49.7, 117.3),
(320.4, 76.5, 166.5, 335.5, 293.4, 46.1,
165.6), (28.8, 36.7, 174.7, 273.2, 40.8, 39.5,
59.8), (360.0, 45.6, 171.0, 251.9,
352.2, 54.3, 101.0))
else:
print(
"Product is not compatible to run this example please contact support with KIN number bellow"
)
print("Product KIN is : " + product.kin())
waypoints = Base_pb2.WaypointList()
waypoints.duration = 0.0
waypoints.use_optimal_blending = False
index = 0
for jointPose in jointPoses:
waypoint = waypoints.waypoints.add()
waypoint.name = "waypoint_" + str(index)
durationFactor = 1
# Joints/motors 5 and 7 are slower and need more time
if (index == 4 or index == 6):
durationFactor = 6 # Min 30 seconds
waypoint.angular_waypoint.CopyFrom(
populateAngularPose(jointPose, durationFactor))
index = index + 1
# Verify validity of waypoints
result = base.ValidateWaypointList(waypoints)
if (len(result.trajectory_error_report.trajectory_error_elements) == 0):
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Reaching angular pose trajectory...")
base.ExecuteWaypointTrajectory(waypoints)
print("Waiting for trajectory to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Angular movement completed")
else:
print("Timeout on action notification wait")
return finished
else:
print("Error found in trajectory")
print(result.trajectory_error_report)
return finished
def example_trajectory(base, base_cyclic):
base_servo_mode = Base_pb2.ServoingModeInformation()
base_servo_mode.servoing_mode = Base_pb2.SINGLE_LEVEL_SERVOING
base.SetServoingMode(base_servo_mode)
product = base.GetProductConfiguration()
waypointsDefinition = tuple(tuple())
if (product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L53 or
product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
if (product.model == Base_pb2.ProductConfiguration__pb2.MODEL_ID_L31):
kTheta_x = 90.6
kTheta_y = -1.0
kTheta_z = 150.0
waypointsDefinition = ((0.439, 0.194, 0.448, 0.0, kTheta_x,
kTheta_y, kTheta_z),
(0.200, 0.150, 0.400, 0.0, kTheta_x,
kTheta_y, kTheta_z), (0.350, 0.050, 0.300,
0.0, kTheta_x,
kTheta_y, kTheta_z))
else:
kTheta_x = 90.0
kTheta_y = 0.0
kTheta_z = 90.0
waypointsDefinition = ((0.7, 0.0, 0.5, 0.0, kTheta_x, kTheta_y,
kTheta_z), (0.7, 0.0, 0.33, 0.1, kTheta_x,
kTheta_y, kTheta_z),
(0.7, 0.48, 0.33, 0.1, kTheta_x, kTheta_y,
kTheta_z), (0.61, 0.22, 0.4, 0.1, kTheta_x,
kTheta_y, kTheta_z),
(0.7, 0.48, 0.33, 0.1, kTheta_x, kTheta_y,
kTheta_z), (0.63, -0.22, 0.45, 0.1,
kTheta_x, kTheta_y, kTheta_z),
(0.65, 0.05, 0.33, 0.0, kTheta_x, kTheta_y,
kTheta_z))
else:
print(
"Product is not compatible to run this example please contact support with KIN number bellow"
)
print("Product KIN is : " + product.kin())
waypoints = Base_pb2.WaypointList()
waypoints.duration = 0.0
waypoints.use_optimal_blending = False
index = 0
for waypointDefinition in waypointsDefinition:
waypoint = waypoints.waypoints.add()
waypoint.name = "waypoint_" + str(index)
waypoint.cartesian_waypoint.CopyFrom(
populateCartesianCoordinate(waypointDefinition))
index = index + 1
# Verify validity of waypoints
result = base.ValidateWaypointList(waypoints)
if (len(result.trajectory_error_report.trajectory_error_elements) == 0):
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Moving cartesian trajectory...")
base.ExecuteWaypointTrajectory(waypoints)
print("Waiting for trajectory to finish ...")
finished = e.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle)
if finished:
print("Cartesian trajectory with no optimization completed ")
e_opt = threading.Event()
notification_handle_opt = base.OnNotificationActionTopic(
check_for_end_or_abort(e_opt), Base_pb2.NotificationOptions())
waypoints.use_optimal_blending = True
base.ExecuteWaypointTrajectory(waypoints)
print("Waiting for trajectory to finish ...")
finished_opt = e_opt.wait(TIMEOUT_DURATION)
base.Unsubscribe(notification_handle_opt)
if (finished_opt):
print("Cartesian trajectory with optimization completed ")
else:
print(
"Timeout on action notification wait for optimized trajectory"
)
return finished_opt
else:
print(
"Timeout on action notification wait for non-optimized trajectory"
)
return finished
else:
print("Error found in trajectory")
result.trajectory_error_report.PrintDebugString()