arm_ik.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, 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  * Author: Melonee Wise
00035  *********************************************************************/
00036 
00037 
00038 #include <ros/ros.h>
00039 #include <actionlib/server/simple_action_server.h>
00040 #include <actionlib/client/simple_action_client.h>
00041 #include <pr2_controllers_msgs/QueryTrajectoryState.h>
00042 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00043 #include <tf/transform_listener.h>
00044 #include <geometry_msgs/PoseStamped.h>
00045 #include <pr2_arm_kinematics/pr2_arm_ik_solver.h>
00046 #include <pr2_common_action_msgs/ArmMoveIKAction.h>
00047 #include <kdl/frames_io.hpp>
00048 #include <urdf/model.h>
00049 
00050 class ArmMoveIKAction
00051 {
00052 
00053 public:
00054 
00055   ArmMoveIKAction(std::string name) :
00056     nh_("~"),
00057     dimension_(7),
00058     action_name_(name),
00059     as_(name, boost::bind(&ArmMoveIKAction::executeCB, this, _1))
00060   {
00061     //register the goal and feeback callbacks
00062     as_.registerPreemptCallback(boost::bind(&ArmMoveIKAction::preemptCB, this));
00063 
00064     ros::NodeHandle nh_toplevel;
00065 
00066     // Load Robot Model
00067     ROS_INFO("%s: Loading robot model", action_name_.c_str());
00068     std::string xml_string;
00069     if (!nh_toplevel.getParam(std::string("robot_description"), xml_string))
00070     {
00071       ROS_ERROR("Could not find parameter robot_description on parameter server.");
00072       ros::shutdown();
00073       exit(1);
00074     }
00075     if(!robot_model_.initString(xml_string))
00076     {
00077       ROS_ERROR("Could not load robot model.");
00078       ros::shutdown();
00079       exit(1);
00080     }
00081 
00082     // Get Parameters
00083     nh_.param<std::string>("arm", arm_, "r");
00084     nh_.param("joint_trajectory_action", joint_action_, std::string("joint_trajectory_action"));
00085     nh_.param("free_angle", free_angle_, 2);
00086     nh_.param("search_discretization", search_discretization_, 0.01);
00087     nh_.param("ik_timeout", timeout_, 5.0);
00088     root_name_ = "torso_lift_link";
00089     tip_name_ = arm_ + "_wrist_roll_link";
00090 
00091 
00092     // Init pose suggestion
00093     jnt_pos_suggestion_.resize(dimension_);
00094 
00095     ROS_DEBUG("%s: Loading KDL chain", action_name_.c_str());
00096     KDL::Tree tree;
00097     if (!kdl_parser::treeFromUrdfModel(robot_model_, tree))
00098     {
00099       ROS_ERROR("%s: Could not load the KDL tree from the robot model", action_name_.c_str());
00100       ros::shutdown();
00101       exit(1);
00102     }
00103     if (!tree.getChain(root_name_, tip_name_, kdl_chain_))
00104     {
00105       ROS_ERROR("%s: Could not create the KDL chain", action_name_.c_str());
00106       ros::shutdown();
00107       exit(1);
00108     }
00109 
00110     // Init IK
00111     ROS_DEBUG("Starting with search_discretization %f and ik_timeout %f", search_discretization_,timeout_);
00112     pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model_, root_name_, tip_name_, search_discretization_, free_angle_));
00113 
00114     if(!pr2_arm_ik_solver_->active_)
00115     {
00116       ROS_ERROR("%s: Could not load pr2 arm IK solver", action_name_.c_str());
00117       ros::shutdown();
00118       exit(1);
00119     }
00120 
00121     std::string trajectory_action_name = joint_action_;
00122     trajectory_action_ = new actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>(trajectory_action_name, true);
00123 
00124     double counter = 0;
00125     while(!trajectory_action_->waitForServer(ros::Duration(80.0)))
00126     {
00127       ROS_DEBUG("%s: Waiting for %s action server to come up", action_name_.c_str(), trajectory_action_name.c_str());
00128       counter++;
00129       if(counter > 3)
00130       {
00131         ROS_ERROR("%s: %s action server took too long to start", action_name_.c_str(), trajectory_action_name.c_str());
00132         //set the action state to aborted
00133         ros::shutdown();
00134         exit(1);
00135       }
00136     }
00137     //Action ready
00138     ROS_INFO("%s: Initialized", action_name_.c_str());
00139   }
00140 
00141   ~ArmMoveIKAction(void)
00142   {
00143     delete trajectory_action_;
00144   }
00145 
00146   void preemptCB()
00147   {
00148     ROS_INFO("%s: Preempt", action_name_.c_str());
00149     // set the action state to preempted
00150     trajectory_action_->cancelGoal();
00151     as_.setPreempted();
00152   }
00153 
00154   void executeCB(const pr2_common_action_msgs::ArmMoveIKGoalConstPtr & goal)
00155   {
00156     // accept the new goal
00157     ROS_INFO("%s: Accepted Goal", action_name_.c_str() );
00158 
00159 
00160     // Determines the tool frame pose
00161     tf::Pose tip_to_tool;
00162     if (goal->tool_frame.header.frame_id == "")
00163     {
00164       tip_to_tool.setIdentity();
00165     }
00166     else
00167     {
00168       try {
00169         geometry_msgs::PoseStamped tip_to_tool_msg;
00170         tf_.waitForTransform(tip_name_, goal->tool_frame.header.frame_id, goal->tool_frame.header.stamp,
00171                              ros::Duration(5.0));
00172         tf_.transformPose(tip_name_, goal->tool_frame, tip_to_tool_msg);
00173         tf::poseMsgToTF(tip_to_tool_msg.pose, tip_to_tool);
00174       }
00175       catch (const tf::TransformException &ex) {
00176         ROS_ERROR("Failed to transform tool_frame into \"%s\": %s", tip_name_.c_str(), ex.what());
00177         as_.setAborted();
00178         return;
00179       }
00180     }
00181 
00182     // Transforms the (tool) goal into the root frame
00183     tf::Pose root_to_tool_goal;
00184     try
00185     {
00186       geometry_msgs::PoseStamped root_to_tool_goal_msg;
00187       tf_.waitForTransform(root_name_, goal->pose.header.frame_id, goal->pose.header.stamp,
00188                            ros::Duration(5.0));
00189       tf_.transformPose(root_name_, goal->pose, root_to_tool_goal_msg);
00190       tf::poseMsgToTF(root_to_tool_goal_msg.pose, root_to_tool_goal);
00191     }
00192     catch (const tf::TransformException &ex)
00193     {
00194       ROS_ERROR("Failed to transform goal into \"%s\": %s", root_name_.c_str(), ex.what());
00195       as_.setAborted();
00196       return;
00197     }
00198 
00199     // Determines the goal of the tip from the goal of the tool
00200     tf::Pose root_to_tip_goal = root_to_tool_goal * tip_to_tool.inverse();
00201 
00202     // Converts into KDL
00203     KDL::Frame desired_pose;
00204     tf::PoseTFToKDL(root_to_tip_goal, desired_pose);
00205 
00206     if(goal->ik_seed.name.size() < dimension_ )
00207     {
00208       ROS_ERROR("The goal ik_seed must be size %d but is size %lu",dimension_, goal->ik_seed.name.size());
00209       as_.setAborted();
00210       return;
00211     }
00212     // Get the IK seed from the goal
00213     for(int i=0; i < dimension_; i++)
00214     {
00215       jnt_pos_suggestion_(getJointIndex(goal->ik_seed.name[i])) = goal->ik_seed.position[i];
00216     }
00217 
00218     ROS_DEBUG("calling IK solver");
00219     KDL::JntArray jnt_pos_out;
00220     bool is_valid = (pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_suggestion_, desired_pose, jnt_pos_out, timeout_)>=0);
00221     if(!is_valid)
00222     {
00223       ROS_ERROR("%s: Aborted: IK invalid.  Cannot get to (%.3f,%.3f,%.3f)(%.2f,%.2f,%.2f,%.2f)", action_name_.c_str(),
00224                 root_to_tip_goal.getOrigin()[0], root_to_tip_goal.getOrigin()[1], root_to_tip_goal.getOrigin()[2],
00225                 root_to_tip_goal.getRotation().x(),root_to_tip_goal.getRotation().y(),root_to_tip_goal.getRotation().z(),root_to_tip_goal.getRotation().w());
00226       //set the action state to aborted
00227       as_.setAborted();
00228       return;
00229     }
00230 
00231     std::vector<double> traj_desired(dimension_);
00232     std::vector<std::string> traj_names(dimension_);
00233 
00234     for(int i=0; i < dimension_; i++)
00235     {
00236       traj_names[i] = goal->ik_seed.name[i];
00237       traj_desired[i] = jnt_pos_out(getJointIndex(goal->ik_seed.name[i]));
00238     }
00239 
00240     pr2_controllers_msgs::JointTrajectoryGoal traj_goal ;
00241     std::vector<double> velocities(dimension_, 0.0);
00242     traj_goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.03);
00243     traj_goal.trajectory.points.resize(1);
00244     traj_goal.trajectory.joint_names = traj_names;
00245 
00246     traj_goal.trajectory.points[0].positions = traj_desired;
00247     traj_goal.trajectory.points[0].velocities = velocities;
00248     traj_goal.trajectory.points[0].time_from_start = goal->move_duration;
00249 
00250     // Send goal
00251     trajectory_action_->sendGoal(traj_goal);
00252     trajectory_action_->waitForResult();
00253     if(trajectory_action_->getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00254     {
00255       ROS_ERROR("%s: Aborted: trajectory action failed to achieve goal", action_name_.c_str());
00256       //set the action state to aborted
00257       as_.setAborted(result_);
00258       return;
00259     }
00260 
00261     ROS_INFO("%s: Succeeded", action_name_.c_str());
00262     // set the action state to succeeded
00263     as_.setSucceeded(result_);
00264   }
00265 
00266 
00267   int getJointIndex(const std::string &name)
00268   {
00269     int i=0; // segment number
00270     int j=0; // joint number
00271     while(j < dimension_ && i < (int) kdl_chain_.getNrOfSegments())
00272     {
00273       if(kdl_chain_.getSegment(i).getJoint().getType() == KDL::Joint::None)
00274       {
00275         i++;
00276         continue;
00277       }
00278       if(kdl_chain_.getSegment(i).getJoint().getName() == name)
00279       {
00280         return j;
00281       }
00282       i++;
00283       j++;
00284     }
00285     return -1;
00286   }
00287 
00288 
00289 protected:
00290 
00291   ros::NodeHandle nh_;
00292   urdf::Model robot_model_;
00293   std::string joint_action_;
00294   int dimension_, free_angle_;
00295   double search_discretization_, timeout_;
00296   std::string action_name_, arm_, arm_controller_, root_name_, tip_name_;
00297 
00298   KDL::Chain kdl_chain_;
00299   KDL::JntArray jnt_pos_suggestion_;
00300 
00301   actionlib::SimpleActionServer<pr2_common_action_msgs::ArmMoveIKAction> as_;
00302   actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction>* trajectory_action_;
00303 
00304   boost::shared_ptr<pr2_arm_kinematics::PR2ArmIKSolver> pr2_arm_ik_solver_;
00305   tf::TransformListener tf_;
00306 
00307   pr2_common_action_msgs::ArmMoveIKResult result_;
00308 
00309 };
00310 
00311 int main(int argc, char** argv)
00312 {
00313   ros::init(argc, argv, "pr2_arm_ik");
00314 
00315   ArmMoveIKAction pr2_arm_ik(ros::this_node::getName());
00316   ros::spin();
00317 
00318   return 0;
00319 }


pr2_arm_move_ik
Author(s): Melonee Wise, Wim Meeussen
autogenerated on Sat Dec 28 2013 17:24:55