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


chomp_motion_planner
Author(s): Gil Jones
autogenerated on Mon Jul 24 2017 02:21:07