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:
65  AttachedBody(const LinkModel* link, const std::string& id, const Eigen::Isometry3d& pose,
66  const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
67  const std::set<std::string>& touch_links, const trajectory_msgs::JointTrajectory& attach_posture,
69 
70  ~AttachedBody();
71 
73  const std::string& getName() const
74  {
75  return id_;
76  }
77 
79  const Eigen::Isometry3d& getPose() const
80  {
81  return pose_;
82  }
83 
85  const Eigen::Isometry3d& getGlobalPose() const
86  {
87  return global_pose_;
88  }
89 
91  const std::string& getAttachedLinkName() const
92  {
93  return parent_link_model_->getName();
94  }
95 
97  const LinkModel* getAttachedLink() const
98  {
99  return parent_link_model_;
100  }
101 
103  const std::vector<shapes::ShapeConstPtr>& getShapes() const
104  {
105  return shapes_;
106  }
107 
110  const EigenSTL::vector_Isometry3d& getShapePoses() const
111  {
112  return shape_poses_;
113  }
114 
116  const std::set<std::string>& getTouchLinks() const
117  {
118  return touch_links_;
119  }
120 
123  const trajectory_msgs::JointTrajectory& getDetachPosture() const
124  {
125  return detach_posture_;
126  }
127 
130  const EigenSTL::vector_Isometry3d& getShapePosesInLinkFrame() const
131  {
133  }
134 
138  [[deprecated]] const EigenSTL::vector_Isometry3d& getFixedTransforms() const
139  {
141  }
142 
146  {
147  return subframe_poses_;
148  }
149 
152  {
153  return global_subframe_poses_;
154  }
155 
162  {
163  for (const auto& t : subframe_poses)
164  {
165  ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry
166  }
167  subframe_poses_ = subframe_poses;
168  }
169 
175  const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
176 
182  const Eigen::Isometry3d& getSubframeTransformInLinkFrame(const std::string& frame_name, bool* found = nullptr) const;
183 
188  const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
189 
194  bool hasSubframeTransform(const std::string& frame_name) const;
195 
198  const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const
199  {
201  }
202 
204  void setPadding(double padding);
205 
207  void setScale(double scale);
208 
210  void computeTransform(const Eigen::Isometry3d& parent_link_global_transform);
211 
212 private:
215 
217  std::string id_;
218 
220  Eigen::Isometry3d pose_;
221 
223  Eigen::Isometry3d global_pose_;
224 
226  std::vector<shapes::ShapeConstPtr> shapes_;
227 
229  EigenSTL::vector_Isometry3d shape_poses_;
230 
232  EigenSTL::vector_Isometry3d shape_poses_in_link_frame_;
233 
235  EigenSTL::vector_Isometry3d global_collision_body_transforms_;
236 
238  std::set<std::string> touch_links_;
239 
242  trajectory_msgs::JointTrajectory detach_posture_;
243 
246 
249 };
250 } // namespace core
251 } // 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:187
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:202
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:194
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:180
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:309
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:149
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:312
moveit::core::AttachedBody::AttachedBody
AttachedBody(const LinkModel *link, 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 &attach_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::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:296
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:281
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:293
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:262
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:209
moveit::core::AttachedBody::getGlobalSubframeTransforms
const moveit::core::FixedTransformsMap & getGlobalSubframeTransforms() const
Get subframes of this object (in the world frame)
Definition: attached_body.h:215
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:161
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:284
moveit::core::AttachedBody::getAttachedLinkName
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
Definition: attached_body.h:155
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:299
moveit::core::AttachedBody::getShapes
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:167
moveit::core::AttachedBody::setSubframeTransforms
void setSubframeTransforms(const moveit::core::FixedTransformsMap &subframe_poses)
Set all subframes of this object.
Definition: attached_body.h:225
moveit::core::AttachedBody::parent_link_model_
const LinkModel * parent_link_model_
The link that owns this attached body.
Definition: attached_body.h:278
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:290
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:143
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:174
moveit::core::AttachedBody::getName
const std::string & getName() const
Get the name of the attached body.
Definition: attached_body.h:137
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:306
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:302
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:287


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Sep 15 2021 02:23:15