virtual_force_publisher.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2012, JSK, The University of Tokyo.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Kei Okada */
00036 
00037 #include <boost/scoped_ptr.hpp>
00038 #include <ros/ros.h>
00039 #include <urdf/model.h>
00040 #include <kdl/tree.hpp>
00041 #include <kdl/chainjnttojacsolver.hpp>
00042 #include <kdl_parser/kdl_parser.hpp>
00043 #include <tf_conversions/tf_kdl.h>
00044 #include <geometry_msgs/WrenchStamped.h>
00045 #include <sensor_msgs/JointState.h>
00046 #include <tf/transform_listener.h>
00047 #include <Eigen/Core>
00048 #include <Eigen/Geometry>
00049 
00050 typedef boost::shared_ptr<sensor_msgs::JointState const> JointStateConstPtr;
00051 
00052 namespace virtual_force_publisher{
00053 
00054     class VirtualForcePublisher
00055     {
00056     private:
00057         KDL::JntArray jnt_pos_;
00058         KDL::Jacobian jacobian_;
00059         KDL::Chain chain_;
00060         boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
00061         ros::Subscriber joint_state_sub_;
00062         ros::Publisher  wrench_stamped_pub_;
00063         std::string root, tip;
00064 
00065         ros::Duration publish_interval_;
00066         std::map<std::string, ros::Time> last_publish_time_;
00067         double t_const_;
00068         KDL::Wrench F_pre_;
00069         tf::TransformListener listener_;
00070     public:
00071 
00072         VirtualForcePublisher()
00073         {
00074             ros::NodeHandle n_tilde("~");
00075             ros::NodeHandle n;
00076 
00077             // subscribe to joint state
00078             joint_state_sub_ = n.subscribe("joint_states", 1, &VirtualForcePublisher::callbackJointState, this);
00079             wrench_stamped_pub_ = n.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
00080 
00081             // set publish frequency
00082             double publish_freq;
00083             n_tilde.param("publish_frequency", publish_freq, 50.0);
00084             publish_interval_ = ros::Duration(1.0/std::max(publish_freq,1.0));
00085 
00086             //set time constant of low pass filter
00087             n_tilde.param("time_constant", t_const_, 0.3);
00088             // set root and tip
00089             n_tilde.param("root", root, std::string("torso_lift_link"));
00090             n_tilde.param("tip", tip, std::string("l_gripper_tool_frame"));
00091             
00092             // load robot model
00093             urdf::Model robot_model;
00094             robot_model.initParam("robot_description");
00095 
00096             KDL::Tree kdl_tree;
00097             kdl_parser::treeFromUrdfModel(robot_model, kdl_tree);
00098             kdl_tree.getChain(root, tip, chain_);
00099             jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
00100 
00101             jnt_pos_.resize(chain_.getNrOfJoints());
00102             jacobian_.resize(chain_.getNrOfJoints());
00103 
00104             for (size_t i=0; i<chain_.getNrOfSegments(); i++){
00105                 if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None){
00106                     std::cerr << "kdl_chain(" << i << ") " << chain_.getSegment(i).getJoint().getName().c_str() << std::endl;
00107                 }
00108             }
00109         }
00110         ~VirtualForcePublisher() { }
00111 
00112         void callbackJointState(const JointStateConstPtr& state)
00113         {
00114             std::map<std::string, double> joint_name_position;
00115             if (state->name.size() != state->position.size()){
00116                 ROS_ERROR("Robot state publisher received an invalid joint state vector");
00117                 return;
00118             }
00119 
00120             // determine least recently published joint
00121             ros::Time last_published = ros::Time::now();
00122             for (unsigned int i=0; i<state->name.size(); i++) {
00123                 ros::Time t = last_publish_time_[state->name[i]];
00124                 last_published = (t < last_published) ? t : last_published;
00125             }
00126 
00127             if (state->header.stamp >= last_published + publish_interval_){
00128                 // get joint positions from state message
00129                 std::map<std::string, double> joint_positions;
00130                 std::map<std::string, double> joint_efforts;
00131                 for (unsigned int i=0; i<state->name.size(); i++) {
00132                     joint_positions.insert(make_pair(state->name[i], state->position[i]));
00133                     joint_efforts.insert(make_pair(state->name[i], state->effort[i]));
00134                 }
00135 
00136                 KDL::JntArray  tau;
00137                 KDL::Wrench    F;
00138                 Eigen::Matrix<double,Eigen::Dynamic,6> jac_t;
00139                 Eigen::Matrix<double,6,Eigen::Dynamic> jac_t_pseudo_inv;
00140                 tf::StampedTransform transform;
00141                 KDL::Wrench    F_pub;
00142                 tf::Vector3 tf_force;
00143                 tf::Vector3 tf_torque;
00144 
00145                 tau.resize(chain_.getNrOfJoints());
00146 
00147                 //getPositions;
00148                 for (size_t i=0, j=0; i<chain_.getNrOfSegments(); i++){
00149                     if (chain_.getSegment(i).getJoint().getType() == KDL::Joint::None)
00150                         continue;
00151 
00152                     // check
00153                     std::string joint_name = chain_.getSegment(i).getJoint().getName();
00154                     if ( joint_positions.find(joint_name) == joint_positions.end() ) {
00155                         ROS_ERROR("Joint '%s' is not found in joint state vector", joint_name.c_str());
00156                     }
00157 
00158                     // set position
00159                     jnt_pos_(j) = joint_positions[joint_name];
00160                     tau(j) = joint_efforts[joint_name];
00161                     j++;
00162                 }
00163                 
00164                 jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
00165                 jac_t = jacobian_.data.transpose();
00166                 jac_t_pseudo_inv =(jac_t.transpose() * jac_t).inverse() *  jac_t.transpose();
00167                 // f = - inv(jt) * effort
00168                 for (unsigned int j=0; j<6; j++)
00169                     {
00170                         F(j) = 0;
00171                         for (unsigned int i = 0; i < jnt_pos_.rows(); i++)
00172                             {
00173                                 F(j) += -1 * jac_t_pseudo_inv(j, i) * tau(i);
00174                             }
00175                     }
00176 
00177                 //low pass filter
00178                 F += (F_pre_ - F) * exp(-1.0 / t_const_);
00179                 for (unsigned int j=0; j<6; j++){
00180                   F_pre_(j) = 0;
00181                 }
00182                 F_pre_ += F;
00183 
00184                 //tf transformation
00185                 tf::vectorKDLToTF(F.force, tf_force);
00186                 tf::vectorKDLToTF(F.torque, tf_torque);
00187                 try{
00188                   listener_.waitForTransform( tip, root, state->header.stamp, ros::Duration(1.0));
00189                   listener_.lookupTransform( tip, root, state->header.stamp , transform);
00190                 }   
00191                 catch (tf::TransformException ex){
00192                   ROS_ERROR("%s",ex.what());
00193                   ros::Duration(1.0).sleep();
00194                 }
00195                 for (unsigned int j=0; j<3; j++){
00196                   F_pub.force[j] = transform.getBasis()[j].dot(tf_force);
00197                   F_pub.torque[j] = transform.getBasis()[j].dot(tf_torque);
00198                 }
00199 
00200                 geometry_msgs::WrenchStamped msg;
00201                 msg.header.stamp = state->header.stamp;
00202                 msg.header.frame_id = tip;
00203                 // http://code.ros.org/lurker/message/20110107.151127.7177af06.nl.html
00204                 //tf::WrenchKDLToMsg(F,msg.wrench);
00205                 msg.wrench.force.x = F_pub.force[0];
00206                 msg.wrench.force.y = F_pub.force[1];
00207                 msg.wrench.force.z = F_pub.force[2];
00208                 msg.wrench.torque.x = F_pub.torque[0];
00209                 msg.wrench.torque.y = F_pub.torque[1];
00210                 msg.wrench.torque.z = F_pub.torque[2];
00211                 wrench_stamped_pub_.publish(msg);
00212 
00213                 {
00214                     ROS_INFO("jacobian : ");
00215                     for (unsigned int i = 0; i < jnt_pos_.rows(); i++) {
00216                         std::stringstream ss;
00217                         for (unsigned int j=0; j<6; j++)
00218                             ss << jacobian_(j,i) << " ";
00219                         ROS_INFO_STREAM(ss.str());
00220                     }
00221                     ROS_INFO("effort : ");
00222                     std::stringstream sstau;
00223                     for (unsigned int i = 0; i < tau.rows(); i++) {
00224                         sstau << tau(i) << " ";
00225                     }
00226                     ROS_INFO_STREAM(sstau.str());
00227                     ROS_INFO("force : ");
00228                     std::stringstream ssf;
00229                     for (unsigned int j = 0; j < 6; j++) {
00230                         ssf << F(j) << " ";
00231                     }
00232                     ROS_INFO_STREAM(ssf.str());
00233                 }
00234 
00235                 // store publish time in joint map
00236                 for (unsigned int i=0; i<state->name.size(); i++)
00237                     last_publish_time_[state->name[i]] = state->header.stamp;
00238             }
00239         }
00240     };
00241 };
00242 
00243 using namespace virtual_force_publisher;
00244 
00245 int main (int argc, char ** argv) {
00246 
00247     // Initialize ros
00248     ros::init(argc, argv, "virtual_force_publisher");
00249 
00250     VirtualForcePublisher virtual_force_publisher;
00251 
00252     ros::spin();
00253 
00254     return 0;
00255 }


virtual_force_publisher
Author(s): Kei Okada
autogenerated on Sun Jan 25 2015 12:37:51