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_wrench_controller.h"
00035 #include <algorithm>
00036 #include "pluginlib/class_list_macros.h"
00037
00038
00039 using namespace KDL;
00040
00041 PLUGINLIB_EXPORT_CLASS( controller::CartesianWrenchController, pr2_controller_interface::Controller)
00042
00043 namespace controller {
00044
00045 CartesianWrenchController::CartesianWrenchController()
00046 : robot_state_(NULL),
00047 jnt_to_jac_solver_(NULL)
00048 {}
00049
00050
00051
00052 CartesianWrenchController::~CartesianWrenchController()
00053 {
00054 sub_command_.shutdown();
00055 }
00056
00057
00058 bool CartesianWrenchController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
00059 {
00060
00061 assert(robot);
00062 robot_state_ = robot;
00063
00064 node_ = n;
00065
00066
00067 std::string root_name, tip_name;
00068 if (!node_.getParam("root_name", root_name)){
00069 ROS_ERROR("CartesianWrenchController: No root name found on parameter server (namespace: %s)",
00070 node_.getNamespace().c_str());
00071 return false;
00072 }
00073 if (!node_.getParam("tip_name", tip_name)){
00074 ROS_ERROR("CartesianWrenchController: No tip name found on parameter server (namespace: %s)",
00075 node_.getNamespace().c_str());
00076 return false;
00077 }
00078
00079
00080 if (!chain_.init(robot_state_, root_name, tip_name)){
00081 ROS_ERROR("Initializing chain from %s to %s failed", root_name.c_str(), tip_name.c_str());
00082 return false;
00083 }
00084 if (!chain_.allCalibrated())
00085 {
00086 ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
00087 return false;
00088 }
00089 chain_.toKDL(kdl_chain_);
00090
00091
00092 jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
00093 jnt_pos_.resize(kdl_chain_.getNrOfJoints());
00094 jnt_eff_.resize(kdl_chain_.getNrOfJoints());
00095 jacobian_.resize(kdl_chain_.getNrOfJoints());
00096
00097
00098
00099 sub_command_ = node_.subscribe<geometry_msgs::Wrench>
00100 ("command", 1, &CartesianWrenchController::command, this);
00101
00102 return true;
00103 }
00104
00105 void CartesianWrenchController::starting()
00106 {
00107
00108 wrench_desi_ = Wrench::Zero();
00109 }
00110
00111
00112
00113 void CartesianWrenchController::update()
00114 {
00115
00116 if (!chain_.allCalibrated()){
00117 return;
00118 }
00119
00120
00121 chain_.getPositions(jnt_pos_);
00122
00123
00124 jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
00125
00126
00127 for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++){
00128 jnt_eff_(i) = 0;
00129 for (unsigned int j=0; j<6; j++)
00130 jnt_eff_(i) += (jacobian_(j,i) * wrench_desi_(j));
00131 }
00132
00133
00134 chain_.setEfforts(jnt_eff_);
00135 }
00136
00137
00138
00139 void CartesianWrenchController::command(const geometry_msgs::WrenchConstPtr& wrench_msg)
00140 {
00141
00142 wrench_desi_.force(0) = wrench_msg->force.x;
00143 wrench_desi_.force(1) = wrench_msg->force.y;
00144 wrench_desi_.force(2) = wrench_msg->force.z;
00145 wrench_desi_.torque(0) = wrench_msg->torque.x;
00146 wrench_desi_.torque(1) = wrench_msg->torque.y;
00147 wrench_desi_.torque(2) = wrench_msg->torque.z;
00148 }
00149
00150 };