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