collision_proximity_planner: collision_proximity_planner: collision_proximity

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 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 COLLISION_PROXIMITY_PLANNER_H_
00038 #define COLLISION_PROXIMITY_PLANNER_H_
00039 
00040 #include <ros/ros.h>
00041 #include <collision_proximity_planner/chomp_robot_model.h>
00042 #include <collision_proximity/collision_proximity_space.h>
00043 
00044 // System
00045 #include <Eigen/Geometry>
00046 #include <Eigen/Core>
00047 #include <algorithm>
00048 #include <vector>
00049 #include <string>
00050 #include <map>
00051 
00052 // KDL
00053 #include <kdl/jntarray.hpp>
00054 #include <kdl/frames.hpp>
00055 
00056 // ROS msgs
00057 #include <arm_navigation_msgs/FilterJointTrajectory.h>
00058 #include <arm_navigation_msgs/RobotTrajectory.h>
00059 #include <arm_navigation_msgs/RobotState.h>
00060 #include <visualization_msgs/MarkerArray.h>
00061 #include <arm_navigation_msgs/GetMotionPlan.h>
00062 #include <arm_navigation_msgs/DisplayTrajectory.h>
00063 
00064 // Arm Navigation
00065 #include <spline_smoother/cubic_trajectory.h>
00066 
00067 // MISC
00068 #include <angles/angles.h>
00069 
00070 namespace collision_proximity_planner
00071 {
00072   class CollisionProximityPlanner
00073   {
00074   public:
00075 
00076     CollisionProximityPlanner(const std::string& robot_description_name);
00077 
00078     virtual ~CollisionProximityPlanner();    
00079 
00080     void fillInGroupState(arm_navigation_msgs::RobotState &robot_state,
00081                           const arm_navigation_msgs::RobotState &group_state);
00082 
00083     bool findPathToFreeState(const arm_navigation_msgs::RobotState &robot_state, 
00084                              arm_navigation_msgs::RobotTrajectory &robot_trajectory);
00085 
00092     bool setRobotState(const arm_navigation_msgs::RobotState &robot_state);
00093 
00100     bool setGroupState(const arm_navigation_msgs::RobotState &group_state);
00101 
00107     bool getStateGradient(const arm_navigation_msgs::RobotState &group_state,
00108                           double &distance,
00109                           arm_navigation_msgs::RobotState &gradient);
00110 
00111     bool refineState(const arm_navigation_msgs::RobotState &group_state, 
00112                      arm_navigation_msgs::RobotTrajectory &robot_trajectory);
00113 
00114     void clear();
00115 
00116   private:
00117     ros::Publisher display_trajectory_publisher_;
00118 
00119     bool initializeForGroup(const std::string& group_name);
00120 
00121     bool mapGroupState(const arm_navigation_msgs::RobotState &group_state,const std::vector<int>& mapping);
00122     bool calculateCollisionIncrements(Eigen::MatrixXd &collision_increments, double &distance);
00123     void isParentJoint(const int& link_index, const int& joint_index);
00124     void setPlanningMonitorToCurrentState();
00125     void performForwardKinematics(KDL::JntArray &jnt_array, const bool& full);
00126     void updateJointState(KDL::JntArray &jnt_array, Eigen::MatrixXd &collision_increments);
00127     void getGroupArray(const KDL::JntArray &jnt_array,
00128                        const std::vector<int> &group_joint_to_kdl_joint_index,
00129                        KDL::JntArray &jnt_array_group);
00130     void fillInGroupArray(const KDL::JntArray &jnt_array_group,
00131                           const std::vector<int> &group_joint_to_kdl_joint_index,
00132                           KDL::JntArray &jnt_array);
00133     void updateGroupRobotState(const KDL::JntArray &jnt_array);
00134     void updateCollisionProximitySpace(const arm_navigation_msgs::RobotState &group_state);
00135     void kdlJointTrajectoryToRobotTrajectory(std::vector<KDL::JntArray> &jnt_trajectory,
00136                                              arm_navigation_msgs::RobotTrajectory &robot_trajectory);
00137 
00138     arm_navigation_msgs::RobotState robot_state_group_;
00139 
00140     ros::NodeHandle private_handle_, root_handle_;
00141     collision_proximity::CollisionProximitySpace* cps_;
00142     std::string reference_frame_, group_name_cps_;
00143     int num_joints_;
00144     chomp::ChompRobotModel chomp_robot_model_;
00145     const chomp::ChompRobotModel::ChompPlanningGroup *planning_group_;
00146     int max_iterations_;
00147     double max_joint_update_;
00148 
00149     std::vector<std::string> current_link_names_;
00150     std::vector<std::string> current_attached_body_names_;
00151 
00152     bool use_pseudo_inverse_;
00153     Eigen::MatrixXd jacobian_;
00154     Eigen::MatrixXd jacobian_pseudo_inverse_;
00155     Eigen::MatrixXd jacobian_jacobian_tranpose_;
00156     Eigen::MatrixXd collision_increments_;
00157     
00158     std::vector<KDL::Vector> joint_axis_;
00159     std::vector<KDL::Vector> joint_pos_;
00160     std::vector<KDL::Frame> segment_frames_;
00161     std::vector<KDL::Vector> collision_point_pos_;
00162     
00163     std::vector<Eigen::Map<Eigen::Vector3d> > joint_axis_eigen_;
00164     std::vector<Eigen::Map<Eigen::Vector3d> > joint_pos_eigen_;
00165     std::vector<Eigen::Map<Eigen::Vector3d> > collision_point_pos_eigen_;
00166     std::vector<double> collision_point_vel_mag_;
00167     std::vector<Eigen::Vector3d> collision_point_potential_gradient_;
00168 
00169     std::vector<int> group_joint_to_kdl_joint_index_;
00170     
00171     std::vector<std::vector<int> > active_joints_;
00172     
00173     ros::Publisher vis_marker_array_publisher_, vis_marker_publisher_;
00174     template<typename Derived>
00175     void getJacobian(const int& link_index,
00176                      std::vector<Eigen::Map<Eigen::Vector3d> >& joint_pos, 
00177                      std::vector<Eigen::Map<Eigen::Vector3d> >& joint_axis,
00178                      Eigen::Vector3d& collision_point_pos, 
00179                      Eigen::MatrixBase<Derived>& jacobian, 
00180                      const std::vector<int>& group_joint_to_kdl_joint_index) const;
00181     inline bool isParentJoint(const int& link_index, const int& joint_index) const;
00182 
00183     bool getFreePath(arm_navigation_msgs::GetMotionPlan::Request &req,
00184                      arm_navigation_msgs::GetMotionPlan::Response &res);
00185 
00186     ros::ServiceServer planning_service_;
00187 
00188     KDL::JntArray jnt_array_, jnt_array_group_;
00189     std::vector<int> group_state_joint_array_group_mapping_, joint_array_group_group_state_mapping_;
00190   };
00191 
00192 template<typename Derived>
00193 void CollisionProximityPlanner::getJacobian(const int& link_index,
00194                                             std::vector<Eigen::Map<Eigen::Vector3d> >& joint_pos, 
00195                                             std::vector<Eigen::Map<Eigen::Vector3d> >& joint_axis,
00196                                             Eigen::Vector3d& collision_point_pos, 
00197                                             Eigen::MatrixBase<Derived>& jacobian, 
00198                                             const std::vector<int>& group_joint_to_kdl_joint_index) const
00199 {
00200   for(unsigned int joint = 0; joint < group_joint_to_kdl_joint_index.size(); joint++) 
00201   {
00202     if(!isParentJoint(link_index, group_joint_to_kdl_joint_index[joint]))
00203     {
00204       // since the joint is not active, fill the jacobian column with zeros
00205       jacobian.col(joint).setZero();
00206     }
00207     else
00208     {
00209       int kj = group_joint_to_kdl_joint_index[joint];
00210       jacobian.col(joint) = joint_axis[kj].cross(collision_point_pos - joint_pos[kj]);
00211     }
00212   }
00213 }
00214 
00215 inline bool CollisionProximityPlanner::isParentJoint(const int& link_index, const int& joint_index) const
00216 {
00217   return(find(active_joints_[link_index].begin(), active_joints_[link_index].end(), joint_index) != active_joints_[link_index].end());
00218 }
00219 
00220 } // namespace collision_proximity_planner
00221 
00222 #endif
collision_proximity_planner.h
