Component for joint space trajectory generation from Cartesian waypoints. More...

#include <cp_joint_space_trajectory_planner.hpp>

Inheritance diagram for cl_moveit2z::CpJointSpaceTrajectoryPlanner:

Collaboration diagram for cl_moveit2z::CpJointSpaceTrajectoryPlanner:

Public Member Functions
	CpJointSpaceTrajectoryPlanner ()=default

virtual	~CpJointSpaceTrajectoryPlanner ()=default

void	onInitialize () override
	Initialize the component and create IK service client.

JointTrajectoryResult	planFromWaypoints (const std::vector< geometry_msgs::msg::PoseStamped > &waypoints, const JointTrajectoryOptions &options={})
	Compute joint space trajectory from Cartesian waypoints.

Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Private Attributes
ClMoveit2z *	moveit2zClient_ = nullptr

rclcpp::Client< moveit_msgs::srv::GetPositionIK >::SharedPtr	ikClient_

Additional Inherited Members
Protected Member Functions inherited from smacc2::ISmaccComponent
template<typename TOrthogonal , typename TClient >
void	onComponentInitialization ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TClient >
void	requiresClient (TClient *&requiredClientStorage)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingComponent (TArgs... targs)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingNamedComponent (std::string name, TArgs... targs)

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger () const

ISmaccStateMachine *	getStateMachine ()

Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Component for joint space trajectory generation from Cartesian waypoints.

This component centralizes IK-based trajectory generation:

Computes IK solutions for Cartesian waypoint sequences
Validates trajectory continuity
Detects and reports joint discontinuities
Handles initial state validation

Pattern: Trajectory planner with IK service integration

Definition at line 94 of file cp_joint_space_trajectory_planner.hpp.

Constructor & Destructor Documentation

◆ CpJointSpaceTrajectoryPlanner()

cl_moveit2z::CpJointSpaceTrajectoryPlanner::CpJointSpaceTrajectoryPlanner ( )

default

◆ ~CpJointSpaceTrajectoryPlanner()

virtual cl_moveit2z::CpJointSpaceTrajectoryPlanner::~CpJointSpaceTrajectoryPlanner ( )

virtualdefault

Member Function Documentation

◆ onInitialize()

void cl_moveit2z::CpJointSpaceTrajectoryPlanner::onInitialize ( )

inlineoverridevirtual

Initialize the component and create IK service client.

Reimplemented from smacc2::ISmaccComponent.

Definition at line 103 of file cp_joint_space_trajectory_planner.hpp.

  {
    this->requiresClient(moveit2zClient_);
 
    // Create IK service client
    ikClient_ = getNode()->create_client<moveit_msgs::srv::GetPositionIK>("/compute_ik");
 
    RCLCPP_INFO(getLogger(), "[CpJointSpaceTrajectoryPlanner] Component initialized");
  }

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), ikClient_, moveit2zClient_, and smacc2::ISmaccComponent::requiresClient().

Here is the call graph for this function:

◆ planFromWaypoints()

JointTrajectoryResult cl_moveit2z::CpJointSpaceTrajectoryPlanner::planFromWaypoints	(	const std::vector< geometry_msgs::msg::PoseStamped > &	waypoints,
		const JointTrajectoryOptions &	options = {} )

inline

Compute joint space trajectory from Cartesian waypoints.

Parameters

waypoints	Vector of Cartesian poses to follow
options	Planning configuration options

Returns: JointTrajectoryResult with success status and computed trajectory

