chomp_optimizer.h
Go to the documentation of this file.
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_OPTIMIZER_H_
00038 #define CHOMP_OPTIMIZER_H_
00039 
00040 #include <chomp_motion_planner/chomp_parameters.h>
00041 #include <chomp_motion_planner/chomp_trajectory.h>
00042 #include <chomp_motion_planner/chomp_cost.h>
00043 #include <chomp_motion_planner/multivariate_gaussian.h>
00044 #include <planning_models/robot_model.h>
00045 #include <planning_scene/planning_scene.h>
00046 #include <collision_distance_field/collision_robot_hybrid.h>
00047 #include <collision_distance_field/collision_world_hybrid.h>
00048 
00049 
00050 #include <Eigen/Core>
00051 
00052 #include <vector>
00053 
00054 namespace chomp
00055 {
00056 
00057 class ChompOptimizer
00058 {
00059 public:
00060   ChompOptimizer(ChompTrajectory *trajectory,
00061                  const planning_scene::PlanningSceneConstPtr& planning_scene,
00062                  const std::string& planning_group,
00063                  const ChompParameters *parameters,
00064                  const planning_models::RobotState& start_state);
00065 
00066   virtual ~ChompOptimizer();
00067 
00068   void optimize();
00069 
00070   inline void destroy()
00071   {
00072     //Nothing for now.
00073   }
00074 
00075   bool isInitialized() const {
00076     return initialized_;
00077   }
00078 
00079 private:
00080 
00081   inline double getPotential(double field_distance, double radius, double clearence)
00082   {
00083     double d = field_distance - radius;
00084     double potential = 0.0;
00085 
00086     // three cases below:
00087     if (d >= clearence)
00088     {
00089       potential = 0.0;
00090     }
00091     else if (d >= 0.0)
00092     {
00093       double diff = (d - clearence);
00094       double gradient_magnitude = diff * clearence; // (diff / clearance)
00095       potential = 0.5*gradient_magnitude*diff;
00096     }
00097     else // if d < 0.0
00098     {
00099       potential = -d + 0.5 * clearence;
00100     }
00101 
00102     return potential;
00103   }
00104   template<typename Derived>
00105   void getJacobian(int trajectoryPoint,
00106                    Eigen::Vector3d& collision_point_pos,
00107                    std::string& jointName,
00108                    Eigen::MatrixBase<Derived>& jacobian) const;
00109 
00110   // void getRandomState(const planning_models::RobotState& currentState,
00111   //                     const std::string& group_name,
00112   //                     Eigen::VectorXd& state_vec);
00113 
00114   void setRobotStateFromPoint(ChompTrajectory& group_trajectory,
00115                               int i);
00116 
00117   //collision_proximity::CollisionProximitySpace::TrajectorySafety checkCurrentIterValidity();
00118 
00119   int num_joints_;
00120   int num_vars_free_;
00121   int num_vars_all_;
00122   int num_collision_points_;
00123   int free_vars_start_;
00124   int free_vars_end_;
00125   int iteration_;
00126   unsigned int collision_free_iteration_;
00127 
00128   ChompTrajectory *full_trajectory_;
00129   const planning_models::RobotModelConstPtr& kmodel_;
00130   std::string planning_group_;
00131   const ChompParameters *parameters_;
00132   ChompTrajectory group_trajectory_;
00133   planning_scene::PlanningSceneConstPtr planning_scene_;
00134   planning_models::RobotState *state_;
00135   planning_models::RobotState *start_state_;
00136   const planning_models::RobotModel::JointModelGroup* joint_model_group_;
00137   const collision_detection::CollisionWorldHybrid* hy_world_;
00138   const collision_detection::CollisionRobotHybrid* hy_robot_;
00139 
00140   std::vector<ChompCost> joint_costs_;
00141   boost::shared_ptr<collision_detection::GroupStateRepresentation> gsr_;
00142   bool initialized_;
00143 
00144   std::vector<std::vector<std::string> > collision_point_joint_names_;
00145   std::vector<std::vector<Eigen::Vector3d > > collision_point_pos_eigen_;
00146   std::vector<std::vector<Eigen::Vector3d > > collision_point_vel_eigen_;
00147   std::vector<std::vector<Eigen::Vector3d > > collision_point_acc_eigen_;
00148   std::vector<std::vector<double> > collision_point_potential_;
00149   std::vector<std::vector<double> > collision_point_vel_mag_;
00150   std::vector<std::vector<Eigen::Vector3d> > collision_point_potential_gradient_;
00151   std::vector<std::vector<Eigen::Vector3d> > joint_axes_;
00152   std::vector<std::vector<Eigen::Vector3d> > joint_positions_;
00153   Eigen::MatrixXd group_trajectory_backup_;
00154   Eigen::MatrixXd best_group_trajectory_;
00155   double best_group_trajectory_cost_;
00156   int last_improvement_iteration_;
00157   unsigned int num_collision_free_iterations_;
00158 
00159   // HMC stuff:
00160   Eigen::MatrixXd momentum_;
00161   Eigen::MatrixXd random_momentum_;
00162   Eigen::VectorXd random_joint_momentum_; //temporary variable
00163   std::vector<MultivariateGaussian> multivariate_gaussian_;
00164   double stochasticity_factor_;
00165 
00166   std::vector<int> state_is_in_collision_;      
00167   std::vector<std::vector<int> > point_is_in_collision_;
00168   bool is_collision_free_;
00169   double worst_collision_cost_state_;
00170 
00171   Eigen::MatrixXd smoothness_increments_;
00172   Eigen::MatrixXd collision_increments_;
00173   Eigen::MatrixXd final_increments_;
00174 
00175   // temporary variables for all functions:
00176   Eigen::VectorXd smoothness_derivative_;
00177   Eigen::MatrixXd jacobian_;
00178   Eigen::MatrixXd jacobian_pseudo_inverse_;
00179   Eigen::MatrixXd jacobian_jacobian_tranpose_;
00180   Eigen::VectorXd random_state_;
00181   Eigen::VectorXd joint_state_velocities_;
00182 
00183   std::vector<std::string> joint_names_;
00184   std::map<std::string, std::map<std::string, bool> > joint_parent_map_;
00185 
00186   inline bool isParent(const std::string& childLink, const std::string& parentLink) const
00187   {
00188     if(childLink == parentLink)
00189     {
00190       return true;
00191     }
00192 
00193     if(joint_parent_map_.find(childLink) == joint_parent_map_.end())
00194     {
00195       //ROS_ERROR("%s was not in joint parent map! for lookup of %s", childLink.c_str(), parentLink.c_str());
00196       return false;
00197     }
00198     const std::map<std::string, bool>& parents = joint_parent_map_.at(childLink);
00199     return (parents.find(parentLink) != parents.end() && parents.at(parentLink));
00200   }
00201 
00202   void registerParents(const planning_models::RobotModel::JointModel* model);
00203   void initialize();
00204   void calculateSmoothnessIncrements();
00205   void calculateCollisionIncrements();
00206   void calculateTotalIncrements();
00207   void performForwardKinematics();
00208   void addIncrementsToTrajectory();
00209   void updateFullTrajectory();
00210   void debugCost();
00211   void handleJointLimits();
00212   double getTrajectoryCost();
00213   double getSmoothnessCost();
00214   double getCollisionCost();
00215   void perturbTrajectory();
00216   void getRandomMomentum();
00217   void updateMomentum();
00218   void updatePositionFromMomentum();
00219   void calculatePseudoInverse();
00220   void computeJointProperties(int trajectoryPoint);
00221   bool isCurrentTrajectoryMeshToMeshCollisionFree() const;
00222 };
00223 
00224 }
00225 
00226 #endif /* CHOMP_OPTIMIZER_H_ */


chomp_motion_planner
Author(s): Gil Jones
autogenerated on Mon Oct 6 2014 11:12:26