$search
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_DECLARE_CLASS(pr2_manipulation_controllers, CartesianTrajectoryController, 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 //btMatrix3x3 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 btMatrix3x3 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