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