cartesian_pose_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 
00035 
00036 #include "robot_mechanism_controllers/cartesian_pose_controller.h"
00037 #include <algorithm>
00038 #include "kdl/chainfksolverpos_recursive.hpp"
00039 #include "pluginlib/class_list_macros.h"
00040 #include "tf_conversions/tf_kdl.h"
00041 
00042 
00043 using namespace KDL;
00044 using namespace tf;
00045 using namespace std;
00046 
00047 PLUGINLIB_EXPORT_CLASS( controller::CartesianPoseController, pr2_controller_interface::Controller)
00048 
00049 namespace controller {
00050 
00051 CartesianPoseController::CartesianPoseController()
00052 : robot_state_(NULL)
00053 {}
00054 
00055 CartesianPoseController::~CartesianPoseController()
00056 {
00057   command_filter_.reset();
00058 }
00059 
00060 
00061 bool CartesianPoseController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
00062 {
00063   node_ = n;
00064 
00065   // get name of root and tip from the parameter server
00066   std::string tip_name;
00067   if (!node_.getParam("root_name", root_name_)){
00068     ROS_ERROR("CartesianPoseController: No root name found on parameter server (namespace: %s)",
00069               node_.getNamespace().c_str());
00070     return false;
00071   }
00072   if (!node_.getParam("tip_name", tip_name)){
00073     ROS_ERROR("CartesianPoseController: No tip name found on parameter server (namespace: %s)",
00074               node_.getNamespace().c_str());
00075     return false;
00076   }
00077 
00078   // test if we got robot pointer
00079   assert(robot_state);
00080   robot_state_ = robot_state;
00081 
00082   // create robot chain from root to tip
00083   if (!chain_.init(robot_state_, root_name_, tip_name))
00084     return false;
00085   if (!chain_.allCalibrated())
00086   {
00087     ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
00088     return false;
00089   }
00090   chain_.toKDL(kdl_chain_);
00091 
00092   // create solver
00093   jnt_to_pose_solver_.reset(new ChainFkSolverPos_recursive(kdl_chain_));
00094   jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
00095   jnt_pos_.resize(kdl_chain_.getNrOfJoints());
00096   jnt_eff_.resize(kdl_chain_.getNrOfJoints());
00097   jacobian_.resize(kdl_chain_.getNrOfJoints());
00098 
00099   // create pid controller for the translation and for the rotation
00100   control_toolbox::Pid pid_controller;
00101   if (!pid_controller.init(ros::NodeHandle(node_,"fb_trans"))) return false;
00102   for (unsigned int i = 0; i < 3; i++)
00103     pid_controller_.push_back(pid_controller);
00104   if (!pid_controller.init(ros::NodeHandle(node_,"fb_rot"))) return false;
00105   for (unsigned int i = 0; i < 3; i++)
00106     pid_controller_.push_back(pid_controller);
00107 
00108   // subscribe to pose commands
00109   sub_command_.subscribe(node_, "command", 10);
00110   command_filter_.reset(new tf::MessageFilter<geometry_msgs::PoseStamped>(
00111                           sub_command_, tf_, root_name_, 10, node_));
00112   command_filter_->registerCallback(boost::bind(&CartesianPoseController::command, this, _1));
00113 
00114   // realtime publisher for control state
00115   state_error_publisher_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist>(node_, "state/error", 1));
00116   state_pose_publisher_.reset(new realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped>(node_, "state/pose", 1));
00117 
00118   return true;
00119 }
00120 
00121 void CartesianPoseController::starting()
00122 {
00123   // reset pid controllers
00124   for (unsigned int i=0; i<6; i++)
00125     pid_controller_[i].reset();
00126 
00127   // initialize desired pose/twist
00128   twist_ff_ = Twist::Zero();
00129   pose_desi_ = getPose();
00130   last_time_ = robot_state_->getTime();
00131 
00132   loop_count_ = 0;
00133 }
00134 
00135 
00136 
00137 void CartesianPoseController::update()
00138 {
00139   // get time
00140   ros::Time time = robot_state_->getTime();
00141   ros::Duration dt = time - last_time_;
00142   last_time_ = time;
00143 
00144   // get current pose
00145   pose_meas_ = getPose();
00146 
00147   // pose feedback into twist
00148   twist_error_ = diff(pose_desi_, pose_meas_);
00149   KDL::Wrench wrench_desi;
00150   for (unsigned int i=0; i<6; i++)
00151     wrench_desi(i) = pid_controller_[i].updatePid(twist_error_(i), dt);
00152 
00153   // get the chain jacobian
00154   jac_solver_->JntToJac(jnt_pos_, jacobian_);
00155 
00156   // Converts the wrench into joint efforts with a jacbobian-transpose
00157   for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++){
00158     jnt_eff_(i) = 0;
00159     for (unsigned int j=0; j<6; j++)
00160       jnt_eff_(i) += (jacobian_(j,i) * wrench_desi(j));
00161   }
00162 
00163   // set effort to joints
00164   chain_.addEfforts(jnt_eff_);
00165 
00166 
00167   if (++loop_count_ % 100 == 0){
00168     if (state_error_publisher_){
00169       if (state_error_publisher_->trylock()){
00170         state_error_publisher_->msg_.linear.x = twist_error_.vel(0);
00171         state_error_publisher_->msg_.linear.y = twist_error_.vel(1);
00172         state_error_publisher_->msg_.linear.z = twist_error_.vel(2);
00173         state_error_publisher_->msg_.angular.x = twist_error_.rot(0);
00174         state_error_publisher_->msg_.angular.y = twist_error_.rot(1);
00175         state_error_publisher_->msg_.angular.z = twist_error_.rot(2);
00176         state_error_publisher_->unlockAndPublish();
00177       }
00178     }
00179     if (state_pose_publisher_){
00180       if (state_pose_publisher_->trylock()){
00181         Pose tmp;
00182         tf::poseKDLToTF(pose_meas_, tmp);
00183         poseStampedTFToMsg(Stamped<Pose>(tmp, ros::Time::now(), root_name_), state_pose_publisher_->msg_);
00184         state_pose_publisher_->unlockAndPublish();
00185       }
00186     }
00187   }
00188 }
00189 
00190 
00191 
00192 Frame CartesianPoseController::getPose()
00193 {
00194   // get the joint positions and velocities
00195   chain_.getPositions(jnt_pos_);
00196 
00197   // get cartesian pose
00198   Frame result;
00199   jnt_to_pose_solver_->JntToCart(jnt_pos_, result);
00200 
00201   return result;
00202 }
00203 
00204 void CartesianPoseController::command(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
00205 {
00206   // convert message to transform
00207   Stamped<Pose> pose_stamped;
00208   poseStampedMsgToTF(*pose_msg, pose_stamped);
00209 
00210   // convert to reference frame of root link of the controller chain
00211   tf_.transformPose(root_name_, pose_stamped, pose_stamped);
00212   tf::poseTFToKDL(pose_stamped, pose_desi_);
00213 }
00214 
00215 } // namespace
00216 


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Thu Apr 24 2014 15:44:43