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