00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef PR2_CONTROLLERS_JT_CARTESIAN_CONTROLLER_H
00038 #define PR2_CONTROLLERS_JT_CARTESIAN_CONTROLLER_H
00039
00040 #include <Eigen/Core>
00041 #include <Eigen/Geometry>
00042
00043 #include <ros/ros.h>
00044
00045 #include <control_toolbox/pid.h>
00046 #include <eigen_conversions/eigen_kdl.h>
00047 #include <eigen_conversions/eigen_msg.h>
00048 #include <kdl/chainfksolver.hpp>
00049 #include <kdl/chainfksolverpos_recursive.hpp>
00050 #include <kdl/chain.hpp>
00051 #include <kdl/chainjnttojacsolver.hpp>
00052 #include <kdl/frames.hpp>
00053 #include <pr2_controller_interface/controller.h>
00054 #include <pr2_mechanism_model/chain.h>
00055 #include <realtime_tools/realtime_publisher.h>
00056 #include <tf/transform_listener.h>
00057
00058 #include <geometry_msgs/PoseStamped.h>
00059 #include <geometry_msgs/TwistStamped.h>
00060 #include <robot_mechanism_controllers/JTCartesianControllerState.h>
00061
00062
00063 namespace controller {
00064
00065 template <int Joints>
00066 struct Kin
00067 {
00068 typedef Eigen::Matrix<double, Joints, 1> JointVec;
00069 typedef Eigen::Matrix<double, 6, Joints> Jacobian;
00070
00071 Kin(const KDL::Chain &kdl_chain) :
00072 fk_solver_(kdl_chain), jac_solver_(kdl_chain),
00073 kdl_q(Joints), kdl_J(Joints)
00074 {
00075 }
00076 ~Kin()
00077 {
00078 }
00079
00080 void fk(const JointVec &q, Eigen::Affine3d &x)
00081 {
00082 kdl_q.data = q;
00083 KDL::Frame kdl_x;
00084 fk_solver_.JntToCart(kdl_q, kdl_x);
00085 tf::transformKDLToEigen(kdl_x, x);
00086 }
00087 void jac(const JointVec &q, Jacobian &J)
00088 {
00089 kdl_q.data = q;
00090 jac_solver_.JntToJac(kdl_q, kdl_J);
00091 J = kdl_J.data;
00092 }
00093
00094 KDL::ChainFkSolverPos_recursive fk_solver_;
00095 KDL::ChainJntToJacSolver jac_solver_;
00096 KDL::JntArray kdl_q;
00097 KDL::Jacobian kdl_J;
00098 };
00099
00100 class JTCartesianController : public pr2_controller_interface::Controller
00101 {
00102 public:
00103
00104
00105 EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00106 private:
00107 enum { Joints = 7 };
00108 typedef Eigen::Matrix<double, Joints, 1> JointVec;
00109 typedef Eigen::Matrix<double, 6, 1> CartVec;
00110 typedef Eigen::Matrix<double, 6, Joints> Jacobian;
00111 typedef robot_mechanism_controllers::JTCartesianControllerState StateMsg;
00112 public:
00113 JTCartesianController();
00114 ~JTCartesianController();
00115
00116 bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);
00117 void starting();
00118 void update();
00119
00120 Eigen::Affine3d x_desi_, x_desi_filtered_;
00121 CartVec wrench_desi_;
00122
00123 ros::NodeHandle node_;
00124 ros::Subscriber sub_gains_;
00125 ros::Subscriber sub_posture_;
00126 ros::Subscriber sub_pose_;
00127 tf::TransformListener tf_;
00128
00129 realtime_tools::RealtimePublisher<StateMsg> pub_state_;
00130 realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> pub_x_desi_;
00131
00132 std::string root_name_, tip_name_;
00133 ros::Time last_time_;
00134 int loop_count_;
00135 pr2_mechanism_model::RobotState *robot_state_;
00136
00137 pr2_mechanism_model::Chain chain_;
00138 boost::scoped_ptr<Kin<Joints> > kin_;
00139 Eigen::Matrix<double,6,1> Kp, Kd;
00140 double pose_command_filter_;
00141 double vel_saturation_trans_, vel_saturation_rot_;
00142 JointVec saturation_;
00143 JointVec joint_dd_ff_;
00144 double joint_vel_filter_;
00145 double jacobian_inverse_damping_;
00146 JointVec q_posture_;
00147 double k_posture_;
00148 bool use_posture_;
00149
00150
00151 double res_force_, res_position_;
00152 double res_torque_, res_orientation_;
00153
00154 Eigen::Affine3d last_pose_;
00155 CartVec last_wrench_;
00156
00157 JointVec qdot_filtered_;
00158
00159 void setGains(const std_msgs::Float64MultiArray::ConstPtr &msg)
00160 {
00161 if (msg->data.size() >= 6)
00162 for (size_t i = 0; i < 6; ++i)
00163 Kp[i] = msg->data[i];
00164 if (msg->data.size() == 12)
00165 for (size_t i = 0; i < 6; ++i)
00166 Kd[i] = msg->data[6+i];
00167
00168 ROS_INFO("New gains: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
00169 Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
00170 }
00171
00172 void commandPosture(const std_msgs::Float64MultiArray::ConstPtr &msg)
00173 {
00174 if (msg->data.size() == 0) {
00175 use_posture_ = false;
00176 ROS_INFO("Posture turned off");
00177 }
00178 else if ((int)msg->data.size() != q_posture_.size()) {
00179 ROS_ERROR("Posture message had the wrong size: %d", (int)msg->data.size());
00180 return;
00181 }
00182 else
00183 {
00184 use_posture_ = true;
00185 for (int j = 0; j < Joints; ++j)
00186 q_posture_[j] = msg->data[j];
00187 }
00188 }
00189
00190 void commandPose(const geometry_msgs::PoseStamped::ConstPtr &command)
00191 {
00192 geometry_msgs::PoseStamped in_root;
00193 try {
00194 tf_.waitForTransform(root_name_, command->header.frame_id, command->header.stamp, ros::Duration(0.1));
00195 tf_.transformPose(root_name_, *command, in_root);
00196 }
00197 catch (const tf::TransformException &ex)
00198 {
00199 ROS_ERROR("Failed to transform: %s", ex.what());
00200 return;
00201 }
00202
00203 tf::poseMsgToEigen(in_root.pose, x_desi_);
00204 }
00205 };
00206
00207 }
00208
00209 #endif