cl_moveit2z::CbCircularPivotMotion Class Reference

#include <cb_circular_pivot_motion.hpp>

Inheritance diagram for cl_moveit2z::CbCircularPivotMotion:

Collaboration diagram for cl_moveit2z::CbCircularPivotMotion:

Public Member Functions
	CbCircularPivotMotion (std::optional< std::string > tipLink=std::nullopt)
	CbCircularPivotMotion (const geometry_msgs::msg::PoseStamped &planePivotPose, double deltaRadians, std::optional< std::string > tipLink=std::nullopt)
	CbCircularPivotMotion (const geometry_msgs::msg::PoseStamped &planePivotPose, const geometry_msgs::msg::Pose &relativeInitialPose, double deltaRadians, std::optional< std::string > tipLink=std::nullopt)
virtual void	generateTrajectory () override
virtual void	createMarkers () override
	CbCircularPivotMotion (std::optional< std::string > tipLink=std::nullopt)
	CbCircularPivotMotion (const geometry_msgs::msg::PoseStamped &planePivotPose, double deltaRadians, std::optional< std::string > tipLink=std::nullopt)
	CbCircularPivotMotion (const geometry_msgs::msg::PoseStamped &planePivotPose, const geometry_msgs::msg::Pose &relativeInitialPose, double deltaRadians, std::optional< std::string > tipLink=std::nullopt)
virtual void	generateTrajectory () override
virtual void	createMarkers () override
Public Member Functions inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
	CbMoveEndEffectorTrajectory (std::optional< std::string > tipLink=std::nullopt)
	CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::msg::PoseStamped > &endEffectorTrajectory, std::optional< std::string > tipLink=std::nullopt)
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
virtual void	onEntry () override
virtual void	onExit () override
	CbMoveEndEffectorTrajectory (std::optional< std::string > tipLink=std::nullopt)
	CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::msg::PoseStamped > &endEffectorTrajectory, std::optional< std::string > tipLink=std::nullopt)
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
virtual void	onEntry () override
virtual void	onExit () override
virtual void	update () override
Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()
virtual	~SmaccAsyncClientBehavior ()
template<typename TCallback , typename T >
boost::signals2::connection	onSuccess (TCallback callback, T *object)
template<typename TCallback , typename T >
boost::signals2::connection	onFinished (TCallback callback, T *object)
template<typename TCallback , typename T >
boost::signals2::connection	onFailure (TCallback callback, T *object)
void	requestForceFinish ()
void	executeOnEntry () override
void	executeOnExit () override
void	waitOnEntryThread (bool requestFinish)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onSuccess (TCallbackMethod callback, T *object)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFinished (TCallbackMethod callback, T *object)
template<typename TCallbackMethod , typename T >
boost::signals2::connection	onFailure (TCallbackMethod callback, T *object)
Public Member Functions inherited from smacc2::ISmaccClientBehavior
	ISmaccClientBehavior ()
virtual	~ISmaccClientBehavior ()
ISmaccStateMachine *	getStateMachine ()
std::string	getName () const
template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, bool throwExceptionIfNotExist)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage, ComponentRequirement requirementType=ComponentRequirement::SOFT)
Public Member Functions inherited from smacc2::ISmaccUpdatable
	ISmaccUpdatable ()
	ISmaccUpdatable (rclcpp::Duration duration)
void	executeUpdate (rclcpp::Node::SharedPtr node)
void	setUpdatePeriod (rclcpp::Duration duration)

Public Attributes
std::optional< double >	angularSpeed_rad_s_
std::optional< double >	linearSpeed_m_s_
std::optional< geometry_msgs::msg::Pose >	relativeInitialPose_
Public Attributes inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
std::optional< std::string >	group_
std::optional< std::string >	tipLink_
std::optional< bool >	allowInitialTrajectoryStateJointDiscontinuity_

Protected Attributes
geometry_msgs::msg::PoseStamped	planePivotPose_
double	deltaRadians_
Protected Attributes inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
std::vector< geometry_msgs::msg::PoseStamped >	endEffectorTrajectory_
ClMoveit2z *	movegroupClient_ = nullptr
visualization_msgs::msg::MarkerArray	beahiorMarkers_

