chomp_motion_planner: chomp_collision

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, 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 
00037 #ifndef CHOMP_COLLISION_POINT_H_
00038 #define CHOMP_COLLISION_POINT_H_
00039 
00040 #include <kdl/frames.hpp>
00041 #include <vector>
00042 #include <Eigen/Core>
00043 #include <Eigen/Geometry>
00044 #include <algorithm>
00045 
00046 namespace chomp
00047 {
00048 
00049 class ChompCollisionPoint
00050 {
00051 public:
00052   ChompCollisionPoint(const std::vector<int>& parent_joints, double radius, double clearance,
00053       int segment_number, const KDL::Vector& position);
00054   ChompCollisionPoint(const ChompCollisionPoint &point, const std::vector<int>& parent_joints);
00055   virtual ~ChompCollisionPoint();
00056 
00057   bool isParentJoint(int joint) const;
00058   double getRadius() const;
00059   double getVolume() const;
00060   double getClearance() const;
00061   double getInvClearance() const;
00062   int getSegmentNumber() const;
00063   const KDL::Vector& getPosition() const;
00064 
00065   const std::vector<int>& getParentJoints() const {
00066     return parent_joints_;
00067   }
00068 
00069   void getTransformedPosition(std::vector<KDL::Frame>& segment_frames, KDL::Vector& position) const;
00070 
00071   template<typename Derived>
00072   void getJacobian(std::vector<Eigen::Map<Eigen::Vector3d> >& joint_pos, std::vector<Eigen::Map<Eigen::Vector3d> >& joint_axis,
00073       Eigen::Map<Eigen::Vector3d>& collision_point_pos, Eigen::MatrixBase<Derived>& jacobian, const std::vector<int>& group_joint_to_kdl_joint_index) const;
00074 
00075 private:
00076   std::vector<int> parent_joints_;      
00077   double radius_;                       
00078   double volume_;                       
00079   double clearance_;                    
00080   double inv_clearance_;                
00081   int segment_number_;                  
00082   KDL::Vector position_;                
00083 };
00084 
00085 inline bool ChompCollisionPoint::isParentJoint(int joint) const
00086 {
00087   return(find(parent_joints_.begin(), parent_joints_.end(), joint) != parent_joints_.end());
00088 }
00089 
00090 inline double ChompCollisionPoint::getRadius() const
00091 {
00092   return radius_;
00093 }
00094 
00095 inline double ChompCollisionPoint::getVolume() const
00096 {
00097   return volume_;
00098 }
00099 
00100 inline double ChompCollisionPoint::getClearance() const
00101 {
00102   return clearance_;
00103 }
00104 
00105 inline double ChompCollisionPoint::getInvClearance() const
00106 {
00107   return inv_clearance_;
00108 }
00109 
00110 inline int ChompCollisionPoint::getSegmentNumber() const
00111 {
00112   return segment_number_;
00113 }
00114 
00115 inline const KDL::Vector& ChompCollisionPoint::getPosition() const
00116 {
00117   return position_;
00118 }
00119 
00120 template<typename Derived>
00121 void ChompCollisionPoint::getJacobian(std::vector<Eigen::Map<Eigen::Vector3d> >& joint_pos, std::vector<Eigen::Map<Eigen::Vector3d> >& joint_axis,
00122     Eigen::Map<Eigen::Vector3d>& collision_point_pos, Eigen::MatrixBase<Derived>& jacobian, const std::vector<int>& group_joint_to_kdl_joint_index) const
00123 {
00124 
00125   for(unsigned int joint = 0; joint < group_joint_to_kdl_joint_index.size(); joint++) {
00126     if(!isParentJoint(group_joint_to_kdl_joint_index[joint])) {
00127       // since the joint is not active, fill the jacobian column with zeros
00128       jacobian.col(joint).setZero();
00129     }
00130     else
00131     {
00132       int kj = group_joint_to_kdl_joint_index[joint];
00133       jacobian.col(joint) = joint_axis[kj].cross(collision_point_pos - joint_pos[kj]);
00134     }
00135   }
00136 }
00137 
00138 inline void ChompCollisionPoint::getTransformedPosition(std::vector<KDL::Frame>& segment_frames, KDL::Vector& position) const
00139 {
00140   position = segment_frames[segment_number_] * position_;
00141 }
00142 
00143 } // namespace chomp
00144 
00145 #endif /* CHOMP_COLLISION_POINT_H_ */
chomp_collision_point.h
