cl_moveit2z::CbCircularPouringMotion Class Reference

#include <cb_pouring_motion.hpp>

Inheritance diagram for cl_moveit2z::CbCircularPouringMotion:

Collaboration diagram for cl_moveit2z::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_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_
geometry_msgs::msg::Vector3	directionVector_
geometry_msgs::msg::Pose	pointerRelativePose_
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::Point	relativePivotPoint_
double	deltaHeight_
std::vector< geometry_msgs::msg::PoseStamped >	pointerTrajectory_
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 ()

Private Attributes
std::string	globalFrame_

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

Constructor & Destructor Documentation

CbCircularPouringMotion()

cl_moveit2z::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_moveit2z::CbCircularPouringMotion::computeCurrentEndEffectorPoseRelativeToPivot ( )

private

createMarkers()

void cl_moveit2z::CbCircularPouringMotion::createMarkers ( )

overridevirtual

Reimplemented from cl_moveit2z::CbMoveEndEffectorTrajectory.

Definition at line 146 of file cb_pouring_motion.cpp.

{
  CbMoveEndEffectorTrajectory::createMarkers();
 
  tf2::Stamped<tf2::Transform> currentEndEffectorTransform;
  this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform);
  tf2::Vector3 pivotPoint;
 
  tf2::fromMsg(this->relativePivotPoint_, pivotPoint);
 
  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;
 
  tf2::toMsg(pivot, marker.pose.position);
 
  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_moveit2z::CbMoveEndEffectorTrajectory::beahiorMarkers_, cl_moveit2z::CbMoveEndEffectorTrajectory::createMarkers(), cl_moveit2z::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose(), smacc2::ISmaccClientBehavior::getNode(), globalFrame_, pointerTrajectory_, and relativePivotPoint_.

Here is the call graph for this function:

generateTrajectory()

void cl_moveit2z::CbCircularPouringMotion::generateTrajectory ( )

overridevirtual

Implements cl_moveit2z::CbMoveEndEffectorTrajectory.

Definition at line 38 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;
 
  tf2::fromMsg(this->relativePivotPoint_, pivotPoint);
 
  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_moveit2z::CbMoveEndEffectorTrajectory::endEffectorTrajectory_, cl_moveit2z::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_moveit2z::CbCircularPouringMotion::angularSpeed_rad_s_

Definition at line 30 of file cb_pouring_motion.hpp.

deltaHeight_

double cl_moveit2z::CbCircularPouringMotion::deltaHeight_

protected

Definition at line 52 of file cb_pouring_motion.hpp.

Referenced by generateTrajectory().

directionVector_

geometry_msgs::msg::Vector3 cl_moveit2z::CbCircularPouringMotion::directionVector_

Definition at line 42 of file cb_pouring_motion.hpp.

globalFrame_

std::string cl_moveit2z::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_moveit2z::CbCircularPouringMotion::linearSpeed_m_s_

Definition at line 32 of file cb_pouring_motion.hpp.

Referenced by generateTrajectory().

pointerRelativePose_

geometry_msgs::msg::Pose cl_moveit2z::CbCircularPouringMotion::pointerRelativePose_

Definition at line 46 of file cb_pouring_motion.hpp.

Referenced by generateTrajectory().

pointerTrajectory_

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

protected

Definition at line 54 of file cb_pouring_motion.hpp.

Referenced by createMarkers(), and generateTrajectory().

relativePivotPoint_

geometry_msgs::msg::Point cl_moveit2z::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/moveit2z_client/include/moveit2z_client/client_behaviors/cb_pouring_motion.hpp
• smacc2_client_library/moveit2z_client/src/moveit2z_client/client_behaviors/cb_pouring_motion.cpp