chomp_planner.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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: E. Gil Jones */
00036 
00037 #include <ros/ros.h>
00038 #include <chomp_motion_planner/chomp_planner.h>
00039 #include <chomp_motion_planner/chomp_trajectory.h>
00040 #include <chomp_motion_planner/chomp_optimizer.h>
00041 #include <planning_models/conversions.h>
00042 
00043 namespace chomp {
00044 
00045 ChompPlanner::ChompPlanner(const planning_models::RobotModelConstPtr& kmodel)
00046 {
00047 }
00048 
00049 bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
00050                          const moveit_msgs::GetMotionPlan::Request &req,
00051                          const chomp::ChompParameters& params,
00052                          moveit_msgs::GetMotionPlan::Response &res) const
00053 {
00054   ros::WallTime start_time = ros::WallTime::now();
00055   ChompTrajectory trajectory(planning_scene->getRobotModel(),
00056                              3.0,
00057                              .03,
00058                              req.motion_plan_request.group_name);
00059   jointStateToArray(planning_scene->getRobotModel(),
00060                     req.motion_plan_request.start_state.joint_state,
00061                     req.motion_plan_request.group_name,
00062                     trajectory.getTrajectoryPoint(0));
00063 
00064   int goal_index = trajectory.getNumPoints()- 1;
00065   trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
00066   sensor_msgs::JointState js;
00067   for(unsigned int i = 0; i < req.motion_plan_request.goal_constraints[0].joint_constraints.size(); i++) {
00068     js.name.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name);
00069     js.position.push_back(req.motion_plan_request.goal_constraints[0].joint_constraints[i].position);
00070     ROS_INFO_STREAM("Setting joint " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].joint_name
00071                     << " to position " << req.motion_plan_request.goal_constraints[0].joint_constraints[i].position);
00072   }
00073   jointStateToArray(planning_scene->getRobotModel(),
00074                     js,
00075                     req.motion_plan_request.group_name,
00076                     trajectory.getTrajectoryPoint(goal_index));
00077   const planning_models::RobotModel::JointModelGroup* model_group =
00078     planning_scene->getRobotModel()->getJointModelGroup(req.motion_plan_request.group_name);
00079   // fix the goal to move the shortest angular distance for wrap-around joints:
00080   for (size_t i = 0; i < model_group->getJointModels().size(); i++)
00081   {
00082     const planning_models::RobotModel::JointModel* model = model_group->getJointModels()[i];
00083     const planning_models::RobotModel::RevoluteJointModel* revolute_joint = dynamic_cast<const planning_models::RobotModel::RevoluteJointModel*>(model);
00084 
00085     if (revolute_joint != NULL)
00086     {
00087       if(revolute_joint->isContinuous())
00088       {
00089         double start = (trajectory)(0, i);
00090         double end = (trajectory)(goal_index, i);
00091         ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
00092         (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
00093       }
00094     }
00095   }
00096 
00097   // fill in an initial quintic spline trajectory
00098   trajectory.fillInMinJerk();
00099 
00100   // optimize!
00101   planning_models::RobotState *start_state(planning_scene->getCurrentState());
00102   planning_models::robotStateMsgToRobotState(*planning_scene->getTransforms(), req.motion_plan_request.start_state, start_state);
00103 
00104   ros::WallTime create_time = ros::WallTime::now();
00105   ChompOptimizer optimizer(&trajectory,
00106                            planning_scene,
00107                            req.motion_plan_request.group_name,
00108                            &params,
00109                            start_state);
00110   if(!optimizer.isInitialized()) {
00111     ROS_WARN_STREAM("Could not initialize optimizer");
00112     res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00113     return false;
00114   }
00115   ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
00116   ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
00117   optimizer.optimize();
00118   ROS_INFO("Optimization actually took %f sec to run", (ros::WallTime::now() - create_time).toSec());
00119   create_time = ros::WallTime::now();
00120   // assume that the trajectory is now optimized, fill in the output structure:
00121 
00122   ROS_INFO("Output trajectory has %d joints", trajectory.getNumJoints());
00123 
00124   // fill in joint names:
00125   res.trajectory.joint_trajectory.joint_names.resize(trajectory.getNumJoints());
00126   for (size_t i = 0; i < model_group->getJointModels().size(); i++)
00127   {
00128     res.trajectory.joint_trajectory.joint_names[i] = model_group->getJointModels()[i]->getName();
00129   }
00130 
00131   res.trajectory.joint_trajectory.header = req.motion_plan_request.start_state.joint_state.header; // @TODO this is probably a hack
00132 
00133   // fill in the entire trajectory
00134   res.trajectory.joint_trajectory.points.resize(trajectory.getNumPoints());
00135   for (int i=0; i < trajectory.getNumPoints(); i++)
00136   {
00137     res.trajectory.joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
00138     for (size_t j=0; j < res.trajectory.joint_trajectory.points[i].positions.size(); j++)
00139     {
00140       res.trajectory.joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
00141       if(i == trajectory.getNumPoints()-1) {
00142         ROS_INFO_STREAM("Joint " << j << " " << res.trajectory.joint_trajectory.points[i].positions[j]);
00143       }
00144     }
00145     // Setting invalid timestamps.
00146     // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints.
00147     res.trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
00148   }
00149 
00150   ROS_INFO("Bottom took %f sec to create", (ros::WallTime::now() - create_time).toSec());
00151   ROS_INFO("Serviced planning request in %f wall-seconds, trajectory duration is %f", (ros::WallTime::now() - start_time).toSec(), res.trajectory.joint_trajectory.points[goal_index].time_from_start.toSec());
00152   res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00153   res.planning_time = ros::Duration((ros::WallTime::now() - start_time).toSec());
00154   return true;
00155 }
00156 
00157 }


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