cl_move_group_interface::CbCircularPivotMotion Class Reference

#include <cb_circular_pivot_motion.hpp>

Inheritance diagram for cl_move_group_interface::CbCircularPivotMotion:

Collaboration diagram for cl_move_group_interface::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_move_group_interface::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)
virtual void	onEntry ()
virtual void	onExit ()
virtual void	executeOnEntry ()
virtual void	executeOnExit ()
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_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory
std::vector< geometry_msgs::msg::PoseStamped >	endEffectorTrajectory_
ClMoveGroup *	movegroupClient_ = nullptr
visualization_msgs::msg::MarkerArray	beahiorMarkers_

Private Member Functions
void	computeCurrentEndEffectorPoseRelativeToPivot ()

Additional Inherited Members
Protected Member Functions inherited from cl_move_group_interface::CbMoveEndEffectorTrajectory
ComputeJointTrajectoryErrorCode	computeJointSpaceTrajectory (moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
void	executeJointSpaceTrajectory (const moveit_msgs::msg::RobotTrajectory &computedJointTrajectory)
virtual void	generateTrajectory ()=0
virtual void	createMarkers ()
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 ()
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 void	dispose ()
virtual rclcpp::Node::SharedPtr	getNode () const
virtual rclcpp::Logger	getLogger () const
virtual void	update ()=0

Detailed Description

Definition at line 29 of file cb_circular_pivot_motion.hpp.

Constructor & Destructor Documentation

CbCircularPivotMotion() [1/3]

cl_move_group_interface::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_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory::tipLink_.

CbCircularPivotMotion() [3/3]

cl_move_group_interface::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_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory::movegroupClient_, cl_move_group_interface::ClMoveGroup::moveGroupClientInterface, planePivotPose_, relativeInitialPose_, and cl_move_group_interface::CbMoveEndEffectorTrajectory::tipLink_.

Referenced by generateTrajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

createMarkers()

void cl_move_group_interface::CbCircularPivotMotion::createMarkers ( )

overridevirtual

Reimplemented from cl_move_group_interface::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_move_group_interface::CbMoveEndEffectorTrajectory::beahiorMarkers_, cl_move_group_interface::CbMoveEndEffectorTrajectory::createMarkers(), smacc2::ISmaccClientBehavior::getNode(), and planePivotPose_.

Here is the call graph for this function:

generateTrajectory()

void cl_move_group_interface::CbCircularPivotMotion::generateTrajectory ( )

overridevirtual

Implements cl_move_group_interface::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_move_group_interface::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_move_group_interface::CbCircularPivotMotion::angularSpeed_rad_s_

Definition at line 32 of file cb_circular_pivot_motion.hpp.

Referenced by generateTrajectory().

deltaRadians_

double cl_move_group_interface::CbCircularPivotMotion::deltaRadians_

protected

Definition at line 57 of file cb_circular_pivot_motion.hpp.

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

linearSpeed_m_s_

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

Definition at line 33 of file cb_circular_pivot_motion.hpp.

Referenced by generateTrajectory().

planePivotPose_

geometry_msgs::msg::PoseStamped cl_move_group_interface::CbCircularPivotMotion::planePivotPose_

protected

Definition at line 55 of file cb_circular_pivot_motion.hpp.

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

relativeInitialPose_

std::optional<geometry_msgs::msg::Pose> cl_move_group_interface::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/move_group_interface_client/include/move_group_interface_client/client_behaviors/cb_circular_pivot_motion.hpp
• smacc2_client_library/move_group_interface_client/src/move_group_interface_client/client_behaviors/cb_circular_pivot_motion.cpp