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
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
virtual void	update () override
Public Member Functions inherited from smacc2::SmaccAsyncClientBehavior
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
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=false)
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 ()

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)
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

Detailed Description

Definition at line 29 of file cb_circular_pivot_motion.hpp.

Constructor & Destructor Documentation

CbCircularPivotMotion() [1/3]

cl_moveit2z::CbCircularPivotMotion::CbCircularPivotMotion

(

std::optional< std::string >

tipLink = std::nullopt

)

Definition at line 29 of file cb_circular_pivot_motion.cpp.

: CbMoveEndEffectorTrajectory(tipLink)
{
}

CbCircularPivotMotion() [2/3]

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

Definition at line 34 of file cb_circular_pivot_motion.cpp.

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

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

CbCircularPivotMotion() [3/3]

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
	)

Definition at line 42 of file cb_circular_pivot_motion.cpp.

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

Member Function Documentation

computeCurrentEndEffectorPoseRelativeToPivot()

void cl_moveit2z::CbCircularPivotMotion::computeCurrentEndEffectorPoseRelativeToPivot ( )

private

Definition at line 159 of file cb_circular_pivot_motion.cpp.

{
  //auto currentRobotEndEffectorPose = this->movegroupClient_->moveGroupClientInterface.getCurrentPose();
 
  tf2_ros::Buffer tfBuffer(getNode()->get_clock());
  tf2_ros::TransformListener tfListener(tfBuffer);
 
  // tf2::Stamped<tf2::Transform>  globalBaseLink;
  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_ << "'");
    tf2::fromMsg(
      tfBuffer.lookupTransform(
        planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
      endEffectorInPivotFrame);
 
    //endEffectorInPivotFrame = tfBuffer.lookupTransform(planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(0));
 
    // we define here the global frame as the pivot frame id
    // tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(10));
    // tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), globalBaseLink);
  }
  catch (const std::exception & e)
  {
    std::cerr << e.what() << '\n';
  }
 
  // 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::CbMoveEndEffectorTrajectory::movegroupClient_, cl_moveit2z::ClMoveit2z::moveGroupClientInterface, planePivotPose_, relativeInitialPose_, and cl_moveit2z::CbMoveEndEffectorTrajectory::tipLink_.

Referenced by generateTrajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

createMarkers()

void cl_moveit2z::CbCircularPivotMotion::createMarkers ( )

overridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Definition at line 213 of file cb_circular_pivot_motion.cpp.

{
  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:

generateTrajectory()

void cl_moveit2z::CbCircularPivotMotion::generateTrajectory ( )

overridevirtual

Implements cl_moveit2z::CbMoveEndEffectorTrajectory.

Definition at line 53 of file cb_circular_pivot_motion.cpp.

{
  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:

Member Data Documentation

angularSpeed_rad_s_

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

Definition at line 32 of file cb_circular_pivot_motion.hpp.

Referenced by generateTrajectory().

deltaRadians_

double cl_moveit2z::CbCircularPivotMotion::deltaRadians_

protected

Definition at line 57 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 33 of file cb_circular_pivot_motion.hpp.

Referenced by generateTrajectory().

planePivotPose_

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

protected

Definition at line 55 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 36 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/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