jt_cartesian_controller.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
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: Stuart Glaser
00036 
00037 #ifndef PR2_CONTROLLERS_JT_CARTESIAN_CONTROLLER_H
00038 #define PR2_CONTROLLERS_JT_CARTESIAN_CONTROLLER_H
00039 
00040 #include <Eigen/Core>
00041 #include <Eigen/Geometry>
00042 
00043 #include <ros/ros.h>
00044 
00045 #include <control_toolbox/pid.h>
00046 #include <eigen_conversions/eigen_kdl.h>
00047 #include <eigen_conversions/eigen_msg.h>
00048 #include <kdl/chainfksolver.hpp>
00049 #include <kdl/chainfksolverpos_recursive.hpp>
00050 #include <kdl/chain.hpp>
00051 #include <kdl/chainjnttojacsolver.hpp>
00052 #include <kdl/frames.hpp>
00053 #include <pr2_controller_interface/controller.h>
00054 #include <pr2_mechanism_model/chain.h>
00055 #include <realtime_tools/realtime_publisher.h>
00056 #include <tf/transform_listener.h>
00057 
00058 #include <geometry_msgs/PoseStamped.h>
00059 #include <geometry_msgs/TwistStamped.h>
00060 #include <robot_mechanism_controllers/JTCartesianControllerState.h>
00061 
00062 
00063 namespace controller {
00064 
00065 template <int Joints>
00066 struct Kin
00067 {
00068   typedef Eigen::Matrix<double, Joints, 1> JointVec;
00069   typedef Eigen::Matrix<double, 6, Joints> Jacobian;
00070 
00071   Kin(const KDL::Chain &kdl_chain) :
00072     fk_solver_(kdl_chain), jac_solver_(kdl_chain),
00073     kdl_q(Joints), kdl_J(Joints)
00074   {
00075   }
00076   ~Kin()
00077   {
00078   }
00079 
00080   void fk(const JointVec &q, Eigen::Affine3d &x)
00081   {
00082     kdl_q.data = q;
00083     KDL::Frame kdl_x;
00084     fk_solver_.JntToCart(kdl_q, kdl_x);
00085     tf::transformKDLToEigen(kdl_x, x);
00086   }
00087   void jac(const JointVec &q, Jacobian &J)
00088   {
00089     kdl_q.data = q;
00090     jac_solver_.JntToJac(kdl_q, kdl_J);
00091     J = kdl_J.data;
00092   }
00093 
00094   KDL::ChainFkSolverPos_recursive fk_solver_;
00095   KDL::ChainJntToJacSolver jac_solver_;
00096   KDL::JntArray kdl_q;
00097   KDL::Jacobian kdl_J;
00098 };
00099 
00100 class JTCartesianController : public pr2_controller_interface::Controller
00101 {
00102 public:
00103   // Ensure 128-bit alignment for Eigen
00104   // See also http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
00105   EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00106 private:
00107   enum { Joints = 7 };
00108   typedef Eigen::Matrix<double, Joints, 1> JointVec;
00109   typedef Eigen::Matrix<double, 6, 1> CartVec;
00110   typedef Eigen::Matrix<double, 6, Joints> Jacobian;
00111   typedef robot_mechanism_controllers::JTCartesianControllerState StateMsg;
00112 public:
00113   JTCartesianController();
00114   ~JTCartesianController();
00115 
00116   bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
00117   void starting();
00118   void update();
00119 
00120   Eigen::Affine3d x_desi_, x_desi_filtered_;
00121   CartVec wrench_desi_;
00122 
00123   ros::NodeHandle node_;
00124   ros::Subscriber sub_gains_;
00125   ros::Subscriber sub_posture_;
00126   ros::Subscriber sub_pose_;
00127   tf::TransformListener tf_;
00128 
00129   realtime_tools::RealtimePublisher<StateMsg> pub_state_;
00130   realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> pub_x_desi_;
00131 
00132   std::string root_name_, tip_name_;
00133   ros::Time last_time_;
00134   int loop_count_;
00135   pr2_mechanism_model::RobotState *robot_state_;
00136 
00137   pr2_mechanism_model::Chain chain_;
00138   boost::scoped_ptr<Kin<Joints> > kin_;
00139   Eigen::Matrix<double,6,1> Kp, Kd;
00140   double pose_command_filter_;
00141   double vel_saturation_trans_, vel_saturation_rot_;
00142   JointVec saturation_;
00143   JointVec joint_dd_ff_;
00144   double joint_vel_filter_;
00145   double jacobian_inverse_damping_;
00146   JointVec q_posture_;
00147   double k_posture_;
00148   bool use_posture_;
00149 
00150   // Minimum resolutions
00151   double res_force_, res_position_;
00152   double res_torque_, res_orientation_;
00153 
00154   Eigen::Affine3d last_pose_;
00155   CartVec last_wrench_;
00156 
00157   JointVec qdot_filtered_;
00158 
00159   void setGains(const std_msgs::Float64MultiArray::ConstPtr &msg)
00160   {
00161     if (msg->data.size() >= 6)
00162       for (size_t i = 0; i < 6; ++i)
00163         Kp[i] = msg->data[i];
00164     if (msg->data.size() == 12)
00165       for (size_t i = 0; i < 6; ++i)
00166         Kd[i] = msg->data[6+i];
00167 
00168     ROS_INFO("New gains: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
00169              Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
00170   }
00171 
00172   void commandPosture(const std_msgs::Float64MultiArray::ConstPtr &msg)
00173   {
00174     if (msg->data.size() == 0) {
00175       use_posture_ = false;
00176       ROS_INFO("Posture turned off");
00177     }
00178     else if ((int)msg->data.size() != q_posture_.size()) {
00179       ROS_ERROR("Posture message had the wrong size: %d", (int)msg->data.size());
00180       return;
00181     }
00182     else
00183     {
00184       use_posture_ = true;
00185       for (int j = 0; j < Joints; ++j)
00186         q_posture_[j] = msg->data[j];
00187     }
00188   }
00189 
00190   void commandPose(const geometry_msgs::PoseStamped::ConstPtr &command)
00191   {
00192     geometry_msgs::PoseStamped in_root;
00193     try {
00194       tf_.waitForTransform(root_name_, command->header.frame_id, command->header.stamp, ros::Duration(0.1));
00195       tf_.transformPose(root_name_, *command, in_root);
00196     }
00197     catch (const tf::TransformException &ex)
00198     {
00199       ROS_ERROR("Failed to transform: %s", ex.what());
00200       return;
00201     }
00202 
00203     tf::poseMsgToEigen(in_root.pose, x_desi_);
00204   }
00205 };
00206 
00207 } // namespace
00208 
00209 #endif


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