00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
00062 as_.registerPreemptCallback(boost::bind(&ArmMoveIKAction::preemptCB, this));
00063
00064 ros::NodeHandle nh_toplevel;
00065
00066
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
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
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
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
00133 ros::shutdown();
00134 exit(1);
00135 }
00136 }
00137
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
00150 trajectory_action_->cancelGoal();
00151 as_.setPreempted();
00152 }
00153
00154 void executeCB(const pr2_common_action_msgs::ArmMoveIKGoalConstPtr & goal)
00155 {
00156
00157 ROS_INFO("%s: Accepted Goal", action_name_.c_str() );
00158
00159
00160
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
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
00200 tf::Pose root_to_tip_goal = root_to_tool_goal * tip_to_tool.inverse();
00201
00202
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
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
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
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
00257 as_.setAborted(result_);
00258 return;
00259 }
00260
00261 ROS_INFO("%s: Succeeded", action_name_.c_str());
00262
00263 as_.setSucceeded(result_);
00264 }
00265
00266
00267 int getJointIndex(const std::string &name)
00268 {
00269 int i=0;
00270 int j=0;
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 }
