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 #ifndef MOVEIT_ROBOT_STATE_ATTACHED_BODY_
38 #define MOVEIT_ROBOT_STATE_ATTACHED_BODY_
39 
42 #include <boost/function.hpp>
43 #include <trajectory_msgs/JointTrajectory.h>
44 #include <set>
45 
46 namespace moveit
47 {
48 namespace core
49 {
51 typedef boost::function<void(AttachedBody* body, bool attached)> AttachedBodyCallback;
52 
57 {
58 public:
63  AttachedBody(const LinkModel* link, const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
64  const EigenSTL::vector_Affine3d& attach_trans, const std::set<std::string>& touch_links,
65  const trajectory_msgs::JointTrajectory& attach_posture);
66 
67  ~AttachedBody();
68 
70  const std::string& getName() const
71  {
72  return id_;
73  }
74 
76  const std::string& getAttachedLinkName() const
77  {
78  return parent_link_model_->getName();
79  }
80 
82  const LinkModel* getAttachedLink() const
83  {
84  return parent_link_model_;
85  }
86 
88  const std::vector<shapes::ShapeConstPtr>& getShapes() const
89  {
90  return shapes_;
91  }
92 
94  const std::set<std::string>& getTouchLinks() const
95  {
96  return touch_links_;
97  }
98 
102  const trajectory_msgs::JointTrajectory& getDetachPosture() const
103  {
104  return detach_posture_;
105  }
106 
109  {
110  return attach_trans_;
111  }
112 
115  {
117  }
118 
120  void setPadding(double padding);
121 
123  void setScale(double scale);
124 
126  void computeTransform(const Eigen::Affine3d& parent_link_global_transform)
127  {
128  for (std::size_t i = 0; i < global_collision_body_transforms_.size(); ++i)
129  global_collision_body_transforms_[i] = parent_link_global_transform * attach_trans_[i];
130  }
131 
132 private:
135 
137  std::string id_;
138 
140  std::vector<shapes::ShapeConstPtr> shapes_;
141 
144 
146  std::set<std::string> touch_links_;
147 
150  trajectory_msgs::JointTrajectory detach_posture_;
151 
154 };
155 }
156 }
157 
158 #endif
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
Definition: attached_body.h:76
const std::string & getName() const
The name of this link.
Definition: link_model.h:81
EigenSTL::vector_Affine3d attach_trans_
The constant transforms applied to the link (needs to be specified by user)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
const std::string & getName() const
Get the name of the attached body.
Definition: attached_body.h:70
std::vector< shapes::ShapeConstPtr > shapes_
The geometries of the attached body.
trajectory_msgs::JointTrajectory detach_posture_
Posture of links for releasing the object (if any). This is useful for example when storing the confi...
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
Definition: attached_body.h:88
std::string id_
string id for reference
const EigenSTL::vector_Affine3d & getFixedTransforms() const
Get the fixed transform (the transforms to the shapes associated with this body)
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
Definition: attached_body.h:50
EigenSTL::vector_Affine3d global_collision_body_transforms_
The global transforms for these attached bodies (computed by forward kinematics)
std::set< std::string > touch_links_
The set of links this body is allowed to touch.
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
Definition: attached_body.h:94
const LinkModel * getAttachedLink() const
Get the model of the link this body is attached to.
Definition: attached_body.h:82
void computeTransform(const Eigen::Affine3d &parent_link_global_transform)
Recompute global_collision_body_transform given the transform of the parent link. ...
const EigenSTL::vector_Affine3d & getGlobalCollisionBodyTransforms() const
Get the global transforms for the collision bodies.
AttachedBody(const LinkModel *link, const std::string &id, const std::vector< shapes::ShapeConstPtr > &shapes, const EigenSTL::vector_Affine3d &attach_trans, const std::set< std::string > &touch_links, const trajectory_msgs::JointTrajectory &attach_posture)
Construct an attached body for a specified link. The name of this body is id and it consists of shape...
const LinkModel * parent_link_model_
The link that owns this attached body.
A link from the robot. Contains the constant transform applied to the link and its geometry...
Definition: link_model.h:72
Main namespace for MoveIt!
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
void setScale(double scale)
Set the scale for the shapes of this attached object.
void setPadding(double padding)
Set the padding for the shapes of this attached object.
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...


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:04