chomp_motion_planner: chomp_cost_server.cpp Source File

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00037 #include <chomp_motion_planner/chomp_cost_server.h>
00038 
00039 //#define ANIMATE 1
00040 
00041 namespace chomp
00042 {
00043   ChompCostServer::ChompCostServer()
00044   {
00045   }
00046 
00047   ChompCostServer::~ChompCostServer()
00048   {
00049   }
00050 
00051   bool ChompCostServer::init(bool advertise_service)
00052   {
00053     // build the robot model
00054     if (!chomp_robot_model_.init())
00055       return false;
00056 
00057     // initialize the collision space
00058     if (!chomp_collision_space_.init(chomp_robot_model_.getMaxRadiusClearance()))
00059       return false;
00060 
00061     // initialize the visualization publisher:
00062     vis_marker_array_publisher_ = node_.advertise<visualization_msgs::MarkerArray>( "visualization_marker_array", 0 );
00063 
00064     // listen to mechanism_state
00065     mechanism_state_subscriber_ = node_.subscribe(std::string("/mechanism_state"), 1, &ChompCostServer::mechanismStateCallback, this);
00066 
00067     // advertise the cost service
00068     if (advertise_service)
00069     {
00070       get_chomp_collision_cost_server_ = node_.advertiseService("get_chomp_collision_cost", &ChompCostServer::getChompCollisionCost, this);
00071       get_state_cost_server_ = node_.advertiseService("get_state_cost", &ChompCostServer::getStateCost, this);
00072     }
00073     return true;
00074   }
00075 
00076   bool ChompCostServer::getChompCollisionCost(chomp_motion_planner::GetChompCollisionCost::Request& request, chomp_motion_planner::GetChompCollisionCost::Response& response)
00077   {
00078     if(request.state.joint_state.name.size() != request.state.joint_state.position.size())
00079     {
00080       ROS_ERROR("Number of joint states does not match number of joint positions");
00081       return false;
00082     }
00083 
00084     for (unsigned int i=0; i<request.state.joint_state.name.size(); ++i)
00085     {
00086       ROS_DEBUG("%s = %f", request.state.joint_state.name[i].c_str(), request.state.joint_state.position[i]);
00087     }
00088     KDL::JntArray joint_array(chomp_robot_model_.getNumKDLJoints());
00089 
00090     fillDefaultJointArray(joint_array);
00091     chomp_robot_model_.jointStateToArray(request.state.joint_state, joint_array);
00092 
00093     std::vector<KDL::Vector> joint_axis;
00094     std::vector<KDL::Vector> joint_pos;
00095     std::vector<KDL::Frame> segment_frames;
00096     chomp_robot_model_.getForwardKinematicsSolver()->JntToCart(joint_array, joint_pos, joint_axis, segment_frames);
00097 
00098     int num_links = request.links.size();
00099     response.costs.resize(num_links);
00100     response.gradient.resize(num_links);
00101 
00102     std::vector<ChompCollisionPoint> points;
00103     KDL::Vector position;
00104     Eigen::Vector3d potential_gradient;
00105     Eigen::Vector3d position_eigen;
00106     double potential;
00107     int num_collision_points=0;
00108 
00109     // for each link, get its collision points and accumulate the cost:
00110     for (unsigned int l=0; l<request.links.size(); ++l)
00111     {
00112       response.costs[l] = 0.0;
00113       chomp_robot_model_.getLinkCollisionPoints(request.links[l], points);
00114       //ROS_INFO("Link %s has %d collision points", request.links[l].c_str(), points.size());
00115       for (unsigned int i=0; i<points.size(); i++)
00116       {
00117         ++num_collision_points;
00118         points[i].getTransformedPosition(segment_frames, position);
00119 
00120         position_eigen = Eigen::Map<Eigen::Vector3d>(position.data);
00121 
00122         //bool colliding =
00123         chomp_collision_space_.getCollisionPointPotentialGradient(points[i],
00124                                                                   position_eigen, potential, potential_gradient);
00125         response.costs[l] += potential * points[i].getVolume();
00126         //ROS_INFO("\tPos=%f,%f,%f  cost = %f", position_eigen(0), position_eigen(1), position_eigen(2), response.costs[l]*1e8);
00127       }
00128     }
00129 #ifdef ANIMATE
00130     visualization_msgs::MarkerArray msg;
00131     msg.markers.resize(num_collision_points);
00132     int i=0;
00133     for (unsigned int l=0; l<request.links.size(); ++l)
00134     {
00135       chomp_robot_model_.getLinkCollisionPoints(request.links[l], points);
00136       for (unsigned int p=0; p<points.size(); p++)
00137       {
00138         points[p].getTransformedPosition(segment_frames, position);
00139         msg.markers[i].header.frame_id = chomp_robot_model_.getReferenceFrame();
00140         msg.markers[i].header.stamp = ros::Time::now();
00141         msg.markers[i].ns = "chomp_collisions";
00142         msg.markers[i].id = i;
00143         msg.markers[i].type = visualization_msgs::Marker::SPHERE;
00144         msg.markers[i].action = visualization_msgs::Marker::ADD;
00145         msg.markers[i].pose.position.x = position.x();
00146         msg.markers[i].pose.position.y = position.y();
00147         msg.markers[i].pose.position.z = position.z();
00148         msg.markers[i].pose.orientation.x = 0.0;
00149         msg.markers[i].pose.orientation.y = 0.0;
00150         msg.markers[i].pose.orientation.z = 0.0;
00151         msg.markers[i].pose.orientation.w = 1.0;
00152         double scale = points[p].getRadius()*2;
00153         msg.markers[i].scale.x = scale;
00154         msg.markers[i].scale.y = scale;
00155         msg.markers[i].scale.z = scale;
00156         msg.markers[i].color.a = 0.9;
00157         msg.markers[i].color.r = 0.5;
00158         msg.markers[i].color.g = 1.0;
00159         msg.markers[i].color.b = 0.3;
00160         ++i;
00161       }
00162     }
00163     vis_marker_array_publisher_.publish(msg);
00164     ros::WallDuration(0.1).sleep();
00165 #endif
00166     return true;
00167   }
00168 
00169 
00170 
00171   bool ChompCostServer::getStateCost(chomp_motion_planner::GetStateCost::Request& request, chomp_motion_planner::GetStateCost::Response& response)
00172   {
00173     chomp_motion_planner::GetChompCollisionCost::Request  chomp_request;
00174     chomp_motion_planner::GetChompCollisionCost::Response chomp_response;
00175 
00176     chomp_request.links = request.link_names;
00177     chomp_request.state = request.robot_state;
00178     bool success = getChompCollisionCost(chomp_request,chomp_response);
00179     
00180     if(success)
00181     {
00182       response.valid = chomp_response.in_collision;
00183       response.costs = chomp_response.costs;
00184     }
00185     return success;
00186   }
00187 
00188 
00189 
00190   void ChompCostServer::mechanismStateCallback(const sensor_msgs::JointStateConstPtr& mech_state)
00191   {
00192     if (mechanism_state_mutex_.try_lock())
00193     {
00194       mechanism_state_ = *mech_state;
00195       mechanism_state_mutex_.unlock();
00196     }
00197   }
00198 
00199   void ChompCostServer::fillDefaultJointArray(KDL::JntArray& jnt_array)
00200   {
00201     mechanism_state_mutex_.lock();
00202 
00203     int num_joints = mechanism_state_.name.size();
00204     for (int i=0; i<num_joints; i++)
00205     {
00206       std::string& name = mechanism_state_.name[i];
00207       int num = chomp_robot_model_.urdfNameToKdlNumber(name);
00208       if (num>=0)
00209         jnt_array(num) = mechanism_state_.position[i];
00210     }
00211     mechanism_state_mutex_.unlock();
00212   }
00213 
00214   int ChompCostServer::run()
00215   {
00216     ros::spin();
00217     return 0;
00218   }
00219 
00220 } // namespace chomp
00221 
00222 using namespace chomp;
00223 
00224 int main(int argc, char** argv)
00225 {
00226   ros::init(argc, argv, "chomp_cost_server");
00227   ChompCostServer chomp_cost_server;
00228 
00229   if (chomp_cost_server.init(true))
00230     return chomp_cost_server.run();
00231   else
00232     return 1;
00233 }