cl_move_group_interface::CbCircularPouringMotion Class Reference

#include <cb_pouring_motion.hpp>

Inheritance diagram for cl_move_group_interface::CbCircularPouringMotion:

Collaboration diagram for cl_move_group_interface::CbCircularPouringMotion:

Public Member Functions
	CbCircularPouringMotion (const geometry_msgs::msg::Point &pivotPoint, double deltaHeight, std::string tipLink, std::string globalFrame)
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_
geometry_msgs::msg::Vector3	directionVector_
geometry_msgs::msg::Pose	pointerRelativePose_
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::Point	relativePivotPoint_
double	deltaHeight_
std::vector< geometry_msgs::msg::PoseStamped >	pointerTrajectory_
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 ()

Private Attributes
std::string	globalFrame_

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 27 of file cb_pouring_motion.hpp.

Constructor & Destructor Documentation

CbCircularPouringMotion()

cl_move_group_interface::CbCircularPouringMotion::CbCircularPouringMotion	(	const geometry_msgs::msg::Point &	pivotPoint,
		double	deltaHeight,
		std::string	tipLink,
		std::string	globalFrame
	)

Definition at line 27 of file cb_pouring_motion.cpp.

: CbMoveEndEffectorTrajectory(tipLink),
  relativePivotPoint_(relativePivotPoint),
  deltaHeight_(deltaHeight),
  globalFrame_(globalFrame)
 
{
}

Member Function Documentation

computeCurrentEndEffectorPoseRelativeToPivot()

void cl_move_group_interface::CbCircularPouringMotion::computeCurrentEndEffectorPoseRelativeToPivot ( )

private

createMarkers()

void cl_move_group_interface::CbCircularPouringMotion::createMarkers ( )

overridevirtual

Reimplemented from cl_move_group_interface::CbMoveEndEffectorTrajectory.

Definition at line 175 of file cb_pouring_motion.cpp.

{
  CbMoveEndEffectorTrajectory::createMarkers();
 
  tf2::Stamped<tf2::Transform> currentEndEffectorTransform;
  this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform);
  tf2::Vector3 pivotPoint;
 
#ifdef ROS_ROLLING
  tf2::fromMsg(this->relativePivotPoint_, pivotPoint);
#else
  fromMsg(this->relativePivotPoint_, pivotPoint);
 
#endif
  tf2::Vector3 pivot = (currentEndEffectorTransform * pivotPoint);
 
  visualization_msgs::msg::Marker marker;
 
  marker.ns = "trajectory";
  marker.id = beahiorMarkers_.markers.size();
  marker.type = visualization_msgs::msg::Marker::SPHERE;
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.scale.x = 0.01;
  marker.scale.y = 0.01;
  marker.scale.z = 0.01;
 
  marker.color.a = 1.0;
  marker.color.r = 0.0;
  marker.color.g = 0;
  marker.color.b = 1.0;
 
#ifdef ROS_ROLLING
  tf2::toMsg(pivot, marker.pose.position);
#else
  toMsg(pivot, marker.pose.position);
