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:
64  AttachedBody(const LinkModel* link, const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
65  const EigenSTL::vector_Isometry3d& attach_trans, const std::set<std::string>& touch_links,
66  const trajectory_msgs::JointTrajectory& attach_posture,
68 
69  ~AttachedBody();
70 
72  const std::string& getName() const
73  {
74  return id_;
75  }
76 
78  const std::string& getAttachedLinkName() const
79  {
80  return parent_link_model_->getName();
81  }
82 
84  const LinkModel* getAttachedLink() const
85  {
86  return parent_link_model_;
87  }
88 
90  const std::vector<shapes::ShapeConstPtr>& getShapes() const
91  {
92  return shapes_;
93  }
94 
96  const std::set<std::string>& getTouchLinks() const
97  {
98  return touch_links_;
99  }
100 
104  const trajectory_msgs::JointTrajectory& getDetachPosture() const
105  {
106  return detach_posture_;
107  }
108 
111  const EigenSTL::vector_Isometry3d& getFixedTransforms() const
112  {
113  return attach_trans_;
114  }
115 
119  {
120  return subframe_poses_;
121  }
122 
125  {
126  return global_subframe_poses_;
127  }
128 
134  void setSubframeTransforms(const moveit::core::FixedTransformsMap& subframe_poses)
135  {
136  for (const auto& t : subframe_poses)
137  {
138  ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry
139  }
140  subframe_poses_ = subframe_poses;
141  }
142 
148  const Eigen::Isometry3d& getSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
149 
154  const Eigen::Isometry3d& getGlobalSubframeTransform(const std::string& frame_name, bool* found = nullptr) const;
155 
160  bool hasSubframeTransform(const std::string& frame_name) const;
161 
164  const EigenSTL::vector_Isometry3d& getGlobalCollisionBodyTransforms() const
165  {
167  }
168 
170  void setPadding(double padding);
171 
173  void setScale(double scale);
174 
176  void computeTransform(const Eigen::Isometry3d& parent_link_global_transform);
177 
178 private:
181 
183  std::string id_;
184 
186  std::vector<shapes::ShapeConstPtr> shapes_;
187 
189  EigenSTL::vector_Isometry3d attach_trans_;
190 
192  std::set<std::string> touch_links_;
193 
196  trajectory_msgs::JointTrajectory detach_posture_;
197 
199  EigenSTL::vector_Isometry3d global_collision_body_transforms_;
200 
203 
206 };
207 } // namespace core
208 } // 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:156
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:168
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:175
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. The frame_name needs to have the object's n...
Definition: attached_body.cpp:205
check_isometry.h
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:160
moveit::core::AttachedBody::subframe_poses_
moveit::core::FixedTransformsMap subframe_poses_
Transforms to subframes on the object. Transforms are relative to the link.
Definition: attached_body.h:266
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:269
transforms.h
moveit::core::AttachedBody::setScale
void setScale(double scale)
Set the scale for the shapes of this attached object.
Definition: attached_body.cpp:139
moveit::core::AttachedBody::id_
std::string id_
string id for reference
Definition: attached_body.h:247
moveit::core::AttachedBody::setPadding
void setPadding(double padding)
Set the padding for the shapes of this attached object.
Definition: attached_body.cpp:170
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:223
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
moveit::core::AttachedBody::getGlobalCollisionBodyTransforms
const EigenSTL::vector_Isometry3d & getGlobalCollisionBodyTransforms() const
Get the global transforms for the collision bodies. The returned transforms are guaranteed to be vali...
Definition: attached_body.h:228
moveit::core::AttachedBody::getGlobalSubframeTransforms
const moveit::core::FixedTransformsMap & getGlobalSubframeTransforms() const
Get subframes of this object (in the world frame)
Definition: attached_body.h:188
moveit::core::AttachedBody::attach_trans_
EigenSTL::vector_Isometry3d attach_trans_
The constant transforms applied to the link (needs to be specified by user)
Definition: attached_body.h:253
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:148
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 robot link)
Definition: attached_body.cpp:187
moveit::core::AttachedBody::getSubframeTransforms
const moveit::core::FixedTransformsMap & getSubframeTransforms() const
Get subframes of this object (relative to the link). The returned transforms are guaranteed to be val...
Definition: attached_body.h:182
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
moveit::core::AttachedBody::getAttachedLinkName
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
Definition: attached_body.h:142
moveit::core::AttachedBody::global_collision_body_transforms_
EigenSTL::vector_Isometry3d global_collision_body_transforms_
The global transforms for these attached bodies (computed by forward kinematics)
Definition: attached_body.h:263
moveit::core::AttachedBody::getShapes
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:154
moveit::core::AttachedBody::setSubframeTransforms
void setSubframeTransforms(const moveit::core::FixedTransformsMap &subframe_poses)
Set all subframes of this object.
Definition: attached_body.h:198
moveit::core::AttachedBody::parent_link_model_
const LinkModel * parent_link_model_
The link that owns this attached body.
Definition: attached_body.h:244
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:250
moveit::core::AttachedBody::AttachedBody
AttachedBody(const LinkModel *link, const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Isometry3d &attach_trans, 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::getName
const std::string & getName() const
Get the name of the attached body.
Definition: attached_body.h:136
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:260
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:256


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Nov 24 2020 03:26:39