SMACC2
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | Protected Attributes | Private Member Functions | List of all members
cl_move_group_interface::CbCircularPivotMotion Class Reference

#include <cb_circular_pivot_motion.hpp>

Inheritance diagram for cl_move_group_interface::CbCircularPivotMotion:
Inheritance graph
Collaboration diagram for cl_move_group_interface::CbCircularPivotMotion:
Collaboration graph

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 ()
 
ISmaccStateMachinegetStateMachine ()
 
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< boolallowInitialTrajectoryStateJointDiscontinuity_
 

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_
 
ClMoveGroupmovegroupClient_ = 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 ()
 
ISmaccStategetCurrentState ()
 
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.

31{
32}
CbMoveEndEffectorTrajectory(std::optional< std::string > tipLink=std::nullopt)

◆ CbCircularPivotMotion() [2/3]

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

◆ 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.

47 relativeInitialPose_(relativeInitialPose),
48 planePivotPose_(planePivotPose),
49 deltaRadians_(deltaRadians)
50{
51}
std::optional< geometry_msgs::msg::Pose > relativeInitialPose_

Member Function Documentation

◆ computeCurrentEndEffectorPoseRelativeToPivot()

void cl_move_group_interface::CbCircularPivotMotion::computeCurrentEndEffectorPoseRelativeToPivot ( )
private

Definition at line 159 of file cb_circular_pivot_motion.cpp.

160{
161 //auto currentRobotEndEffectorPose = this->movegroupClient_->moveGroupClientInterface.getCurrentPose();
162
163 tf2_ros::Buffer tfBuffer(getNode()->get_clock());
164 tf2_ros::TransformListener tfListener(tfBuffer);
165
166 // tf2::Stamped<tf2::Transform> globalBaseLink;
167 tf2::Stamped<tf2::Transform> endEffectorInPivotFrame;
168
169 try
170 {
171 if (!tipLink_ || *tipLink_ == "")
172 {
173 tipLink_ = this->movegroupClient_->moveGroupClientInterface->getEndEffectorLink();
174 }
175
176 RCLCPP_INFO_STREAM(
177 getLogger(), "[" << getName() << "] waiting transform, pivot: '"
178 << planePivotPose_.header.frame_id << "' tipLink: '" << *tipLink_ << "'");
179 tf2::fromMsg(
180 tfBuffer.lookupTransform(
181 planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(), rclcpp::Duration(10s)),
182 endEffectorInPivotFrame);
183
184 //endEffectorInPivotFrame = tfBuffer.lookupTransform(planePivotPose_.header.frame_id, *tipLink_, rclcpp::Time(0));
185
186 // we define here the global frame as the pivot frame id
187 // tfListener.waitForTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), rclcpp::Duration(10));
188 // tfListener.lookupTransform(currentRobotEndEffectorPose.header.frame_id, planePivotPose_.header.frame_id, rclcpp::Time(0), globalBaseLink);
189 }
190 catch (const std::exception & e)
191 {
192 std::cerr << e.what() << '\n';
193 }
194
195 // tf2::Transform endEffectorInBaseLinkFrame;
196 // tf2::fromMsg(currentRobotEndEffectorPose.pose, endEffectorInBaseLinkFrame);
197
198 // tf2::Transform endEffectorInPivotFrame = globalBaseLink * endEffectorInBaseLinkFrame; // pose composition
199
200 // now pivot and EndEffector share a common reference frame (let say map)
201 // now get the current pose from the pivot reference frame with inverse composition
202 tf2::Transform pivotTransform;
203 tf2::fromMsg(planePivotPose_.pose, pivotTransform);
204 tf2::Transform invertedNewReferenceFrame = pivotTransform.inverse();
205
206 tf2::Transform currentPoseRelativeToPivot = invertedNewReferenceFrame * endEffectorInPivotFrame;
207
208 geometry_msgs::msg::Pose finalEndEffectorRelativePose;
209 tf2::toMsg(currentPoseRelativeToPivot, finalEndEffectorRelativePose);
210 relativeInitialPose_ = finalEndEffectorRelativePose;
211}
std::shared_ptr< moveit::planning_interface::MoveGroupInterface > moveGroupClientInterface
virtual rclcpp::Logger getLogger() const
virtual rclcpp::Node::SharedPtr getNode() const

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.