#endif
  marker.header.frame_id = globalFrame_;
  marker.header.stamp = getNode()->now();
 
  beahiorMarkers_.markers.push_back(marker);
 
  tf2::Transform localdirection;
  localdirection.setIdentity();
  localdirection.setOrigin(tf2::Vector3(0.05, 0, 0));
  auto frameid = this->pointerTrajectory_.front().header.frame_id;
 
  for (auto & pose : this->pointerTrajectory_)
  {
    visualization_msgs::msg::Marker marker;
    marker.header.frame_id = frameid;
    marker.header.stamp = getNode()->now();
    marker.ns = "trajectory";
    marker.id = this->beahiorMarkers_.markers.size();
    marker.type = visualization_msgs::msg::Marker::ARROW;
    marker.action = visualization_msgs::msg::Marker::ADD;
    marker.scale.x = 0.005;
    marker.scale.y = 0.01;
    marker.scale.z = 0.01;
    marker.color.a = 0.8;
    marker.color.r = 0.0;
    marker.color.g = 1;
    marker.color.b = 0.0;
 
    geometry_msgs::msg::Point start, end;
    start.x = 0;
    start.y = 0;
    start.z = 0;
 
    tf2::Transform basetransform;
    tf2::fromMsg(pose.pose, basetransform);
    // tf2::Transform endarrow = localdirection * basetransform;
 
    end.x = localdirection.getOrigin().x();
    end.y = localdirection.getOrigin().y();
    end.z = localdirection.getOrigin().z();
 
    marker.pose.position = pose.pose.position;
    marker.pose.orientation = pose.pose.orientation;
    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(), cl_move_group_interface::fromMsg(), cl_move_group_interface::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose(), smacc2::ISmaccClientBehavior::getNode(), globalFrame_, pointerTrajectory_, relativePivotPoint_, and cl_move_group_interface::toMsg().

Here is the call graph for this function:

generateTrajectory()

void cl_move_group_interface::CbCircularPouringMotion::generateTrajectory ( )

overridevirtual

Implements cl_move_group_interface::CbMoveEndEffectorTrajectory.

Definition at line 62 of file cb_pouring_motion.cpp.

{
  // at least 1 sample per centimeter (average)
  const double METERS_PER_SAMPLE = 0.001;
 
  float dist_meters = 0;
  float totallineardist = fabs(this->deltaHeight_);
 
  int totalSamplesCount = totallineardist / METERS_PER_SAMPLE;
  int steps = totallineardist / METERS_PER_SAMPLE;
 
  float interpolation_factor_step = 1.0 / totalSamplesCount;
 
  double secondsPerSample;
 
  if (linearSpeed_m_s_)
  {
    secondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_);
  }
  else
  {
    secondsPerSample = std::numeric_limits<double>::max();
  }
 
  tf2::Stamped<tf2::Transform> currentEndEffectorTransform;
 
  this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform);
 
  tf2::Transform lidEndEffectorTransform;
  tf2::fromMsg(this->pointerRelativePose_, lidEndEffectorTransform);
 
  tf2::Vector3 v0, v1;
  v0 = (currentEndEffectorTransform * lidEndEffectorTransform).getOrigin();
 
  tf2::Vector3 pivotPoint;
 
#ifdef ROS_ROLLING
  tf2::fromMsg(this->relativePivotPoint_, pivotPoint);
#else
  fromMsg(this->relativePivotPoint_, pivotPoint);
 
