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_angular_trajectory_movement(base):
constrained_joint_angles = Base_pb2.ConstrainedJointAngles()
actuator_count = base.GetActuatorCount()
# Place arm straight up
for joint_id in range(actuator_count.count):
joint_angle = constrained_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
joint_angle.value = 0
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 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 joint_angle_command(base_client_service, j_id, value):
constrained_joint_angles = Base_pb2.ConstrainedJointAngles()
state = read_joints(base_client_service)
act_count = base_client_service.GetActuatorCount()
for joint_id in range(act_count.count):
joint_angle = constrained_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
if joint_id == j_id:
joint_angle.value = value
else:
joint_angle.value = state[joint_id]
print("joint ID {} Value {}".format(joint_angle.joint_identifier,
joint_angle.value))
base_client_service.PlayJointTrajectory(constrained_joint_angles)
def example_angular_trajectory_movement(base):
constrained_joint_angles = Base_pb2.ConstrainedJointAngles()
actuator_count = base.GetActuatorCount()
# Place arm straight up
for joint_id in range(actuator_count.count):
joint_angle = constrained_joint_angles.joint_angles.joint_angles.add()
joint_angle.joint_identifier = joint_id
joint_angle.value = 0
print("Reaching joint angles...")
base.PlayJointTrajectory(constrained_joint_angles)
print("Waiting 20 seconds for movement to finish ...")
time.sleep(20)
print("Joint angles reached")