214{
216
217 tf2::Transform localdirection;
218 localdirection.setIdentity();
219 localdirection.setOrigin(tf2::Vector3(0.12, 0, 0));
220 auto frameid = planePivotPose_.header.frame_id;
221
222 visualization_msgs::msg::Marker marker;
223 marker.header.frame_id = frameid;
224 marker.header.stamp = getNode()->now();
225 marker.ns = "trajectory";
226 marker.id = beahiorMarkers_.markers.size();
227 marker.type = visualization_msgs::msg::Marker::ARROW;
228 marker.action = visualization_msgs::msg::Marker::ADD;
229 marker.scale.x = 0.01;
230 marker.scale.y = 0.02;
231 marker.scale.z = 0.02;
232 marker.color.a = 1.0;
233 marker.color.r = 0.0;
234 marker.color.g = 0;
235 marker.color.b = 1.0;
236
237 geometry_msgs::msg::Point start, end;
238 start.x = planePivotPose_.pose.position.x;
239 start.y = planePivotPose_.pose.position.y;
240 start.z = planePivotPose_.pose.position.z;
241
242 tf2::Transform basetransform;
243 tf2::fromMsg(planePivotPose_.pose, basetransform);
244 tf2::Transform endarrow = localdirection * basetransform;
245
246 end.x = endarrow.getOrigin().x();
247 end.y = endarrow.getOrigin().y();
248 end.z = endarrow.getOrigin().z();
249
250 marker.pose.orientation.w = 1;
251 marker.points.push_back(start);
252 marker.points.push_back(end);
253
254 beahiorMarkers_.markers.push_back(marker);
255}
const std::string frameid

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.

54{
56 {
58 }
59
60 // project offset into the xy-plane
61 // get the radius
62 double radius = sqrt(
63 relativeInitialPose_->position.z * relativeInitialPose_->position.z +
64 relativeInitialPose_->position.y * relativeInitialPose_->position.y);
65 double initialAngle = atan2(relativeInitialPose_->position.z, relativeInitialPose_->position.y);
66
67 double totallineardist = fabs(radius * deltaRadians_);
68 double totalangulardist = fabs(deltaRadians_);
69
70 // at least 1 sample per centimeter (average)
71 // at least 1 sample per ~1.1 degrees (average)
72
73 const double RADS_PER_SAMPLE = 0.02;
74 const double METERS_PER_SAMPLE = 0.01;
75
76 int totalSamplesCount =
77 std::max(totallineardist / METERS_PER_SAMPLE, totalangulardist / RADS_PER_SAMPLE);
78
79 double linearSecondsPerSample;
80 double angularSecondsPerSamples;
81 double secondsPerSample;
82
84 {
85 linearSecondsPerSample = METERS_PER_SAMPLE / (*linearSpeed_m_s_);
86 }
87 else
88 {
89 linearSecondsPerSample = std::numeric_limits<double>::max();
90 }
91
93 {
94 angularSecondsPerSamples = RADS_PER_SAMPLE / (*angularSpeed_rad_s_);
95 }
96 else
97 {
98 angularSecondsPerSamples = std::numeric_limits<double>::max();
99 }
100
102 {
103 secondsPerSample = 0.5;
104 }
105 else
106 {
107 secondsPerSample = std::min(linearSecondsPerSample, angularSecondsPerSamples);
108 }
109
110 double currentAngle = initialAngle;
111
112 double angleStep = deltaRadians_ / (double)totalSamplesCount;
113
114 tf2::Transform tfBasePose;
115 tf2::fromMsg(planePivotPose_.pose, tfBasePose);
116
117 RCLCPP_INFO_STREAM(
118 getLogger(),
119 "[" << getName() << "] generated trajectory, total samples: " << totalSamplesCount);
120 for (int i = 0; i < totalSamplesCount; i++)
121 {
122 // relativePose i
123 double y = radius * cos(currentAngle);
124 double z = radius * sin(currentAngle);
125
126 geometry_msgs::msg::Pose relativeCurrentPose;
127
128 relativeCurrentPose.position.x = relativeInitialPose_->position.x;
129 relativeCurrentPose.position.y = y;
130 relativeCurrentPose.position.z = z;
131
132 tf2::Quaternion localquat;
133 localquat.setEuler(currentAngle, 0, 0);
134
135 //relativeCurrentPose.orientation = relativeInitialPose_.orientation;
136 //tf2::toMsg(localquat, relativeCurrentPose.orientation);
137 relativeCurrentPose.orientation.w = 1;
138
139 tf2::Transform tfRelativeCurrentPose;
140 tf2::fromMsg(relativeCurrentPose, tfRelativeCurrentPose);
141
142 tf2::Transform tfGlobalPose = tfRelativeCurrentPose * tfBasePose;
143
144 tfGlobalPose.setRotation(tfGlobalPose.getRotation() * localquat);
145
146 geometry_msgs::msg::PoseStamped globalPose;
147 tf2::toMsg(tfGlobalPose, globalPose.pose);
148 globalPose.header.frame_id = planePivotPose_.header.frame_id;
149 globalPose.header.stamp = rclcpp::Time(planePivotPose_.header.stamp) +
150 rclcpp::Duration::from_seconds(i * secondsPerSample);
151 RCLCPP_INFO_STREAM(
152 getLogger(), "[" << getName() << "]" << rclcpp::Time(globalPose.header.stamp).nanoseconds());
153
154 this->endEffectorTrajectory_.push_back(globalPose);
155 currentAngle += angleStep;
156 }
157}
std::vector< geometry_msgs::msg::PoseStamped > endEffectorTrajectory_

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

◆ 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

◆ relativeInitialPose_

std::optional<geometry_msgs::msg::Pose> cl_move_group_interface::CbCircularPivotMotion::relativeInitialPose_

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