jt_cartesian_controller.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 // Author: Stuart Glaser
36 
37 #ifndef PR2_CONTROLLERS_JT_CARTESIAN_CONTROLLER_H
38 #define PR2_CONTROLLERS_JT_CARTESIAN_CONTROLLER_H
39 
40 #include <Eigen/Core>
41 #include <Eigen/Geometry>
42 
43 #include <ros/ros.h>
44 
45 #include <control_toolbox/pid.h>
48 #include <kdl/chainfksolver.hpp>
49 #include <kdl/chainfksolverpos_recursive.hpp>
50 #include <kdl/chain.hpp>
51 #include <kdl/chainjnttojacsolver.hpp>
52 #include <kdl/frames.hpp>
56 #include <tf/transform_listener.h>
57 
58 #include <geometry_msgs/PoseStamped.h>
59 #include <geometry_msgs/TwistStamped.h>
60 #include <robot_mechanism_controllers/JTCartesianControllerState.h>
61 
62 
63 namespace controller {
64 
65 template <int Joints>
66 struct Kin
67 {
68  typedef Eigen::Matrix<double, Joints, 1> JointVec;
69  typedef Eigen::Matrix<double, 6, Joints> Jacobian;
70 
71  Kin(const KDL::Chain &kdl_chain) :
72  fk_solver_(kdl_chain), jac_solver_(kdl_chain),
73  kdl_q(Joints), kdl_J(Joints)
74  {
75  }
76  ~Kin()
77  {
78  }
79 
80  void fk(const JointVec &q, Eigen::Affine3d &x)
81  {
82  kdl_q.data = q;
83  KDL::Frame kdl_x;
84  fk_solver_.JntToCart(kdl_q, kdl_x);
85  tf::transformKDLToEigen(kdl_x, x);
86  }
87  void jac(const JointVec &q, Jacobian &J)
88  {
89  kdl_q.data = q;
91  J = kdl_J.data;
92  }
93 
98 };
99 
101 {
102 public:
103  // Ensure 128-bit alignment for Eigen
104  // See also http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
106 private:
107  enum { Joints = 7 };
108  typedef Eigen::Matrix<double, Joints, 1> JointVec;
109  typedef Eigen::Matrix<double, 6, 1> CartVec;
110  typedef Eigen::Matrix<double, 6, Joints> Jacobian;
111  typedef robot_mechanism_controllers::JTCartesianControllerState StateMsg;
112 public:
115 
116  bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
117  void starting();
118  void update();
119 
120  Eigen::Affine3d x_desi_, x_desi_filtered_;
121  CartVec wrench_desi_;
122 
128 
131 
132  std::string root_name_, tip_name_;
136 
138  boost::scoped_ptr<Kin<Joints> > kin_;
139  Eigen::Matrix<double,6,1> Kp, Kd;
141  double vel_saturation_trans_, vel_saturation_rot_;
142  JointVec saturation_;
143  JointVec joint_dd_ff_;
146  JointVec q_posture_;
147  double k_posture_;
149 
150  // Minimum resolutions
151  double res_force_, res_position_;
152  double res_torque_, res_orientation_;
153 
154  Eigen::Affine3d last_pose_;
155  CartVec last_wrench_;
156 
157  JointVec qdot_filtered_;
158 
159  void setGains(const std_msgs::Float64MultiArray::ConstPtr &msg)
160  {
161  if (msg->data.size() >= 6)
162  for (size_t i = 0; i < 6; ++i)
163  Kp[i] = msg->data[i];
164  if (msg->data.size() == 12)
165  for (size_t i = 0; i < 6; ++i)
166  Kd[i] = msg->data[6+i];
167 
168  ROS_INFO("New gains: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
169  Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
170  }
171 
172  void commandPosture(const std_msgs::Float64MultiArray::ConstPtr &msg)
173  {
174  if (msg->data.size() == 0) {
175  use_posture_ = false;
176  ROS_INFO("Posture turned off");
177  }
178  else if ((int)msg->data.size() != q_posture_.size()) {
179  ROS_ERROR("Posture message had the wrong size: %d", (int)msg->data.size());
180  return;
181  }
182  else
183  {
184  use_posture_ = true;
185  for (int j = 0; j < Joints; ++j)
186  q_posture_[j] = msg->data[j];
187  }
188  }
189 
190  void commandPose(const geometry_msgs::PoseStamped::ConstPtr &command)
191  {
192  geometry_msgs::PoseStamped in_root;
193  try {
194  tf_.waitForTransform(root_name_, command->header.frame_id, command->header.stamp, ros::Duration(0.1));
195  tf_.transformPose(root_name_, *command, in_root);
196  }
197  catch (const tf::TransformException &ex)
198  {
199  ROS_ERROR("Failed to transform: %s", ex.what());
200  return;
201  }
202 
203  tf::poseMsgToEigen(in_root.pose, x_desi_);
204  }
205 };
206 
207 } // namespace
208 
209 #endif
KDL::ChainFkSolverPos_recursive fk_solver_
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
void fk(const JointVec &q, Eigen::Affine3d &x)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
realtime_tools::RealtimePublisher< geometry_msgs::PoseStamped > pub_x_desi_
void commandPosture(const std_msgs::Float64MultiArray::ConstPtr &msg)
realtime_tools::RealtimePublisher< StateMsg > pub_state_
Eigen::Matrix< double, 6, 1 > Kp
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Eigen::Matrix< double, 6, Eigen::Dynamic > data
void setGains(const std_msgs::Float64MultiArray::ConstPtr &msg)
void jac(const JointVec &q, Jacobian &J)
Eigen::Matrix< double, 6, Joints > Jacobian
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Eigen::Matrix< double, 6, 1 > CartVec
#define ROS_INFO(...)
Kin(const KDL::Chain &kdl_chain)
Eigen::VectorXd data
Eigen::Matrix< double, Joints, 1 > JointVec
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Eigen::Matrix< double, 6, Joints > Jacobian
void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
robot_mechanism_controllers::JTCartesianControllerState StateMsg
pr2_mechanism_model::RobotState * robot_state_
boost::scoped_ptr< Kin< Joints > > kin_
#define ROS_ERROR(...)
if((endptr==val_str)||(endptr< (val_str+strlen(val_str))))
KDL::ChainJntToJacSolver jac_solver_
void commandPose(const geometry_msgs::PoseStamped::ConstPtr &command)
Eigen::Matrix< double, Joints, 1 > JointVec


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Mon Jun 10 2019 14:26:26