Go to the documentation of this file.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 #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
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
00077 assert(robot_state);
00078 robot_state_ = robot_state;
00079
00080
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
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
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
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
00109 node_.param("ff_trans", ff_trans_, 0.0) ;
00110 node_.param("ff_rot", ff_rot_, 0.0) ;
00111
00112
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
00122 for (unsigned int i=0; i<6; i++)
00123 fb_pid_controller_[i].reset();
00124
00125
00126 last_time_ = robot_state_->getTime();
00127
00128
00129 twist_desi_ = Twist::Zero();
00130 }
00131
00132
00133
00134 void CartesianTwistController::update()
00135 {
00136
00137 if (!chain_.allCalibrated())
00138 return;
00139
00140
00141 ros::Time time = robot_state_->getTime();
00142 ros::Duration dt = time - last_time_;
00143 last_time_ = time;
00144
00145
00146 chain_.getVelocities(jnt_posvel_);
00147
00148
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
00155 jac_solver_->JntToJac(jnt_posvel_.q, jacobian_);
00156
00157
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
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
00172 chain_.addEfforts(jnt_eff_);
00173 }
00174
00175
00176 void CartesianTwistController::command(const geometry_msgs::TwistConstPtr& twist_msg)
00177 {
00178
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 }