chomp_optimizer.cpp
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 #include <chomp_motion_planner/chomp_optimizer.h>
00038 #include <ros/ros.h>
00039 #include <visualization_msgs/MarkerArray.h>
00040 #include <chomp_motion_planner/chomp_utils.h>
00041 #include <moveit/robot_model/robot_model.h>
00042 #include <moveit/robot_state/conversions.h>
00043 #include <moveit/planning_scene/planning_scene.h>
00044 #include <eigen3/Eigen/LU>
00045 #include <eigen3/Eigen/Core>
00046 
00047 namespace chomp
00048 {
00049 double getRandomDouble()
00050 {
00051   return ((double)random() / (double)RAND_MAX);
00052 }
00053 
00054 ChompOptimizer::ChompOptimizer(ChompTrajectory* trajectory, const planning_scene::PlanningSceneConstPtr& planning_scene,
00055                                const std::string& planning_group, const ChompParameters* parameters,
00056                                const moveit::core::RobotState& start_state)
00057   : full_trajectory_(trajectory)
00058   , kmodel_(planning_scene->getRobotModel())
00059   , planning_group_(planning_group)
00060   , parameters_(parameters)
00061   , group_trajectory_(*full_trajectory_, planning_group_, DIFF_RULE_LENGTH)
00062   , planning_scene_(planning_scene)
00063   , state_(start_state)
00064   , start_state_(start_state)
00065   , initialized_(false)
00066 {
00067   std::vector<std::string> cd_names;
00068   planning_scene->getCollisionDetectorNames(cd_names);
00069 
00070   ROS_INFO_STREAM("The following collision detectors are active in the planning scene.");
00071   for (int i = 0; i < cd_names.size(); i++)
00072   {
00073     ROS_INFO_STREAM(cd_names[i]);
00074   }
00075 
00076   ROS_INFO_STREAM("Active collision detector is: " + planning_scene->getActiveCollisionDetectorName());
00077 
00078   //  hy_world_ = dynamic_cast<const
00079   //  collision_detection::CollisionWorldHybrid*>(planning_scene->getCollisionWorld().get());
00080   hy_world_ = dynamic_cast<const collision_detection::CollisionWorldHybrid*>(
00081       planning_scene->getCollisionWorld(planning_scene->getActiveCollisionDetectorName()).get());
00082   if (!hy_world_)
00083   {
00084     ROS_WARN_STREAM("Could not initialize hybrid collision world from planning scene");
00085     return;
00086   }
00087 
00088   hy_robot_ = dynamic_cast<const collision_detection::CollisionRobotHybrid*>(
00089       planning_scene->getCollisionRobot(planning_scene->getActiveCollisionDetectorName()).get());
00090   if (!hy_robot_)
00091   {
00092     ROS_WARN_STREAM("Could not initialize hybrid collision robot from planning scene");
00093     return;
00094   }
00095   initialize();
00096 }
00097 
00098 void ChompOptimizer::initialize()
00099 {
00100   // init some variables:
00101   num_vars_free_ = group_trajectory_.getNumFreePoints();
00102   num_vars_all_ = group_trajectory_.getNumPoints();
00103   num_joints_ = group_trajectory_.getNumJoints();
00104 
00105   free_vars_start_ = group_trajectory_.getStartIndex();
00106   free_vars_end_ = group_trajectory_.getEndIndex();
00107 
00108   collision_detection::CollisionRequest req;
00109   collision_detection::CollisionResult res;
00110   req.group_name = planning_group_;
00111   ros::WallTime wt = ros::WallTime::now();
00112   hy_world_->getCollisionGradients(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_,
00113                                    &planning_scene_->getAllowedCollisionMatrix(), gsr_);
00114   ROS_INFO_STREAM("First coll check took " << (ros::WallTime::now() - wt));
00115   num_collision_points_ = 0;
00116   for (size_t i = 0; i < gsr_->gradients_.size(); i++)
00117   {
00118     num_collision_points_ += gsr_->gradients_[i].gradients.size();
00119   }
00120 
00121   // set up the joint costs:
00122   joint_costs_.reserve(num_joints_);
00123 
00124   double max_cost_scale = 0.0;
00125 
00126   joint_model_group_ = planning_scene_->getRobotModel()->getJointModelGroup(planning_group_);
00127 
00128   const std::vector<const moveit::core::JointModel*> joint_models = joint_model_group_->getActiveJointModels();
00129   for (size_t i = 0; i < joint_models.size(); i++)
00130   {
00131     const moveit::core::JointModel* model = joint_models[i];
00132     double joint_cost = 1.0;
00133     std::string joint_name = model->getName();
00134     // nh.param("joint_costs/" + joint_name, joint_cost, 1.0);
00135     std::vector<double> derivative_costs(3);
00136     derivative_costs[0] = joint_cost * parameters_->getSmoothnessCostVelocity();
00137     derivative_costs[1] = joint_cost * parameters_->getSmoothnessCostAcceleration();
00138     derivative_costs[2] = joint_cost * parameters_->getSmoothnessCostJerk();
00139     joint_costs_.push_back(ChompCost(group_trajectory_, i, derivative_costs, parameters_->getRidgeFactor()));
00140     double cost_scale = joint_costs_[i].getMaxQuadCostInvValue();
00141     if (max_cost_scale < cost_scale)
00142       max_cost_scale = cost_scale;
00143   }
00144 
00145   // scale the smoothness costs
00146   for (int i = 0; i < num_joints_; i++)
00147   {
00148     joint_costs_[i].scale(max_cost_scale);
00149   }
00150 
00151   // allocate memory for matrices:
00152   smoothness_increments_ = Eigen::MatrixXd::Zero(num_vars_free_, num_joints_);
00153   collision_increments_ = Eigen::MatrixXd::Zero(num_vars_free_, num_joints_);
00154   final_increments_ = Eigen::MatrixXd::Zero(num_vars_free_, num_joints_);
00155   smoothness_derivative_ = Eigen::VectorXd::Zero(num_vars_all_);
00156   jacobian_ = Eigen::MatrixXd::Zero(3, num_joints_);
00157   jacobian_pseudo_inverse_ = Eigen::MatrixXd::Zero(num_joints_, 3);
00158   jacobian_jacobian_tranpose_ = Eigen::MatrixXd::Zero(3, 3);
00159   random_state_ = Eigen::VectorXd::Zero(num_joints_);
00160   joint_state_velocities_ = Eigen::VectorXd::Zero(num_joints_);
00161 
00162   group_trajectory_backup_ = group_trajectory_.getTrajectory();
00163   best_group_trajectory_ = group_trajectory_.getTrajectory();
00164 
00165   collision_point_joint_names_.resize(num_vars_all_, std::vector<std::string>(num_collision_points_));
00166   collision_point_pos_eigen_.resize(num_vars_all_, std::vector<Eigen::Vector3d>(num_collision_points_));
00167   collision_point_vel_eigen_.resize(num_vars_all_, std::vector<Eigen::Vector3d>(num_collision_points_));
00168   collision_point_acc_eigen_.resize(num_vars_all_, std::vector<Eigen::Vector3d>(num_collision_points_));
00169   joint_axes_.resize(num_vars_all_, std::vector<Eigen::Vector3d>(num_joints_));
00170   joint_positions_.resize(num_vars_all_, std::vector<Eigen::Vector3d>(num_joints_));
00171 
00172   collision_point_potential_.resize(num_vars_all_, std::vector<double>(num_collision_points_));
00173   collision_point_vel_mag_.resize(num_vars_all_, std::vector<double>(num_collision_points_));
00174   collision_point_potential_gradient_.resize(num_vars_all_, std::vector<Eigen::Vector3d>(num_collision_points_));
00175 
00176   collision_free_iteration_ = 0;
00177   is_collision_free_ = false;
00178   state_is_in_collision_.resize(num_vars_all_);
00179   point_is_in_collision_.resize(num_vars_all_, std::vector<int>(num_collision_points_));
00180 
00181   last_improvement_iteration_ = -1;
00182 
00183   // HMC initialization:
00184   momentum_ = Eigen::MatrixXd::Zero(num_vars_free_, num_joints_);
00185   random_momentum_ = Eigen::MatrixXd::Zero(num_vars_free_, num_joints_);
00186   random_joint_momentum_ = Eigen::VectorXd::Zero(num_vars_free_);
00187   multivariate_gaussian_.clear();
00188   stochasticity_factor_ = 1.0;
00189   for (int i = 0; i < num_joints_; i++)
00190   {
00191     multivariate_gaussian_.push_back(
00192         MultivariateGaussian(Eigen::VectorXd::Zero(num_vars_free_), joint_costs_[i].getQuadraticCostInverse()));
00193   }
00194 
00195   std::map<std::string, std::string> fixed_link_resolution_map;
00196   for (int i = 0; i < num_joints_; i++)
00197   {
00198     joint_names_.push_back(joint_model_group_->getActiveJointModels()[i]->getName());
00199     // ROS_INFO("Got joint %s", joint_names_[i].c_str());
00200     registerParents(joint_model_group_->getActiveJointModels()[i]);
00201     fixed_link_resolution_map[joint_names_[i]] = joint_names_[i];
00202   }
00203 
00204   for (size_t i = 0; i < joint_model_group_->getFixedJointModels().size(); i++)
00205   {
00206     const moveit::core::JointModel* model = joint_model_group_->getFixedJointModels()[i];
00207     fixed_link_resolution_map[model->getName()] = model->getParentLinkModel()->getParentJointModel()->getName();
00208   }
00209 
00210   // TODO - is this just the joint_roots_?
00211   for (size_t i = 0; i < joint_model_group_->getUpdatedLinkModels().size(); i++)
00212   {
00213     if (fixed_link_resolution_map.find(
00214             joint_model_group_->getUpdatedLinkModels()[i]->getParentJointModel()->getName()) ==
00215         fixed_link_resolution_map.end())
00216     {
00217       const moveit::core::JointModel* parent_model = NULL;
00218       bool found_root = false;
00219 
00220       while (!found_root)
00221       {
00222         if (parent_model == NULL)
00223         {
00224           parent_model = joint_model_group_->getUpdatedLinkModels()[i]->getParentJointModel();
00225         }
00226         else
00227         {
00228           parent_model = parent_model->getParentLinkModel()->getParentJointModel();
00229           for (size_t j = 0; j < joint_names_.size(); j++)
00230           {
00231             if (parent_model->getName() == joint_names_[j])
00232             {
00233               found_root = true;
00234             }
00235           }
00236         }
00237       }
00238       fixed_link_resolution_map[joint_model_group_->getUpdatedLinkModels()[i]->getParentJointModel()->getName()] =
00239           parent_model->getName();
00240     }
00241   }
00242 
00243   // for(map<string, map<string, bool> >::iterator it = joint_parent_map_.begin(); it != joint_parent_map_.end(); it++)
00244   // {
00245   //   stringstream ss;
00246   //   ss << it->first << " Parents : {";
00247 
00248   //   for(map<string, bool>::iterator it2 = it->second.begin(); it2 != it->second.end(); it2++)
00249   //   {
00250   //     ss << it2->first << ",";
00251   //   }
00252   //   ss << "}";
00253   //   ROS_INFO("%s",ss.str().c_str());
00254   // }
00255 
00256   int start = free_vars_start_;
00257   int end = free_vars_end_;
00258   for (int i = start; i <= end; ++i)
00259   {
00260     size_t j = 0;
00261     for (size_t g = 0; g < gsr_->gradients_.size(); g++)
00262     {
00263       collision_detection::GradientInfo& info = gsr_->gradients_[g];
00264 
00265       for (size_t k = 0; k < info.sphere_locations.size(); k++)
00266       {
00267         if (fixed_link_resolution_map.find(info.joint_name) != fixed_link_resolution_map.end())
00268         {
00269           collision_point_joint_names_[i][j] = fixed_link_resolution_map[info.joint_name];
00270         }
00271         else
00272         {
00273           ROS_ERROR("Couldn't find joint %s!", info.joint_name.c_str());
00274         }
00275         j++;
00276       }
00277     }
00278   }
00279   initialized_ = true;
00280 }
00281 
00282 ChompOptimizer::~ChompOptimizer()
00283 {
00284   destroy();
00285 }
00286 
00287 void ChompOptimizer::registerParents(const moveit::core::JointModel* model)
00288 {
00289   const moveit::core::JointModel* parent_model = NULL;
00290   bool found_root = false;
00291 
00292   if (model == kmodel_->getRootJoint())
00293     return;
00294 
00295   while (!found_root)
00296   {
00297     if (parent_model == NULL)
00298     {
00299       if (model->getParentLinkModel() == NULL)
00300       {
00301         ROS_ERROR_STREAM("Model " << model->getName() << " not root but has NULL link model parent");
00302         return;
00303       }
00304       else if (model->getParentLinkModel()->getParentJointModel() == NULL)
00305       {
00306         ROS_ERROR_STREAM("Model " << model->getName() << " not root but has NULL joint model parent");
00307         return;
00308       }
00309       parent_model = model->getParentLinkModel()->getParentJointModel();
00310     }
00311     else
00312     {
00313       if (parent_model == kmodel_->getRootJoint())
00314       {
00315         found_root = true;
00316       }
00317       else
00318       {
00319         parent_model = parent_model->getParentLinkModel()->getParentJointModel();
00320       }
00321     }
00322     joint_parent_map_[model->getName()][parent_model->getName()] = true;
00323   }
00324 }
00325 
00326 void ChompOptimizer::optimize()
00327 {
00328   ros::WallTime start_time = ros::WallTime::now();
00329   double averageCostVelocity = 0.0;
00330   int currentCostIter = 0;
00331   int costWindow = 10;
00332   std::vector<double> costs(costWindow, 0.0);
00333   double minimaThreshold = 0.05;
00334   bool should_break_out = false;
00335 
00336   // if(parameters_->getAnimatePath())
00337   // {
00338   //   animatePath();
00339   // }
00340 
00341   // iterate
00342   for (iteration_ = 0; iteration_ < parameters_->getMaxIterations(); iteration_++)
00343   {
00344     ros::WallTime for_time = ros::WallTime::now();
00345     performForwardKinematics();
00346     ROS_INFO_STREAM("Forward kinematics took " << (ros::WallTime::now() - for_time));
00347     double cCost = getCollisionCost();
00348     double sCost = getSmoothnessCost();
00349     double cost = cCost + sCost;
00350 
00351     // ROS_INFO_STREAM("Collision cost " << cCost << " smoothness cost " << sCost);
00352 
00353     // if(parameters_->getAddRandomness() && currentCostIter != -1)
00354     // {
00355     //   costs[currentCostIter] = cCost;
00356     //   currentCostIter++;
00357 
00358     //   if(currentCostIter >= costWindow)
00359     //   {
00360     //     for(int i = 1; i < costWindow; i++)
00361     //     {
00362     //       averageCostVelocity += (costs.at(i) - costs.at(i - 1));
00363     //     }
00364 
00365     //     averageCostVelocity /= (double)(costWindow);
00366     //     currentCostIter = -1;
00367     //   }
00368     // }
00369     if (iteration_ == 0)
00370     {
00371       best_group_trajectory_ = group_trajectory_.getTrajectory();
00372       best_group_trajectory_cost_ = cost;
00373     }
00374     else
00375     {
00376       if (cost < best_group_trajectory_cost_)
00377       {
00378         best_group_trajectory_ = group_trajectory_.getTrajectory();
00379         best_group_trajectory_cost_ = cost;
00380         last_improvement_iteration_ = iteration_;
00381       }
00382     }
00383     calculateSmoothnessIncrements();
00384     ros::WallTime coll_time = ros::WallTime::now();
00385     calculateCollisionIncrements();
00386     // ROS_INFO_STREAM("Collision increments took " << (ros::WallTime::now()-coll_time));
00387     calculateTotalIncrements();
00388 
00389     // if(!parameters_->getUseHamiltonianMonteCarlo())
00390     // {
00391     //   // non-stochastic version:
00392     addIncrementsToTrajectory();
00393     // }
00394     // else
00395     // {
00396     //   // hamiltonian monte carlo updates:
00397     //   getRandomMomentum();
00398     //   updateMomentum();
00399     //   updatePositionFromMomentum();
00400     //   stochasticity_factor_ *= parameters_->getHmcAnnealingFactor();
00401     // }
00402     handleJointLimits();
00403     updateFullTrajectory();
00404 
00405     if (iteration_ % 10 == 0)
00406     {
00407       ROS_INFO("iteration: %d", iteration_);
00408       if (isCurrentTrajectoryMeshToMeshCollisionFree())
00409       {
00410         num_collision_free_iterations_ = 0;
00411         ROS_INFO("Chomp Got mesh to mesh safety at iter %d. Breaking out early.", iteration_);
00412         is_collision_free_ = true;
00413         iteration_++;
00414         should_break_out = true;
00415       }
00416       // } else if(safety == CollisionProximitySpace::InCollisionSafe) {
00417 
00418       // ROS_DEBUG("Trajectory cost: %f (s=%f, c=%f)", getTrajectoryCost(), getSmoothnessCost(), getCollisionCost());
00419       // CollisionProximitySpace::TrajectorySafety safety = checkCurrentIterValidity();
00420       // if(safety == CollisionProximitySpace::MeshToMeshSafe)
00421       // {
00422       //   num_collision_free_iterations_ = 0;
00423       //   ROS_INFO("Chomp Got mesh to mesh safety at iter %d. Breaking out early.", iteration_);
00424       //   is_collision_free_ = true;
00425       //   iteration_++;
00426       //   should_break_out = true;
00427       // } else if(safety == CollisionProximitySpace::InCollisionSafe) {
00428       //   num_collision_free_iterations_ = parameters_->getMaxIterationsAfterCollisionFree();
00429       //   ROS_INFO("Chomp Got in collision safety at iter %d. Breaking out soon.", iteration_);
00430       //   is_collision_free_ = true;
00431       //   iteration_++;
00432       //   should_break_out = true;
00433       // }
00434       // else
00435       // {
00436       //   is_collision_free_ = false;
00437       // }
00438     }
00439 
00440     if (!parameters_->getFilterMode())
00441     {
00442       if (cCost < parameters_->getCollisionThreshold())
00443       {
00444         num_collision_free_iterations_ = parameters_->getMaxIterationsAfterCollisionFree();
00445         is_collision_free_ = true;
00446         iteration_++;
00447         should_break_out = true;
00448       }
00449       else
00450       {
00451         // ROS_INFO_STREAM("cCost " << cCost << " over threshold " << parameters_->getCollisionThreshold());
00452       }
00453     }
00454 
00455     if ((ros::WallTime::now() - start_time).toSec() > parameters_->getPlanningTimeLimit() &&
00456         !parameters_->getAnimatePath() && !parameters_->getAnimateEndeffector())
00457     {
00458       ROS_WARN("Breaking out early due to time limit constraints.");
00459       break;
00460     }
00461 
00462     // if(fabs(averageCostVelocity) < minimaThreshold && currentCostIter == -1 && !is_collision_free_ &&
00463     // parameters_->getAddRandomness())
00464     // {
00465     //   ROS_INFO("Detected local minima. Attempting to break out!");
00466     //   int iter = 0;
00467     //   bool success = false;
00468     //   while(iter < 20 && !success)
00469     //   {
00470     //     performForwardKinematics();
00471     //     double original_cost = getTrajectoryCost();
00472     //     group_trajectory_backup_ = group_trajectory_.getTrajectory();
00473     //     perturbTrajectory();
00474     //     handleJointLimits();
00475     //     updateFullTrajectory();
00476     //     performForwardKinematics();
00477     //     double new_cost = getTrajectoryCost();
00478     //     iter ++;
00479     //     if(new_cost < original_cost)
00480     //     {
00481     //       ROS_INFO("Got out of minimum in %d iters!", iter);
00482     //       averageCostVelocity = 0.0;
00483     //       currentCostIter = 0;
00484     //       success = true;
00485     //     }
00486     //     else
00487     //     {
00488     //       group_trajectory_.getTrajectory() = group_trajectory_backup_;
00489     //       updateFullTrajectory();
00490     //       currentCostIter = 0;
00491     //       averageCostVelocity = 0.0;
00492     //       success = false;
00493     //     }
00494 
00495     //   }
00496 
00497     //   if(!success)
00498     //   {
00499     //     ROS_INFO("Failed to exit minimum!");
00500     //   }
00501     //}
00502     else if (currentCostIter == -1)
00503     {
00504       currentCostIter = 0;
00505       averageCostVelocity = 0.0;
00506     }
00507 
00508     // if(parameters_->getAnimateEndeffector())
00509     // {
00510     //   animateEndeffector();
00511     // }
00512 
00513     // if(parameters_->getAnimatePath() && iteration_ % 25 == 0)
00514     // {
00515     //   animatePath();
00516     // }
00517 
00518     if (should_break_out)
00519     {
00520       collision_free_iteration_++;
00521       if (num_collision_free_iterations_ == 0)
00522       {
00523         break;
00524       }
00525       else if (collision_free_iteration_ > num_collision_free_iterations_)
00526       {
00527         // CollisionProximitySpace::TrajectorySafety safety = checkCurrentIterValidity();
00528         // if(safety != CollisionProximitySpace::MeshToMeshSafe &&
00529         //    safety != CollisionProximitySpace::InCollisionSafe) {
00530         //   ROS_WARN_STREAM("Apparently regressed");
00531         // }
00532         break;
00533       }
00534     }
00535   }
00536 
00537   if (is_collision_free_)
00538   {
00539     ROS_INFO("Chomp path is collision free");
00540   }
00541   else
00542   {
00543     ROS_ERROR("Chomp path is not collision free!");
00544   }
00545 
00546   // if(parameters_->getAnimatePath())
00547   // {
00548   //   animatePath();
00549   // }
00550 
00551   group_trajectory_.getTrajectory() = best_group_trajectory_;
00552   updateFullTrajectory();
00553 
00554   // if(parameters_->getAnimatePath())
00555   //   animatePath();
00556 
00557   ROS_INFO("Terminated after %d iterations, using path from iteration %d", iteration_, last_improvement_iteration_);
00558   ROS_INFO("Optimization core finished in %f sec", (ros::WallTime::now() - start_time).toSec());
00559   ROS_INFO_STREAM("Time per iteration " << (ros::WallTime::now() - start_time).toSec() / (iteration_ * 1.0));
00560 }
00561 
00562 bool ChompOptimizer::isCurrentTrajectoryMeshToMeshCollisionFree() const
00563 {
00564   moveit_msgs::RobotTrajectory traj;
00565   traj.joint_trajectory.joint_names = joint_names_;
00566 
00567   for (int i = 0; i < group_trajectory_.getNumPoints(); i++)
00568   {
00569     trajectory_msgs::JointTrajectoryPoint point;
00570     for (int j = 0; j < group_trajectory_.getNumJoints(); j++)
00571     {
00572       point.positions.push_back(best_group_trajectory_(i, j));
00573     }
00574     traj.joint_trajectory.points.push_back(point);
00575   }
00576   moveit_msgs::RobotState start_state_msg;
00577   moveit::core::robotStateToRobotStateMsg(start_state_, start_state_msg);
00578   return planning_scene_->isPathValid(start_state_msg, traj);
00579 }
00580 
00581 // CollisionProximitySpace::TrajectorySafety ChompOptimizer::checkCurrentIterValidity()
00582 // {
00583 //   JointTrajectory jointTrajectory;
00584 //   jointTrajectory.joint_names = joint_names_;
00585 //   jointTrajectory.header.frame_id = collision_space_->getCollisionModelsInterface()->getRobotFrameId();
00586 //   jointTrajectory.header.stamp = ros::Time::now();
00587 //   Constraints goalConstraints;
00588 //   Constraints pathConstraints;
00589 //   ArmNavigationErrorCodes errorCode;
00590 //   vector<ArmNavigationErrorCodes> trajectoryErrorCodes;
00591 //   for(int i = 0; i < group_trajectory_.getNumPoints(); i++)
00592 //   {
00593 //     JointTrajectoryPoint point;
00594 //     for(int j = 0; j < group_trajectory_.getNumJoints(); j++)
00595 //     {
00596 //       point.positions.push_back(best_group_trajectory_(i, j));
00597 //     }
00598 //     jointTrajectory.points.push_back(point);
00599 //   }
00600 
00601 //   return collision_space_->isTrajectorySafe(jointTrajectory, goalConstraints, pathConstraints, planning_group_);
00602 //   /*
00603 //     bool valid = collision_space_->getCollisionModelsInterface()->isJointTrajectoryValid(*state_,
00604 //     jointTrajectory,
00605 //     goalConstraints,
00606 //     pathConstraints, errorCode,
00607 //     trajectoryErrorCodes, false);
00608 //   */
00609 
00610 // }
00611 
00612 void ChompOptimizer::calculateSmoothnessIncrements()
00613 {
00614   for (int i = 0; i < num_joints_; i++)
00615   {
00616     joint_costs_[i].getDerivative(group_trajectory_.getJointTrajectory(i), smoothness_derivative_);
00617     smoothness_increments_.col(i) = -smoothness_derivative_.segment(group_trajectory_.getStartIndex(), num_vars_free_);
00618   }
00619 }
00620 
00621 void ChompOptimizer::calculateCollisionIncrements()
00622 {
00623   double potential;
00624   double vel_mag_sq;
00625   double vel_mag;
00626   Eigen::Vector3d potential_gradient;
00627   Eigen::Vector3d normalized_velocity;
00628   Eigen::Matrix3d orthogonal_projector;
00629   Eigen::Vector3d curvature_vector;
00630   Eigen::Vector3d cartesian_gradient;
00631 
00632   collision_increments_.setZero(num_vars_free_, num_joints_);
00633 
00634   int startPoint = 0;
00635   int endPoint = free_vars_end_;
00636 
00637   // In stochastic descent, simply use a random point in the trajectory, rather than all the trajectory points.
00638   // This is faster and guaranteed to converge, but it may take more iterations in the worst case.
00639   if (parameters_->getUseStochasticDescent())
00640   {
00641     startPoint = (int)(((double)random() / (double)RAND_MAX) * (free_vars_end_ - free_vars_start_) + free_vars_start_);
00642     if (startPoint < free_vars_start_)
00643       startPoint = free_vars_start_;
00644     if (startPoint > free_vars_end_)
00645       startPoint = free_vars_end_;
00646     endPoint = startPoint;
00647   }
00648   else
00649   {
00650     startPoint = free_vars_start_;
00651   }
00652 
00653   for (int i = startPoint; i <= endPoint; i++)
00654   {
00655     for (int j = 0; j < num_collision_points_; j++)
00656     {
00657       potential = collision_point_potential_[i][j];
00658 
00659       if (potential < 0.0001)
00660         continue;
00661 
00662       potential_gradient = -collision_point_potential_gradient_[i][j];
00663 
00664       vel_mag = collision_point_vel_mag_[i][j];
00665       vel_mag_sq = vel_mag * vel_mag;
00666 
00667       // all math from the CHOMP paper:
00668 
00669       normalized_velocity = collision_point_vel_eigen_[i][j] / vel_mag;
00670       orthogonal_projector = Eigen::Matrix3d::Identity() - (normalized_velocity * normalized_velocity.transpose());
00671       curvature_vector = (orthogonal_projector * collision_point_acc_eigen_[i][j]) / vel_mag_sq;
00672       cartesian_gradient = vel_mag * (orthogonal_projector * potential_gradient - potential * curvature_vector);
00673 
00674       // pass it through the jacobian transpose to get the increments
00675       getJacobian(i, collision_point_pos_eigen_[i][j], collision_point_joint_names_[i][j], jacobian_);
00676 
00677       if (parameters_->getUsePseudoInverse())
00678       {
00679         calculatePseudoInverse();
00680         collision_increments_.row(i - free_vars_start_).transpose() -= jacobian_pseudo_inverse_ * cartesian_gradient;
00681       }
00682       else
00683       {
00684         collision_increments_.row(i - free_vars_start_).transpose() -= jacobian_.transpose() * cartesian_gradient;
00685       }
00686 
00687       /*
00688         if(point_is_in_collision_[i][j])
00689         {
00690         break;
00691         }
00692       */
00693     }
00694   }
00695   // cout << collision_increments_ << endl;
00696 }
00697 
00698 void ChompOptimizer::calculatePseudoInverse()
00699 {
00700   jacobian_jacobian_tranpose_ =
00701       jacobian_ * jacobian_.transpose() + Eigen::MatrixXd::Identity(3, 3) * parameters_->getPseudoInverseRidgeFactor();
00702   jacobian_pseudo_inverse_ = jacobian_.transpose() * jacobian_jacobian_tranpose_.inverse();
00703 }
00704 
00705 void ChompOptimizer::calculateTotalIncrements()
00706 {
00707   for (int i = 0; i < num_joints_; i++)
00708   {
00709     final_increments_.col(i) =
00710         parameters_->getLearningRate() * (joint_costs_[i].getQuadraticCostInverse() *
00711                                           (parameters_->getSmoothnessCostWeight() * smoothness_increments_.col(i) +
00712                                            parameters_->getObstacleCostWeight() * collision_increments_.col(i)));
00713   }
00714 }
00715 
00716 void ChompOptimizer::addIncrementsToTrajectory()
00717 {
00718   const std::vector<const moveit::core::JointModel*>& joint_models = joint_model_group_->getActiveJointModels();
00719   for (size_t i = 0; i < joint_models.size(); i++)
00720   {
00721     double scale = 1.0;
00722     double max = final_increments_.col(i).maxCoeff();
00723     double min = final_increments_.col(i).minCoeff();
00724     double max_scale = parameters_->getJointUpdateLimit() / fabs(max);
00725     double min_scale = parameters_->getJointUpdateLimit() / fabs(min);
00726     if (max_scale < scale)
00727       scale = max_scale;
00728     if (min_scale < scale)
00729       scale = min_scale;
00730     group_trajectory_.getFreeTrajectoryBlock().col(i) += scale * final_increments_.col(i);
00731   }
00732   // ROS_DEBUG("Scale: %f",scale);
00733   // group_trajectory_.getFreeTrajectoryBlock() += scale * final_increments_;
00734 }
00735 
00736 void ChompOptimizer::updateFullTrajectory()
00737 {
00738   full_trajectory_->updateFromGroupTrajectory(group_trajectory_);
00739 }
00740 
00741 void ChompOptimizer::debugCost()
00742 {
00743   double cost = 0.0;
00744   for (int i = 0; i < num_joints_; i++)
00745     cost += joint_costs_[i].getCost(group_trajectory_.getJointTrajectory(i));
00746   std::cout << "Cost = " << cost << std::endl;
00747 }
00748 
00749 double ChompOptimizer::getTrajectoryCost()
00750 {
00751   return getSmoothnessCost() + getCollisionCost();
00752 }
00753 
00754 double ChompOptimizer::getSmoothnessCost()
00755 {
00756   double smoothness_cost = 0.0;
00757   // joint costs:
00758   for (int i = 0; i < num_joints_; i++)
00759     smoothness_cost += joint_costs_[i].getCost(group_trajectory_.getJointTrajectory(i));
00760 
00761   return parameters_->getSmoothnessCostWeight() * smoothness_cost;
00762 }
00763 
00764 double ChompOptimizer::getCollisionCost()
00765 {
00766   double collision_cost = 0.0;
00767 
00768   double worst_collision_cost = 0.0;
00769   worst_collision_cost_state_ = -1;
00770 
00771   // collision costs:
00772   for (int i = free_vars_start_; i <= free_vars_end_; i++)
00773   {
00774     double state_collision_cost = 0.0;
00775     for (int j = 0; j < num_collision_points_; j++)
00776     {
00777       state_collision_cost += collision_point_potential_[i][j] * collision_point_vel_mag_[i][j];
00778     }
00779     collision_cost += state_collision_cost;
00780     if (state_collision_cost > worst_collision_cost)
00781     {
00782       worst_collision_cost = state_collision_cost;
00783       worst_collision_cost_state_ = i;
00784     }
00785   }
00786 
00787   return parameters_->getObstacleCostWeight() * collision_cost;
00788 }
00789 
00790 void ChompOptimizer::computeJointProperties(int trajectory_point)
00791 {
00792   // tf::Transform inverseWorldTransform = collision_space_->getInverseWorldTransform(*state_);
00793   for (int j = 0; j < num_joints_; j++)
00794   {
00795     const moveit::core::JointModel* joint_model = state_.getJointModel(joint_names_[j]);
00796     const moveit::core::RevoluteJointModel* revolute_joint =
00797         dynamic_cast<const moveit::core::RevoluteJointModel*>(joint_model);
00798     const moveit::core::PrismaticJointModel* prismatic_joint =
00799         dynamic_cast<const moveit::core::PrismaticJointModel*>(joint_model);
00800 
00801     std::string parent_link_name = joint_model->getParentLinkModel()->getName();
00802     std::string child_link_name = joint_model->getChildLinkModel()->getName();
00803     Eigen::Affine3d joint_transform =
00804         state_.getGlobalLinkTransform(parent_link_name) *
00805         (kmodel_->getLinkModel(child_link_name)->getJointOriginTransform() * (state_.getJointTransform(joint_model)));
00806 
00807     // joint_transform = inverseWorldTransform * jointTransform;
00808     Eigen::Vector3d axis;
00809 
00810     if (revolute_joint != NULL)
00811     {
00812       axis = revolute_joint->getAxis();
00813     }
00814     else if (prismatic_joint != NULL)
00815     {
00816       axis = prismatic_joint->getAxis();
00817     }
00818     else
00819     {
00820       axis = Eigen::Vector3d::Identity();
00821     }
00822 
00823     axis = joint_transform * axis;
00824 
00825     joint_axes_[trajectory_point][j] = axis;
00826     joint_positions_[trajectory_point][j] = joint_transform.translation();
00827   }
00828 }
00829 
00830 template <typename Derived>
00831 void ChompOptimizer::getJacobian(int trajectory_point, Eigen::Vector3d& collision_point_pos, std::string& joint_name,
00832                                  Eigen::MatrixBase<Derived>& jacobian) const
00833 {
00834   for (int j = 0; j < num_joints_; j++)
00835   {
00836     if (isParent(joint_name, joint_names_[j]))
00837     {
00838       Eigen::Vector3d column = joint_axes_[trajectory_point][j].cross(
00839           Eigen::Vector3d(collision_point_pos(0), collision_point_pos(1), collision_point_pos(2)) -
00840           joint_positions_[trajectory_point][j]);
00841 
00842       jacobian.col(j)[0] = column.x();
00843       jacobian.col(j)[1] = column.y();
00844       jacobian.col(j)[2] = column.z();
00845     }
00846     else
00847     {
00848       jacobian.col(j)[0] = 0.0;
00849       jacobian.col(j)[1] = 0.0;
00850       jacobian.col(j)[2] = 0.0;
00851     }
00852   }
00853 }
00854 
00855 void ChompOptimizer::handleJointLimits()
00856 {
00857   const std::vector<const moveit::core::JointModel*> joint_models = joint_model_group_->getActiveJointModels();
00858   for (size_t joint_i = 0; joint_i < joint_models.size(); joint_i++)
00859   {
00860     const moveit::core::JointModel* joint_model = joint_models[joint_i];
00861 
00862     if (joint_model->getType() == moveit::core::JointModel::REVOLUTE)
00863     {
00864       const moveit::core::RevoluteJointModel* revolute_joint =
00865           dynamic_cast<const moveit::core::RevoluteJointModel*>(joint_model);
00866       if (revolute_joint->isContinuous())
00867       {
00868         continue;
00869       }
00870     }
00871 
00872     const moveit::core::JointModel::Bounds& bounds = joint_model->getVariableBounds();
00873 
00874     double joint_max = -DBL_MAX;
00875     double joint_min = DBL_MAX;
00876 
00877     for (moveit::core::JointModel::Bounds::const_iterator it = bounds.begin(); it != bounds.end(); it++)
00878     {
00879       if (it->min_position_ < joint_min)
00880       {
00881         joint_min = it->min_position_;
00882       }
00883 
00884       if (it->max_position_ > joint_max)
00885       {
00886         joint_max = it->max_position_;
00887       }
00888     }
00889 
00890     int count = 0;
00891 
00892     bool violation = false;
00893     do
00894     {
00895       double max_abs_violation = 1e-6;
00896       double max_violation = 0.0;
00897       int max_violation_index = 0;
00898       violation = false;
00899       for (int i = free_vars_start_; i <= free_vars_end_; i++)
00900       {
00901         double amount = 0.0;
00902         double absolute_amount = 0.0;
00903         if (group_trajectory_(i, joint_i) > joint_max)
00904         {
00905           amount = joint_max - group_trajectory_(i, joint_i);
00906           absolute_amount = fabs(amount);
00907         }
00908         else if (group_trajectory_(i, joint_i) < joint_min)
00909         {
00910           amount = joint_min - group_trajectory_(i, joint_i);
00911           absolute_amount = fabs(amount);
00912         }
00913         if (absolute_amount > max_abs_violation)
00914         {
00915           max_abs_violation = absolute_amount;
00916           max_violation = amount;
00917           max_violation_index = i;
00918           violation = true;
00919         }
00920       }
00921 
00922       if (violation)
00923       {
00924         int free_var_index = max_violation_index - free_vars_start_;
00925         double multiplier =
00926             max_violation / joint_costs_[joint_i].getQuadraticCostInverse()(free_var_index, free_var_index);
00927         group_trajectory_.getFreeJointTrajectoryBlock(joint_i) +=
00928             multiplier * joint_costs_[joint_i].getQuadraticCostInverse().col(free_var_index);
00929       }
00930       if (++count > 10)
00931         break;
00932     } while (violation);
00933   }
00934 }
00935 
00936 void ChompOptimizer::performForwardKinematics()
00937 {
00938   double inv_time = 1.0 / group_trajectory_.getDiscretization();
00939   double inv_time_sq = inv_time * inv_time;
00940 
00941   // calculate the forward kinematics for the fixed states only in the first iteration:
00942   int start = free_vars_start_;
00943   int end = free_vars_end_;
00944   if (iteration_ == 0)
00945   {
00946     start = 0;
00947     end = num_vars_all_ - 1;
00948   }
00949 
00950   is_collision_free_ = true;
00951 
00952   ros::WallDuration total_dur(0.0);
00953 
00954   // for each point in the trajectory
00955   for (int i = start; i <= end; ++i)
00956   {
00957     // Set Robot state from trajectory point...
00958     collision_detection::CollisionRequest req;
00959     collision_detection::CollisionResult res;
00960     req.group_name = planning_group_;
00961     setRobotStateFromPoint(group_trajectory_, i);
00962     ros::WallTime grad = ros::WallTime::now();
00963 
00964     hy_world_->getCollisionGradients(req, res, *hy_robot_->getCollisionRobotDistanceField().get(), state_, NULL, gsr_);
00965     total_dur += (ros::WallTime::now() - grad);
00966     computeJointProperties(i);
00967     state_is_in_collision_[i] = false;
00968 
00969     // Keep vars in scope
00970     {
00971       size_t j = 0;
00972       for (size_t g = 0; g < gsr_->gradients_.size(); g++)
00973       {
00974         collision_detection::GradientInfo& info = gsr_->gradients_[g];
00975 
00976         for (size_t k = 0; k < info.sphere_locations.size(); k++)
00977         {
00978           collision_point_pos_eigen_[i][j][0] = info.sphere_locations[k].x();
00979           collision_point_pos_eigen_[i][j][1] = info.sphere_locations[k].y();
00980           collision_point_pos_eigen_[i][j][2] = info.sphere_locations[k].z();
00981 
00982           collision_point_potential_[i][j] =
00983               getPotential(info.distances[k], info.sphere_radii[k], parameters_->getMinClearence());
00984           collision_point_potential_gradient_[i][j][0] = info.gradients[k].x();
00985           collision_point_potential_gradient_[i][j][1] = info.gradients[k].y();
00986           collision_point_potential_gradient_[i][j][2] = info.gradients[k].z();
00987 
00988           point_is_in_collision_[i][j] = (info.distances[k] - info.sphere_radii[k] < info.sphere_radii[k]);
00989 
00990           if (point_is_in_collision_[i][j])
00991           {
00992             state_is_in_collision_[i] = true;
00993             // if(is_collision_free_ == true) {
00994             //   ROS_INFO_STREAM("We know it's not collision free " << g);
00995             //   ROS_INFO_STREAM("Sphere location " << info.sphere_locations[k].x() << " " <<
00996             //   info.sphere_locations[k].y() << " " << info.sphere_locations[k].z());
00997             //   ROS_INFO_STREAM("Gradient " << info.gradients[k].x() << " " << info.gradients[k].y() << " " <<
00998             //   info.gradients[k].z() << " distance " << info.distances[k] << " radii " << info.sphere_radii[k]);
00999             //   ROS_INFO_STREAM("Radius " << info.sphere_radii[k] << " potential " <<
01000             //   collision_point_potential_[i][j]);
01001             // }
01002             is_collision_free_ = false;
01003           }
01004           j++;
01005         }
01006       }
01007     }
01008   }
01009 
01010   // ROS_INFO_STREAM("Total dur " << total_dur << " total checks " << end-start+1);
01011 
01012   // now, get the vel and acc for each collision point (using finite differencing)
01013   for (int i = free_vars_start_; i <= free_vars_end_; i++)
01014   {
01015     for (int j = 0; j < num_collision_points_; j++)
01016     {
01017       collision_point_vel_eigen_[i][j] = Eigen::Vector3d(0, 0, 0);
01018       collision_point_acc_eigen_[i][j] = Eigen::Vector3d(0, 0, 0);
01019       for (int k = -DIFF_RULE_LENGTH / 2; k <= DIFF_RULE_LENGTH / 2; k++)
01020       {
01021         collision_point_vel_eigen_[i][j] +=
01022             (inv_time * DIFF_RULES[0][k + DIFF_RULE_LENGTH / 2]) * collision_point_pos_eigen_[i + k][j];
01023         collision_point_acc_eigen_[i][j] +=
01024             (inv_time_sq * DIFF_RULES[1][k + DIFF_RULE_LENGTH / 2]) * collision_point_pos_eigen_[i + k][j];
01025       }
01026 
01027       // get the norm of the velocity:
01028       collision_point_vel_mag_[i][j] = collision_point_vel_eigen_[i][j].norm();
01029     }
01030   }
01031 }
01032 
01033 void ChompOptimizer::setRobotStateFromPoint(ChompTrajectory& group_trajectory, int i)
01034 {
01035   const Eigen::MatrixXd::RowXpr& point = group_trajectory.getTrajectoryPoint(i);
01036 
01037   std::vector<double> joint_states;
01038   for (int j = 0; j < group_trajectory.getNumJoints(); j++)
01039   {
01040     joint_states.push_back(point(0, j));
01041   }
01042 
01043   state_.setJointGroupPositions(planning_group_, joint_states);
01044   state_.update();
01045 }
01046 
01047 void ChompOptimizer::perturbTrajectory()
01048 {
01049   // int mid_point = (free_vars_start_ + free_vars_end_) / 2;
01050   if (worst_collision_cost_state_ < 0)
01051     return;
01052   int mid_point = worst_collision_cost_state_;
01053   moveit::core::RobotState random_state = state_;
01054   const moveit::core::JointModelGroup* planning_group = state_.getJointModelGroup(planning_group_);
01055   random_state.setToRandomPositions(planning_group);
01056   std::vector<double> vals;
01057   random_state.copyJointGroupPositions(planning_group_, vals);
01058   double* ptr = &vals[0];
01059   Eigen::Map<Eigen::VectorXd> random_matrix(ptr, vals.size());
01060   // Eigen::VectorXd random_matrix = vals;
01061 
01062   // convert the state into an increment
01063   random_matrix -= group_trajectory_.getTrajectoryPoint(mid_point).transpose();
01064 
01065   // project the increment orthogonal to joint velocities
01066   group_trajectory_.getJointVelocities(mid_point, joint_state_velocities_);
01067   joint_state_velocities_.normalize();
01068   random_matrix = (Eigen::MatrixXd::Identity(num_joints_, num_joints_) -
01069                    joint_state_velocities_ * joint_state_velocities_.transpose()) *
01070                   random_matrix;
01071 
01072   int mp_free_vars_index = mid_point - free_vars_start_;
01073   for (int i = 0; i < num_joints_; i++)
01074   {
01075     group_trajectory_.getFreeJointTrajectoryBlock(i) +=
01076         joint_costs_[i].getQuadraticCostInverse().col(mp_free_vars_index) * random_state_(i);
01077   }
01078 }
01079 
01080 // void ChompOptimizer::getRandomState(const RobotState currentState, const string& groupName, Eigen::VectorXd&
01081 // state_vec)
01082 // {
01083 //   const vector<RobotState *::JointState*>& jointStates =
01084 //     currentState->getJointStateGroup(groupName)->getJointStateVector();
01085 //   for(size_t i = 0; i < jointStates.size(); i++)
01086 //   {
01087 
01088 //     bool continuous = false;
01089 
01090 //     RobotState *::JointState* jointState = jointStates[i];
01091 //     const RevoluteJointModel* revolute_joint
01092 //       = dynamic_cast<const RevoluteJointModel*>(jointState->getJointModel());
01093 //     if(revolute_joint && revolute_joint->continuous_) {
01094 //       continuous = true;
01095 //     }
01096 
01097 //     map<string, pair<double, double> > bounds = jointState->getJointModel()->getAllVariableBounds();
01098 //     int j = 0;
01099 //     for(map<string, pair<double, double> >::iterator it = bounds.begin(); it != bounds.end(); it++)
01100 //     {
01101 //       double randVal = jointState->getJointStateValues()[j] + (getRandomDouble()
01102 //                                                                * (parameters_->getRandomJumpAmount()) -
01103 //                                                                getRandomDouble() *
01104 //                                                                (parameters_->getRandomJumpAmount()));
01105 
01106 //       if(!continuous)
01107 //       {
01108 //         if(randVal > it->second.second)
01109 //         {
01110 //           randVal = it->second.second;
01111 //         }
01112 //         else if(randVal < it->second.first)
01113 //         {
01114 //           randVal = it->second.first;
01115 //         }
01116 //       }
01117 
01118 //       ROS_DEBUG_STREAM("Joint " << it->first << " old value " << jointState->getJointStateValues()[j] << " new value
01119 //       " << randVal);
01120 //       state_vec(i) = randVal;
01121 
01122 //       j++;
01123 //     }
01124 //   }
01125 // }
01126 
01127 void ChompOptimizer::getRandomMomentum()
01128 {
01129   if (is_collision_free_)
01130     random_momentum_.setZero(num_vars_free_, num_joints_);
01131   else
01132     for (int i = 0; i < num_joints_; ++i)
01133     {
01134       multivariate_gaussian_[i].sample(random_joint_momentum_);
01135       random_momentum_.col(i) = stochasticity_factor_ * random_joint_momentum_;
01136     }
01137 }
01138 
01139 void ChompOptimizer::updateMomentum()
01140 {
01141   // double alpha = 1.0 - parameters_->getHmcStochasticity();
01142   double eps = parameters_->getHmcDiscretization();
01143   if (iteration_ > 0)
01144     momentum_ = (momentum_ + eps * final_increments_);
01145   else
01146     momentum_ = random_momentum_;
01147   // momentum_ = alpha * (momentum_ + eps*final_increments_) + sqrt(1.0-alpha*alpha)*random_momentum_;
01148 }
01149 
01150 void ChompOptimizer::updatePositionFromMomentum()
01151 {
01152   double eps = parameters_->getHmcDiscretization();
01153   group_trajectory_.getFreeTrajectoryBlock() += eps * momentum_;
01154 }
01155 
01156 // void ChompOptimizer::animatePath()
01157 // {
01158 //   for(int i = free_vars_start_; i <= free_vars_end_; i++)
01159 //   {
01160 //     visualizeState(i);
01161 //     //ros::WallDuration(group_trajectory_.getDiscretization()).sleep();
01162 //     ros::WallDuration(.05).sleep();
01163 //   }
01164 // }
01165 
01166 // void ChompOptimizer::animateEndeffector()
01167 // {
01168 //   visualization_msgs::Marker msg;
01169 //   msg.points.resize(num_vars_free_);
01170 //   // int joint_index = planning_group_->chomp_joints_[num_joints_-1].kdl_joint_index_;
01171 //   int sn = (int)(num_collision_points_ - 1);
01172 //   for(int i = 0; i < num_vars_free_; ++i)
01173 //   {
01174 //     int j = i + free_vars_start_;
01175 //     msg.points[i].x = collision_point_pos_eigen_[j][sn][0];
01176 //     msg.points[i].y = collision_point_pos_eigen_[j][sn][1];
01177 //     msg.points[i].z = collision_point_pos_eigen_[j][sn][2];
01178 //   }
01179 //   msg.header.frame_id = collision_space_->getCollisionModelsInterface()->getRobotFrameId();
01180 //   msg.header.stamp = ros::Time();
01181 //   msg.ns = "chomp_endeffector";
01182 //   msg.id = 0;
01183 //   msg.type = visualization_msgs::Marker::SPHERE_LIST;
01184 //   msg.action = visualization_msgs::Marker::ADD;
01185 //   double scale = 0.05;
01186 //   msg.scale.x = scale;
01187 //   msg.scale.y = scale;
01188 //   msg.scale.z = scale;
01189 //   msg.color.a = 0.6;
01190 //   msg.color.r = 0.5;
01191 //   msg.color.g = 1.0;
01192 //   msg.color.b = 0.3;
01193 //   vis_marker_pub_.publish(msg);
01194 //   ros::WallDuration(0.1).sleep();
01195 
01196 // }
01197 
01198 // void ChompOptimizer::visualizeState(int index)
01199 // {
01200 
01201 //   visualization_msgs::MarkerArray msg;
01202 //   msg.markers.resize(num_collision_points_ + num_joints_);
01203 //   int num_arrows = 0;
01204 //   double potential_threshold = 1e-10;
01205 //   for(int i = 0; i < num_collision_points_; i++)
01206 //   {
01207 //     msg.markers[i].header.frame_id = collision_space_->getCollisionModelsInterface()->getRobotFrameId();
01208 //     msg.markers[i].header.stamp = ros::Time();
01209 //     msg.markers[i].ns = "chomp_collisions";
01210 //     msg.markers[i].id = i;
01211 //     msg.markers[i].type = visualization_msgs::Marker::SPHERE;
01212 //     msg.markers[i].action = visualization_msgs::Marker::ADD;
01213 //     msg.markers[i].pose.position.x = collision_point_pos_eigen_[index][i][0];
01214 //     msg.markers[i].pose.position.y = collision_point_pos_eigen_[index][i][1];
01215 //     msg.markers[i].pose.position.z = collision_point_pos_eigen_[index][i][2];
01216 //     msg.markers[i].pose.orientation.x = 0.0;
01217 //     msg.markers[i].pose.orientation.y = 0.0;
01218 //     msg.markers[i].pose.orientation.z = 0.0;
01219 //     msg.markers[i].pose.orientation.w = 1.0;
01220 //     double scale = 0.1;
01221 //     msg.markers[i].scale.x = scale;
01222 //     msg.markers[i].scale.y = scale;
01223 //     msg.markers[i].scale.z = scale;
01224 //     msg.markers[i].color.a = 0.6;
01225 //     msg.markers[i].color.r = 0.5;
01226 //     msg.markers[i].color.g = 1.0;
01227 //     msg.markers[i].color.b = 0.3;
01228 //     if(collision_point_potential_[index][i] > potential_threshold)
01229 //       num_arrows++;
01230 //   }
01231 
01232 //   vis_marker_array_pub_.publish(msg);
01233 
01234 //   // publish arrows for distance field:
01235 //   msg.markers.resize(0);
01236 //   msg.markers.resize(num_collision_points_);
01237 //   for(int i = 0; i < num_collision_points_; i++)
01238 //   {
01239 //     msg.markers[i].header.frame_id = collision_space_->getCollisionModelsInterface()->getRobotFrameId();
01240 //     msg.markers[i].header.stamp = ros::Time();
01241 //     msg.markers[i].ns = "chomp_arrows";
01242 //     msg.markers[i].id = i;
01243 //     msg.markers[i].type = visualization_msgs::Marker::ARROW;
01244 //     msg.markers[i].action = visualization_msgs::Marker::ADD;
01245 //     msg.markers[i].points.resize(2);
01246 //     msg.markers[i].points[0].x = collision_point_pos_eigen_[index][i](0);
01247 //     msg.markers[i].points[0].y = collision_point_pos_eigen_[index][i](1);
01248 //     msg.markers[i].points[0].z = collision_point_pos_eigen_[index][i](2);
01249 //     msg.markers[i].points[1] = msg.markers[i].points[0];
01250 //     double scale = 0.25f;
01251 //     if(collision_point_potential_[index][i] <= potential_threshold)
01252 //       scale = 0.0;
01253 //     msg.markers[i].points[1].x += scale * collision_point_potential_gradient_[index][i](0);
01254 //     msg.markers[i].points[1].y += scale * collision_point_potential_gradient_[index][i](1);
01255 //     msg.markers[i].points[1].z += scale * collision_point_potential_gradient_[index][i](2);
01256 //     msg.markers[i].scale.x = 0.01;
01257 //     msg.markers[i].scale.y = 0.03;
01258 //     msg.markers[i].color.a = 0.5;
01259 //     msg.markers[i].color.r = 0.5;
01260 //     msg.markers[i].color.g = 0.5;
01261 //     msg.markers[i].color.b = 1.0;
01262 //   }
01263 //   vis_marker_array_pub_.publish(msg);
01264 
01265 // }
01266 
01267 }  // namespace chomp


chomp_motion_planner
Author(s): Gil Jones , Mrinal Kalakrishnan
autogenerated on Wed Jun 19 2019 19:24:07