jinv_experimental_controller.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, 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 #include <boost/shared_ptr.hpp>
00036 #include <boost/scoped_ptr.hpp>
00037 
00038 #include <ros/ros.h>
00039 #include <rosrt/rosrt.h>
00040 #include <angles/angles.h>
00041 #include <control_toolbox/pid.h>
00042 #include <eigen_conversions/eigen_kdl.h>
00043 #include <eigen_conversions/eigen_msg.h>
00044 #include <pr2_controller_interface/controller.h>
00045 #include <pr2_mechanism_model/chain.h>
00046 #include <realtime_tools/realtime_publisher.h>
00047 #include <tf/transform_listener.h>
00048 
00049 #include <kdl/chain.hpp>
00050 #include <kdl/chainfksolver.hpp>
00051 #include <kdl/chainjnttojacsolver.hpp>
00052 #include <kdl/chainfksolvervel_recursive.hpp>
00053 
00054 #include <Eigen/Geometry>
00055 #include <Eigen/LU>
00056 #include <Eigen/Core>
00057 
00058 #include <geometry_msgs/TwistStamped.h>
00059 #include <pr2_manipulation_controllers/JinvExperimentalControllerState.h>
00060 
00061 namespace pr2_manipulation_controllers {
00062 
00063 template <int JOINTS>
00064 class JinvExperimentalController : public pr2_controller_interface::Controller
00065 {
00066 public:
00067   // Ensure 128-bit alignment for Eigen
00068   // See also http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
00069   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00070 private:
00071   typedef Eigen::Matrix<double, JOINTS, 1> JointVector;
00072   typedef Eigen::Matrix<double, 6, 1> CartVector;
00073   typedef pr2_manipulation_controllers::JinvExperimentalControllerState StateMsg;
00074 public:
00075 
00076   JinvExperimentalController();
00077 
00078   ~JinvExperimentalController();
00079 
00080   bool init(pr2_mechanism_model::RobotState *r, ros::NodeHandle &n);
00081 
00082   void starting();
00083 
00084   void update();
00085 
00086 private:
00087 
00088   pr2_mechanism_model::RobotState *robot_;
00089 
00090   ros::NodeHandle node_;
00091 
00092   ros::Subscriber sub_command_;
00093   ros::Subscriber sub_command_pose_;
00094   ros::Subscriber sub_posture_;
00095 
00096   realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> pub_x_;
00097   realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> pub_x_desi_;
00098   realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> pub_x_err_;
00099 
00100   rosrt::Publisher<StateMsg> pub_state_;
00101   ros::Publisher pub_transformed_twist_;
00102   ros::Publisher pub_transformed_rot_vel_;
00103 
00104   tf::TransformListener tf_;
00105 
00106   CartVector Kp_x, Kd_x;
00107   JointVector Kp_j, Kd_j, pclamp_j;
00108   JointVector saturation_;
00109   double jacobian_inverse_damping_;
00110   double pose_command_filter_;
00111 
00112   ros::Time last_command_time_;
00113   Eigen::Affine3d x_desi_, x_desi_filtered_;
00114   CartVector xd_desi;
00115   double xd_trans_limit_;
00116   double xd_rot_limit_;
00117   JointVector q_proxy_;
00118 
00119   double k_posture_;
00120   bool use_posture_;
00121   JointVector q_posture_;
00122 
00123   ros::Time last_time_;
00124   int loop_count_;
00125 
00126   std::string root_name_;
00127   pr2_mechanism_model::Chain chain_;
00128   KDL::Chain kdl_chain_;
00129   boost::scoped_ptr<KDL::ChainFkSolverVel> fk_solver_;
00130   boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_;
00131 
00132   // All local variables of update.  Brought outside so they can be recorded
00133   JointVector q, qd;
00134   Eigen::Affine3d x;
00135   CartVector xd;
00136   CartVector x_err;
00137   CartVector xd_ref;
00138   JointVector qd_pose;
00139   JointVector tau;
00140 
00141   void commandPoseCB(const geometry_msgs::PoseStamped::ConstPtr &command)
00142   {
00143     geometry_msgs::PoseStamped in_root;
00144     try {
00145       tf_.waitForTransform(root_name_, command->header.frame_id, command->header.stamp, ros::Duration(0.1));
00146       tf_.transformPose(root_name_, *command, in_root);
00147     }
00148     catch (const tf::TransformException &ex)
00149     {
00150       ROS_ERROR("Failed to transform: %s", ex.what());
00151       return;
00152     }
00153 
00154     tf::poseMsgToEigen(in_root.pose, x_desi_);
00155     xd_desi.setZero();
00156   }
00157 
00158   void commandPostureCB(const std_msgs::Float64MultiArray::ConstPtr &msg)
00159   {
00160     if (msg->data.size() == 0) {
00161       use_posture_ = false;
00162       ROS_INFO("Posture turned off");
00163     }
00164     else if ((int)msg->data.size() != JOINTS) {
00165       ROS_ERROR("Posture message had the wrong size: %d", (int)msg->data.size());
00166       return;
00167     }
00168     else
00169     {
00170       use_posture_ = true;
00171       for (int j = 0; j < JOINTS; ++j)
00172         q_posture_[j] = msg->data[j];
00173     }
00174   }
00175 };
00176 
00177 
00178 template <int JOINTS>
00179 JinvExperimentalController<JOINTS>::JinvExperimentalController()
00180 {
00181   loop_count_ = 0;
00182 }
00183 
00184 template <int JOINTS>
00185 JinvExperimentalController<JOINTS>::~JinvExperimentalController()
00186 {
00187   sub_command_.shutdown();
00188   sub_command_pose_.shutdown();
00189   sub_posture_.shutdown();
00190 }
00191 
00192 template <int JOINTS>
00193 bool JinvExperimentalController<JOINTS>::init(pr2_mechanism_model::RobotState *r, ros::NodeHandle &n)
00194 {
00195   rosrt::init();
00196   node_ = n;
00197   robot_ = r;
00198 
00199   // Initialized the chain
00200   std::string tip_name;
00201   if (!node_.getParam("root_name", root_name_))
00202   {
00203     ROS_ERROR("No \"root_name\" found on parameter server");
00204     return false;
00205   }
00206   if (!node_.getParam("tip_name", tip_name))
00207   {
00208     ROS_ERROR("No \"tip_name\" found on parameter server");
00209     return false;
00210   }
00211   if (!chain_.init(robot_, root_name_, tip_name))
00212     return false;
00213   chain_.toKDL(kdl_chain_);
00214 
00215   // TODO: check chain size again JOINTS
00216 
00217   // Creates the KDL solvers
00218   fk_solver_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_));
00219   jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
00220 
00221   // Cartesian gains
00222   double kp_trans, kd_trans, kp_rot, kd_rot;
00223   if (!node_.getParam("cart_gains/trans/p", kp_trans) ||
00224       !node_.getParam("cart_gains/trans/d", kd_trans))
00225   {
00226     ROS_ERROR("P and D translational gains not specified (namespace: %s)", node_.getNamespace().c_str());
00227     return false;
00228   }
00229   if (!node_.getParam("cart_gains/rot/p", kp_rot) ||
00230       !node_.getParam("cart_gains/rot/d", kd_rot))
00231   {
00232     ROS_ERROR("P and D rotational gains not specified (namespace: %s)", node_.getNamespace().c_str());
00233     return false;
00234   }
00235   Kp_x << kp_trans, kp_trans, kp_trans,  kp_rot, kp_rot, kp_rot;
00236   Kd_x << kd_trans, kd_trans, kd_trans,  kd_rot, kd_rot, kd_rot;
00237 
00238   // Gets the joint gains
00239   for (size_t j = 0; j < JOINTS; ++j)
00240   {
00241     const std::string prefix = std::string("joints/" + chain_.getJoint(j)->joint_->name);
00242     node_.param(prefix + "/p", Kp_j[j], 0.0);
00243     node_.param(prefix + "/d", Kd_j[j], 0.0);
00244     node_.param(prefix + "/p_clamp", pclamp_j[j], 0.0);
00245   }
00246 
00247   node_.param("k_posture", k_posture_, 0.0);
00248   node_.param("jacobian_inverse_damping", jacobian_inverse_damping_, 0.0);
00249   node_.param("pose_command_filter", pose_command_filter_, 1.0);
00250 
00251   // Advertises state topics
00252   pub_x_.init(node_, "state/x", 3);
00253   pub_x_desi_.init(node_, "state/x_desi", 3);
00254   pub_x_err_.init(node_, "state/x_error", 3);
00255   pub_transformed_twist_ = node_.advertise<geometry_msgs::TwistStamped>("state/transformed_twist", 3);
00256   pub_transformed_rot_vel_ = node_.advertise<geometry_msgs::Vector3Stamped>("state/transformed_rot_vel", 3);
00257 
00258   // Subscribe to desired posture
00259   //sub_command_ = node_.subscribe("command", 1, &JinvExperimentalController<JOINTS>::commandCB, this);
00260   sub_command_pose_ = node_.subscribe("command_pose", 20, &JinvExperimentalController<JOINTS>::commandPoseCB, this);
00261   sub_posture_ = node_.subscribe("command_posture", 3, &JinvExperimentalController<JOINTS>::commandPostureCB, this);
00262 
00263   StateMsg state_template;
00264   state_template.header.frame_id = root_name_;
00265   state_template.x.header.frame_id = root_name_;
00266   state_template.x_desi.header.frame_id = root_name_;
00267   state_template.x_desi_filtered.header.frame_id = root_name_;
00268   state_template.q_proxy.resize(JOINTS);
00269   state_template.qd_pose.resize(JOINTS);
00270   state_template.qd_posture.resize(JOINTS);
00271   state_template.qd_posture_raw.resize(JOINTS);
00272   state_template.qd_desi.resize(JOINTS);
00273   state_template.tau.resize(JOINTS);
00274   state_template.J.layout.dim.resize(2);
00275   state_template.J.data.resize(6*JOINTS);
00276   state_template.N.layout.dim.resize(2);
00277   state_template.N.data.resize(JOINTS*JOINTS);
00278 
00279   state_template.J_singular_values.resize(6);
00280 
00281   pub_state_.initialize(node_.advertise<StateMsg>("state", 20), 20, state_template);
00282 
00283   ROS_INFO(" --------- EXPERIMENTAL CONTROLLER INITIALIZED ----------------------");
00284 
00285   return true;
00286 }
00287 
00288 
00289 static void computePoseError(const Eigen::Affine3d &xact, const Eigen::Affine3d &xdes, Eigen::Matrix<double,6,1> &err)
00290 {
00291   err.head<3>() = xact.translation() - xdes.translation();
00292   err.tail<3>()   = 0.5 * (xdes.linear().col(0).cross(xact.linear().col(0)) +
00293                           xdes.linear().col(1).cross(xact.linear().col(1)) +
00294                           xdes.linear().col(2).cross(xact.linear().col(2)));
00295 }
00296 
00297 template <int JOINTS>
00298 void JinvExperimentalController<JOINTS>::starting()
00299 {
00300   last_time_ = robot_->getTime();
00301 
00302   KDL::JntArrayVel q_qd(JOINTS);
00303   chain_.getVelocities(q_qd);
00304   JointVector q(q_qd.q.data), qd(q_qd.qdot.data);
00305 
00306   // Forward kinematics to get current x
00307   Eigen::Affine3d x;
00308   KDL::FrameVel x_framevel;
00309   fk_solver_->JntToCart(q_qd, x_framevel);
00310   tf::transformKDLToEigen(x_framevel.GetFrame(), x);
00311   q_proxy_ = q;
00312 
00313   x_desi_ = x;
00314   x_desi_filtered_ = x;
00315   xd_desi.setZero();
00316   use_posture_ = false;
00317 }
00318 
00319 template <class Derived>
00320 Eigen::Matrix<double,3,3> eulerRot(double dt, const Eigen::MatrixBase<Derived>& e)
00321 {
00322   double n = e.norm();
00323   if (fabs(n) < 1e-8)
00324     return Eigen::Matrix<double,3,3>::Identity();
00325   return Eigen::AngleAxis<double>(n * dt, e.normalized()).toRotationMatrix();
00326 }
00327 
00328 template <int JOINTS>
00329 void JinvExperimentalController<JOINTS>::update()
00330 {
00331   ros::Time time = robot_->getTime();
00332   ros::Duration dt = time - last_time_;
00333 
00334   KDL::JntArrayVel q_qd(JOINTS);
00335   chain_.getVelocities(q_qd);
00336   q = q_qd.q.data;
00337   qd = q_qd.qdot.data;
00338 
00339   KDL::Jacobian kdl_jacobian(JOINTS);
00340   jac_solver_->JntToJac(q_qd.q, kdl_jacobian);
00341   Eigen::Matrix<double, 6, JOINTS> J(kdl_jacobian.data);
00342 
00343   Eigen::JacobiSVD<Eigen::MatrixXd> svd(J);
00344 
00345   // Forward kinematics to get x and xd
00346   KDL::FrameVel x_framevel;
00347   fk_solver_->JntToCart(q_qd, x_framevel);
00348   tf::transformKDLToEigen(x_framevel.GetFrame(), x);
00349   tf::twistKDLToEigen(x_framevel.GetTwist(), xd);
00350 
00351   {
00352     // Filters the desired pose
00353     Eigen::Vector3d p0(x_desi_filtered_.translation());
00354     Eigen::Vector3d p1(x_desi_.translation());
00355     Eigen::Quaterniond q0(x_desi_filtered_.linear());
00356     Eigen::Quaterniond q1(x_desi_.linear());
00357     q0.normalize();
00358     q1.normalize();
00359     
00360     tf::Quaternion tf_q0(q0.x(), q0.y(), q0.z(), q0.w());
00361     tf::Quaternion tf_q1(q1.x(), q1.y(), q1.z(), q1.w());
00362     tf::Quaternion tf_q = tf_q0.slerp(tf_q1, pose_command_filter_);
00363     
00364     Eigen::Vector3d p = p0 + pose_command_filter_ * (p1 - p0);
00365     Eigen::Quaterniond qu(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z());
00366     x_desi_filtered_ = Eigen::Translation3d(p) * qu;
00367   }
00368     
00369   computePoseError(x, x_desi_filtered_, x_err);
00370 
00371   // Computes the reference twist from the pose error and the desired twist.
00372   xd_ref = xd_desi.array() - (Kp_x.array() / Kd_x.array()) * x_err.array();
00373 
00374   // Applies velocity limits
00375   // TODO
00376 
00377   // ======== J psuedo-inverse and Nullspace computation
00378 
00379   // Computes pseudo-inverse of J
00380 
00381   Eigen::Matrix<double,6,6> I6; I6.setIdentity();
00382   Eigen::Matrix<double,6,6> JJt = J * J.transpose();
00383   Eigen::Matrix<double,6,6> JJt_inv;
00384   JJt_inv = JJt.inverse();
00385   Eigen::Matrix<double,JOINTS,6> J_pinv = J.transpose() * JJt_inv;
00386   Eigen::Matrix<double,6,6> JJt_damped = J * J.transpose() + jacobian_inverse_damping_ * I6;
00387   Eigen::Matrix<double,6,6> JJt_inv_damped;
00388   JJt_inv_damped = JJt_damped.inverse();
00389   Eigen::Matrix<double,JOINTS,6> J_pinv_damped = J.transpose() * JJt_inv_damped;
00390 
00391   // Computes the nullspace of J
00392   Eigen::Matrix<double,JOINTS,JOINTS> I;
00393   I.setIdentity();
00394   Eigen::Matrix<double,JOINTS,JOINTS> N = I - J_pinv * J;
00395 
00396   // ======== Pose Control
00397   
00398   // Computes the desired joint velocities for achieving the pose.
00399   qd_pose = J_pinv_damped * xd_ref;  // J-inverse
00400   JointVector qd_desi = qd_pose;
00401 
00402   // ======== Posture control
00403 
00404   // Computes the desired joint velocities for achieving the posture
00405   JointVector qd_posture;
00406   JointVector qd_posture_raw;
00407   if (use_posture_)
00408   {
00409     JointVector posture_err = q_posture_ - q;
00410     for (size_t j = 0; j < JOINTS; ++j)
00411     {
00412       if (chain_.getJoint(j)->joint_->type == urdf::Joint::CONTINUOUS)
00413         posture_err[j] = angles::normalize_angle(posture_err[j]);
00414     }
00415 
00416     for (size_t j = 0; j < JOINTS; ++j) {
00417       if (fabs(q_posture_[j] - 9999) < 1e-5)
00418         posture_err[j] = 0.0;
00419     }
00420 
00421     qd_posture_raw = k_posture_ * posture_err;
00422     qd_posture = N * qd_posture_raw;
00423     qd_desi += qd_posture;
00424   }
00425 
00426   // Clamps the proxy
00427   // aleeper:
00428   // Doesn't do anything because q_proxy_ and q should never differ
00429   for (int j = 0; j < JOINTS; ++j)
00430     q_proxy_[j] = std::max(q[j] - pclamp_j[j], std::min(q_proxy_[j], q[j] + pclamp_j[j]));
00431 
00432   // Convertes the desired joint velocities into joint torques
00433   tau = Kp_j.array() * (q_proxy_ - q).array() + Kd_j.array() * (qd_desi - qd).array();
00434   // aleeper: changed to qd_desi from qd... is this right?
00435   q_proxy_ += qd_desi * dt.toSec();
00436   // q_proxy_ += qd * dt.toSec();
00437 
00438   // ======== Torque Saturation
00439   // Copied from JT controller ... do we want this here, or does it even make sense?
00440   /*
00441   double sat_scaling = 1.0;
00442   for (int i = 0; i < Joints; ++i) {
00443     if (saturation_[i] > 0.0)
00444       sat_scaling = std::min(sat_scaling, fabs(saturation_[i] / tau[i]));
00445   }
00446   JointVec tau_sat = sat_scaling * tau;
00447   */
00448 
00449   // ======== Apply torques to arm
00450   chain_.addEfforts(tau);
00451 
00452   // ================ END OF CONTROLLER ================
00453    
00454   // ======== Publish state
00455   if (loop_count_ % 10 == 0)
00456   {
00457     if (pub_x_.trylock())
00458     {
00459       pub_x_.msg_.header.stamp = time;
00460       pub_x_.msg_.header.frame_id = root_name_;  // NOT RT SAFE
00461       tf::poseEigenToMsg(x, pub_x_.msg_.pose);
00462       pub_x_.unlockAndPublish();
00463     }
00464 
00465     if (pub_x_desi_.trylock())
00466     {
00467       pub_x_desi_.msg_.header.stamp = time;
00468       pub_x_desi_.msg_.header.frame_id = root_name_;  // NOT RT SAFE
00469       tf::poseEigenToMsg(x_desi_, pub_x_desi_.msg_.pose);
00470       pub_x_desi_.unlockAndPublish();
00471     }
00472     if (pub_x_err_.trylock())
00473     {
00474       pub_x_err_.msg_.header.stamp = time;
00475       tf::twistEigenToMsg(x_err, pub_x_err_.msg_.twist);
00476       pub_x_err_.unlockAndPublish();
00477     }
00478 
00479     StateMsg::Ptr state_msg;
00480     if (state_msg = pub_state_.allocate()) {
00481       state_msg->header.stamp = time;
00482       state_msg->x.header.stamp = time;
00483       tf::poseEigenToMsg(x, state_msg->x.pose);
00484       state_msg->x_desi.header.stamp = time;
00485       tf::poseEigenToMsg(x_desi_, state_msg->x_desi.pose);
00486       state_msg->x_desi_filtered.header.stamp = time;
00487       tf::poseEigenToMsg(x_desi_filtered_, state_msg->x_desi_filtered.pose);
00488       tf::twistEigenToMsg(x_err, state_msg->x_err);
00489       tf::twistEigenToMsg(xd, state_msg->xd);
00490       tf::twistEigenToMsg(xd_desi, state_msg->xd_desi);
00491       tf::matrixEigenToMsg(J, state_msg->J);
00492       tf::matrixEigenToMsg(N, state_msg->N);
00493       for (size_t j = 0; j < JOINTS; ++j) {
00494         state_msg->q_proxy[j] = q_proxy_[j];
00495         state_msg->qd_pose[j] = qd_pose[j];
00496         state_msg->qd_posture[j] = qd_posture[j];
00497         state_msg->qd_posture_raw[j] = qd_posture_raw[j];
00498         state_msg->qd_desi[j] = qd_desi[j];
00499         state_msg->tau[j] = tau[j];
00500       }
00501       for (size_t j=0; j<6; j++)
00502         {
00503           state_msg->J_singular_values[j] = svd.singularValues()[j];
00504         }
00505       
00506       pub_state_.publish(state_msg);
00507     }
00508   }
00509 
00510   last_time_ = time;
00511   ++loop_count_;
00512 }
00513 
00514 } // namespace
00515 
00516 #include <pluginlib/class_list_macros.h>
00517 PLUGINLIB_EXPORT_CLASS(pr2_manipulation_controllers::JinvExperimentalController<7>,pr2_controller_interface::Controller)
00518 


pr2_manipulation_controllers
Author(s): Kaijen Hsiao, Stu Glaser, Adam Leeper
autogenerated on Mon Oct 6 2014 12:10:46