Private Member Functions
void	computeCurrentEndEffectorPoseRelativeToPivot ()
void	computeCurrentEndEffectorPoseRelativeToPivot ()

Additional Inherited Members
Protected Member Functions inherited from cl_moveit2z::CbMoveEndEffectorTrajectory
ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
void	executeJointSpaceTrajectory (const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
void	getCurrentEndEffectorPose (std::string globalFrame, tf2::Stamped< tf2::Transform > &currentEndEffectorTransform)
ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
void	executeJointSpaceTrajectory (const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
void	getCurrentEndEffectorPose (std::string globalFrame, tf2::Stamped< tf2::Transform > &currentEndEffectorTransform)
Protected Member Functions inherited from smacc2::SmaccAsyncClientBehavior
void	postSuccessEvent ()
void	postFailureEvent ()
virtual void	dispose () override
bool	isShutdownRequested ()
	onEntry is executed in a new thread. However the current state cannot be left until the onEntry thread finishes. This flag can be checked from the onEntry thread to force finishing the thread.
Protected Member Functions inherited from smacc2::ISmaccClientBehavior
virtual void	runtimeConfigure ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
ISmaccState *	getCurrentState ()
virtual rclcpp::Node::SharedPtr	getNode () const
virtual rclcpp::Logger	getLogger () const
Protected Member Functions inherited from smacc2::ISmaccUpdatable

Detailed Description

Definition at line 29 of file cb_circular_pivot_motion.hpp.

Constructor & Destructor Documentation

CbCircularPivotMotion() [1/6]

cl_moveit2z::CbCircularPivotMotion::CbCircularPivotMotion ( std::optional< std::string > tipLink = std::nullopt )

inline

Definition at line 43 of file cb_circular_pivot_motion.hpp.

  : CbMoveEndEffectorTrajectory(tipLink)
  {
  }

CbCircularPivotMotion() [2/6]

cl_moveit2z::CbCircularPivotMotion::CbCircularPivotMotion ( const geometry_msgs::msg::PoseStamped & planePivotPose, double deltaRadians, std::optional< std::string > tipLink = std::nullopt )

inline

Definition at line 48 of file cb_circular_pivot_motion.hpp.

  : CbMoveEndEffectorTrajectory(tipLink),
    planePivotPose_(planePivotPose),
    deltaRadians_(deltaRadians)
  {
    if (tipLink_) planePivotPose_.header.frame_id = *tipLink;
  }

References planePivotPose_, and cl_moveit2z::CbMoveEndEffectorTrajectory::tipLink_.

CbCircularPivotMotion() [3/6]

cl_moveit2z::CbCircularPivotMotion::CbCircularPivotMotion ( const geometry_msgs::msg::PoseStamped & planePivotPose, const geometry_msgs::msg::Pose & relativeInitialPose, double deltaRadians, std::optional< std::string > tipLink = std::nullopt )

inline

Definition at line 58 of file cb_circular_pivot_motion.hpp.

  : CbMoveEndEffectorTrajectory(tipLink),
    relativeInitialPose_(relativeInitialPose),
    planePivotPose_(planePivotPose),
    deltaRadians_(deltaRadians)
  {
  }

CbCircularPivotMotion() [4/6]

cl_moveit2z::CbCircularPivotMotion::CbCircularPivotMotion

(

std::optional< std::string >

tipLink = std::nullopt

)

CbCircularPivotMotion() [5/6]

cl_moveit2z::CbCircularPivotMotion::CbCircularPivotMotion	(	const geometry_msgs::msg::PoseStamped &	planePivotPose,
		double	deltaRadians,
		std::optional< std::string >	tipLink = std::nullopt )

CbCircularPivotMotion() [6/6]

cl_moveit2z::CbCircularPivotMotion::CbCircularPivotMotion	(	const geometry_msgs::msg::PoseStamped &	planePivotPose,
		const geometry_msgs::msg::Pose &	relativeInitialPose,
		double	deltaRadians,
		std::optional< std::string >	tipLink = std::nullopt )

Member Function Documentation

computeCurrentEndEffectorPoseRelativeToPivot() [1/2]

void cl_moveit2z::CbCircularPivotMotion::computeCurrentEndEffectorPoseRelativeToPivot ( )

inlineprivate

Definition at line 227 of file cb_circular_pivot_motion.hpp.

  {
    // Use CpTfListener component for transform lookups
    CpTfListener * tfListener = nullptr;
    this->requiresComponent(tfListener, false);  // Optional component
 
    tf2::Stamped<tf2::Transform> endEffectorInPivotFrame;
 
    try
    {
      if (!tipLink_ || *tipLink_ == "")
      {
        tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
      }
 
      RCLCPP_INFO_STREAM(
        getLogger(), "[" << getName() << "] waiting transform, pivot: '"
                         << planePivotPose_.header.frame_id << "' tipLink: '" << *tipLink_ << "'");
 
      if (tfListener != nullptr)
      {
        // Use component-based TF listener (preferred)
        auto transformOpt =
          tfListener->lookupTransform(planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time());
 
        if (transformOpt)
        {
          tf2::fromMsg(transformOpt.value(), endEffectorInPivotFrame);
        }
        else
        {
          RCLCPP_ERROR_STREAM(
            getLogger(), "[" << getName() << "] Failed to lookup transform from " << *tipLink_
                             << " to " << planePivotPose_.header.frame_id);
          return;
        }
      }
      else
      {
        // Fallback to legacy TF2 usage if component not available
        RCLCPP_WARN_STREAM(
          getLogger(), "[" << getName()
                           << "] CpTfListener component not available, using legacy TF2 (consider "
                              "adding CpTfListener component)");
        tf2_ros::Buffer tfBuffer(getNode()->get_clock());
        tf2_ros::TransformListener tfListenerLegacy(tfBuffer);
 
        tf2::fromMsg(
          tfBuffer.lookupTransform(
            planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
          endEffectorInPivotFrame);
      }
    }
    catch (const std::exception & e)
    {
      RCLCPP_ERROR_STREAM(
        getLogger(),
        "[" << getName()
            << "] Exception in computeCurrentEndEffectorPoseRelativeToPivot: " << e.what());
      return;
    }
 
    // tf2::Transform endEffectorInBaseLinkFrame;
    // tf2::fromMsg(currentRobotEndEffectorPose.pose, endEffectorInBaseLinkFrame);
 
    // tf2::Transform endEffectorInPivotFrame = globalBaseLink * endEffectorInBaseLinkFrame; // pose composition
 
    // now pivot and EndEffector share a common reference frame (let say map)
    // now get the current pose from the pivot reference frame with inverse composition
    tf2::Transform pivotTransform;
    tf2::fromMsg(planePivotPose_.pose, pivotTransform);
    tf2::Transform invertedNewReferenceFrame = pivotTransform.inverse();
 
    tf2::Transform currentPoseRelativeToPivot = invertedNewReferenceFrame * endEffectorInPivotFrame;
 
    geometry_msgs::msg::Pose finalEndEffectorRelativePose;
    tf2::toMsg(currentPoseRelativeToPivot, finalEndEffectorRelativePose);
    relativeInitialPose_ = finalEndEffectorRelativePose;
  }

References smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), cl_moveit2z::CpTfListener::lookupTransform(), cl_moveit2z::CbMoveEndEffectorTrajectory::movegroupClient_, cl_moveit2z::ClMoveit2z::moveGroupClientInterface, planePivotPose_, relativeInitialPose_, smacc2::ISmaccClientBehavior::requiresComponent(), and cl_moveit2z::CbMoveEndEffectorTrajectory::tipLink_.

Referenced by generateTrajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

computeCurrentEndEffectorPoseRelativeToPivot() [2/2]

void cl_moveit2z::CbCircularPivotMotion::computeCurrentEndEffectorPoseRelativeToPivot ( )

private

createMarkers() [1/2]

void cl_moveit2z::CbCircularPivotMotion::createMarkers ( )

inlineoverridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Definition at line 176 of file cb_circular_pivot_motion.hpp.

  {
    CbMoveEndEffectorTrajectory::createMarkers();
 
    tf2::Transform localdirection;
    localdirection.setIdentity();
    localdirection.setOrigin(tf2::Vector3(0.12, 0, 0));
    auto frameid = planePivotPose_.header.frame_id;
 
    visualization_msgs::msg::Marker marker;
    marker.header.frame_id = frameid;
    marker.header.stamp = getNode()->now();
    marker.ns = "trajectory";
    marker.id = beahiorMarkers_.markers.size();
    marker.type = visualization_msgs::msg::Marker::ARROW;
    marker.action = visualization_msgs::msg::Marker::ADD;
    marker.scale.x = 0.01;
    marker.scale.y = 0.02;
    marker.scale.z = 0.02;
    marker.color.a = 1.0;
    marker.color.r = 0.0;
    marker.color.g = 0;
    marker.color.b = 1.0;
 
    geometry_msgs::msg::Point start, end;
    start.x = planePivotPose_.pose.position.x;
    start.y = planePivotPose_.pose.position.y;
    start.z = planePivotPose_.pose.position.z;
 
    tf2::Transform basetransform;
    tf2::fromMsg(planePivotPose_.pose, basetransform);
    tf2::Transform endarrow = localdirection * basetransform;
 
    end.x = endarrow.getOrigin().x();
    end.y = endarrow.getOrigin().y();
    end.z = endarrow.getOrigin().z();
 
    marker.pose.orientation.w = 1;
    marker.points.push_back(start);
    marker.points.push_back(end);
 
    beahiorMarkers_.markers.push_back(marker);
  }

References cl_moveit2z::CbMoveEndEffectorTrajectory::beahiorMarkers_, cl_moveit2z::CbMoveEndEffectorTrajectory::createMarkers(), smacc2::ISmaccClientBehavior::getNode(), and planePivotPose_.

Here is the call graph for this function:

createMarkers() [2/2]

virtual void cl_moveit2z::CbCircularPivotMotion::createMarkers ( )

overridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

generateTrajectory() [1/2]

void cl_moveit2z::CbCircularPivotMotion::generateTrajectory ( )

inlineoverridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Definition at line 69 of file cb_circular_pivot_motion.hpp.

  {
    if (!relativeInitialPose_)
    {
      this->computeCurrentEndEffectorPoseRelativeToPivot();
    }
 
    // project offset into the xy-plane
    // get the radius
    double radius = sqrt(
      relativeInitialPose_->position.z * relativeInitialPose_->position.z +
      relativeInitialPose_->position.y * relativeInitialPose_->position.y);
    double initialAngle = atan2(relativeInitialPose_->position.z, relativeInitialPose_->position.y);
 
    double totallineardist = fabs(radius * deltaRadians_);
    double totalangulardist = fabs(deltaRadians_);
 
    // at least 1 sample per centimeter (average)
    // at least 1 sample per ~1.1 degrees (average)
 
    const double RADS_PER_SAMPLE = 0.02;
    const double METERS_PER_SAMPLE = 0.01;
 
    int totalSamplesCount =
      std::max(totallineardist / METERS_PER_SAMPLE, totalangulardist / RADS_PER_SAMPLE);
 
    double linearSecondsPerSample;
    double angularSecondsPerSamples;
    double secondsPerSample;
 
    if (linearSpeed_m_s_)
    {
      linearSecondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_);
    }
    else
    {
      linearSecondsPerSample = std::numeric_limits<double>::max();
    }
 
    if (angularSpeed_rad_s_)
    {
      angularSecondsPerSamples = RADS_PER_SAMPLE / (*angularSpeed_rad_s_);
    }
    else
    {
      angularSecondsPerSamples = std::numeric_limits<double>::max();
    }
 
    if (!linearSpeed_m_s_ && !angularSpeed_rad_s_)
    {
      secondsPerSample = 0.5;
    }
    else
    {
      secondsPerSample = std::min(linearSecondsPerSample, angularSecondsPerSamples);
    }
 
    double currentAngle = initialAngle;
 
    double angleStep = deltaRadians_ / (double)totalSamplesCount;
 
    tf2::Transform tfBasePose;
    tf2::fromMsg(planePivotPose_.pose, tfBasePose);
 
    RCLCPP_INFO_STREAM(
      getLogger(),
      "[" << getName() << "] generated trajectory, total samples: " << totalSamplesCount);
    for (int i = 0; i < totalSamplesCount; i++)
    {
      // relativePose i
      double y = radius * cos(currentAngle);
      double z = radius * sin(currentAngle);
 
      geometry_msgs::msg::Pose relativeCurrentPose;
 
      relativeCurrentPose.position.x = relativeInitialPose_->position.x;
      relativeCurrentPose.position.y = y;
      relativeCurrentPose.position.z = z;
 
      tf2::Quaternion localquat;
      localquat.setEuler(currentAngle, 0, 0);
 
      //relativeCurrentPose.orientation = relativeInitialPose_.orientation;
      //tf2::toMsg(localquat, relativeCurrentPose.orientation);
      relativeCurrentPose.orientation.w = 1;
 
      tf2::Transform tfRelativeCurrentPose;
      tf2::fromMsg(relativeCurrentPose, tfRelativeCurrentPose);
 
      tf2::Transform tfGlobalPose = tfRelativeCurrentPose * tfBasePose;
 
      tfGlobalPose.setRotation(tfGlobalPose.getRotation() * localquat);
 
      geometry_msgs::msg::PoseStamped globalPose;
      tf2::toMsg(tfGlobalPose, globalPose.pose);
      globalPose.header.frame_id = planePivotPose_.header.frame_id;
      globalPose.header.stamp = rclcpp::Time(planePivotPose_.header.stamp) +
                                rclcpp::Duration::from_seconds(i * secondsPerSample);
      RCLCPP_INFO_STREAM(
        getLogger(),
        "[" << getName() << "]" << rclcpp::Time(globalPose.header.stamp).nanoseconds());
 
      this->endEffectorTrajectory_.push_back(globalPose);
      currentAngle += angleStep;
    }
  }

References angularSpeed_rad_s_, computeCurrentEndEffectorPoseRelativeToPivot(), deltaRadians_, cl_moveit2z::CbMoveEndEffectorTrajectory::endEffectorTrajectory_, smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), linearSpeed_m_s_, planePivotPose_, and relativeInitialPose_.

