## For instruction on writing tutorials ## http://www.ros.org/wiki/WritingTutorials #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = This tutorial assumes that you are in the middle of the tutorial: [[pr2_mechanism/Tutorials/Implementing a realtime Cartesian controller|Implementing a realtime Cartesian controller]] ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= ## descriptive title for the tutorial ## title = Coding a realtime Cartesian controller with Eigen ## multi-line description to be displayed in search ## description = This tutorial gives example code for a realtime Cartesian controller using KDL and Eigen ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link= ## next.1.link= ## what level user is this tutorial for ## level= AdvancedCategory ## keywords = #################################### <> <> == Introduction == The following gives example code for a realtime Cartesian controller, to be run from the [[pr2_mechanism/Tutorials/Implementing a realtime Cartesian controller|previous tutorial]]. It stands in contrast to the previous [[pr2_mechanism/Tutorials/Coding a realtime Cartesian controller with KDL|code example using KDL]] Like the [[pr2_mechanism/Tutorials/Writing a realtime joint controller|joint controller]], we create a class which inherits from the [[http://www.ros.org/doc/api/pr2_controller_interface/html/classcontroller_1_1Controller.html|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 [[http://www.ros.org/doc/api/pr2_mechanism_model/html/classpr2__mechanism__model_1_1Chain.html|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 [[pr2_mechanism/Tutorials/Coding a realtime Cartesian controller with KDL|example code using KDL]], we perform the kinematic calculations using the [[kdl|KDL]] library, but immediately transform to [[eigen|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''': {{{ #!cplusplus block=controller_h #include #include #include #include #include #include #include #include #include #include #include #include namespace my_controller_ns{ class MyCartControllerClass: public pr2_controller_interface::Controller { // Declare the number of joints. enum { Joints = 7 }; // Define the joint/cart vector types accordingly (using a fixed // size to avoid dynamic allocations and make the code realtime safe). typedef Eigen::Matrix JointVector; typedef Eigen::Transform3d CartPose; typedef Eigen::Matrix Cart3Vector; typedef Eigen::Matrix Cart6Vector; typedef Eigen::Matrix JacobianMatrix; // Ensure 128-bit alignment for Eigen // See also http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: // The current robot state (to get the time stamp) pr2_mechanism_model::RobotState* robot_state_; // The chain of links and joints pr2_mechanism_model::Chain chain_; // KDL Solvers performing the actual computations boost::scoped_ptr jnt_to_pose_solver_; boost::scoped_ptr jnt_to_jac_solver_; // Temporary variables for KDL and helper functions // to convert to Eigen, etc. KDL::JntArray qtmp_; // Joint vector KDL::Frame xtmp_; // Tip pose KDL::Jacobian Jtmp_; // Jacobian pr2_mechanism_model::KinematicHelpers kin_; // The trajectory initial value CartPose x0_; // Tip initial pose // The controller parameters Cart6Vector Kp_; // Proportional gains Cart6Vector Kd_; // Derivative gains // The trajectory variables double circle_phase_; // Phase along the circle ros::Time last_time_; // Time of the last servo cycle public: bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); void starting(); void update(); void stopping(); }; } }}} In comparison to the header file [[pr2_mechanism/Tutorials/Coding a realtime Cartesian controller with KDL|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. <> Notice that the vector/matrix dimensions are fixed. And be aware that [[http://eigen.tuxfamily.org/dox/StructHavingEigenMembers.html|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''': {{{ #!cplusplus block=controller_c #include "my_controller_pkg/my_cart_controller_file.h" #include using namespace my_controller_ns; /// Controller initialization in non-realtime bool MyCartControllerClass::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) { // Get the root and tip link names from parameter server. std::string root_name, tip_name; if (!n.getParam("root_name", root_name)) { ROS_ERROR("No root name given in namespace: %s)", n.getNamespace().c_str()); return false; } if (!n.getParam("tip_name", tip_name)) { ROS_ERROR("No tip name given in namespace: %s)", n.getNamespace().c_str()); return false; } // Construct a chain from the root to the tip and prepare the kinematics. // Note the joints must be calibrated. if (!chain_.init(robot, root_name, tip_name)) { ROS_ERROR("MyCartController could not use the chain from '%s' to '%s'", root_name.c_str(), tip_name.c_str()); return false; } // Store the robot handle for later use (to get time). robot_state_ = robot; // Construct the kdl solvers in non-realtime. KDL::Chain kdl_chain; chain_.toKDL(kdl_chain); jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain)); jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain)); // Resize (pre-allocate) the temporary variables in non-realtime. qtmp_.resize(chain_.getNumOfJoints()); Jtmp_.resize(chain_.getNumOfJoints()); // Verify the number of joints. if (chain_.getNumOfJoints() != Joints) { ROS_ERROR("The chain from '%s' to '%s' does not contain %d joints", root_name.c_str(), tip_name.c_str(), Joints); return false; } // Pick the gains. Kp_(0) = 100.0; Kd_(0) = 1.0; // Translation x Kp_(1) = 100.0; Kd_(1) = 1.0; // Translation y Kp_(2) = 100.0; Kd_(2) = 1.0; // Translation z Kp_(3) = 100.0; Kd_(3) = 1.0; // Rotation x Kp_(4) = 100.0; Kd_(4) = 1.0; // Rotation y Kp_(5) = 100.0; Kd_(5) = 1.0; // Rotation z return true; } /// Controller startup in realtime void MyCartControllerClass::starting() { // Get the current joint values to compute the initial tip location. chain_.getJointPositions(qtmp_); jnt_to_pose_solver_->JntToCart(qtmp_, xtmp_); kin_.KDLtoEigen(xtmp_, x0_); // Initialize the phase of the circle as zero. circle_phase_ = 0.0; // Also reset the time-of-last-servo-cycle. last_time_ = robot_state_->getTime(); } /// Controller update loop in realtime void MyCartControllerClass::update() { double dt; // Servo loop time step JointVector q, qdot, tau; // Joint position, velocity, torques CartPose x, xd; // Tip pose, desired pose Cart6Vector xerr, xdot, F; // Cart error,velocity,effort JacobianMatrix J; // Jacobian // Calculate the dt between servo cycles. dt = (robot_state_->getTime() - last_time_).toSec(); last_time_ = robot_state_->getTime(); // Get the current joint positions and velocities. chain_.getJointPositions(q); chain_.getJointVelocities(qdot); // Compute the forward kinematics and Jacobian (at this location). kin_.EigentoKDL(q, qtmp_); jnt_to_pose_solver_->JntToCart(qtmp_, xtmp_); jnt_to_jac_solver_->JntToJac(qtmp_, Jtmp_); kin_.KDLtoEigen(xtmp_, x); kin_.KDLtoEigen(Jtmp_, J); xdot = J * qdot; // Follow a circle of 10cm at 3 rad/sec. circle_phase_ += 3.0 * dt; Cart3Vector circle(0,0,0); circle.x() = 0.1 * sin(circle_phase_); circle.y() = 0.1 * (1 - cos(circle_phase_)); xd = x0_; xd.translation() += circle; // Calculate a Cartesian restoring force. kin_.computeCartError(x,xd,xerr); F = - Kp_.asDiagonal() * xerr - Kd_.asDiagonal() * xdot; // Convert the force into a set of joint torques. tau = J.transpose() * F; // And finally send these torques out. chain_.setJointEfforts(tau); } /// Controller stopping in realtime void MyCartControllerClass::stopping() {} /// Register controller to pluginlib PLUGINLIB_REGISTER_CLASS(MyCartControllerPlugin, my_controller_ns::MyCartControllerClass, pr2_controller_interface::Controller) }}} In comparison to the source code [[pr2_mechanism/Tutorials/Coding a realtime Cartesian controller with KDL|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 <> * 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. <> <> * 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. <> == Compiling and Running == To compile and run the controller, go back to the [[pr2_mechanism/Tutorials/Implementing a realtime Cartesian controller|previous tutorial]]. ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## WritingYourselfCategory ## CartesianSpaceCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE