def example_cartesian_trajectory_movement(base, base_cyclic):
constrained_pose = Base_pb2.ConstrainedPose()
feedback = base_cyclic.RefreshFeedback()
cartesian_pose = constrained_pose.target_pose
cartesian_pose.x = feedback.base.tool_pose_x # (meters)
cartesian_pose.y = feedback.base.tool_pose_y - 0.1 # (meters)
cartesian_pose.z = feedback.base.tool_pose_z - 0.2 # (meters)
cartesian_pose.theta_x = feedback.base.tool_pose_theta_x # (degrees)
cartesian_pose.theta_y = feedback.base.tool_pose_theta_y # (degrees)
cartesian_pose.theta_z = feedback.base.tool_pose_theta_z # (degrees)
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e), Base_pb2.NotificationOptions())
print("Reaching cartesian pose...")
base.PlayCartesianTrajectory(constrained_pose)
print("Waiting for movement 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
def waypoint_trajectory_movement(base, base_cyclic,waypoint,speed=0.1,watch_for_flag=False):
constrained_pose = Base_pb2.ConstrainedPose()
constrained_pose.constraint.speed.translation = speed # m/s
constrained_pose.constraint.speed.orientation = 30 # deg/sec
cartesian_pose = constrained_pose.target_pose
cartesian_pose.x = waypoint[0] # (meters)
cartesian_pose.y = waypoint[1] # (meters)
cartesian_pose.z = waypoint[2] # (meters)
cartesian_pose.theta_x = waypoint[3] # (degrees)
cartesian_pose.theta_y = waypoint[4] # (degrees)
cartesian_pose.theta_z = waypoint[5] # (degrees)
e = threading.Event()
notification_handle = base.OnNotificationActionTopic(
check_for_end_or_abort(e),
Base_pb2.NotificationOptions()
)
print("Reaching cartesian pose...")
base.PlayCartesianTrajectory(constrained_pose)
if watch_for_flag==True:
while success_flag_shared.value==0 or e.is_set()==False:
time.sleep(0.1)
finished=True
base.stop()
base.Unsubscribe(notification_handle)
else:
t0=time.time()
while e.is_set()==False and time.time()-t0<TIMEOUT_DURATION:
if e0.is_set()==True: #all processes were terminated from GUI
base.Stop()
base.Unsubscribe(notification_handle)
return False
finished = e.wait(TIMEOUT_DURATION) #waits until True
base.Unsubscribe(notification_handle)
if finished:
print("Angular movement completed")
else:
print("Timeout on action notification wait")
return finished
def example_cartesian_trajectory_movement(base):
constrained_pose = Base_pb2.ConstrainedPose()
cartesian_pose = constrained_pose.target_pose
cartesian_pose.x = 0.80 # (meters)
cartesian_pose.y = 0 # (meters)
cartesian_pose.z = 0.36 # (meters)
cartesian_pose.theta_x = 10 # (degrees)
cartesian_pose.theta_y = 90 # (degrees)
cartesian_pose.theta_z = 10 # (degrees)
print("Reaching cartesian pose...")
base.PlayCartesianTrajectory(constrained_pose)
print("Waiting 20 seconds for movement to finish ...")
time.sleep(20)
print("Cartesian pose reached")