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


chomp_motion_planner
Author(s): Gil Jones
autogenerated on Wed Sep 16 2015 04:42:45