cl_move_group_interface::CbCircularPouringMotion Class Reference

#include <cb_pouring_motion.h>

Inheritance diagram for cl_move_group_interface::CbCircularPouringMotion:

Collaboration diagram for cl_move_group_interface::CbCircularPouringMotion:

Public Member Functions
	CbCircularPouringMotion (const geometry_msgs::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::string tipLink="")
	CbMoveEndEffectorTrajectory (const std::vector< geometry_msgs::PoseStamped > &endEffectorTrajectory, std::string tipLink="")
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
virtual void	onEntry () override
virtual void	onExit () override
virtual void	update () override
Public Member Functions inherited from smacc::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)
Public Member Functions inherited from smacc::ISmaccClientBehavior
	ISmaccClientBehavior ()
virtual	~ISmaccClientBehavior ()
ISmaccStateMachine *	getStateMachine ()
std::string	getName () const
template<typename SmaccClientType >
void	requiresClient (SmaccClientType *&storage)
template<typename SmaccComponentType >
void	requiresComponent (SmaccComponentType *&storage)
ros::NodeHandle	getNode ()
Public Member Functions inherited from smacc::ISmaccUpdatable
	ISmaccUpdatable ()
	ISmaccUpdatable (ros::Duration duration)
void	executeUpdate ()
void	setUpdatePeriod (ros::Duration duration)

Public Attributes
boost::optional< double >	angularSpeed_rad_s_
boost::optional< double >	linearSpeed_m_s_
geometry_msgs::Vector3	directionVector_
geometry_msgs::Pose	pointerRelativePose_
Public Attributes inherited from cl_move_group_interface::CbMoveEndEffectorTrajectory
boost::optional< std::string >	group_
boost::optional< std::string >	tipLink_
boost::optional< bool >	allowInitialTrajectoryStateJointDiscontinuity_

Protected Attributes
geometry_msgs::Point	relativePivotPoint_
double	deltaHeight_
std::vector< geometry_msgs::PoseStamped >	pointerTrajectory_
Protected Attributes inherited from cl_move_group_interface::CbMoveEndEffectorTrajectory
std::vector< geometry_msgs::PoseStamped >	endEffectorTrajectory_
ClMoveGroup *	movegroupClient_ = nullptr
visualization_msgs::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::RobotTrajectory &computedJointTrajectory)
void	executeJointSpaceTrajectory (const moveit_msgs::RobotTrajectory &computedJointTrajectory)
virtual void	generateTrajectory ()
virtual void	createMarkers ()
void	getCurrentEndEffectorPose (std::string globalFrame, tf::StampedTransform &currentEndEffectorTransform)
Protected Member Functions inherited from smacc::SmaccAsyncClientBehavior
virtual void	executeOnEntry () override
virtual void	executeOnExit () override
void	postSuccessEvent ()
void	postFailureEvent ()
virtual void	dispose () override
Protected Member Functions inherited from smacc::ISmaccClientBehavior
virtual void	runtimeConfigure ()
virtual void	onEntry ()
virtual void	onExit ()
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
ISmaccState *	getCurrentState ()
virtual void	executeOnEntry ()
virtual void	executeOnExit ()
virtual void	dispose ()
virtual void	update ()=0

Detailed Description

Definition at line 14 of file cb_pouring_motion.h.

Constructor & Destructor Documentation

CbCircularPouringMotion()

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

