link_model.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, 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 the Willow Garage 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 #include <moveit/robot_model/link_model.h>
00038 #include <moveit/robot_model/joint_model.h>
00039 #include <geometric_shapes/shape_operations.h>
00040 #include <moveit/robot_model/aabb.h>
00041 
00042 #include <map>
00043 #include <boost/thread/mutex.hpp>
00044 #include <boost/thread/lock_guard.hpp>
00045 
00051 std::map<const moveit::core::LinkModel* const, Eigen::Vector3d> centered_bounding_box_offsets;
00052 boost::mutex mutex_centered_bounding_box_offsets;
00053 
00054 moveit::core::LinkModel::LinkModel(const std::string& name)
00055   : name_(name)
00056   , parent_joint_model_(NULL)
00057   , parent_link_model_(NULL)
00058   , is_parent_joint_fixed_(false)
00059   , joint_origin_transform_is_identity_(true)
00060   , first_collision_body_transform_index_(-1)
00061   , link_index_(-1)
00062 {
00063   joint_origin_transform_.setIdentity();
00064   centered_bounding_box_offsets[this] = Eigen::Vector3d::Zero();
00065 }
00066 
00067 moveit::core::LinkModel::~LinkModel()
00068 {
00069   centered_bounding_box_offsets.erase(this);
00070 }
00071 
00072 void moveit::core::LinkModel::setJointOriginTransform(const Eigen::Affine3d& transform)
00073 {
00074   joint_origin_transform_ = transform;
00075   joint_origin_transform_is_identity_ =
00076       joint_origin_transform_.rotation().isIdentity() &&
00077       joint_origin_transform_.translation().norm() < std::numeric_limits<double>::epsilon();
00078 }
00079 
00080 void moveit::core::LinkModel::setParentJointModel(const JointModel* joint)
00081 {
00082   parent_joint_model_ = joint;
00083   is_parent_joint_fixed_ = joint->getType() == JointModel::FIXED;
00084 }
00085 
00086 void moveit::core::LinkModel::setGeometry(const std::vector<shapes::ShapeConstPtr>& shapes,
00087                                           const EigenSTL::vector_Affine3d& origins)
00088 {
00089   shapes_ = shapes;
00090   collision_origin_transform_ = origins;
00091   collision_origin_transform_is_identity_.resize(collision_origin_transform_.size());
00092 
00093   core::AABB aabb;
00094 
00095   for (std::size_t i = 0; i < shapes_.size(); ++i)
00096   {
00097     collision_origin_transform_is_identity_[i] =
00098         (collision_origin_transform_[i].rotation().isIdentity() &&
00099          collision_origin_transform_[i].translation().norm() < std::numeric_limits<double>::epsilon()) ?
00100             1 :
00101             0;
00102     Eigen::Affine3d transform = collision_origin_transform_[i];
00103 
00104     if (shapes_[i]->type != shapes::MESH)
00105     {
00106       Eigen::Vector3d extents = shapes::computeShapeExtents(shapes_[i].get());
00107       aabb.extendWithTransformedBox(transform, extents);
00108     }
00109     else
00110     {
00111       // we cannot use shapes::computeShapeExtents() for meshes, since that method does not provide information about
00112       // the offset of the mesh origin
00113       const shapes::Mesh* mesh = dynamic_cast<const shapes::Mesh*>(shapes_[i].get());
00114       for (unsigned int j = 0; j < mesh->vertex_count; ++j)
00115       {
00116         aabb.extend(transform * Eigen::Map<Eigen::Vector3d>(&mesh->vertices[3 * j]));
00117       }
00118     }
00119   }
00120 
00121   {
00122     boost::lock_guard<boost::mutex> lock(mutex_centered_bounding_box_offsets);
00123     centered_bounding_box_offsets[this] = aabb.center();
00124   }
00125   shape_extents_ = aabb.sizes();
00126 }
00127 
00128 void moveit::core::LinkModel::setVisualMesh(const std::string& visual_mesh, const Eigen::Affine3d& origin,
00129                                             const Eigen::Vector3d& scale)
00130 {
00131   visual_mesh_filename_ = visual_mesh;
00132   visual_mesh_origin_ = origin;
00133   visual_mesh_scale_ = scale;
00134 }
00135 
00136 const Eigen::Vector3d moveit::core::LinkModel::getCenteredBoundingBoxOffset() const
00137 {
00138   Eigen::Vector3d offset;
00139   boost::lock_guard<boost::mutex> lock(mutex_centered_bounding_box_offsets);
00140 
00141   offset = centered_bounding_box_offsets.at(this);
00142 
00143   return offset;
00144 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jan 17 2018 03:31:36