Definition at line 120 of file cp_joint_space_trajectory_planner.hpp.

                                             {})
  {
    JointTrajectoryResult result;
 
    if (waypoints.empty())
    {
      result.errorCode = JointTrajectoryErrorCode::EMPTY_WAYPOINT_LIST;
      result.errorMessage = "No waypoints provided";
      RCLCPP_WARN(getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str());
      return result;
    }
 
    if (!moveit2zClient_)
    {
      result.errorMessage = "ClMoveit2z client not available";
      RCLCPP_ERROR(getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str());
      return result;
    }
 
    try
    {
      auto & moveGroup = moveit2zClient_->moveGroupClientInterface;
 
      // Get current robot state
      RCLCPP_INFO(
        getLogger(),
        "[CpJointSpaceTrajectoryPlanner] Getting current state for trajectory planning");
      auto currentState = moveGroup->getCurrentState(100);
      if (!currentState)
      {
        result.errorMessage = "Failed to get current robot state";
        RCLCPP_ERROR(
          getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str());
        return result;
      }
 
      // Determine group name and tip link
      std::string groupName = options.groupName.value_or(moveGroup->getName());
      std::string tipLink = options.tipLink.value_or(moveGroup->getEndEffectorLink());
 
      RCLCPP_INFO(
        getLogger(), "[CpJointSpaceTrajectoryPlanner] Planning for group '%s' with tip link '%s'",
        groupName.c_str(), tipLink.c_str());
 
      // Get joint names
      auto jointNames = currentState->getJointModelGroup(groupName)->getActiveJointModelNames();
 
      // Initialize trajectory with current state
      std::vector<double> jointPositions;
      currentState->copyJointGroupPositions(groupName, jointPositions);
 
      std::vector<std::vector<double>> trajectory;
      std::vector<rclcpp::Duration> trajectoryTimeStamps;
 
      trajectory.push_back(jointPositions);
      trajectoryTimeStamps.push_back(rclcpp::Duration(0s));
 
      // Reference time for trajectory timestamps
      auto & first = waypoints.front();
      rclcpp::Time referenceTime(first.header.stamp);
 
      // Compute IK for each waypoint
      int ikAttemptsRemaining = options.ikAttempts;
      for (size_t k = 0; k < waypoints.size(); k++)
      {
        auto & pose = waypoints[k];
 
        // Create IK request
        auto req = std::make_shared<moveit_msgs::srv::GetPositionIK::Request>();
        req->ik_request.ik_link_name = tipLink;
        req->ik_request.robot_state.joint_state.name = jointNames;
        req->ik_request.robot_state.joint_state.position = jointPositions;
        req->ik_request.group_name = groupName;
        req->ik_request.avoid_collisions = options.avoidCollisions;
        req->ik_request.pose_stamped = pose;
 
        RCLCPP_DEBUG(
          getLogger(), "[CpJointSpaceTrajectoryPlanner] Computing IK for waypoint %lu/%lu", k + 1,
          waypoints.size());
 
        // Call IK service
        auto resFuture = ikClient_->async_send_request(req);
        auto status = resFuture.wait_for(options.ikTimeout);
 
        if (status != std::future_status::ready)
        {
          result.errorMessage = "IK service timeout at waypoint " + std::to_string(k);
          RCLCPP_ERROR(
            getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str());
          result.errorCode = JointTrajectoryErrorCode::IK_SOLUTION_FAILED;
          return result;
        }
 
        auto res = resFuture.get();
        auto & prevTrajPoint = trajectory.back();
 
        // Extract joint positions from IK solution
        for (size_t j = 0; j < res->solution.joint_state.position.size(); j++)
        {
          auto & jointName = res->solution.joint_state.name[j];
          auto it = std::find(jointNames.begin(), jointNames.end(), jointName);
          if (it != jointNames.end())
          {
            int index = std::distance(jointNames.begin(), it);
            jointPositions[index] = res->solution.joint_state.position[j];
          }
        }
 
        // Continuity check
        bool shouldCheck = k > 0 || !options.allowInitialDiscontinuity;
        int discontinuityJointIndex = -1;
        double discontinuityDelta = -1;
 
        if (shouldCheck)
        {
          for (size_t jointIndex = 0; jointIndex < jointPositions.size(); jointIndex++)
          {
            double deltaJoint = jointPositions[jointIndex] - prevTrajPoint[jointIndex];
            if (fabs(deltaJoint) > options.maxJointDiscontinuity)
            {
              discontinuityDelta = deltaJoint;
              discontinuityJointIndex = jointIndex;
              break;
            }
          }
        }
 
        // Handle discontinuity
        if (ikAttemptsRemaining > 0 && discontinuityJointIndex != -1)
        {
          // Retry IK
          k--;
          ikAttemptsRemaining--;
          RCLCPP_WARN(
            getLogger(),
            "[CpJointSpaceTrajectoryPlanner] IK discontinuity detected, retrying (%d attempts "
            "remaining)",
            ikAttemptsRemaining);
          continue;
        }
        else
        {
          // Record discontinuity or accept trajectory point
          if (ikAttemptsRemaining == 0 && discontinuityJointIndex != -1)
          {
            result.discontinuityIndexes.push_back(k);
 
            RCLCPP_ERROR(
              getLogger(),
              "[CpJointSpaceTrajectoryPlanner] Joint discontinuity at waypoint %lu: joint '%s' "
              "delta=%.3f rad",
              k, jointNames[discontinuityJointIndex].c_str(), discontinuityDelta);
 
            if (k == 0)
            {
              RCLCPP_ERROR(
                getLogger(),
                "[CpJointSpaceTrajectoryPlanner] Initial posture discontinuity detected");
            }
          }
 
          // Reset retry counter
          ikAttemptsRemaining = options.ikAttempts;
 
          // Add point to trajectory
          trajectory.push_back(jointPositions);
          rclcpp::Duration durationFromStart = rclcpp::Time(pose.header.stamp) - referenceTime;
          trajectoryTimeStamps.push_back(durationFromStart);
        }
      }
 
      // Build RobotTrajectory message
      result.trajectory.joint_trajectory.joint_names = jointNames;
 
      // Skip first point (current state) to avoid discontinuity in some behaviors
      for (size_t i = 1; i < trajectory.size(); i++)
      {
        trajectory_msgs::msg::JointTrajectoryPoint jp;
        jp.positions = trajectory[i];
        jp.time_from_start = trajectoryTimeStamps[i];
        result.trajectory.joint_trajectory.points.push_back(jp);
      }
 
      // Determine success
      if (!result.discontinuityIndexes.empty())
      {
        if (result.discontinuityIndexes[0] == 0)
        {
          result.errorCode = JointTrajectoryErrorCode::INCORRECT_INITIAL_STATE;
          result.errorMessage = "Incorrect initial state";
        }
        else
        {
          result.errorCode = JointTrajectoryErrorCode::JOINT_TRAJECTORY_DISCONTINUITY;
          result.errorMessage = "Joint trajectory discontinuity detected";
        }
        result.success = false;
        RCLCPP_WARN(getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str());
      }
      else
      {
        result.success = true;
        result.errorCode = JointTrajectoryErrorCode::SUCCESS;
        RCLCPP_INFO(
          getLogger(),
          "[CpJointSpaceTrajectoryPlanner] Successfully computed trajectory with %lu points",
          result.trajectory.joint_trajectory.points.size());
      }
    }
    catch (const std::exception & e)
    {
      result.success = false;
      result.errorMessage = std::string("Exception during trajectory planning: ") + e.what();
      RCLCPP_ERROR(getLogger(), "[CpJointSpaceTrajectoryPlanner] %s", result.errorMessage.c_str());
    }
 
    return result;
  }

Referenced by cl_moveit2z::CbMoveEndEffectorTrajectory::computeJointSpaceTrajectory().

Here is the caller graph for this function:

Member Data Documentation

◆ ikClient_

rclcpp::Client<moveit_msgs::srv::GetPositionIK>::SharedPtr cl_moveit2z::CpJointSpaceTrajectoryPlanner::ikClient_

private

Definition at line 343 of file cp_joint_space_trajectory_planner.hpp.

Referenced by onInitialize().

◆ moveit2zClient_

ClMoveit2z* cl_moveit2z::CpJointSpaceTrajectoryPlanner::moveit2zClient_ = nullptr