Here is the call graph for this function:

generateTrajectory() [2/2]

virtual void cl_moveit2z::CbCircularPivotMotion::generateTrajectory ( )

overridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Member Data Documentation

angularSpeed_rad_s_

std::optional< double > cl_moveit2z::CbCircularPivotMotion::angularSpeed_rad_s_

Definition at line 37 of file cb_circular_pivot_motion.hpp.

Referenced by generateTrajectory().

deltaRadians_

double cl_moveit2z::CbCircularPivotMotion::deltaRadians_

protected

Definition at line 224 of file cb_circular_pivot_motion.hpp.

Referenced by cl_moveit2z::CbEndEffectorRotate::CbEndEffectorRotate(), and generateTrajectory().

linearSpeed_m_s_

std::optional< double > cl_moveit2z::CbCircularPivotMotion::linearSpeed_m_s_

Definition at line 38 of file cb_circular_pivot_motion.hpp.

Referenced by generateTrajectory().

planePivotPose_

geometry_msgs::msg::PoseStamped cl_moveit2z::CbCircularPivotMotion::planePivotPose_

protected

Definition at line 222 of file cb_circular_pivot_motion.hpp.

Referenced by CbCircularPivotMotion(), computeCurrentEndEffectorPoseRelativeToPivot(), createMarkers(), generateTrajectory(), and cl_moveit2z::CbEndEffectorRotate::onEntry().

relativeInitialPose_

std::optional< geometry_msgs::msg::Pose > cl_moveit2z::CbCircularPivotMotion::relativeInitialPose_

Definition at line 41 of file cb_circular_pivot_motion.hpp.

Referenced by computeCurrentEndEffectorPoseRelativeToPivot(), and generateTrajectory().

---

The documentation for this class was generated from the following files:

• smacc2_client_library/cl_moveit2z/include/cl_moveit2z/client_behaviors/cb_circular_pivot_motion.hpp
• smacc2_client_library/moveit2z_client/include/moveit2z_client/client_behaviors/cb_circular_pivot_motion.hpp
• smacc2_client_library/moveit2z_client/src/moveit2z_client/client_behaviors/cb_circular_pivot_motion.cpp