cartesian_trajectory_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Wim Meeussen
00032  */
00033 
00034 #include <algorithm>
00035 #include "kdl/chainfksolverpos_recursive.hpp"
00036 #include "pr2_manipulation_controllers/CheckMoving.h"
00037 #include "pr2_manipulation_controllers/cartesian_trajectory_controller.h"
00038 #include "pluginlib/class_list_macros.h"
00039 
00040 
00041 using namespace KDL;
00042 using namespace tf;
00043 using namespace ros;
00044 using namespace controller;
00045 
00046 PLUGINLIB_EXPORT_CLASS(pr2_manipulation_controllers::CartesianTrajectoryController,pr2_controller_interface::Controller)
00047 
00048 namespace pr2_manipulation_controllers{
00049 
00050 
00051 CartesianTrajectoryController::CartesianTrajectoryController()
00052 : jnt_to_pose_solver_(NULL),
00053   motion_profile_(6, VelocityProfile_Trap(0,0))
00054 {}
00055 
00056 CartesianTrajectoryController::~CartesianTrajectoryController()
00057 {}
00058 
00059 
00060 bool CartesianTrajectoryController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle& n)
00061 {
00062   node_ = n;
00063 
00064   // get name of root and tip from the parameter server
00065   std::string tip_name;
00066   if (!node_.getParam("root_name", root_name_)){
00067     ROS_ERROR("CartesianTrajectoryController: No root name found on parameter server");
00068     return false;
00069   }
00070   if (!node_.getParam("tip_name", tip_name)){
00071     ROS_ERROR("CartesianTrajectoryController: No tip name found on parameter server");
00072     return false;
00073   }
00074 
00075   // test if we got robot pointer
00076   assert(robot_state);
00077   robot_state_ = robot_state;
00078 
00079   // create robot chain from root to tip
00080   if (!chain_.init(robot_state, root_name_, tip_name))
00081     return false;
00082   chain_.toKDL(kdl_chain_);
00083 
00084   // create solver
00085   jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
00086   jnt_pos_.resize(kdl_chain_.getNrOfJoints());
00087 
00088   // initialize motion profile
00089   double max_vel_trans, max_vel_rot, max_acc_trans, max_acc_rot;
00090   node_.param("max_vel_trans", max_vel_trans, 0.0) ;
00091   node_.param("max_vel_rot", max_vel_rot, 0.0) ;
00092   node_.param("max_acc_trans", max_acc_trans, 0.0) ;
00093   node_.param("max_acc_rot", max_acc_rot, 0.0) ;
00094   for (unsigned int i=0; i<3; i++){
00095     motion_profile_[i  ].SetMax(max_vel_trans, max_acc_trans);
00096     motion_profile_[i+3].SetMax(max_vel_rot,   max_acc_rot);
00097   }
00098 
00099   // get a pointer to the pose controller
00100   std::string output;
00101   if (!node_.getParam("output", output)){
00102     ROS_ERROR("CartesianTrajectoryController: No ouptut name found on parameter server");
00103     return false;
00104   }
00105   if (!getController<CartesianPoseController>(output, AFTER_ME, pose_controller_)){
00106     ROS_ERROR("CartesianTrajectoryController: could not connect to pose controller");
00107     return false;
00108   }
00109 
00110   // subscribe to pose commands
00111   /*
00112   command_notifier_.reset(new MessageFilter<geometry_msgs::PoseStamped>(tf_,
00113                                                                        boost::bind(&CartesianTrajectoryController::command, this, _1),
00114                                                                        node_.getNamespace() + "/command", root_name_, 1));
00115   */
00116   //command_sub_ = node_.subscribe(node_.getNamespace() + "/command", 1, &CartesianTrajectoryController::command, this);
00117   
00118 
00119   // advertise services
00120   move_to_srv_ = node_.advertiseService("move_to", &CartesianTrajectoryController::moveTo, this);
00121   preempt_srv_ = node_.advertiseService("preempt", &CartesianTrajectoryController::preempt, this);
00122   check_moving_srv_ = node_.advertiseService("check_moving", &CartesianTrajectoryController::checkMoving, this);
00123 
00124   return true;
00125 }
00126 
00127 
00128 
00129 
00130 //service callback to return is_moving_
00131 bool CartesianTrajectoryController::checkMoving(pr2_manipulation_controllers::CheckMoving::Request &req, pr2_manipulation_controllers::CheckMoving::Response &res)
00132 {
00133   res.ismoving = is_moving_;
00134   return true;
00135 }
00136 
00137 
00138 
00139 
00140 bool CartesianTrajectoryController::moveTo(const geometry_msgs::PoseStamped& pose, const geometry_msgs::Twist& tolerance, double duration)
00141 {
00142   //ROS_INFO("Cartesian trajectory controller moving to new pose");
00143 
00144   // don't do anything when still moving
00145   if (is_moving_){
00146     ROS_WARN("Controller is still moving!  Rejecting new pose");
00147     return false;
00148   }
00149 
00150   //normalize pose orientation
00151   geometry_msgs::PoseStamped pose_normalized;
00152   pose_normalized.header = pose.header;
00153   pose_normalized.pose.position = pose.pose.position;
00154   double x = pose.pose.orientation.x;
00155   double y = pose.pose.orientation.y;
00156   double z = pose.pose.orientation.z;
00157   double w = pose.pose.orientation.w;
00158   double mag = sqrt(x*x+y*y+z*z+w*w);
00159   pose_normalized.pose.orientation.x = x/mag;
00160   pose_normalized.pose.orientation.y = y/mag;
00161   pose_normalized.pose.orientation.z = z/mag;
00162   pose_normalized.pose.orientation.w = w/mag;
00163   //ROS_ERROR("normalized pose: %2.3f %2.3f %2.3f %2.3f", pose_normalized.pose.orientation.x, pose_normalized.pose.orientation.y, pose_normalized.pose.orientation.z, pose_normalized.pose.orientation.w);
00164 
00165   // convert message to transform
00166   Stamped<Pose> pose_stamped;
00167   poseStampedMsgToTF(pose_normalized, pose_stamped);
00168 
00169   // convert to reference frame of root link of the controller chain
00170   Duration timeout = Duration().fromSec(2.0);
00171   std::string error_msg;
00172   if (!tf_.waitForTransform(root_name_, pose.header.frame_id, pose_stamped.stamp_, timeout, Duration(0.01), &error_msg)){
00173     ROS_ERROR("CartesianTrajectoryController: %s", error_msg.c_str());
00174     return false;
00175   }
00176   tf_.transformPose(root_name_, pose_stamped, pose_stamped);
00177 
00178   // trajectory from pose_begin to pose_end
00179   TransformToFrame(pose_stamped, pose_end_);
00180   pose_begin_ = pose_current_;
00181 
00182   //tf::Matrix3x3 bt_mat = pose_stamped.getBasis();
00183   //ROS_ERROR("pose_stamped:\n %0.2f %0.2f %0.2f\n %0.2f %0.2f %0.2f\n %0.2f %0.2f %0.2f\n", bt_mat[0][0],bt_mat[0][1],bt_mat[0][2],bt_mat[1][0],bt_mat[1][1],bt_mat[1][2],bt_mat[2][0],bt_mat[2][1],bt_mat[2][2]);
00184 
00185   //KDL::Rotation mat = pose_end_.M;
00186   //ROS_ERROR("pose_end:\n %0.2f %0.2f %0.2f\n %0.2f %0.2f %0.2f\n %0.2f %0.2f %0.2f\n", mat(0,0),mat(0,1),mat(0,2),mat(1,0),mat(1,1),mat(1,2),mat(2,0),mat(2,1),mat(2,2));
00187 
00188 
00189   max_duration_ = 0;
00190   Twist twist_move = diff(pose_begin_, pose_end_);
00191 
00192   // Set motion profiles
00193   for (unsigned int i=0; i<6; i++){
00194     motion_profile_[i].SetProfileDuration( 0, twist_move(i), duration);
00195     max_duration_ = max( max_duration_, motion_profile_[i].Duration() );
00196   }
00197 
00198   // Rescale trajectories to maximal duration
00199   for (unsigned int i=0; i<6; i++)
00200     motion_profile_[i].SetProfileDuration( 0, twist_move(i), max_duration_ );
00201 
00202   // set tolerance
00203   tolerance_.vel(0) = tolerance.linear.x;
00204   tolerance_.vel(1) = tolerance.linear.y;
00205   tolerance_.vel(2) = tolerance.linear.z;
00206   tolerance_.rot(0) = tolerance.angular.x;
00207   tolerance_.rot(1) = tolerance.angular.y;
00208   tolerance_.rot(2) = tolerance.angular.z;
00209 
00210   time_passed_ = 0;
00211 
00212   exceed_tolerance_ = false;
00213   request_preempt_ = false;
00214   is_moving_ = true;
00215 
00216   ROS_INFO("CartesianTrajectoryController: %s will move to new pose in %f seconds", controller_name_.c_str(), max_duration_);
00217 
00218   return true;
00219 }
00220 
00221 
00222 
00223 void CartesianTrajectoryController::starting()
00224 {
00225   // time
00226   last_time_ = robot_state_->getTime();
00227 
00228   // set desired pose to current pose
00229   pose_current_ = getPose();
00230   twist_current_ = Twist::Zero();
00231 
00232   // start not moving
00233   is_moving_ = false;
00234   request_preempt_ = false;
00235 }
00236 
00237 
00238 
00239 
00240 void CartesianTrajectoryController::update()
00241 {
00242   // get time
00243   ros::Time time = robot_state_->getTime();
00244   ros::Duration dt = time - last_time_;
00245   last_time_ = time;
00246 
00247   // preempt trajectory
00248   if (request_preempt_){
00249     twist_current_ = Twist::Zero();
00250     is_moving_ = false;
00251   }
00252 
00253   // if we are moving
00254   if (is_moving_){
00255     time_passed_ += dt.toSec();
00256 
00257     // check tolerance
00258     for (unsigned int i=0; i<6; i++){
00259       if (tolerance_[i] != 0 && pose_controller_->twist_error_[i] > tolerance_[i]){
00260         exceed_tolerance_ = true;
00261         is_moving_ = false;
00262         ROS_INFO("Cartesian trajectory exceeded tolerance");
00263       }
00264     }
00265 
00266     // ended trajectory
00267     if (time_passed_ > max_duration_){
00268       twist_current_ = Twist::Zero();
00269       pose_current_  = pose_end_;
00270       is_moving_ = false;
00271       KDL::Rotation mat = pose_current_.M;
00272       //ROS_ERROR("pose_current:\n %0.2f %0.2f %0.2f\n %0.2f %0.2f %0.2f\n %0.2f %0.2f %0.2f\n", mat(0,0),mat(0,1),mat(0,2),mat(1,0),mat(1,1),mat(1,2),mat(2,0),mat(2,1),mat(2,2));
00273 
00274 
00275     }
00276     // still in trajectory
00277     else{
00278       // pose
00279       Twist twist_begin_current = Twist(Vector(motion_profile_[0].Pos(time_passed_),
00280                                                motion_profile_[1].Pos(time_passed_),
00281                                                motion_profile_[2].Pos(time_passed_)),
00282                                         Vector(motion_profile_[3].Pos(time_passed_),
00283                                                motion_profile_[4].Pos(time_passed_),
00284                                                motion_profile_[5].Pos(time_passed_)) );
00285       pose_current_ = Frame( pose_begin_.M * Rot( pose_begin_.M.Inverse( twist_begin_current.rot ) ),
00286                              pose_begin_.p + twist_begin_current.vel);
00287 
00288       // twist
00289       for(unsigned int i=0; i<6; i++)
00290         twist_current_(i) = motion_profile_[i].Vel( time_passed_ );
00291     }
00292   }
00293 
00294   // send output to pose controller
00295   pose_controller_->pose_desi_ = pose_current_;
00296   pose_controller_->twist_ff_ = twist_current_;
00297 }
00298 
00299 
00300 
00301 Frame CartesianTrajectoryController::getPose()
00302 {
00303   // get the joint positions and velocities
00304   chain_.getPositions(jnt_pos_);
00305 
00306   // get cartesian pose
00307   Frame result;
00308   jnt_to_pose_solver_->JntToCart(jnt_pos_, result);
00309 
00310   return result;
00311 }
00312 
00313 
00314 bool CartesianTrajectoryController::moveTo(pr2_manipulation_controllers::MoveToPose::Request &req,
00315                                            pr2_manipulation_controllers::MoveToPose::Response &resp)
00316 {
00317   ROS_DEBUG("in cartesian traj move_to service");
00318 
00319   if (!moveTo(req.pose, req.tolerance, 0.0)){
00320     ROS_ERROR("CartesianTrajectoryController: not starting trajectory because either previous one is still running or the transform frame could not be found");
00321     return false;
00322   }
00323   /*
00324   ros::Duration timeout = Duration().fromSec(3.0);
00325   ros::Duration traj_duration = Duration().fromSec(max_duration_);
00326   ros::Time start_time = ros::Time::now();
00327 
00328 
00329 
00330   while (is_moving_){
00331     Duration().fromSec(0.1).sleep();
00332     if (ros::Time::now() > start_time + timeout + traj_duration){
00333       ROS_ERROR("CartesianTrajectoryController: timeout");
00334       return false;
00335     }
00336   }
00337 
00338   if (request_preempt_){
00339     ROS_ERROR("CartesianTrajectoryController: trajectory preempted");
00340     return false;
00341   }
00342   else if (exceed_tolerance_){
00343     ROS_ERROR("CartesianTrajectoryController: exceeded trajectory tolerance");
00344     return false;
00345   }
00346   else{
00347     ROS_DEBUG("CartesianTrajectoryController: moveto finished successfully");
00348     return true;
00349     }*/
00350   return true;
00351 }
00352 
00353 
00354 /*
00355 void CartesianTrajectoryController::command(const MessageFilter<geometry_msgs::PoseStamped>::MessagePtr& pose_msg)
00356 {
00357   moveTo(*pose_msg);
00358   }*/
00359 
00360 
00361 bool CartesianTrajectoryController::preempt(std_srvs::Empty::Request &req,
00362                                             std_srvs::Empty::Response &resp)
00363 {
00364   // you can only preempt is the robot is moving
00365   if (!is_moving_)
00366     return false;
00367 
00368   request_preempt_ = true;
00369 
00370   // wait for robot to stop moving
00371   Duration sleep_time = Duration().fromSec(0.01);
00372   while (is_moving_)
00373     sleep_time.sleep();
00374 
00375   return true;
00376 }
00377 
00378 
00379 
00380 void CartesianTrajectoryController::TransformToFrame(const Transform& trans, Frame& frame)
00381 {
00382   frame.p(0) = trans.getOrigin().x();
00383   frame.p(1) = trans.getOrigin().y();
00384   frame.p(2) = trans.getOrigin().z();
00385 
00386   int i, j;
00387   tf::Matrix3x3 bt_mat = trans.getBasis();
00388   for(i=0; i<3; i++){
00389     for(j=0; j<3; j++){
00390       frame.M(i,j) = bt_mat[i][j];
00391     }
00392   }
00393 }
00394 
00395 
00396 
00397 }; // namespace
00398 


pr2_manipulation_controllers
Author(s): Kaijen Hsiao, Stu Glaser, Adam Leeper
autogenerated on Mon Oct 6 2014 12:10:46