def follow_trajectory_goal(self,
traj_msg: FollowJointTrajectoryGoal,
feedback_cb: Optional[Callable] = None,
stop_cb: Optional[Callable] = None):
result = FollowJointTrajectoryResult()
# Interpolate the trajectory to a fine resolution
# if you set max_step_size to be large and position tolerance to be small, then things will be jerky
if len(traj_msg.trajectory.points) == 0:
rospy.loginfo("Ignoring empty trajectory")
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
result.error_string = "empty trajectory"
return result
# construct a list of the tolerances in order of the joint names
trajectory_joint_names = traj_msg.trajectory.joint_names
tolerance = get_ordered_tolerance_list(trajectory_joint_names, traj_msg.path_tolerance)
goal_tolerance = get_ordered_tolerance_list(trajectory_joint_names, traj_msg.goal_tolerance, is_goal=True)
interpolated_points = interpolate_joint_trajectory_points(traj_msg.trajectory.points, max_step_size=0.1)
if len(interpolated_points) == 0:
rospy.loginfo("Trajectory was empty after interpolation")
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
result.error_string = "empty trajectory"
return result
rospy.logdebug(interpolated_points)
trajectory_point_idx = 0
t0 = rospy.Time.now()
while True:
# tiny sleep lets the listeners process messages better, results in smoother following
rospy.sleep(1e-6)
desired_point = interpolated_points[trajectory_point_idx]
command_failed, command_failed_msg = self.robot.send_joint_command(trajectory_joint_names, desired_point)
# get feedback
actual_point = self.get_actual_trajectory_point(trajectory_joint_names)
# let the caller stop
if stop_cb is not None:
stop, stop_msg = stop_cb(actual_point)
else:
stop = None
stop_msg = ""
dt = rospy.Time.now() - t0
error = waypoint_error(actual_point, desired_point)
rospy.logdebug_throttle(1, f"{error} {desired_point.time_from_start.to_sec()} {dt.to_sec()}")
if desired_point.time_from_start.to_sec() > 0 and dt > desired_point.time_from_start * 5.0:
stop = True
if trajectory_point_idx == len(interpolated_points) - 1:
stop_msg = f"timeout. expected t={desired_point.time_from_start.to_sec()} but t={dt.to_sec()}." \
+ f" error to waypoint is {error}, tolerance is {goal_tolerance}"
else:
stop_msg = f"timeout. expected t={desired_point.time_from_start.to_sec()} but t={dt.to_sec()}." \
+ f" error to waypoint is {error}, tolerance is {tolerance}"
if command_failed or stop:
# command the current configuration
self.robot.send_joint_command(trajectory_joint_names, actual_point)
rospy.loginfo("Preempt requested, aborting.")
if command_failed_msg:
rospy.loginfo(command_failed_msg)
if stop_msg:
rospy.logwarn(stop_msg)
result.error_code = -10
result.error_string = stop_msg
break
# If we're close enough, advance
if trajectory_point_idx >= len(interpolated_points) - 1:
if waypoint_reached(desired_point, actual_point, goal_tolerance):
# we're done!
result.error_code = FollowJointTrajectoryResult.SUCCESSFUL
break
else:
if waypoint_reached(desired_point, actual_point, tolerance):
trajectory_point_idx += 1
if feedback_cb is not None:
feedback = FollowJointTrajectoryFeedback()
feedback.header.stamp = rospy.Time.now()
feedback.joint_names = trajectory_joint_names
feedback.desired = desired_point
feedback.actual = actual_point
feedback_cb(feedback)
return result
def execute_joint_trajectory_cb(self, goal):
rospy.loginfo("Trajectory received with %d points",
len(goal.trajectory.points))
feedback = FollowJointTrajectoryFeedback()
result = FollowJointTrajectoryResult()
current_status = self._driver.get_current_gripper_status()
# Check trajectory joint names
joint_names = goal.trajectory.joint_names
if len(joint_names
) != 1 and joint_names[0] != self._driver._joint_name:
msg = "Joint trajectory joints do not match gripper joint"
rospy.logerr(msg)
result.error_code = result.INVALID_JOINTS
result.error_string = msg
self._joint_trajectory_action_server.set_aborted(result)
return
# Check trajectory points
if len(goal.trajectory.points) == 0:
msg = "Ignoring empty trajectory "
rospy.logerr(msg)
result.error_code = result.INVALID_GOAL
result.error_string = msg
self._joint_trajectory_action_server.set_aborted(result)
return
# Process goal trajectory
self._processing_goal = True
self._is_stalled = False
goal_command = CommandRobotiqGripperGoal()
feedback.joint_names = goal.trajectory.joint_names
watchdog = rospy.Timer(rospy.Duration(
goal.trajectory.points[-1].time_from_start.to_sec() + 0.5),
self._execution_timeout,
oneshot=True)
# Follow trajectory points
goal_trajectory_point = goal.trajectory.points[-1]
# Validate trajectory point
if len(goal_trajectory_point.positions) != 1:
result.error_code = result.INVALID_GOAL
result.error_string = "Invalid joint position on trajectory point "
self._joint_trajectory_action_server.set_aborted(result)
return
target_speed = 0.1
#target_speed = goal.trajectory.points[int(len(goal.trajectory.points)/2)].velocities[0] if goal.trajectory.points[int(len(goal.trajectory.points)/2)].velocities > 0 else 0.1
#print("Target gripper speed: " + target_speed)
#target_speed = goal_trajectory_point.velocities[0] if len(goal_trajectory_point.velocities) > 0 else 0.01
target_force = goal_trajectory_point.effort[0] if len(
goal_trajectory_point.effort) > 0 else 0.1
goal_command.position = self._driver.from_radians_to_distance(
goal_trajectory_point.positions[0])
goal_command.speed = abs(target_speed) # To-Do: Convert to rad/s
goal_command.force = target_force
# Send incoming command to gripper driver
self._driver.update_gripper_command(goal_command)
# Set feedback desired value
feedback.desired.positions = [goal_trajectory_point.positions[0]]
while not rospy.is_shutdown(
) and self._processing_goal and not self._is_stalled:
current_status = self._driver.get_current_gripper_status()
feedback.actual.positions = [
self._driver.get_current_joint_position()
]
error = abs(feedback.actual.positions[0] -
feedback.desired.positions[0])
rospy.logdebug("Error : %.3f -- Actual: %.3f -- Desired: %.3f",
error, self._driver.get_current_joint_position(),
feedback.desired.positions[0])
feedback.error.positions = [error]
self._joint_trajectory_action_server.publish_feedback(feedback)
# Check for errors
if current_status.fault_status != 0 and not self._is_stalled:
self._is_stalled = True
self._processing_goal = False
rospy.logerr(msg)
result.error_code = -6
result.error_string = "Gripper fault status (gFLT): " + current_status.fault_status
self._joint_trajectory_action_server.set_aborted(result)
return
# Check if object was detected
if current_status.obj_detected:
watchdog.shutdown() # Stop timeout watchdog.
self._processing_goal = False
self._is_stalled = False
result.error_code = result.SUCCESSFUL
result.error_string = "Object detected/grasped"
self._joint_trajectory_action_server.set_succeeded(result)
return
# Check if current trajectory point was reached
if error < GOAL_DETECTION_THRESHOLD:
break
# Entire trajectory was followed/reached
watchdog.shutdown()
rospy.logdebug(self._action_name + ": goal reached")
result.error_code = result.SUCCESSFUL
result.error_string = "Goal reached"
self._joint_trajectory_action_server.set_succeeded(result)
self._processing_goal = False
self._is_stalled = False
