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 <boost/scoped_ptr.hpp>
41 
42 #include <Eigen/Core>
43 #include <Eigen/Geometry>
44 
45 #include <ros/ros.h>
46 
47 #include <control_toolbox/pid.h>
50 #include <kdl/chainfksolver.hpp>
51 #include <kdl/chainfksolverpos_recursive.hpp>
52 #include <kdl/chain.hpp>
53 #include <kdl/chainjnttojacsolver.hpp>
54 #include <kdl/frames.hpp>
58 #include <tf/transform_listener.h>
59 
60 #include <geometry_msgs/PoseStamped.h>
61 #include <geometry_msgs/TwistStamped.h>
62 #include <robot_mechanism_controllers/JTCartesianControllerState.h>
63 
64 
65 namespace controller {
66 
67 template <int Joints>
68 struct Kin
69 {
70  typedef Eigen::Matrix<double, Joints, 1> JointVec;
71  typedef Eigen::Matrix<double, 6, Joints> Jacobian;
72 
73  Kin(const KDL::Chain &kdl_chain) :
74  fk_solver_(kdl_chain), jac_solver_(kdl_chain),
75  kdl_q(Joints), kdl_J(Joints)
76  {
77  }
78  ~Kin()
79  {
80  }
81 
82  void fk(const JointVec &q, Eigen::Affine3d &x)
83  {
84  kdl_q.data = q;
85  KDL::Frame kdl_x;
86  fk_solver_.JntToCart(kdl_q, kdl_x);
87  tf::transformKDLToEigen(kdl_x, x);
88  }
89  void jac(const JointVec &q, Jacobian &J)
90  {
91  kdl_q.data = q;
92  jac_solver_.JntToJac(kdl_q, kdl_J);
93  J = kdl_J.data;
94  }
95 
96  KDL::ChainFkSolverPos_recursive fk_solver_;
97  KDL::ChainJntToJacSolver jac_solver_;
98  KDL::JntArray kdl_q;
99  KDL::Jacobian kdl_J;
100 };
101 
103 {
104 public:
105  // Ensure 128-bit alignment for Eigen
106  // See also http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
108 private:
109  enum { Joints = 7 };
110  typedef Eigen::Matrix<double, Joints, 1> JointVec;
111  typedef Eigen::Matrix<double, 6, 1> CartVec;
112  typedef Eigen::Matrix<double, 6, Joints> Jacobian;
113  typedef robot_mechanism_controllers::JTCartesianControllerState StateMsg;
114 public:
117 
119  void starting();
120  void update();
121 
122  Eigen::Affine3d x_desi_, x_desi_filtered_;
124 
130 
133 
134  std::string root_name_, tip_name_;
136  int loop_count_;
138 
140  boost::scoped_ptr<Kin<Joints> > kin_;
141  Eigen::Matrix<double,6,1> Kp, Kd;
149  double k_posture_;
150  bool use_posture_;
151 
152  // Minimum resolutions
155 
156  Eigen::Affine3d last_pose_;
158 
160 
161  void setGains(const std_msgs::Float64MultiArray::ConstPtr &msg)
162  {
163  if (msg->data.size() >= 6)
164  for (size_t i = 0; i < 6; ++i)
165  Kp[i] = msg->data[i];
166  if (msg->data.size() == 12)
167  for (size_t i = 0; i < 6; ++i)
168  Kd[i] = msg->data[6+i];
169 
170  ROS_INFO("New gains: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
171  Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
172  }
173 
174  void commandPosture(const std_msgs::Float64MultiArray::ConstPtr &msg)
175  {
176  if (msg->data.size() == 0) {
177  use_posture_ = false;
178  ROS_INFO("Posture turned off");
179  }
180  else if ((int)msg->data.size() != q_posture_.size()) {
181  ROS_ERROR("Posture message had the wrong size: %d", (int)msg->data.size());
182  return;
183  }
184  else
185  {
186  use_posture_ = true;
187  for (int j = 0; j < Joints; ++j)
188  q_posture_[j] = msg->data[j];
189  }
190  }
191 
192  void commandPose(const geometry_msgs::PoseStamped::ConstPtr &command)
193  {
194  geometry_msgs::PoseStamped in_root;
195  try {
196  tf_.waitForTransform(root_name_, command->header.frame_id, command->header.stamp, ros::Duration(0.1));
197  tf_.transformPose(root_name_, *command, in_root);
198  }
199  catch (const tf::TransformException &ex)
200  {
201  ROS_ERROR("Failed to transform: %s", ex.what());
202  return;
203  }
204 
205  tf::poseMsgToEigen(in_root.pose, x_desi_);
206  }
207 };
208 
209 } // namespace
210 
211 #endif
tf::poseMsgToEigen
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
controller::Kin::jac_solver_
KDL::ChainJntToJacSolver jac_solver_
Definition: jt_cartesian_controller.h:161
realtime_publisher.h
msg
msg
controller::JTCartesianController::kin_
boost::scoped_ptr< Kin< Joints > > kin_
Definition: jt_cartesian_controller.h:172
controller::JTCartesianController::pub_x_desi_
realtime_tools::RealtimePublisher< geometry_msgs::PoseStamped > pub_x_desi_
Definition: jt_cartesian_controller.h:164
controller::JTCartesianController::tip_name_
std::string tip_name_
Definition: jt_cartesian_controller.h:166
controller::JTCartesianController::pub_state_
realtime_tools::RealtimePublisher< StateMsg > pub_state_
Definition: jt_cartesian_controller.h:163
i
int i
controller::JTCartesianController::res_orientation_
double res_orientation_
Definition: jt_cartesian_controller.h:186
controller::JTCartesianController::commandPose
void commandPose(const geometry_msgs::PoseStamped::ConstPtr &command)
Definition: jt_cartesian_controller.h:224
controller::Kin::kdl_q
KDL::JntArray kdl_q
Definition: jt_cartesian_controller.h:162
controller::JTCartesianController::x_desi_filtered_
Eigen::Affine3d x_desi_filtered_
Definition: jt_cartesian_controller.h:154
controller::JTCartesianController::use_posture_
bool use_posture_
Definition: jt_cartesian_controller.h:182
controller::JTCartesianController
Definition: jt_cartesian_controller.h:134
controller::Kin::jac
void jac(const JointVec &q, Jacobian &J)
Definition: jt_cartesian_controller.h:153
tf::TransformListener::transformPose
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
ros.h
pr2_mechanism_model::Chain
command
ROSLIB_DECL std::string command(const std::string &cmd)
controller::JTCartesianController::last_time_
ros::Time last_time_
Definition: jt_cartesian_controller.h:167
controller::JTCartesianController::Kd
Eigen::Matrix< double, 6, 1 > Kd
Definition: jt_cartesian_controller.h:173
controller::Kin::~Kin
~Kin()
Definition: jt_cartesian_controller.h:142
controller::JTCartesianController::wrench_desi_
CartVec wrench_desi_
Definition: jt_cartesian_controller.h:155
controller::JTCartesianController::init
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
Definition: jt_cartesian_controller.cpp:95
controller::JTCartesianController::qdot_filtered_
JointVec qdot_filtered_
Definition: jt_cartesian_controller.h:191
controller::JTCartesianController::sub_gains_
ros::Subscriber sub_gains_
Definition: jt_cartesian_controller.h:158
controller::JTCartesianController::joint_vel_filter_
double joint_vel_filter_
Definition: jt_cartesian_controller.h:178
controller::JTCartesianController::setGains
void setGains(const std_msgs::Float64MultiArray::ConstPtr &msg)
Definition: jt_cartesian_controller.h:193
controller::Kin::fk_solver_
KDL::ChainFkSolverPos_recursive fk_solver_
Definition: jt_cartesian_controller.h:160
controller::JTCartesianController::EIGEN_MAKE_ALIGNED_OPERATOR_NEW
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Definition: jt_cartesian_controller.h:139
controller::JTCartesianController::last_wrench_
CartVec last_wrench_
Definition: jt_cartesian_controller.h:189
controller::JTCartesianController::x_desi_
Eigen::Affine3d x_desi_
Definition: jt_cartesian_controller.h:154
realtime_tools::RealtimePublisher< StateMsg >
controller::JTCartesianController::update
void update()
Definition: jt_cartesian_controller.cpp:232
controller::JTCartesianController::chain_
pr2_mechanism_model::Chain chain_
Definition: jt_cartesian_controller.h:171
controller::Kin::fk
void fk(const JointVec &q, Eigen::Affine3d &x)
Definition: jt_cartesian_controller.h:146
controller::JTCartesianController::Joints
@ Joints
Definition: jt_cartesian_controller.h:141
controller::JTCartesianController::pose_command_filter_
double pose_command_filter_
Definition: jt_cartesian_controller.h:174
controller::JTCartesianController::vel_saturation_trans_
double vel_saturation_trans_
Definition: jt_cartesian_controller.h:175
controller::JTCartesianController::res_force_
double res_force_
Definition: jt_cartesian_controller.h:185
controller::JTCartesianController::saturation_
JointVec saturation_
Definition: jt_cartesian_controller.h:176
controller.h
controller
pr2_mechanism_model::RobotState
controller::JTCartesianController::res_torque_
double res_torque_
Definition: jt_cartesian_controller.h:186
controller::JTCartesianController::jacobian_inverse_damping_
double jacobian_inverse_damping_
Definition: jt_cartesian_controller.h:179
controller::JTCartesianController::root_name_
std::string root_name_
Definition: jt_cartesian_controller.h:166
controller::JTCartesianController::JointVec
Eigen::Matrix< double, Joints, 1 > JointVec
Definition: jt_cartesian_controller.h:142
eigen_msg.h
ROS_ERROR
#define ROS_ERROR(...)
controller::JTCartesianController::commandPosture
void commandPosture(const std_msgs::Float64MultiArray::ConstPtr &msg)
Definition: jt_cartesian_controller.h:206
controller::JTCartesianController::Kp
Eigen::Matrix< double, 6, 1 > Kp
Definition: jt_cartesian_controller.h:173
controller::JTCartesianController::node_
ros::NodeHandle node_
Definition: jt_cartesian_controller.h:157
controller::JTCartesianController::starting
void starting()
Definition: jt_cartesian_controller.cpp:206
controller::JTCartesianController::k_posture_
double k_posture_
Definition: jt_cartesian_controller.h:181
controller::JTCartesianController::JTCartesianController
JTCartesianController()
Definition: jt_cartesian_controller.cpp:83
tf::Transformer::waitForTransform
bool waitForTransform(const std::string &target_frame, const ros::Time &target_time, const std::string &source_frame, const ros::Time &source_time, const std::string &fixed_frame, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
controller::JTCartesianController::tf_
tf::TransformListener tf_
Definition: jt_cartesian_controller.h:161
controller::JTCartesianController::Jacobian
Eigen::Matrix< double, 6, Joints > Jacobian
Definition: jt_cartesian_controller.h:144
controller::JTCartesianController::~JTCartesianController
~JTCartesianController()
Definition: jt_cartesian_controller.cpp:87
pr2_controller_interface::Controller
controller::JTCartesianController::CartVec
Eigen::Matrix< double, 6, 1 > CartVec
Definition: jt_cartesian_controller.h:143
transform_listener.h
controller::JTCartesianController::vel_saturation_rot_
double vel_saturation_rot_
Definition: jt_cartesian_controller.h:175
controller::JTCartesianController::StateMsg
robot_mechanism_controllers::JTCartesianControllerState StateMsg
Definition: jt_cartesian_controller.h:145
controller::JTCartesianController::res_position_
double res_position_
Definition: jt_cartesian_controller.h:185
controller::JTCartesianController::q_posture_
JointVec q_posture_
Definition: jt_cartesian_controller.h:180
controller::JTCartesianController::robot_state_
pr2_mechanism_model::RobotState * robot_state_
Definition: jt_cartesian_controller.h:169
ros::Time
chain.h
controller::Kin::Jacobian
Eigen::Matrix< double, 6, Joints > Jacobian
Definition: jt_cartesian_controller.h:135
tf::transformKDLToEigen
void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
controller::JTCartesianController::last_pose_
Eigen::Affine3d last_pose_
Definition: jt_cartesian_controller.h:188
controller::Kin::kdl_J
KDL::Jacobian kdl_J
Definition: jt_cartesian_controller.h:163
controller::Kin::JointVec
Eigen::Matrix< double, Joints, 1 > JointVec
Definition: jt_cartesian_controller.h:134
tf::TransformListener
if
if((endptr==val_str)||(endptr<(val_str+strlen(val_str))))
pid.h
controller::JTCartesianController::sub_posture_
ros::Subscriber sub_posture_
Definition: jt_cartesian_controller.h:159
eigen_kdl.h
tf2::TransformException
ROS_INFO
#define ROS_INFO(...)
ros::Duration
controller::Kin::Kin
Kin(const KDL::Chain &kdl_chain)
Definition: jt_cartesian_controller.h:137
controller::JTCartesianController::joint_dd_ff_
JointVec joint_dd_ff_
Definition: jt_cartesian_controller.h:177
controller::JTCartesianController::loop_count_
int loop_count_
Definition: jt_cartesian_controller.h:168
controller::JTCartesianController::sub_pose_
ros::Subscriber sub_pose_
Definition: jt_cartesian_controller.h:160
ros::NodeHandle
ros::Subscriber


robot_mechanism_controllers
Author(s): John Hsu, Melonee Wise, Stuart Glaser
autogenerated on Sat Nov 12 2022 03:33:22