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 <moveit/robot_state/conversions.h>
00042 #include <moveit_msgs/MotionPlanRequest.h>
00043 
00044 namespace chomp
00045 {
00046 ChompPlanner::ChompPlanner()
00047 {
00048 }
00049 
00050 bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
00051                          const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
00052                          moveit_msgs::MotionPlanDetailedResponse& res) const
00053 {
00054   if (!planning_scene)
00055     ROS_ERROR_STREAM("No planning scene initialized.");
00056   else
00057   {
00058     ROS_INFO_STREAM("Planning scene is configured. Beginning to plan.");
00059   }
00060   ros::WallTime start_time = ros::WallTime::now();
00061   ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);
00062   jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name,
00063                     trajectory.getTrajectoryPoint(0));
00064 
00065   int goal_index = trajectory.getNumPoints() - 1;
00066   trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
00067   sensor_msgs::JointState js;
00068   for (unsigned int i = 0; i < req.goal_constraints[0].joint_constraints.size(); i++)
00069   {
00070     js.name.push_back(req.goal_constraints[0].joint_constraints[i].joint_name);
00071     js.position.push_back(req.goal_constraints[0].joint_constraints[i].position);
00072     ROS_INFO_STREAM("Setting joint " << req.goal_constraints[0].joint_constraints[i].joint_name << " to position "
00073                                      << req.goal_constraints[0].joint_constraints[i].position);
00074   }
00075   jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index));
00076 
00077   const moveit::core::JointModelGroup* model_group =
00078       planning_scene->getRobotModel()->getJointModelGroup(req.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->getActiveJointModels().size(); i++)
00081   {
00082     const moveit::core::JointModel* model = model_group->getActiveJointModels()[i];
00083     const moveit::core::RevoluteJointModel* revolute_joint =
00084         dynamic_cast<const moveit::core::RevoluteJointModel*>(model);
00085 
00086     if (revolute_joint != NULL)
00087     {
00088       if (revolute_joint->isContinuous())
00089       {
00090         double start = (trajectory)(0, i);
00091         double end = (trajectory)(goal_index, i);
00092         ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
00093         (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
00094       }
00095     }
00096   }
00097 
00098   // fill in an initial quintic spline trajectory
00099   trajectory.fillInMinJerk();
00100 
00101   // optimize!
00102   moveit::core::RobotState start_state(planning_scene->getCurrentState());
00103   moveit::core::robotStateMsgToRobotState(req.start_state, start_state);
00104   start_state.update();
00105 
00106   ros::WallTime create_time = ros::WallTime::now();
00107   ChompOptimizer optimizer(&trajectory, planning_scene, req.group_name, &params, start_state);
00108   if (!optimizer.isInitialized())
00109   {
00110     ROS_WARN_STREAM("Could not initialize optimizer");
00111     res.error_code.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
00112     return false;
00113   }
00114   ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
00115   ROS_INFO("Optimization took %f sec to create", (ros::WallTime::now() - create_time).toSec());
00116   optimizer.optimize();
00117   ROS_INFO("Optimization actually took %f sec to run", (ros::WallTime::now() - create_time).toSec());
00118   create_time = ros::WallTime::now();
00119   // assume that the trajectory is now optimized, fill in the output structure:
00120 
00121   ROS_INFO("Output trajectory has %d joints", trajectory.getNumJoints());
00122 
00123   res.trajectory.resize(1);
00124 
00125   for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++)
00126   {
00127     res.trajectory[0].joint_trajectory.joint_names.push_back(model_group->getActiveJointModels()[i]->getName());
00128   }
00129 
00130   res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header;  // @TODO this is probably a hack
00131 
00132   // fill in the entire trajectory
00133   res.trajectory[0].joint_trajectory.points.resize(trajectory.getNumPoints());
00134   for (int i = 0; i < trajectory.getNumPoints(); i++)
00135   {
00136     res.trajectory[0].joint_trajectory.points[i].positions.resize(trajectory.getNumJoints());
00137     for (size_t j = 0; j < res.trajectory[0].joint_trajectory.points[i].positions.size(); j++)
00138     {
00139       res.trajectory[0].joint_trajectory.points[i].positions[j] = trajectory.getTrajectoryPoint(i)(j);
00140       if (i == trajectory.getNumPoints() - 1)
00141       {
00142         ROS_INFO_STREAM("Joint " << j << " " << res.trajectory[0].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[0].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",
00152            (ros::WallTime::now() - start_time).toSec(),
00153            res.trajectory[0].joint_trajectory.points[goal_index].time_from_start.toSec());
00154   res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00155   res.processing_time.push_back((ros::WallTime::now() - start_time).toSec());
00156   return true;
00157 }
00158 }


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