chomp_motion_planner: chomp_optimizer.h Source 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_robot_model.h>
00043 #include <chomp_motion_planner/chomp_cost.h>
00044 #include <chomp_motion_planner/chomp_collision_space.h>
00045 #include <chomp_motion_planner/multivariate_gaussian.h>
00046 
00047 #include <Eigen/Core>
00048 
00049 #include <vector>
00050 #include <kdl/frames.hpp>
00051 
00052 namespace chomp
00053 {
00054 
00055 class ChompOptimizer
00056 {
00057 public:
00058   ChompOptimizer(ChompTrajectory *trajectory, const ChompRobotModel *robot_model,
00059       const ChompRobotModel::ChompPlanningGroup *planning_group, const ChompParameters *parameters,
00060       const ros::Publisher& vis_marker_array_publisher,
00061       const ros::Publisher& vis_marker_publisher,
00062       ChompCollisionSpace *collision_space);
00063   virtual ~ChompOptimizer();
00064 
00065   void optimize();
00066 
00067 private:
00068 
00069   int num_joints_;
00070   int num_vars_free_;
00071   int num_vars_all_;
00072   int num_collision_points_;
00073   int free_vars_start_;
00074   int free_vars_end_;
00075   int iteration_;
00076   int collision_free_iteration_;
00077   ChompTrajectory *full_trajectory_;
00078   const ChompRobotModel *robot_model_;
00079   const ChompRobotModel::ChompPlanningGroup *planning_group_;
00080   const ChompParameters *parameters_;
00081   ChompCollisionSpace *collision_space_;
00082   ChompTrajectory group_trajectory_;
00083   std::vector<ChompCost> joint_costs_;
00084   std::vector<int> group_joint_to_kdl_joint_index_;
00085 
00086   std::vector<std::vector<KDL::Vector> > joint_axis_;
00087   std::vector<std::vector<KDL::Vector> > joint_pos_;
00088   std::vector<std::vector<KDL::Frame> > segment_frames_;
00089   std::vector<std::vector<KDL::Vector> > collision_point_pos_;
00090   std::vector<std::vector<KDL::Vector> > collision_point_vel_;
00091   std::vector<std::vector<KDL::Vector> > collision_point_acc_;
00092 
00093   std::vector<std::vector<Eigen::Map<Eigen::Vector3d> > > joint_axis_eigen_;
00094   std::vector<std::vector<Eigen::Map<Eigen::Vector3d> > > joint_pos_eigen_;
00095   std::vector<std::vector<Eigen::Map<Eigen::Vector3d> > > collision_point_pos_eigen_;
00096   std::vector<std::vector<Eigen::Map<Eigen::Vector3d> > > collision_point_vel_eigen_;
00097   std::vector<std::vector<Eigen::Map<Eigen::Vector3d> > > collision_point_acc_eigen_;
00098   std::vector<std::vector<double> > collision_point_potential_;
00099   std::vector<std::vector<double> > collision_point_vel_mag_;
00100   std::vector<std::vector<Eigen::Vector3d> > collision_point_potential_gradient_;
00101   Eigen::MatrixXd group_trajectory_backup_;
00102   Eigen::MatrixXd best_group_trajectory_;
00103   double best_group_trajectory_cost_;
00104   int last_improvement_iteration_;
00105 
00106   // HMC stuff:
00107   Eigen::MatrixXd momentum_;
00108   Eigen::MatrixXd random_momentum_;
00109   Eigen::VectorXd random_joint_momentum_; //temporary variable
00110   std::vector<MultivariateGaussian> multivariate_gaussian_;
00111   double stochasticity_factor_;
00112 
00113   std::vector<int> state_is_in_collision_;      
00114   std::vector<std::vector<int> > point_is_in_collision_;
00115   bool is_collision_free_;
00116   double worst_collision_cost_state_;
00117 
00118   Eigen::MatrixXd smoothness_increments_;
00119   Eigen::MatrixXd collision_increments_;
00120   Eigen::MatrixXd final_increments_;
00121 
00122   // temporary variables for all functions:
00123   Eigen::VectorXd smoothness_derivative_;
00124   KDL::JntArray kdl_joint_array_;
00125   Eigen::MatrixXd jacobian_;
00126   Eigen::MatrixXd jacobian_pseudo_inverse_;
00127   Eigen::MatrixXd jacobian_jacobian_tranpose_;
00128   Eigen::VectorXd random_state_;
00129   Eigen::VectorXd joint_state_velocities_;
00130 
00131   ros::Publisher vis_marker_array_pub_;
00132   ros::Publisher vis_marker_pub_;
00133   int animate_endeffector_segment_number_;
00134 
00135   void initialize();
00136   void calculateSmoothnessIncrements();
00137   void calculateCollisionIncrements();
00138   void calculateTotalIncrements();
00139   void performForwardKinematics();
00140   void addIncrementsToTrajectory();
00141   void updateFullTrajectory();
00142   void debugCost();
00143   void eigenMapTest();
00144   void handleJointLimits();
00145   void animatePath();
00146   void animateEndeffector();
00147   void visualizeState(int index);
00148   double getTrajectoryCost();
00149   double getSmoothnessCost();
00150   double getCollisionCost();
00151   void perturbTrajectory();
00152   void getRandomMomentum();
00153   void updateMomentum();
00154   void updatePositionFromMomentum();
00155   void calculatePseudoInverse();
00156 
00157 };
00158 
00159 }
00160 
00161 #endif /* CHOMP_OPTIMIZER_H_ */