Definition at line 13 of file cb_pouring_motion.cpp.

        : relativePivotPoint_(relativePivotPoint)
        , deltaHeight_(deltaHeight)
        , CbMoveEndEffectorTrajectory(tipLink)
        , 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 118 of file cb_pouring_motion.cpp.

    {
        CbMoveEndEffectorTrajectory::createMarkers();
 
        tf::StampedTransform currentEndEffectorTransform;
        this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform);
        tf::Point pivotPoint;
        tf::pointMsgToTF(this->relativePivotPoint_, pivotPoint);
        tf::Vector3 pivot = (currentEndEffectorTransform * pivotPoint);
 
 
        visualization_msgs::Marker marker;
 
        marker.ns = "trajectory";
        marker.id = beahiorMarkers_.markers.size();
        marker.type = visualization_msgs::Marker::SPHERE;
        marker.action = visualization_msgs::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;
 
        tf::pointTFToMsg(pivot, marker.pose.position);
        marker.header.frame_id = globalFrame_;
        marker.header.stamp = ros::Time::now();
 
        beahiorMarkers_.markers.push_back(marker);
 
        tf::Transform localdirection;
        localdirection.setIdentity();
        localdirection.setOrigin(tf::Vector3(0.05, 0, 0));
        auto frameid = this->pointerTrajectory_.front().header.frame_id;
 
        for (auto &pose : this->pointerTrajectory_)
        {
            visualization_msgs::Marker marker;
            marker.header.frame_id = frameid;
            marker.header.stamp = ros::Time::now();
            marker.ns = "trajectory";
            marker.id = this->beahiorMarkers_.markers.size();
            marker.type = visualization_msgs::Marker::ARROW;
            marker.action = visualization_msgs::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::Point start, end;
            start.x = 0;
            start.y = 0;
            start.z = 0;
 
            tf::Transform basetransform;
            tf::poseMsgToTF(pose.pose, basetransform);
            tf::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::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose(), globalFrame_, pointerTrajectory_, and relativePivotPoint_.

Here is the call graph for this function:

generateTrajectory()

void cl_move_group_interface::CbCircularPouringMotion::generateTrajectory ( )

overridevirtual

Reimplemented from cl_move_group_interface::CbMoveEndEffectorTrajectory.

Definition at line 21 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();
        }
 
        tf::StampedTransform currentEndEffectorTransform;
 
        this->getCurrentEndEffectorPose(globalFrame_, currentEndEffectorTransform);
 
        tf::Transform lidEndEffectorTransform;
        tf::poseMsgToTF(this->pointerRelativePose_, lidEndEffectorTransform);
 
        tf::Vector3 v0, v1;
        v0 = (currentEndEffectorTransform * lidEndEffectorTransform).getOrigin();
 
        tf::Point pivotPoint;
        tf::pointMsgToTF(this->relativePivotPoint_, pivotPoint);
 
        tf::Vector3 pivot = (currentEndEffectorTransform * pivotPoint);
 
        v0 = v0 - pivot;
        v1 = v0;
        v1.setZ(v1.z() + this->deltaHeight_);
 
        tf::Vector3 vp1(v1);
        tf::Vector3 vp0(v0);
        tf::Quaternion rotation = tf::shortestArcQuatNormalize2(vp0, vp1);
 
        tf::Quaternion initialEndEffectorOrientation = currentEndEffectorTransform.getRotation();
        auto finalEndEffectorOrientation = initialEndEffectorOrientation* rotation;
 
        tf::Quaternion initialPointerOrientation = initialEndEffectorOrientation * lidEndEffectorTransform.getRotation();
        tf::Quaternion finalPointerOrientation = finalEndEffectorOrientation * lidEndEffectorTransform.getRotation();
 
        auto shortestAngle = tf::angleShortestPath(initialEndEffectorOrientation, finalEndEffectorOrientation);
 
        v0+=pivot;
        v1+=pivot;
 
        float linc = deltaHeight_/steps;// METERS_PER_SAMPLE with sign
        float interpolation_factor = 0;
        tf::Vector3 vi = v0;
 
        tf::Transform invertedLidTransform = lidEndEffectorTransform.inverse();
        ros::Time startTime = ros::Time::now();
 
        for (float i =0; i < steps; i++)
        {
            auto currentEndEffectorOrientation = tf::slerp(initialEndEffectorOrientation, finalEndEffectorOrientation, interpolation_factor);
            auto currentPointerOrientation = tf::slerp(initialPointerOrientation, finalPointerOrientation, interpolation_factor);
 
            interpolation_factor += interpolation_factor_step;
            dist_meters += linc;
 
            vi = v0;
            vi.setZ(vi.getZ() + dist_meters);
 
            tf::Transform pose;
            pose.setOrigin(vi);
            pose.setRotation(currentPointerOrientation);
 
            geometry_msgs::PoseStamped pointerPose;
            tf::poseTFToMsg(pose, pointerPose.pose);
            pointerPose.header.frame_id = globalFrame_;
            pointerPose.header.stamp = startTime + ros::Duration(i * secondsPerSample);
            this->pointerTrajectory_.push_back(pointerPose);
 
            tf::Transform poseEndEffector = pose * invertedLidTransform;
 
            geometry_msgs::PoseStamped globalEndEffectorPose;
            tf::poseTFToMsg(poseEndEffector, globalEndEffectorPose.pose);
            globalEndEffectorPose.header.frame_id = globalFrame_;
            globalEndEffectorPose.header.stamp = startTime + ros::Duration(i * secondsPerSample);
 
            this->endEffectorTrajectory_.push_back(globalEndEffectorPose);
        }
    }

References deltaHeight_, cl_move_group_interface::CbMoveEndEffectorTrajectory::endEffectorTrajectory_, cl_move_group_interface::CbMoveEndEffectorTrajectory::getCurrentEndEffectorPose(), globalFrame_, linearSpeed_m_s_, pointerRelativePose_, pointerTrajectory_, and relativePivotPoint_.

Here is the call graph for this function:

Member Data Documentation

angularSpeed_rad_s_

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

Definition at line 17 of file cb_pouring_motion.h.

deltaHeight_

double cl_move_group_interface::CbCircularPouringMotion::deltaHeight_

protected

Definition at line 36 of file cb_pouring_motion.h.

Referenced by generateTrajectory().

directionVector_

geometry_msgs::Vector3 cl_move_group_interface::CbCircularPouringMotion::directionVector_

Definition at line 27 of file cb_pouring_motion.h.

globalFrame_

std::string cl_move_group_interface::CbCircularPouringMotion::globalFrame_

private

Definition at line 43 of file cb_pouring_motion.h.

Referenced by createMarkers(), and generateTrajectory().

linearSpeed_m_s_

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

Definition at line 19 of file cb_pouring_motion.h.

Referenced by generateTrajectory().

pointerRelativePose_

geometry_msgs::Pose cl_move_group_interface::CbCircularPouringMotion::pointerRelativePose_

Definition at line 31 of file cb_pouring_motion.h.

Referenced by generateTrajectory().

pointerTrajectory_

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

protected

Definition at line 38 of file cb_pouring_motion.h.

Referenced by createMarkers(), and generateTrajectory().

relativePivotPoint_

geometry_msgs::Point cl_move_group_interface::CbCircularPouringMotion::relativePivotPoint_

protected

Definition at line 33 of file cb_pouring_motion.h.

Referenced by createMarkers(), and generateTrajectory().

---

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

• smacc_client_library/move_group_interface_client/include/move_group_interface_client/client_behaviors/cb_pouring_motion.h
• smacc_client_library/move_group_interface_client/src/move_group_interface_client/client_behaviors/cb_pouring_motion.cpp