attached_body.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #pragma once
38 
42 #include <eigen_stl_containers/eigen_stl_containers.h>
43 #include <boost/function.hpp>
44 #include <trajectory_msgs/JointTrajectory.h>
45 #include <set>
46 
47 namespace moveit
48 {
49 namespace core
50 {
51 class AttachedBody;
52 typedef boost::function<void(AttachedBody* body, bool attached)> AttachedBodyCallback;
53 
57 class AttachedBody
58 {
59 public:
66  AttachedBody(const LinkModel* parent, const std::string& id, const Eigen::Isometry3d& pose,
67  const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
68  const std::set<std::string>& touch_links, const trajectory_msgs::JointTrajectory& detach_posture,
70 
71  ~AttachedBody();
72 
74  const std::string& getName() const
75  {
76  return id_;
77  }
78 
80  const Eigen::Isometry3d& getPose() const
81  {
82  return pose_;
83  }
84 
86  const Eigen::Isometry3d& getGlobalPose() const
87  {
88  return global_pose_;
89  }
90 
92  const std::string& getAttachedLinkName() const
93  {
94  return parent_link_model_->getName();
95  }
96 
98  const LinkModel* getAttachedLink() const
99  {
100  return parent_link_model_;
101  }
102 
104  const std::vector<shapes::ShapeConstPtr>& getShapes() const
105  {
106  return shapes_;
107  }
108 
111  const EigenSTL::vector_Isometry3d& getShapePoses() const
112  {
113  return shape_poses_;
114  }
115 
117  const std::set<std::string>& getTouchLinks() const
118  {
119  return touch_links_;
120  }
121 
124  const trajectory_msgs::JointTrajectory& getDetachPosture() const
125  {
126  return detach_posture_;
127  }
128 
131  const EigenSTL::vector_Isometry3d& getShapePosesInLinkFrame() const
132  {
134  }
135 
139  [[deprecated]] const EigenSTL::vector_Isometry3d& getFixedTransforms() const
140  {
142  }
143 
147  {
148  return subframe_poses_;
149  }
150 
153  {
154  return global_subframe_poses_;
155  }
156 
163  {
164  for (const auto& t : subframe_poses)
165  {
166  ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry
167  }
168  subframe_poses_ = subframe_poses;
169  }
170 
176  const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
177 
183  const Eigen::Isometry3d& getSubframeTransformInLinkFrame(const std::string& frame_name, bool* found = nullptr) const;
184 
189  const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
190 
195  bool hasSubframeTransform(const std::string& frame_name) const;
196 
199  const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const
200  {
202  }
203 
205  void setPadding(double padding);
206 
208  void setScale(double scale);
209 
211  void computeTransform(const Eigen::Isometry3d& parent_link_global_transform);
212 
213 private:
216 
218  std::string id_;
219 
221  Eigen::Isometry3d pose_;
222 
224  Eigen::Isometry3d global_pose_;
225 
227  std::vector<shapes::ShapeConstPtr> shapes_;
228 
230  EigenSTL::vector_Isometry3d shape_poses_;
231 
233  EigenSTL::vector_Isometry3d shape_poses_in_link_frame_;
234 
236  EigenSTL::vector_Isometry3d global_collision_body_transforms_;
237 
239  std::set<std::string> touch_links_;
240 
243  trajectory_msgs::JointTrajectory detach_posture_;
244 
247 
250 };
251 } // namespace core
252 } // namespace moveit
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core::AttachedBody::computeTransform
void computeTransform(const Eigen::Isometry3d &parent_link_global_transform)
Recompute global_collision_body_transform given the transform of the parent link.
Definition: attached_body.cpp:168
moveit::core::AttachedBody::getDetachPosture
const trajectory_msgs::JointTrajectory & getDetachPosture() const
Return the posture that is necessary for the object to be released, (if any). This is useful for exam...
Definition: attached_body.h:188
shapes
moveit::core::AttachedBody::getFixedTransforms
const EigenSTL::vector_Isometry3d & getFixedTransforms() const
Get the fixed transforms (the transforms to the shapes of this body, relative to the link)....
Definition: attached_body.h:203
moveit::core::AttachedBody::getGlobalSubframeTransform
const Eigen::Isometry3d & getGlobalSubframeTransform(const std::string &frame_name, bool *found=nullptr) const
Get the fixed transform to a named subframe on this body, relative to the world frame....
Definition: attached_body.cpp:218
check_isometry.h
moveit::core::AttachedBody::getShapePosesInLinkFrame
const EigenSTL::vector_Isometry3d & getShapePosesInLinkFrame() const
Get the fixed transforms (the transforms to the shapes of this body, relative to the link)....
Definition: attached_body.h:195
moveit::core::AttachedBody::AttachedBody
AttachedBody(const LinkModel *parent, const std::string &id, const Eigen::Isometry3d &pose, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &shape_poses, const std::set< std::string > &touch_links, const trajectory_msgs::JointTrajectory &detach_posture, const moveit::core::FixedTransformsMap &subframe_poses=moveit::core::FixedTransformsMap())
Construct an attached body for a specified link.
Definition: attached_body.cpp:110
moveit::core::AttachedBody::~AttachedBody
~AttachedBody()
moveit::core::AttachedBody::getTouchLinks
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
Definition: attached_body.h:181
moveit::core::AttachedBody::subframe_poses_
moveit::core::FixedTransformsMap subframe_poses_
Transforms to subframes on the object, relative to the object's pose.
Definition: attached_body.h:310
moveit::core::AttachedBody::getSubframeTransformInLinkFrame
const Eigen::Isometry3d & getSubframeTransformInLinkFrame(const std::string &frame_name, bool *found=nullptr) const
Get the fixed transform to a named subframe on this body (relative to the robot link)
moveit::core::AttachedBody::getGlobalPose
const Eigen::Isometry3d & getGlobalPose() const
Get the pose of the attached body, relative to the world.
Definition: attached_body.h:150
moveit::core::AttachedBody::global_subframe_poses_
moveit::core::FixedTransformsMap global_subframe_poses_
Transforms to subframes on the object, relative to the model frame.
Definition: attached_body.h:313
moveit::core::AttachedBody::shape_poses_in_link_frame_
EigenSTL::vector_Isometry3d shape_poses_in_link_frame_
The transforms from the link to the object's geometries.
Definition: attached_body.h:297
transforms.h
moveit::core::AttachedBody::setScale
void setScale(double scale)
Set the scale for the shapes of this attached object.
Definition: attached_body.cpp:151
moveit::core::AttachedBody::id_
std::string id_
string id for reference
Definition: attached_body.h:282
moveit::core::AttachedBody::setPadding
void setPadding(double padding)
Set the padding for the shapes of this attached object.
Definition: attached_body.cpp:183
moveit::core::AttachedBody::shape_poses_
EigenSTL::vector_Isometry3d shape_poses_
The transforms from the object's pose to the object's geometries.
Definition: attached_body.h:294
moveit::core::AttachedBody::hasSubframeTransform
bool hasSubframeTransform(const std::string &frame_name) const
Check whether a subframe of given @frame_name is present in this object.
Definition: attached_body.cpp:236
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
moveit::core::AttachedBody::getGlobalCollisionBodyTransforms
const EigenSTL::vector_Isometry3d & getGlobalCollisionBodyTransforms() const
Get the global transforms (in world frame) for the collision bodies. The returned transforms are guar...
Definition: attached_body.h:263
moveit::core::AttachedBody::getSubframes
const moveit::core::FixedTransformsMap & getSubframes() const
Get subframes of this object (relative to the object pose). The returned transforms are guaranteed to...
Definition: attached_body.h:210
moveit::core::AttachedBody::getGlobalSubframeTransforms
const moveit::core::FixedTransformsMap & getGlobalSubframeTransforms() const
Get subframes of this object (in the world frame)
Definition: attached_body.h:216
moveit::core::AttachedBodyCallback
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:115
moveit::core::FixedTransformsMap
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:117
moveit::core::AttachedBody::getAttachedLink
const LinkModel * getAttachedLink() const
Get the model of the link this body is attached to.
Definition: attached_body.h:162
moveit::core::AttachedBody::getSubframeTransform
const Eigen::Isometry3d & getSubframeTransform(const std::string &frame_name, bool *found=nullptr) const
Get the fixed transform to a named subframe on this body (relative to the body's pose)
Definition: attached_body.cpp:200
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
moveit::core::AttachedBody::pose_
Eigen::Isometry3d pose_
The transform from the parent link to the attached body's pose.
Definition: attached_body.h:285
moveit::core::AttachedBody::getAttachedLinkName
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
Definition: attached_body.h:156
moveit::core::AttachedBody::global_collision_body_transforms_
EigenSTL::vector_Isometry3d global_collision_body_transforms_
The global transforms for the attached bodies (computed by forward kinematics)
Definition: attached_body.h:300
moveit::core::AttachedBody::getShapes
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:168
moveit::core::AttachedBody::setSubframeTransforms
void setSubframeTransforms(const moveit::core::FixedTransformsMap &subframe_poses)
Set all subframes of this object.
Definition: attached_body.h:226
moveit::core::AttachedBody::parent_link_model_
const LinkModel * parent_link_model_
The link that owns this attached body.
Definition: attached_body.h:279
moveit::core::LinkModel::getName
const std::string & getName() const
The name of this link.
Definition: link_model.h:80
moveit::core::AttachedBody::shapes_
std::vector< shapes::ShapeConstPtr > shapes_
The geometries of the attached body.
Definition: attached_body.h:291
moveit::core::AttachedBody::getPose
const Eigen::Isometry3d & getPose() const
Get the pose of the attached body relative to the parent link.
Definition: attached_body.h:144
moveit::core::AttachedBody::getShapePoses
const EigenSTL::vector_Isometry3d & getShapePoses() const
Get the shape poses (the transforms to the shapes of this body, relative to the pose)....
Definition: attached_body.h:175
moveit::core::AttachedBody::getName
const std::string & getName() const
Get the name of the attached body.
Definition: attached_body.h:138
moveit::core::AttachedBody::detach_posture_
trajectory_msgs::JointTrajectory detach_posture_
Posture of links for releasing the object (if any). This is useful for example when storing the confi...
Definition: attached_body.h:307
t
geometry_msgs::TransformStamped t
moveit::core::AttachedBody::touch_links_
std::set< std::string > touch_links_
The set of links this body is allowed to touch.
Definition: attached_body.h:303
moveit::core::AttachedBody::global_pose_
Eigen::Isometry3d global_pose_
The transform from the model frame to the attached body's pose
Definition: attached_body.h:288


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jan 15 2022 03:25:06