cartesian_twist_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 "robot_mechanism_controllers/cartesian_twist_controller.h"
00035 #include <algorithm>
00036 #include "kdl/chainfksolvervel_recursive.hpp"
00037 #include "pluginlib/class_list_macros.h"
00038 
00039 using namespace KDL;
00040 
00041 PLUGINLIB_EXPORT_CLASS( controller::CartesianTwistController, pr2_controller_interface::Controller)
00042 
00043 namespace controller {
00044 
00045 
00046 CartesianTwistController::CartesianTwistController()
00047   : robot_state_(NULL),
00048     jnt_to_twist_solver_(NULL)
00049 {}
00050 
00051 
00052 
00053 CartesianTwistController::~CartesianTwistController()
00054 {
00055   sub_command_.shutdown();
00056 }
00057 
00058 
00059 bool CartesianTwistController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
00060 {
00061   node_ = n;
00062 
00063   // get name of root and tip from the parameter server
00064   std::string root_name, tip_name;
00065   if (!node_.getParam("root_name", root_name)){
00066     ROS_ERROR("CartesianTwistController: No root name found on parameter server (namespace: %s)",
00067               node_.getNamespace().c_str());
00068     return false;
00069   }
00070   if (!node_.getParam("tip_name", tip_name)){
00071     ROS_ERROR("CartesianTwistController: No tip name found on parameter server (namespace: %s)",
00072               node_.getNamespace().c_str());
00073     return false;
00074   }
00075 
00076   // test if we got robot pointer
00077   assert(robot_state);
00078   robot_state_ = robot_state;
00079 
00080   // create robot chain from root to tip
00081   if (!chain_.init(robot_state, root_name, tip_name))
00082     return false;
00083   if (!chain_.allCalibrated())
00084   {
00085     ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
00086     return false;
00087   }
00088   chain_.toKDL(kdl_chain_);
00089 
00090   // create solver
00091   jnt_to_twist_solver_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_));
00092   jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
00093   jnt_posvel_.resize(kdl_chain_.getNrOfJoints());
00094   jnt_eff_.resize(kdl_chain_.getNrOfJoints());
00095   jacobian_.resize(kdl_chain_.getNrOfJoints());
00096 
00097   // constructs 3 identical pid controllers: for the x,y and z translations
00098   control_toolbox::Pid pid_controller;
00099   if (!pid_controller.init(ros::NodeHandle(node_, "fb_trans"))) return false;
00100   for (unsigned int i=0; i<3; i++)
00101     fb_pid_controller_.push_back(pid_controller);
00102 
00103   // constructs 3 identical pid controllers: for the x,y and z rotations
00104   if (!pid_controller.init(ros::NodeHandle(node_, "fb_rot"))) return false;
00105   for (unsigned int i=0; i<3; i++)
00106     fb_pid_controller_.push_back(pid_controller);
00107 
00108   // get parameters
00109   node_.param("ff_trans", ff_trans_, 0.0) ;
00110   node_.param("ff_rot", ff_rot_, 0.0) ;
00111 
00112   // subscribe to twist commands
00113   sub_command_ = node_.subscribe<geometry_msgs::Twist>
00114     ("command", 1, &CartesianTwistController::command, this);
00115 
00116   return true;
00117 }
00118 
00119 void CartesianTwistController::starting()
00120 {
00121   // reset pid controllers
00122   for (unsigned int i=0; i<6; i++)
00123     fb_pid_controller_[i].reset();
00124 
00125   // time
00126   last_time_ = robot_state_->getTime();
00127 
00128   // set disired twist to 0
00129   twist_desi_ = Twist::Zero();
00130 }
00131 
00132 
00133 
00134 void CartesianTwistController::update()
00135 {
00136   // check if joints are calibrated
00137   if (!chain_.allCalibrated())
00138     return;
00139 
00140   // get time
00141   ros::Time time = robot_state_->getTime();
00142   ros::Duration dt = time - last_time_;
00143   last_time_ = time;
00144 
00145   // get the joint positions and velocities
00146   chain_.getVelocities(jnt_posvel_);
00147 
00148   // get cartesian twist error
00149   FrameVel twist;
00150   jnt_to_twist_solver_->JntToCart(jnt_posvel_, twist);
00151   twist_meas_ = twist.deriv();
00152   Twist error = twist_desi_ - twist_meas_;
00153 
00154   // get the chain jacobian
00155   jac_solver_->JntToJac(jnt_posvel_.q, jacobian_);
00156 
00157   // pid feedback
00158   for (unsigned int i=0; i<3; i++)
00159     wrench_out_.force(i) = (twist_desi_.vel(i) * ff_trans_) + fb_pid_controller_[i].computeCommand(error.vel(i), dt);
00160 
00161   for (unsigned int i=0; i<3; i++)
00162     wrench_out_.torque(i) = (twist_desi_.rot(i) * ff_rot_) + fb_pid_controller_[i+3].computeCommand(error.rot(i), dt);
00163 
00164   // Converts the wrench into joint efforts with a jacbobian-transpose
00165   for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++){
00166     jnt_eff_(i) = 0;
00167     for (unsigned int j=0; j<6; j++)
00168       jnt_eff_(i) += (jacobian_(j,i) * wrench_out_(j));
00169   }
00170 
00171   // set effort to joints
00172   chain_.addEfforts(jnt_eff_);
00173 }
00174 
00175 
00176 void CartesianTwistController::command(const geometry_msgs::TwistConstPtr& twist_msg)
00177 {
00178   // convert to twist command
00179   twist_desi_.vel(0) = twist_msg->linear.x;
00180   twist_desi_.vel(1) = twist_msg->linear.y;
00181   twist_desi_.vel(2) = twist_msg->linear.z;
00182   twist_desi_.rot(0) = twist_msg->angular.x;
00183   twist_desi_.rot(1) = twist_msg->angular.y;
00184   twist_desi_.rot(2) = twist_msg->angular.z;
00185 }
00186 
00187 } // namespace


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Thu Aug 27 2015 14:22:45