jinv_teleop_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/JinvTeleopControllerState.h>
00060 
00061 namespace pr2_manipulation_controllers {
00062 
00063 template <int JOINTS>
00064 class JinvTeleopController : 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::JinvTeleopControllerState StateMsg;
00074 public:
00075 
00076   JinvTeleopController();
00077 
00078   ~JinvTeleopController();
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   enum {POSE, TWIST} mode_;
00091 
00092   ros::NodeHandle node_;
00093   ros::Subscriber sub_command_;
00094   ros::Subscriber sub_command_pose_;
00095   ros::Subscriber sub_twist_;
00096   ros::Subscriber sub_posture_;
00097   realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> pub_x_;
00098   realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> pub_x_desi_;
00099   realtime_tools::RealtimePublisher<geometry_msgs::TwistStamped> pub_x_err_;
00100   rosrt::Publisher<StateMsg> pub_state_;
00101   ros::Publisher pub_transformed_twist_;
00102   ros::Publisher pub_transformed_rot_vel_;
00103   tf::TransformListener tf_;
00104 
00105   CartVector Kp_x, Kd_x;
00106   JointVector Kp_j, Kd_j, pclamp_j;
00107   JointVector saturation_;
00108   double jacobian_inverse_damping_;
00109   double pose_command_filter_;
00110 
00111   ros::Time last_command_time_;
00112   Eigen::Affine3d x_desi_, x_desi_filtered_;
00113   CartVector xd_desi;
00114   double xd_trans_limit_;
00115   double xd_rot_limit_;
00116   JointVector q_proxy_;
00117 
00118   double k_posture_;
00119   bool use_posture_;
00120   JointVector q_posture_;
00121 
00122   ros::Time last_time_;
00123   int loop_count_;
00124 
00125   std::string root_name_;
00126   pr2_mechanism_model::Chain chain_;
00127   KDL::Chain kdl_chain_;
00128   boost::scoped_ptr<KDL::ChainFkSolverVel> fk_solver_;
00129   boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_;
00130 
00131   // All local variables of update.  Brought outside so they can be recorded
00132   JointVector q, qd;
00133   Eigen::Affine3d x;
00134   CartVector xd;
00135   CartVector x_err;
00136   CartVector xd_ref;
00137   JointVector qd_pose;
00138   JointVector tau;
00139 
00140   /*
00141   void commandCB(const simple_cartesian_controller::CartesianCommand::ConstPtr &command)
00142   {
00143     ROS_ERROR("COMMAND");
00144     // TODO: TF stuff
00145 
00146     tf::poseMsgToEigen(command->pose, x_desi_);
00147     tf::twistMsgToEigen(command->twist, xd_desi);
00148     xd_trans_limit_ = command->max_trans_vel;
00149     xd_rot_limit_ = command->max_rot_vel;
00150     mode_ = POSE;
00151   }
00152   */
00153 
00154   void commandPoseCB(const geometry_msgs::PoseStamped::ConstPtr &command)
00155   {
00156     geometry_msgs::PoseStamped in_root;
00157     try {
00158       tf_.waitForTransform(root_name_, command->header.frame_id, command->header.stamp, ros::Duration(0.1));
00159       tf_.transformPose(root_name_, *command, in_root);
00160     }
00161     catch (const tf::TransformException &ex)
00162     {
00163       ROS_ERROR("Failed to transform: %s", ex.what());
00164       return;
00165     }
00166 
00167     tf::poseMsgToEigen(in_root.pose, x_desi_);
00168     xd_desi.setZero();
00169   }
00170 
00171   void commandTwistCB(const geometry_msgs::TwistStamped::ConstPtr &command)
00172   {
00173     tf::StampedTransform transform;
00174     try {
00175       tf_.waitForTransform(root_name_, command->header.frame_id, command->header.stamp,
00176                            ros::Duration(0.1));
00177       tf_.lookupTransform(root_name_, command->header.frame_id, command->header.stamp, transform);
00178     }
00179     catch (const tf::TransformException &ex)
00180     {
00181       ROS_ERROR("Failed to transform: %s", ex.what());
00182       return;
00183     }
00184 
00185     tf::Vector3 vel(command->twist.linear.x, command->twist.linear.y, command->twist.linear.z);
00186     tf::Vector3 rot_vel(command->twist.angular.x, command->twist.angular.y, command->twist.angular.z);
00187 
00188     //vel = transform.getBasis().inverse() * vel;
00189     //rot_vel = transform.getBasis().inverse() * rot_vel;
00190     vel = transform.getBasis() * vel;
00191     rot_vel = transform.getBasis() * rot_vel;
00192 
00193     xd_desi[0] = vel[0];
00194     xd_desi[1] = vel[1];
00195     xd_desi[2] = vel[2];
00196     xd_desi[3] = rot_vel[0];
00197     xd_desi[4] = rot_vel[1];
00198     xd_desi[5] = rot_vel[2];
00199 
00200     //ROS_INFO("Transformed twist: %.2f, %.2f, %.2f,  %.2f, %.2f, %.2f",
00201     //         xd_desi[0], xd_desi[1], xd_desi[2], xd_desi[3], xd_desi[4], xd_desi[5]);
00202 
00203     xd_trans_limit_ = 0.0;
00204     xd_rot_limit_ = 0.0;
00205 
00206     geometry_msgs::TwistStamped msg;
00207     msg.header.stamp = ros::Time::now();
00208     msg.header.frame_id = root_name_;
00209     tf::twistEigenToMsg(xd_desi, msg.twist);
00210     pub_transformed_twist_.publish(msg);
00211     geometry_msgs::Vector3Stamped vm;
00212     vm.header.stamp = ros::Time::now();
00213     vm.header.frame_id = root_name_;
00214     vm.vector.x = rot_vel[0];
00215     vm.vector.y = rot_vel[1];
00216     vm.vector.z = rot_vel[2];
00217     pub_transformed_rot_vel_.publish(vm);
00218 
00219     last_command_time_ = ros::Time::now();
00220     mode_ = TWIST;
00221   }
00222 
00223   void commandPostureCB(const std_msgs::Float64MultiArray::ConstPtr &msg)
00224   {
00225     if (msg->data.size() == 0) {
00226       use_posture_ = false;
00227       ROS_INFO("Posture turned off");
00228     }
00229     else if ((int)msg->data.size() != JOINTS) {
00230       ROS_ERROR("Posture message had the wrong size: %d", (int)msg->data.size());
00231       return;
00232     }
00233     else
00234     {
00235       use_posture_ = true;
00236       for (int j = 0; j < JOINTS; ++j)
00237         q_posture_[j] = msg->data[j];
00238     }
00239   }
00240 };
00241 
00242 
00243 template <int JOINTS>
00244 JinvTeleopController<JOINTS>::JinvTeleopController()
00245 {
00246   loop_count_ = 0;
00247 }
00248 
00249 template <int JOINTS>
00250 JinvTeleopController<JOINTS>::~JinvTeleopController()
00251 {
00252   sub_command_.shutdown();
00253   sub_command_pose_.shutdown();
00254   sub_twist_.shutdown();
00255   sub_posture_.shutdown();
00256 }
00257 
00258 template <int JOINTS>
00259 bool JinvTeleopController<JOINTS>::init(pr2_mechanism_model::RobotState *r, ros::NodeHandle &n)
00260 {
00261   rosrt::init();
00262   node_ = n;
00263   robot_ = r;
00264 
00265   // Initialized the chain
00266   std::string tip_name;
00267   if (!node_.getParam("root_name", root_name_))
00268   {
00269     ROS_ERROR("No \"root_name\" found on parameter server");
00270     return false;
00271   }
00272   if (!node_.getParam("tip_name", tip_name))
00273   {
00274     ROS_ERROR("No \"tip_name\" found on parameter server");
00275     return false;
00276   }
00277   if (!chain_.init(robot_, root_name_, tip_name))
00278     return false;
00279   chain_.toKDL(kdl_chain_);
00280 
00281   // TODO: check chain size again JOINTS
00282 
00283   // Creates the KDL solvers
00284   fk_solver_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_));
00285   jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
00286 
00287   // Cartesian gains
00288   double kp_trans, kd_trans, kp_rot, kd_rot;
00289   if (!node_.getParam("cart_gains/trans/p", kp_trans) ||
00290       !node_.getParam("cart_gains/trans/d", kd_trans))
00291   {
00292     ROS_ERROR("P and D translational gains not specified (namespace: %s)", node_.getNamespace().c_str());
00293     return false;
00294   }
00295   if (!node_.getParam("cart_gains/rot/p", kp_rot) ||
00296       !node_.getParam("cart_gains/rot/d", kd_rot))
00297   {
00298     ROS_ERROR("P and D rotational gains not specified (namespace: %s)", node_.getNamespace().c_str());
00299     return false;
00300   }
00301   Kp_x << kp_trans, kp_trans, kp_trans,  kp_rot, kp_rot, kp_rot;
00302   Kd_x << kd_trans, kd_trans, kd_trans,  kd_rot, kd_rot, kd_rot;
00303 
00304   // Gets the joint gains
00305   for (size_t j = 0; j < JOINTS; ++j)
00306   {
00307     const std::string prefix = std::string("joints/" + chain_.getJoint(j)->joint_->name);
00308     node_.param(prefix + "/p", Kp_j[j], 0.0);
00309     node_.param(prefix + "/d", Kd_j[j], 0.0);
00310     node_.param(prefix + "/p_clamp", pclamp_j[j], 0.0);
00311   }
00312 
00313   node_.param("k_posture", k_posture_, 0.0);
00314   node_.param("jacobian_inverse_damping", jacobian_inverse_damping_, 0.0);
00315   node_.param("pose_command_filter", pose_command_filter_, 1.0);
00316 
00317   // Advertises state topics
00318   pub_x_.init(node_, "state/x", 3);
00319   pub_x_desi_.init(node_, "state/x_desi", 3);
00320   pub_x_err_.init(node_, "state/x_error", 3);
00321   pub_transformed_twist_ = node_.advertise<geometry_msgs::TwistStamped>("state/transformed_twist", 3);
00322   pub_transformed_rot_vel_ = node_.advertise<geometry_msgs::Vector3Stamped>("state/transformed_rot_vel", 3);
00323 
00324   // Subscribe to desired posture
00325   //sub_command_ = node_.subscribe("command", 1, &JinvTeleopController<JOINTS>::commandCB, this);
00326   sub_command_pose_ = node_.subscribe("command_pose", 20, &JinvTeleopController<JOINTS>::commandPoseCB, this);
00327   sub_twist_ = node_.subscribe("command_twist", 1,
00328                                &JinvTeleopController<JOINTS>::commandTwistCB, this);
00329   sub_posture_ = node_.subscribe("command_posture", 3,
00330                                  &JinvTeleopController<JOINTS>::commandPostureCB, this);
00331 
00332   StateMsg state_template;
00333   state_template.header.frame_id = root_name_;
00334   state_template.x.header.frame_id = root_name_;
00335   state_template.x_desi.header.frame_id = root_name_;
00336   state_template.x_desi_filtered.header.frame_id = root_name_;
00337   state_template.q_proxy.resize(JOINTS);
00338   state_template.qd_pose.resize(JOINTS);
00339   state_template.qd_posture.resize(JOINTS);
00340   state_template.qd_posture_raw.resize(JOINTS);
00341   state_template.qd_desi.resize(JOINTS);
00342   state_template.tau.resize(JOINTS);
00343   state_template.J.layout.dim.resize(2);
00344   state_template.J.data.resize(6*JOINTS);
00345   state_template.N.layout.dim.resize(2);
00346   state_template.N.data.resize(JOINTS*JOINTS);
00347   pub_state_.initialize(node_.advertise<StateMsg>("state", 20), 20, state_template);
00348 
00349   /*
00350   JointVector q, qd;
00351   Eigen::Affine3d x;
00352   Eigen::Matrix<double, 6, 1> xd;
00353   Eigen::Matrix<double, 6, 1> x_err;
00354   Eigen::Matrix<double, 6, 1> xd_ref;
00355   JointVector qd_pose;
00356   JointVector tau;
00357   */
00358   return true;
00359 }
00360 
00361 
00362 static void computePoseError(const Eigen::Affine3d &xact, const Eigen::Affine3d &xdes, Eigen::Matrix<double,6,1> &err)
00363 {
00364   err.head<3>() = xact.translation() - xdes.translation();
00365   err.tail<3>()   = 0.5 * (xdes.linear().col(0).cross(xact.linear().col(0)) +
00366                           xdes.linear().col(1).cross(xact.linear().col(1)) +
00367                           xdes.linear().col(2).cross(xact.linear().col(2)));
00368 }
00369 
00370 template <int JOINTS>
00371 void JinvTeleopController<JOINTS>::starting()
00372 {
00373   last_time_ = robot_->getTime();
00374 
00375   KDL::JntArrayVel q_qd(JOINTS);
00376   chain_.getVelocities(q_qd);
00377   JointVector q(q_qd.q.data), qd(q_qd.qdot.data);
00378 
00379   // Forward kinematics to get current x
00380   Eigen::Affine3d x;
00381   KDL::FrameVel x_framevel;
00382   fk_solver_->JntToCart(q_qd, x_framevel);
00383   tf::transformKDLToEigen(x_framevel.GetFrame(), x);
00384   q_proxy_ = q;
00385 
00386   x_desi_ = x;
00387   x_desi_filtered_ = x;
00388   xd_desi.setZero();
00389   use_posture_ = false;
00390   mode_ = POSE;
00391 }
00392 
00393 template <class Derived>
00394 //Eigen::Quaterniond eulerRot(const Eigen::MatrixBase<Derived>& e)
00395 Eigen::Matrix<double,3,3> eulerRot(double dt, const Eigen::MatrixBase<Derived>& e)
00396 {
00397   double n = e.norm();
00398   if (fabs(n) < 1e-8)
00399     return Eigen::Matrix<double,3,3>::Identity();
00400   return Eigen::AngleAxis<double>(n * dt, e.normalized()).toRotationMatrix();
00401   /*
00402   return
00403     Eigen::AngleAxis<double>(e[2], Eigen::Vector3d(0,0,1)) *
00404     Eigen::AngleAxis<double>(e[1], Eigen::Vector3d(0,1,0)) *
00405     Eigen::AngleAxis<double>(e[0], Eigen::Vector3d(1,0,0));
00406   */
00407 }
00408 
00409 template <int JOINTS>
00410 void JinvTeleopController<JOINTS>::update()
00411 {
00412   ros::Time time = robot_->getTime();
00413   ros::Duration dt = time - last_time_;
00414 
00415   KDL::JntArrayVel q_qd(JOINTS);
00416   chain_.getVelocities(q_qd);
00417   q = q_qd.q.data;
00418   qd = q_qd.qdot.data;
00419 
00420   KDL::Jacobian kdl_jacobian(JOINTS);
00421   jac_solver_->JntToJac(q_qd.q, kdl_jacobian);
00422   Eigen::Matrix<double, 6, JOINTS> J(kdl_jacobian.data);
00423 
00424   // Forward kinematics to get x and xd
00425   KDL::FrameVel x_framevel;
00426   fk_solver_->JntToCart(q_qd, x_framevel);
00427   tf::transformKDLToEigen(x_framevel.GetFrame(), x);
00428   tf::twistKDLToEigen(x_framevel.GetTwist(), xd);
00429 
00430   // Integrates the desired twist to determine the next position.
00431   if (mode_ == TWIST)
00432   {
00433     // Only apply update if command is less than 2 seconds old,
00434     // otherwise timeout and switch back to POSE mode.
00435     // TODO parameterize the timeout duration
00436     if (time < last_command_time_ + ros::Duration(1.0))
00437     {
00438       x_desi_.translation() += xd_desi.head<3>() * dt.toSec();
00439       x_desi_.linear() = eulerRot(dt.toSec(), xd_desi.tail<3>()) * x_desi_.linear();
00440 
00441       // Clamp
00442       Eigen::Vector3d err = x_desi_.translation() - x.translation();
00443       if (err.norm() > 0.1)
00444       {
00445         x_desi_.translation() = x.translation() + err.normalized() * 0.1;
00446       }
00447     }
00448     else
00449     {
00450       x_desi_ = x;
00451       mode_ = POSE;
00452     }
00453     x_desi_filtered_ = x_desi_;
00454   }
00455   else
00456   {
00457     // Filters the desired pose
00458 
00459     Eigen::Vector3d p0(x_desi_filtered_.translation());
00460     Eigen::Vector3d p1(x_desi_.translation());
00461     Eigen::Quaterniond q0(x_desi_filtered_.linear());
00462     Eigen::Quaterniond q1(x_desi_.linear());
00463     q0.normalize();
00464     q1.normalize();
00465 
00466     tf::Quaternion tf_q0(q0.x(), q0.y(), q0.z(), q0.w());
00467     tf::Quaternion tf_q1(q1.x(), q1.y(), q1.z(), q1.w());
00468     tf::Quaternion tf_q = tf_q0.slerp(tf_q1, pose_command_filter_);
00469 
00470     Eigen::Vector3d p = p0 + pose_command_filter_ * (p1 - p0);
00471     //Eigen::Quaterniond q = q0.slerp(pose_command_filter_, q1);
00472     Eigen::Quaterniond q(tf_q.w(), tf_q.x(), tf_q.y(), tf_q.z());
00473     //x_desi_filtered_ = q * Eigen::Translation3d(p);
00474     x_desi_filtered_ = Eigen::Translation3d(p) * q;
00475   }
00476 
00477   //computePoseError(x, x_desi_, x_err);
00478   computePoseError(x, x_desi_filtered_, x_err);
00479 
00480   // Computes the reference twist from the pose error and the desired twist.
00481   xd_ref = xd_desi.array() - (Kp_x.array() / Kd_x.array()) * x_err.array();
00482 
00483   // Applies velocity limits
00484   // TODO
00485 
00486   // ======== J psuedo-inverse and Nullspace computation
00487 
00488   // Computes pseudo-inverse of J
00489 
00490   Eigen::Matrix<double,6,6> I6; I6.setIdentity();
00491   Eigen::Matrix<double,6,6> JJt = J * J.transpose();
00492   Eigen::Matrix<double,6,6> JJt_inv;
00493   JJt_inv = JJt.inverse();
00494   Eigen::Matrix<double,JOINTS,6> J_pinv = J.transpose() * JJt_inv;
00495   Eigen::Matrix<double,6,6> JJt_damped = J * J.transpose() + jacobian_inverse_damping_ * I6;
00496   Eigen::Matrix<double,6,6> JJt_inv_damped;
00497   JJt_inv_damped = JJt_damped.inverse();
00498   Eigen::Matrix<double,JOINTS,6> J_pinv_damped = J.transpose() * JJt_inv_damped;
00499 
00500   // Computes the nullspace of J
00501   Eigen::Matrix<double,JOINTS,JOINTS> I;
00502   I.setIdentity();
00503   Eigen::Matrix<double,JOINTS,JOINTS> N = I - J_pinv * J;
00504 
00505   // ======== Pose Control
00506   
00507   // Computes the desired joint velocities for achieving the pose.
00508   qd_pose = J_pinv_damped * xd_ref;  // J-inverse
00509   //qd_pose = J.transpose() * xd_ref;  // J-transpose
00510   JointVector qd_desi = qd_pose;
00511 
00512 
00513   // ======== Posture control
00514   
00515   // Computes the desired joint velocities for achieving the posture
00516   JointVector qd_posture;
00517   JointVector qd_posture_raw;
00518   if (use_posture_)
00519   {
00520     JointVector posture_err = q_posture_ - q;
00521     for (size_t j = 0; j < JOINTS; ++j)
00522     {
00523       if (chain_.getJoint(j)->joint_->type == urdf::Joint::CONTINUOUS)
00524         posture_err[j] = angles::normalize_angle(posture_err[j]);
00525     }
00526 
00527     for (size_t j = 0; j < JOINTS; ++j) {
00528       if (fabs(q_posture_[j] - 9999) < 1e-5)
00529         posture_err[j] = 0.0;
00530     }
00531 
00532     qd_posture_raw = k_posture_ * posture_err;
00533     qd_posture = N * qd_posture_raw;
00534     qd_desi += qd_posture;
00535   }
00536 
00537   // Clamps the proxy
00538   // aleeper:
00539   // Doesn't do anything because q_proxy_ and q should never differ
00540   for (int j = 0; j < JOINTS; ++j)
00541     q_proxy_[j] = std::max(q[j] - pclamp_j[j], std::min(q_proxy_[j], q[j] + pclamp_j[j]));
00542 
00543   // Convertes the desired joint velocities into joint torques
00544   tau = Kp_j.array() * (q_proxy_ - q).array() + Kd_j.array() * (qd_desi - qd).array();
00545   // aleeper: changed to qd_desi from qd... is this right?
00546   q_proxy_ += qd_desi * dt.toSec();
00547 
00548   // ======== Torque Saturation
00549   // Copied from JT controller ... do we want this here, or does it even make sense?
00550   /*
00551   double sat_scaling = 1.0;
00552   for (int i = 0; i < Joints; ++i) {
00553     if (saturation_[i] > 0.0)
00554       sat_scaling = std::min(sat_scaling, fabs(saturation_[i] / tau[i]));
00555   }
00556   JointVec tau_sat = sat_scaling * tau;
00557   */
00558 
00559   // ======== Apply torques to arm
00560   chain_.addEfforts(tau);
00561 
00562 
00563   // ================ END OF CONTROLLER ================
00564    
00565   // ======== Publish state
00566   if (loop_count_ % 10 == 0)
00567   {
00568     if (pub_x_.trylock())
00569     {
00570       pub_x_.msg_.header.stamp = time;
00571       pub_x_.msg_.header.frame_id = root_name_;  // NOT RT SAFE
00572       tf::poseEigenToMsg(x, pub_x_.msg_.pose);
00573       pub_x_.unlockAndPublish();
00574     }
00575 
00576     if (pub_x_desi_.trylock())
00577     {
00578       pub_x_desi_.msg_.header.stamp = time;
00579       pub_x_desi_.msg_.header.frame_id = root_name_;  // NOT RT SAFE
00580       tf::poseEigenToMsg(x_desi_, pub_x_desi_.msg_.pose);
00581       pub_x_desi_.unlockAndPublish();
00582     }
00583     if (pub_x_err_.trylock())
00584     {
00585       pub_x_err_.msg_.header.stamp = time;
00586       tf::twistEigenToMsg(x_err, pub_x_err_.msg_.twist);
00587       pub_x_err_.unlockAndPublish();
00588     }
00589 
00590     StateMsg::Ptr state_msg;
00591     if (state_msg = pub_state_.allocate()) {
00592       state_msg->header.stamp = time;
00593       state_msg->x.header.stamp = time;
00594       tf::poseEigenToMsg(x, state_msg->x.pose);
00595       state_msg->x_desi.header.stamp = time;
00596       tf::poseEigenToMsg(x_desi_, state_msg->x_desi.pose);
00597       state_msg->x_desi_filtered.header.stamp = time;
00598       tf::poseEigenToMsg(x_desi_filtered_, state_msg->x_desi_filtered.pose);
00599       tf::twistEigenToMsg(x_err, state_msg->x_err);
00600       tf::twistEigenToMsg(xd, state_msg->xd);
00601       tf::twistEigenToMsg(xd_desi, state_msg->xd_desi);
00602       tf::matrixEigenToMsg(J, state_msg->J);
00603       tf::matrixEigenToMsg(N, state_msg->N);
00604       for (size_t j = 0; j < JOINTS; ++j) {
00605         state_msg->q_proxy[j] = q_proxy_[j];
00606         state_msg->qd_pose[j] = qd_pose[j];
00607         state_msg->qd_posture[j] = qd_posture[j];
00608         state_msg->qd_posture_raw[j] = qd_posture_raw[j];
00609         state_msg->qd_desi[j] = qd_desi[j];
00610         state_msg->tau[j] = tau[j];
00611       }
00612       pub_state_.publish(state_msg);
00613     }
00614   }
00615 
00616   last_time_ = time;
00617   ++loop_count_;
00618 }
00619 
00620 } // namespace
00621 
00622 #include <pluginlib/class_list_macros.h>
00623 PLUGINLIB_EXPORT_CLASS(pr2_manipulation_controllers::JinvTeleopController<7>,pr2_controller_interface::Controller)


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