#endif
 
  tf2::Vector3 pivot = (currentEndEffectorTransform * pivotPoint);
 
  v0 = v0 - pivot;
  v1 = v0;
  v1.setZ(v1.z() + this->deltaHeight_);
 
  tf2::Vector3 vp1(v1);
  tf2::Vector3 vp0(v0);
  tf2::Quaternion rotation = tf2::shortestArcQuatNormalize2(vp0, vp1);
 
  tf2::Quaternion initialEndEffectorOrientation = currentEndEffectorTransform.getRotation();
  auto finalEndEffectorOrientation = initialEndEffectorOrientation * rotation;
 
  tf2::Quaternion initialPointerOrientation =
    initialEndEffectorOrientation * lidEndEffectorTransform.getRotation();
  tf2::Quaternion finalPointerOrientation =
    finalEndEffectorOrientation * lidEndEffectorTransform.getRotation();
 
  // auto shortestAngle =
  //   tf2::angleShortestPath(initialEndEffectorOrientation, finalEndEffectorOrientation);
 
  v0 += pivot;
  v1 += pivot;
 
  float linc = deltaHeight_ / steps;  // METERS_PER_SAMPLE with sign
  float interpolation_factor = 0;
  tf2::Vector3 vi = v0;
 
  tf2::Transform invertedLidTransform = lidEndEffectorTransform.inverse();
  rclcpp::Time startTime = getNode()->now();
 
  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] trajectory steps: " << steps);
  for (float i = 0; i < steps; i++)
  {
    // auto currentEndEffectorOrientation =
    //   tf2::slerp(initialEndEffectorOrientation, finalEndEffectorOrientation, interpolation_factor);
    auto currentPointerOrientation =
      tf2::slerp(initialPointerOrientation, finalPointerOrientation, interpolation_factor);
 
    interpolation_factor += interpolation_factor_step;
    dist_meters += linc;
 
    vi = v0;
    vi.setZ(vi.getZ() + dist_meters);
 
    tf2::Transform pose;
    pose.setOrigin(vi);
    pose.setRotation(currentPointerOrientation);
 
    geometry_msgs::msg::PoseStamped pointerPose;
    tf2::toMsg(pose, pointerPose.pose);
    pointerPose.header.frame_id = globalFrame_;
    pointerPose.header.stamp = startTime + rclcpp::Duration::from_seconds(i * secondsPerSample);
    this->pointerTrajectory_.push_back(pointerPose);
 
    tf2::Transform poseEndEffector = pose * invertedLidTransform;
 
    geometry_msgs::msg::PoseStamped globalEndEffectorPose;
    tf2::toMsg(poseEndEffector, globalEndEffectorPose.pose);
    globalEndEffectorPose.header.frame_id = globalFrame_;
    globalEndEffectorPose.header.stamp =
      startTime + rclcpp::Duration::from_seconds(i * secondsPerSample);
 
    this->endEffectorTrajectory_.push_back(globalEndEffectorPose);
    RCLCPP_INFO_STREAM(
      getLogger(), "[" << getName() << "] " << i << " - " << globalEndEffectorPose);
  }
  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "]trajectory generated, size: " << steps);
}

References deltaHeight_, cl_move_group_interface::CbMoveEndEffectorTrajectory::endEffectorTrajectory_, cl_move_group_interface::fromMsg(), cl_move_group_interface::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose(), smacc2::ISmaccClientBehavior::getLogger(), smacc2::ISmaccClientBehavior::getName(), smacc2::ISmaccClientBehavior::getNode(), globalFrame_, linearSpeed_m_s_, pointerRelativePose_, pointerTrajectory_, and relativePivotPoint_.

Here is the call graph for this function:

Member Data Documentation

angularSpeed_rad_s_

std::optional<double> cl_move_group_interface::CbCircularPouringMotion::angularSpeed_rad_s_

Definition at line 30 of file cb_pouring_motion.hpp.

deltaHeight_

double cl_move_group_interface::CbCircularPouringMotion::deltaHeight_

protected

Definition at line 52 of file cb_pouring_motion.hpp.

Referenced by generateTrajectory().

directionVector_

geometry_msgs::msg::Vector3 cl_move_group_interface::CbCircularPouringMotion::directionVector_

Definition at line 42 of file cb_pouring_motion.hpp.

globalFrame_

std::string cl_move_group_interface::CbCircularPouringMotion::globalFrame_

private

Definition at line 59 of file cb_pouring_motion.hpp.

Referenced by createMarkers(), and generateTrajectory().

linearSpeed_m_s_

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

Definition at line 32 of file cb_pouring_motion.hpp.

Referenced by generateTrajectory().

pointerRelativePose_

geometry_msgs::msg::Pose cl_move_group_interface::CbCircularPouringMotion::pointerRelativePose_

Definition at line 46 of file cb_pouring_motion.hpp.

Referenced by generateTrajectory().

pointerTrajectory_

std::vector<geometry_msgs::msg::PoseStamped> cl_move_group_interface::CbCircularPouringMotion::pointerTrajectory_

protected

Definition at line 54 of file cb_pouring_motion.hpp.

Referenced by createMarkers(), and generateTrajectory().

relativePivotPoint_

geometry_msgs::msg::Point cl_move_group_interface::CbCircularPouringMotion::relativePivotPoint_

protected

Definition at line 49 of file cb_pouring_motion.hpp.

Referenced by createMarkers(), 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_pouring_motion.hpp
• smacc2_client_library/move_group_interface_client/src/move_group_interface_client/client_behaviors/cb_pouring_motion.cpp