cartesian_wrench_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_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   // test if we got robot pointer
00061   assert(robot);
00062   robot_state_ = robot;
00063 
00064   node_ = n;
00065 
00066   // get name of root and tip from the parameter server
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   // create robot chain from root to tip
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   // create solver
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   // subscribe to wrench commands
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   // set desired wrench to 0
00108   wrench_desi_ = Wrench::Zero();
00109 }
00110 
00111 
00112 
00113 void CartesianWrenchController::update()
00114 {
00115   // check if joints are calibrated
00116   if (!chain_.allCalibrated()){
00117     return;
00118   }
00119 
00120   // get joint positions
00121   chain_.getPositions(jnt_pos_);
00122 
00123   // get the chain jacobian
00124   jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
00125 
00126   // convert the wrench into joint efforts
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   // set effort to joints
00134   chain_.setEfforts(jnt_eff_);
00135 }
00136 
00137 
00138 
00139 void CartesianWrenchController::command(const geometry_msgs::WrenchConstPtr& wrench_msg)
00140 {
00141   // convert to wrench command
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 }; // namespace


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