| Note: This tutorial assumes that you are in the middle of the tutorial: Implementing a realtime Cartesian controller. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Coding a realtime Cartesian controller with Eigen
Description: This tutorial gives example code for a realtime Cartesian controller using KDL and EigenTutorial Level: ADVANCED
Introduction
The following gives example code for a realtime Cartesian controller, to be run from the previous tutorial. It stands in contrast to the previous code example using KDL
Like the joint controller, we create a class which inherits from the controller::Controller class in the pr2_controller_interface package. As before, it overloads the init, starting, update, and stopping methods to provide the desired realtime functionality.
To access the joints, we use the Chain object, which builds a chain of joints from the specified root to tip frames and then reads the joint position or velocity vector as well as sets the joint torque vector.
Different to the example code using KDL, we perform the kinematic calculations using the KDL library, but immediately transform to Eigen vectors and matrices to allow general linear algebra.
The Header File
The following gives the header file, which should be replace (if it exists) the file include/my_controller_pkg/my_cart_controller_file.h:
1 #include <pr2_controller_interface/controller.h>
2 #include <pr2_mechanism_model/chain.h>
3 #include <pr2_mechanism_model/robot.h>
4
5 #include <boost/scoped_ptr.hpp>
6 #include <kdl/chain.hpp>
7 #include <kdl/chainjnttojacsolver.hpp>
8 #include <kdl/chainfksolverpos_recursive.hpp>
9 #include <kdl/frames.hpp>
10 #include <kdl/jacobian.hpp>
11 #include <kdl/jntarray.hpp>
12
13 #include <pr2_mechanism_model/kinematichelpers.h>
14 #include <Eigen/Geometry>
15
16
17 namespace my_controller_ns{
18
19 class MyCartControllerClass: public pr2_controller_interface::Controller
20 {
21 // Declare the number of joints.
22 enum
23 {
24 Joints = 7
25 };
26
27 // Define the joint/cart vector types accordingly (using a fixed
28 // size to avoid dynamic allocations and make the code realtime safe).
29 typedef Eigen::Matrix<double, Joints, 1> JointVector;
30 typedef Eigen::Transform3d CartPose;
31 typedef Eigen::Matrix<double, 3, 1> Cart3Vector;
32 typedef Eigen::Matrix<double, 6, 1> Cart6Vector;
33 typedef Eigen::Matrix<double, 6, Joints> JacobianMatrix;
34
35 // Ensure 128-bit alignment for Eigen
36 // See also http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39
40 private:
41 // The current robot state (to get the time stamp)
42 pr2_mechanism_model::RobotState* robot_state_;
43
44 // The chain of links and joints
45 pr2_mechanism_model::Chain chain_;
46
47 // KDL Solvers performing the actual computations
48 boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
49 boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
50
51 // Temporary variables for KDL and helper functions
52 // to convert to Eigen, etc.
53 KDL::JntArray qtmp_; // Joint vector
54 KDL::Frame xtmp_; // Tip pose
55 KDL::Jacobian Jtmp_; // Jacobian
56 pr2_mechanism_model::KinematicHelpers kin_;
57
58 // The trajectory initial value
59 CartPose x0_; // Tip initial pose
60
61 // The controller parameters
62 Cart6Vector Kp_; // Proportional gains
63 Cart6Vector Kd_; // Derivative gains
64
65 // The trajectory variables
66 double circle_phase_; // Phase along the circle
67 ros::Time last_time_; // Time of the last servo cycle
68
69 public:
70 bool init(pr2_mechanism_model::RobotState *robot,
71 ros::NodeHandle &n);
72 void starting();
73 void update();
74 void stopping();
75 };
76 }
In comparison to the header file using KDL we notice the following changes:
- Two additional include files
to provide helper functions and Eigen objects.
- The biggest change is the declaration of the number of joints and definition of types using Eigen vectors and matricies.
21 // Declare the number of joints.
22 enum
23 {
24 Joints = 7
25 };
26
27 // Define the joint/cart vector types accordingly (using a fixed
28 // size to avoid dynamic allocations and make the code realtime safe).
29 typedef Eigen::Matrix<double, Joints, 1> JointVector;
30 typedef Eigen::Transform3d CartPose;
31 typedef Eigen::Matrix<double, 3, 1> Cart3Vector;
32 typedef Eigen::Matrix<double, 6, 1> Cart6Vector;
33 typedef Eigen::Matrix<double, 6, Joints> JacobianMatrix;
34
35 // Ensure 128-bit alignment for Eigen
36 // See also http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html
37 public:
38 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Notice that the vector/matrix dimensions are fixed. And be aware that Eigen requires 128 bit alignment of data, which is enforced with the EIGEN_MAKE_ALIGNED_OPERATOR_NEW macro.
- Unlike the previous code, we don't need to declare and pre-allocated all variables. Only the KDL variables used in the KDL solvers, for which we create temporary space
The KinematicHelpers class provides the helper functions, including KDL to Eigen conversion, Eigen to KDL conversion, as well as computation of a Cartesian error.
- The only required variable is the initial pose, defined as an Eigen type. Notice also that the Kp/Kd gains are now a generic Eigen vector.
The Source Code
The following gives the source code file, which should be replace (if it exists) the file src/my_cart_controller_file.cpp:
1 #include "my_controller_pkg/my_cart_controller_file.h"
2 #include <pluginlib/class_list_macros.h>
3
4 using namespace my_controller_ns;
5
6
7 /// Controller initialization in non-realtime
8 bool MyCartControllerClass::init(pr2_mechanism_model::RobotState *robot,
9 ros::NodeHandle &n)
10 {
11 // Get the root and tip link names from parameter server.
12 std::string root_name, tip_name;
13 if (!n.getParam("root_name", root_name))
14 {
15 ROS_ERROR("No root name given in namespace: %s)",
16 n.getNamespace().c_str());
17 return false;
18 }
19 if (!n.getParam("tip_name", tip_name))
20 {
21 ROS_ERROR("No tip name given in namespace: %s)",
22 n.getNamespace().c_str());
23 return false;
24 }
25
26 // Construct a chain from the root to the tip and prepare the kinematics.
27 // Note the joints must be calibrated.
28 if (!chain_.init(robot, root_name, tip_name))
29 {
30 ROS_ERROR("MyCartController could not use the chain from '%s' to '%s'",
31 root_name.c_str(), tip_name.c_str());
32 return false;
33 }
34
35 // Store the robot handle for later use (to get time).
36 robot_state_ = robot;
37
38 // Construct the kdl solvers in non-realtime.
39 KDL::Chain kdl_chain;
40 chain_.toKDL(kdl_chain);
41 jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain));
42 jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain));
43
44 // Resize (pre-allocate) the temporary variables in non-realtime.
45 qtmp_.resize(chain_.getNumOfJoints());
46 Jtmp_.resize(chain_.getNumOfJoints());
47
48 // Verify the number of joints.
49 if (chain_.getNumOfJoints() != Joints)
50 {
51 ROS_ERROR("The chain from '%s' to '%s' does not contain %d joints",
52 root_name.c_str(), tip_name.c_str(), Joints);
53 return false;
54 }
55
56 // Pick the gains.
57 Kp_(0) = 100.0; Kd_(0) = 1.0; // Translation x
58 Kp_(1) = 100.0; Kd_(1) = 1.0; // Translation y
59 Kp_(2) = 100.0; Kd_(2) = 1.0; // Translation z
60 Kp_(3) = 100.0; Kd_(3) = 1.0; // Rotation x
61 Kp_(4) = 100.0; Kd_(4) = 1.0; // Rotation y
62 Kp_(5) = 100.0; Kd_(5) = 1.0; // Rotation z
63
64 return true;
65 }
66
67
68 /// Controller startup in realtime
69 void MyCartControllerClass::starting()
70 {
71 // Get the current joint values to compute the initial tip location.
72 chain_.getJointPositions(qtmp_);
73 jnt_to_pose_solver_->JntToCart(qtmp_, xtmp_);
74 kin_.KDLtoEigen(xtmp_, x0_);
75
76 // Initialize the phase of the circle as zero.
77 circle_phase_ = 0.0;
78
79 // Also reset the time-of-last-servo-cycle.
80 last_time_ = robot_state_->getTime();
81 }
82
83
84 /// Controller update loop in realtime
85 void MyCartControllerClass::update()
86 {
87 double dt; // Servo loop time step
88 JointVector q, qdot, tau; // Joint position, velocity, torques
89 CartPose x, xd; // Tip pose, desired pose
90 Cart6Vector xerr, xdot, F; // Cart error,velocity,effort
91 JacobianMatrix J; // Jacobian
92
93 // Calculate the dt between servo cycles.
94 dt = (robot_state_->getTime() - last_time_).toSec();
95 last_time_ = robot_state_->getTime();
96
97 // Get the current joint positions and velocities.
98 chain_.getJointPositions(q);
99 chain_.getJointVelocities(qdot);
100
101 // Compute the forward kinematics and Jacobian (at this location).
102 kin_.EigentoKDL(q, qtmp_);
103 jnt_to_pose_solver_->JntToCart(qtmp_, xtmp_);
104 jnt_to_jac_solver_->JntToJac(qtmp_, Jtmp_);
105 kin_.KDLtoEigen(xtmp_, x);
106 kin_.KDLtoEigen(Jtmp_, J);
107
108 xdot = J * qdot;
109
110 // Follow a circle of 10cm at 3 rad/sec.
111 circle_phase_ += 3.0 * dt;
112 Cart3Vector circle(0,0,0);
113 circle.x() = 0.1 * sin(circle_phase_);
114 circle.y() = 0.1 * (1 - cos(circle_phase_));
115
116 xd = x0_;
117 xd.translation() += circle;
118
119 // Calculate a Cartesian restoring force.
120 kin_.computeCartError(x,xd,xerr);
121 F = - Kp_.asDiagonal() * xerr - Kd_.asDiagonal() * xdot;
122
123 // Convert the force into a set of joint torques.
124 tau = J.transpose() * F;
125
126 // And finally send these torques out.
127 chain_.setJointEfforts(tau);
128 }
129
130
131 /// Controller stopping in realtime
132 void MyCartControllerClass::stopping()
133 {}
134
135
136
137 /// Register controller to pluginlib
138 PLUGINLIB_REGISTER_CLASS(MyCartControllerPlugin,
139 my_controller_ns::MyCartControllerClass,
140 pr2_controller_interface::Controller)
In comparison to the source code using KDL we notice the following changes:
In the init() method we only need to pre-allocate the temporary space for the KDL solvers. Not the entire variable set.
However, init() should confirm the number of joint matches the expectation set in the header file.
- Also notice the Kp/Kd gains are set as generic 6x1 vectors without explicit translation and orientation fields
56 // Pick the gains.
57 Kp_(0) = 100.0; Kd_(0) = 1.0; // Translation x
58 Kp_(1) = 100.0; Kd_(1) = 1.0; // Translation y
59 Kp_(2) = 100.0; Kd_(2) = 1.0; // Translation z
60 Kp_(3) = 100.0; Kd_(3) = 1.0; // Rotation x
61 Kp_(4) = 100.0; Kd_(4) = 1.0; // Rotation y
62 Kp_(5) = 100.0; Kd_(5) = 1.0; // Rotation z
63
In both the starting() and init() methods, we see that the KDL solvers are now operating in the temporary space and immediately converted to Eigen types.
- Though using Eigen, the calculations are now more straight forward. Both to calculate the tip velocity, tip force, and joint torques.
108 xdot = J * qdot;
- And in Eigen, accessing sub fields of a pose is slightly different. So the trajectory calculations are equivalent but using a different notation.
- Finally, notice the kinematic helper functions include the computation of a Cartesian error.
120 kin_.computeCartError(x,xd,xerr);
Compiling and Running
To compile and run the controller, go back to the previous tutorial.
