link_model.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2013, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage, Inc. nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #ifndef MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_
00038 #define MOVEIT_CORE_ROBOT_MODEL_LINK_MODEL_
00039 
00040 #include <string>
00041 #include <vector>
00042 #include <utility>
00043 #include <map>
00044 #include <Eigen/Geometry>
00045 #include <geometric_shapes/shapes.h>
00046 #include <eigen_stl_containers/eigen_stl_vector_container.h>
00047 
00048 namespace moveit
00049 {
00050 namespace core
00051 {
00052 class JointModel;
00053 class LinkModel;
00054 
00056 typedef std::map<std::string, LinkModel*> LinkModelMap;
00057 
00059 typedef std::map<std::string, const LinkModel*> LinkModelMapConst;
00060 
00062 typedef std::map<const LinkModel*, Eigen::Affine3d, std::less<const LinkModel*>,
00063                  Eigen::aligned_allocator<std::pair<const LinkModel*, Eigen::Affine3d> > >
00064     LinkTransformMap;
00065 
00067 class LinkModel
00068 {
00069 public:
00070   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00071 
00072   LinkModel(const std::string& name);
00073   ~LinkModel();
00074 
00076   const std::string& getName() const
00077   {
00078     return name_;
00079   }
00080 
00082   int getLinkIndex() const
00083   {
00084     return link_index_;
00085   }
00086 
00087   void setLinkIndex(int index)
00088   {
00089     link_index_ = index;
00090   }
00091 
00092   int getFirstCollisionBodyTransformIndex() const
00093   {
00094     return first_collision_body_transform_index_;
00095   }
00096 
00097   void setFirstCollisionBodyTransformIndex(int index)
00098   {
00099     first_collision_body_transform_index_ = index;
00100   }
00101 
00103   const JointModel* getParentJointModel() const
00104   {
00105     return parent_joint_model_;
00106   }
00107 
00108   void setParentJointModel(const JointModel* joint);
00109 
00112   const LinkModel* getParentLinkModel() const
00113   {
00114     return parent_link_model_;
00115   }
00116 
00117   void setParentLinkModel(const LinkModel* link)
00118   {
00119     parent_link_model_ = link;
00120   }
00121 
00123   const std::vector<const JointModel*>& getChildJointModels() const
00124   {
00125     return child_joint_models_;
00126   }
00127 
00128   void addChildJointModel(const JointModel* joint)
00129   {
00130     child_joint_models_.push_back(joint);
00131   }
00132 
00137   const Eigen::Affine3d& getJointOriginTransform() const
00138   {
00139     return joint_origin_transform_;
00140   }
00141 
00142   bool jointOriginTransformIsIdentity() const
00143   {
00144     return joint_origin_transform_is_identity_;
00145   }
00146 
00147   bool parentJointIsFixed() const
00148   {
00149     return is_parent_joint_fixed_;
00150   }
00151 
00152   void setJointOriginTransform(const Eigen::Affine3d& transform);
00153 
00157   const EigenSTL::vector_Affine3d& getCollisionOriginTransforms() const
00158   {
00159     return collision_origin_transform_;
00160   }
00161 
00163   const std::vector<int>& areCollisionOriginTransformsIdentity() const
00164   {
00165     return collision_origin_transform_is_identity_;
00166   }
00167 
00169   const std::vector<shapes::ShapeConstPtr>& getShapes() const
00170   {
00171     return shapes_;
00172   }
00173 
00174   void setGeometry(const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Affine3d& origins);
00175 
00179   const Eigen::Vector3d& getShapeExtentsAtOrigin() const
00180   {
00181     return shape_extents_;
00182   }
00183 
00185   const LinkTransformMap& getAssociatedFixedTransforms() const
00186   {
00187     return associated_fixed_transforms_;
00188   }
00189 
00191   void addAssociatedFixedTransform(const LinkModel* link_model, const Eigen::Affine3d& transform)
00192   {
00193     associated_fixed_transforms_[link_model] = transform;
00194   }
00195 
00197   const std::string& getVisualMeshFilename() const
00198   {
00199     return visual_mesh_filename_;
00200   }
00201 
00203   const Eigen::Vector3d& getVisualMeshScale() const
00204   {
00205     return visual_mesh_scale_;
00206   }
00207 
00209   const Eigen::Affine3d& getVisualMeshOrigin() const
00210   {
00211     return visual_mesh_origin_;
00212   }
00213 
00214   void setVisualMesh(const std::string& visual_mesh, const Eigen::Affine3d& origin, const Eigen::Vector3d& scale);
00215 
00216 private:
00218   std::string name_;
00219 
00221   const JointModel* parent_joint_model_;
00222 
00224   const LinkModel* parent_link_model_;
00225 
00227   std::vector<const JointModel*> child_joint_models_;
00228 
00230   bool is_parent_joint_fixed_;
00231 
00233   bool joint_origin_transform_is_identity_;
00234 
00236   Eigen::Affine3d joint_origin_transform_;
00237 
00239   EigenSTL::vector_Affine3d collision_origin_transform_;
00240 
00243   std::vector<int> collision_origin_transform_is_identity_;
00244 
00246   LinkTransformMap associated_fixed_transforms_;
00247 
00249   std::vector<shapes::ShapeConstPtr> shapes_;
00250 
00252   Eigen::Vector3d shape_extents_;
00253 
00255   std::string visual_mesh_filename_;
00256 
00258   Eigen::Affine3d visual_mesh_origin_;
00259 
00261   Eigen::Vector3d visual_mesh_scale_;
00262 
00265   int first_collision_body_transform_index_;
00266 
00268   int link_index_;
00269 };
00270 }
00271 }
00272 
00273 #endif


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jul 24 2017 02:20:44