ee_cart_imped_control: ee_cart_imped_control: ee_cart_imped

00001 //For class and function comments, see 
00002 //include/ee_cart_imped_control/ee_cart_imped_control.hpp
00003 //or the API docs
00004 //
00005 //The structure of this code borrows from the Realtime controller
00006 //KDL tutorial and the joint trajectory spline controller
00007 
00008 #include "ee_cart_imped_control/ee_cart_imped_control.hpp"
00009 #include <pluginlib/class_list_macros.h>
00010 
00011 using namespace ee_cart_imped_control_ns;
00012 
00013 double EECartImpedControlClass::linearlyInterpolate(double time, 
00014                                                     double startTime, 
00015                                                     double endTime, 
00016                                                     double startValue, 
00017                                                     double endValue) {
00018   return startValue + 
00019     (time - startTime)*
00020     (endValue - startValue)/(endTime - startTime);
00021 }
00022 
00023 ee_cart_imped_msgs::StiffPoint 
00024 EECartImpedControlClass::sampleInterpolation() {
00025 
00026   boost::shared_ptr<const EECartImpedData> desired_poses_ptr;
00027   desired_poses_box_.get(desired_poses_ptr);
00028   if (!desired_poses_ptr) {
00029     ROS_FATAL("ee_cart_imped_control: current trajectory was NULL!");
00030   }
00031   const EECartImpedTraj &desiredPoses = desired_poses_ptr->traj;
00032   const ee_cart_imped_msgs::StiffPoint &initial_point = 
00033     desired_poses_ptr->initial_point;
00034   const ros::Time &current_goal_start_time = desired_poses_ptr->starting_time;
00035   if (desiredPoses.size() == 0) {
00036     ROS_ERROR("ee_cart_imped_control: Empty trajectory");
00037     return last_point_;
00038   }
00039   if (last_goal_starting_time_ != current_goal_start_time.toSec()) {
00040     //we have never seen this goal before
00041     last_point_ = initial_point;
00042     last_point_.time_from_start = ros::Duration(0);
00043   }
00044   last_goal_starting_time_ = current_goal_start_time.toSec();
00045 
00046   // time from the start of the series of points
00047   double time = robot_state_->getTime().toSec();
00048   double timeFromStart = time - current_goal_start_time.toSec();  
00049   ee_cart_imped_msgs::StiffPoint next_point;
00050     
00051 
00052   //Find the correct trajectory segment to use
00053   //We don't want a current_goal_index_ because it has
00054   //the potential to get caught in a bad race condition
00055   int current_goal_index;
00056   for (current_goal_index = 0; 
00057        current_goal_index < (signed int)desiredPoses.size();
00058        current_goal_index++) {
00059     if (desiredPoses[current_goal_index].time_from_start.toSec() >= timeFromStart) {
00060       break;
00061     }
00062   }
00063 
00064   if (current_goal_index >= (signed int)desiredPoses.size()) {
00065     //we're done with the goal, hold the last position
00066     return desiredPoses[current_goal_index-1];
00067   }
00068 
00069   //did we move on to the next point?
00070   if (current_goal_index > 0 && last_point_.time_from_start.toSec() != 
00071       desiredPoses[current_goal_index-1].time_from_start.toSec()) {
00072     //this should be where we CURRENTLY ARE
00073     last_point_.pose.position.x = x_.p(0);
00074     last_point_.pose.position.y = x_.p(1);
00075     last_point_.pose.position.z = x_.p(2);
00076     x_.M.GetQuaternion(last_point_.pose.orientation.x,
00077                        last_point_.pose.orientation.y,
00078                        last_point_.pose.orientation.z,
00079                        last_point_.pose.orientation.w);
00080     last_point_.wrench_or_stiffness = 
00081       desiredPoses[current_goal_index-1].wrench_or_stiffness;
00082     last_point_.isForceX = desiredPoses[current_goal_index-1].isForceX;
00083     last_point_.isForceY = desiredPoses[current_goal_index-1].isForceY;
00084     last_point_.isForceZ = desiredPoses[current_goal_index-1].isForceZ;
00085     last_point_.isTorqueX = desiredPoses[current_goal_index-1].isTorqueX;
00086     last_point_.isTorqueY = desiredPoses[current_goal_index-1].isTorqueY;
00087     last_point_.isTorqueZ = desiredPoses[current_goal_index-1].isTorqueZ;
00088     last_point_.time_from_start = 
00089       desiredPoses[current_goal_index-1].time_from_start;
00090   }
00091 
00092   ee_cart_imped_msgs::StiffPoint start_point;
00093   const ee_cart_imped_msgs::StiffPoint &end_point = 
00094     desiredPoses[current_goal_index];
00095   //actually now last_point_ and initial point should be the
00096   //same if current_goal_index is zero
00097   if (current_goal_index == 0) {
00098     start_point = initial_point;
00099   } else { 
00100     start_point = last_point_;
00101   }
00102 
00103   double segStartTime = 0.0;
00104   if (current_goal_index > 0) {
00105     segStartTime = 
00106       desiredPoses[current_goal_index-1].time_from_start.toSec();
00107   }
00108   double segEndTime = 
00109     desiredPoses[current_goal_index].time_from_start.toSec();
00110   if (segEndTime <= segStartTime) {
00111     //just stay where we currently are
00112     next_point.pose.position.x = x_.p(0);
00113     next_point.pose.position.y = x_.p(1);
00114     next_point.pose.position.z = x_.p(2);
00115     x_.M.GetQuaternion(next_point.pose.orientation.x,
00116                        next_point.pose.orientation.y,
00117                        next_point.pose.orientation.z,
00118                        next_point.pose.orientation.w);
00119 
00120   } else {
00121     next_point.pose.position.x = linearlyInterpolate
00122       (timeFromStart, segStartTime, segEndTime, 
00123        start_point.pose.position.x, end_point.pose.position.x);
00124  
00125     next_point.pose.position.y = linearlyInterpolate
00126       (timeFromStart, segStartTime, segEndTime, 
00127        start_point.pose.position.y, end_point.pose.position.y); 
00128 
00129     next_point.pose.position.z = linearlyInterpolate
00130       (timeFromStart, segStartTime, segEndTime, 
00131        start_point.pose.position.z, end_point.pose.position.z); 
00132 
00133     next_point.pose.orientation.x = linearlyInterpolate
00134       (timeFromStart, segStartTime, segEndTime, 
00135        start_point.pose.orientation.x, end_point.pose.orientation.x); 
00136 
00137     next_point.pose.orientation.y = linearlyInterpolate
00138       (timeFromStart, segStartTime, segEndTime, 
00139        start_point.pose.orientation.y, end_point.pose.orientation.y);
00140  
00141     next_point.pose.orientation.z = linearlyInterpolate
00142       (timeFromStart, segStartTime, segEndTime, 
00143        start_point.pose.orientation.z, end_point.pose.orientation.z); 
00144 
00145     next_point.pose.orientation.w = linearlyInterpolate
00146       (timeFromStart, segStartTime, segEndTime, 
00147        start_point.pose.orientation.w, end_point.pose.orientation.w); 
00148   }
00149   //we don't currently interpolate between wrench
00150   //and stiffness as generally the user wants one
00151   //wrench through a full trajectory point and then
00152   //another at the next trajectory point
00153   //perhaps, however, we should put in some interpolation
00154   //to avoid fast transitions between very different wrenches
00155   //as these can lead to bad behavior
00156   next_point.wrench_or_stiffness = 
00157     desiredPoses[current_goal_index].wrench_or_stiffness;
00158   next_point.isForceX = desiredPoses[current_goal_index].isForceX;
00159   next_point.isForceY = desiredPoses[current_goal_index].isForceY;
00160   next_point.isForceZ = desiredPoses[current_goal_index].isForceZ;
00161   next_point.isTorqueX = desiredPoses[current_goal_index].isTorqueX;
00162   next_point.isTorqueY = desiredPoses[current_goal_index].isTorqueY;
00163   next_point.isTorqueZ = desiredPoses[current_goal_index].isTorqueZ;
00164   next_point.time_from_start = ros::Duration(timeFromStart);
00165   return next_point;
00166 }
00167 
00168 void EECartImpedControlClass::commandCB
00169 (const ee_cart_imped_msgs::EECartImpedGoalConstPtr &msg) {
00170   if ((msg->trajectory).empty()) {
00171     //stop the controller
00172     starting();
00173     return;
00174   }
00175   //this is a new goal
00176   boost::shared_ptr<EECartImpedData> new_traj_ptr
00177     (new EECartImpedData());
00178   if (!new_traj_ptr) {
00179     ROS_ERROR("Null new trajectory.");
00180     starting();
00181     return;
00182   }
00183     
00184   EECartImpedData &new_traj = *new_traj_ptr;
00185   KDL::Frame init_pos;
00186   KDL::JntArray q0(kdl_chain_.getNrOfJoints());
00187   KDL::ChainFkSolverPos_recursive fksolver(kdl_chain_);
00188   //Operation is in fact const (although not listed as such)
00189   read_only_chain_.getPositions(q0);    
00190   fksolver.JntToCart(q0, init_pos);
00191 
00192   new_traj.initial_point.pose.position.x = init_pos.p(0);
00193   new_traj.initial_point.pose.position.y = init_pos.p(1);
00194   new_traj.initial_point.pose.position.z = init_pos.p(2);
00195   init_pos.M.GetQuaternion(new_traj.initial_point.pose.orientation.x,
00196                            new_traj.initial_point.pose.orientation.y,
00197                            new_traj.initial_point.pose.orientation.z,
00198                            new_traj.initial_point.pose.orientation.w);
00199                      
00200   for (size_t i = 0; i < msg->trajectory.size(); i++) {
00201     new_traj.traj.push_back(msg->trajectory[i]);
00202   }
00203   if (!new_traj_ptr) {
00204     ROS_ERROR("Null new trajectory after filling.");
00205     starting();
00206     return;
00207   }
00208   new_traj.starting_time = ros::Time::now();
00209   desired_poses_box_.set(new_traj_ptr);
00210 }
00211 
00212 bool EECartImpedControlClass::init(pr2_mechanism_model::RobotState *robot,
00213         ros::NodeHandle &n) {
00214     std::string root_name, tip_name;
00215     node_ = n;
00216     if (!n.getParam("root_name", root_name))
00217     {
00218         ROS_ERROR("No root name given in namespace: %s)",
00219                 n.getNamespace().c_str());
00220         return false;
00221     }
00222     if (!n.getParam("tip_name", tip_name))
00223     {
00224         ROS_ERROR("No tip name given in namespace: %s)",
00225                 n.getNamespace().c_str());
00226         return false;
00227     }
00228 
00229     // Construct a chain from the root to the tip and prepare the kinematics
00230     // Note the joints must be calibrated
00231     if (!chain_.init(robot, root_name, tip_name))
00232     {
00233         ROS_ERROR("EECartImpedControlClass could not use the chain from '%s' to '%s'",
00234                 root_name.c_str(), tip_name.c_str());
00235         return false;
00236     }
00237 
00238 
00239     if (!read_only_chain_.init(robot, root_name, tip_name))
00240     {
00241         ROS_ERROR
00242           ("EECartImpedControlClass could not use the chain from '%s' to '%s'",
00243            root_name.c_str(), tip_name.c_str());
00244         return false;
00245     }
00246 
00247     // Store the robot handle for later use (to get time)
00248     robot_state_ = robot;
00249 
00250     // Construct the kdl solvers in non-realtime
00251     chain_.toKDL(kdl_chain_);
00252     jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00253     jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
00254 
00255     // Resize (pre-allocate) the variables in non-realtime
00256     q_.resize(kdl_chain_.getNrOfJoints());
00257     qdot_.resize(kdl_chain_.getNrOfJoints());
00258     tau_.resize(kdl_chain_.getNrOfJoints());
00259     tau_act_.resize(kdl_chain_.getNrOfJoints());
00260     J_.resize(kdl_chain_.getNrOfJoints());
00261 
00262     subscriber_ = node_.subscribe("command", 1, &EECartImpedControlClass::commandCB, this);
00263     controller_state_publisher_.reset
00264       (new realtime_tools::RealtimePublisher
00265        <ee_cart_imped_msgs::EECartImpedFeedback>
00266        (node_, "state", 1));
00267     controller_state_publisher_->msg_.requested_joint_efforts.resize
00268       (kdl_chain_.getNrOfJoints());
00269     controller_state_publisher_->msg_.actual_joint_efforts.resize
00270       (kdl_chain_.getNrOfJoints());
00271     updates_ = 0;
00272   
00273     
00274     Kd_.vel(0) = 0.0;        // Translation x                                                   
00275     Kd_.vel(1) = 0.0;        // Translation y
00276     Kd_.vel(2) = 0.0;        // Translation z
00277     Kd_.rot(0) = 0.0;        // Rotation x
00278     Kd_.rot(1) = 0.0;        // Rotation y
00279     Kd_.rot(2) = 0.0;        // Rotation z
00280     
00281     //Create a dummy trajectory
00282     boost::shared_ptr<EECartImpedData> dummy_ptr(new EECartImpedData());
00283     EECartImpedTraj &dummy = dummy_ptr->traj;
00284     dummy.resize(1);
00285     dummy[0].time_from_start = ros::Duration(0);
00286     desired_poses_box_.set(dummy_ptr);
00287     last_goal_starting_time_ = -1;
00288     return true;
00289 }
00290 
00291 void EECartImpedControlClass::starting() {
00292   // Get the current joint values to compute the initial tip location.
00293   KDL::Frame init_pos;
00294   KDL::JntArray q0(kdl_chain_.getNrOfJoints());
00295   KDL::ChainFkSolverPos_recursive fksolver(kdl_chain_);
00296   //this operation is not listed as const but is in fact
00297   //in the current implementation
00298   read_only_chain_.getPositions(q0);
00299   fksolver.JntToCart(q0, init_pos);
00300 
00301 
00302   // Also reset the time-of-last-servo-cycle
00303   last_time_ = robot_state_->getTime();
00305   boost::shared_ptr<EECartImpedData> hold_traj_ptr(new EECartImpedData());
00306   if (!hold_traj_ptr) {
00307     ROS_ERROR("While starting, trajectory pointer was null");
00308     return;
00309   }
00310   EECartImpedData &hold_traj = *hold_traj_ptr;
00311   hold_traj.traj.resize(1);
00312   
00313   hold_traj.traj[0].pose.position.x = init_pos.p(0);
00314   hold_traj.traj[0].pose.position.y = init_pos.p(1);
00315   hold_traj.traj[0].pose.position.z = init_pos.p(2);
00316   init_pos.M.GetQuaternion((hold_traj.traj[0].pose.orientation.x), 
00317                            (hold_traj.traj[0].pose.orientation.y), 
00318                            (hold_traj.traj[0].pose.orientation.z), 
00319                            (hold_traj.traj[0].pose.orientation.w));
00320   hold_traj.traj[0].wrench_or_stiffness.force.x = MAX_STIFFNESS;
00321   hold_traj.traj[0].wrench_or_stiffness.force.y = MAX_STIFFNESS;
00322   hold_traj.traj[0].wrench_or_stiffness.force.z = MAX_STIFFNESS;
00323   hold_traj.traj[0].wrench_or_stiffness.torque.x = ACCEPTABLE_ROT_STIFFNESS;
00324   hold_traj.traj[0].wrench_or_stiffness.torque.y = ACCEPTABLE_ROT_STIFFNESS;
00325   hold_traj.traj[0].wrench_or_stiffness.torque.z = ACCEPTABLE_ROT_STIFFNESS;
00326   hold_traj.traj[0].isForceX = false;
00327   hold_traj.traj[0].isForceY = false;
00328   hold_traj.traj[0].isForceZ = false;
00329   hold_traj.traj[0].isTorqueX = false;
00330   hold_traj.traj[0].isTorqueY = false;
00331   hold_traj.traj[0].isTorqueZ = false;
00332   hold_traj.traj[0].time_from_start = ros::Duration(0);
00333   hold_traj.initial_point = hold_traj.traj[0];
00334   hold_traj.starting_time = ros::Time::now();
00335   if (!hold_traj_ptr) {
00336     ROS_ERROR("During starting hold trajectory was null after filling");
00337     return;
00338   }
00339   //Pass the trajectory through to the update loop
00340   desired_poses_box_.set(hold_traj_ptr);
00341 }
00342 
00343 void EECartImpedControlClass::update()
00344 {
00345     last_time_ = robot_state_->getTime();
00346 
00347     // Get the current joint positions and velocities
00348     chain_.getPositions(q_);
00349     chain_.getVelocities(qdot_);
00350 
00351     // Compute the forward kinematics and Jacobian (at this location)
00352     jnt_to_pose_solver_->JntToCart(q_, x_);
00353     jnt_to_jac_solver_->JntToJac(q_, J_);
00354 
00355     for (unsigned int i = 0 ; i < 6 ; i++)
00356     {
00357         xdot_(i) = 0;
00358         for (unsigned int j = 0 ; j < kdl_chain_.getNrOfJoints() ; j++)
00359             xdot_(i) += J_(i,j) * qdot_.qdot(j);
00360     }
00361 
00362     ee_cart_imped_msgs::StiffPoint desiredPose = sampleInterpolation();
00363     
00364 
00365     Fdes_(0) = desiredPose.wrench_or_stiffness.force.x;
00366     Fdes_(1) = desiredPose.wrench_or_stiffness.force.y;
00367     Fdes_(2) = desiredPose.wrench_or_stiffness.force.z;
00368     Fdes_(3) = desiredPose.wrench_or_stiffness.torque.x;
00369     Fdes_(4) = desiredPose.wrench_or_stiffness.torque.y;
00370     Fdes_(5) = desiredPose.wrench_or_stiffness.torque.z;
00371 
00372     Kp_.vel(0) = desiredPose.wrench_or_stiffness.force.x;
00373     Kp_.vel(1) = desiredPose.wrench_or_stiffness.force.y;
00374     Kp_.vel(2) = desiredPose.wrench_or_stiffness.force.z;
00375     Kp_.rot(0) = desiredPose.wrench_or_stiffness.torque.x;
00376     Kp_.rot(1) = desiredPose.wrench_or_stiffness.torque.y;
00377     Kp_.rot(2) = desiredPose.wrench_or_stiffness.torque.z;
00378 
00379     xd_.p(0) = desiredPose.pose.position.x;
00380     xd_.p(1) = desiredPose.pose.position.y;
00381     xd_.p(2) = desiredPose.pose.position.z;
00382     xd_.M = KDL::Rotation::Quaternion(desiredPose.pose.orientation.x, 
00383                                       desiredPose.pose.orientation.y, 
00384                                       desiredPose.pose.orientation.z, 
00385                                       desiredPose.pose.orientation.w);
00386 
00387     // Calculate a Cartesian restoring force.
00388     xerr_.vel = x_.p - xd_.p;
00389     xerr_.rot = 0.5 * (xd_.M.UnitX() * x_.M.UnitX() +
00390             xd_.M.UnitY() * x_.M.UnitY() +
00391             xd_.M.UnitZ() * x_.M.UnitZ());  // I have no idea what this is
00392 
00393 
00394     // F_ is a vector of forces/wrenches corresponding to x, y, z, tx,ty,tz,tw
00395     if (desiredPose.isForceX) {
00396       F_(0) = Fdes_(0);
00397     } else {
00398       F_(0) = -Kp_(0) * xerr_(0) - Kd_(0)*xdot_(0);
00399     }
00400 
00401     if (desiredPose.isForceY) {
00402       F_(1) = Fdes_(1);
00403     } else {
00404       F_(1) = -Kp_(1) * xerr_(1) - Kd_(1)*xdot_(1);
00405     }
00406 
00407     if (desiredPose.isForceZ) {
00408       F_(2) = Fdes_(2);
00409     } else {
00410       F_(2) = -Kp_(2) * xerr_(2) - Kd_(2)*xdot_(2);
00411     }
00412 
00413     if (desiredPose.isTorqueX) {
00414       F_(3) = Fdes_(3);
00415     } else {
00416       F_(3) = -Kp_(3) * xerr_(3) - Kd_(3)*xdot_(3);
00417     }
00418 
00419     if (desiredPose.isTorqueY) {
00420       F_(4) = Fdes_(4);
00421     } else {
00422       F_(4) = -Kp_(4) * xerr_(4) - Kd_(4)*xdot_(4);
00423     }
00424 
00425     if (desiredPose.isTorqueZ) {
00426       F_(5) = Fdes_(5);
00427     } else {
00428       F_(5) = -Kp_(5) * xerr_(5) - Kd_(5)*xdot_(5);
00429     }
00430 
00431     // Convert the force into a set of joint torques
00432     // tau_ is a vector of joint torques q1...qn
00433     for (unsigned int i = 0 ; i < kdl_chain_.getNrOfJoints() ; i++) {
00434         // iterate through the vector.  every joint torque is contributed to
00435         // by the Jacobian Transpose (note the index switching in J access) times
00436         // the desired force (from impedance OR explicit force)
00437 
00438         // So if a desired end effector wrench is specified, there is no position control on that dof
00439         // if a wrench is not specified, then there is impedance based (basically p-gain) 
00440       //position control on that dof
00441         tau_(i) = 0;
00442         for (unsigned int j = 0 ; j < 6 ; j++) {
00443             tau_(i) += J_(j,i) * F_(j);
00444         }
00445     }
00446 
00447     // And finally send these torques out
00448     chain_.setEfforts(tau_);
00449 
00450     //publish the current state
00451     if (!(updates_ % 10)) {
00452       if (controller_state_publisher_ && 
00453           controller_state_publisher_->trylock()) {
00454         controller_state_publisher_->msg_.header.stamp = last_time_;
00455         controller_state_publisher_->msg_.desired = 
00456           desiredPose;
00457         controller_state_publisher_->msg_.actual_pose.pose.position.x = 
00458           x_.p.x();
00459         controller_state_publisher_->msg_.actual_pose.pose.position.y = 
00460           x_.p.y();
00461         controller_state_publisher_->msg_.actual_pose.pose.position.z = 
00462           x_.p.z();
00463         x_.M.GetQuaternion
00464           (controller_state_publisher_->msg_.actual_pose.pose.orientation.x,
00465            controller_state_publisher_->msg_.actual_pose.pose.orientation.y,
00466            controller_state_publisher_->msg_.actual_pose.pose.orientation.z,
00467            controller_state_publisher_->msg_.actual_pose.pose.orientation.w);
00468         controller_state_publisher_->msg_.actual_pose.
00469           wrench_or_stiffness.force.x = F_(0);
00470         controller_state_publisher_->msg_.actual_pose.
00471           wrench_or_stiffness.force.y = F_(1);
00472         controller_state_publisher_->msg_.actual_pose.
00473           wrench_or_stiffness.force.z = F_(2);
00474         controller_state_publisher_->msg_.actual_pose.
00475           wrench_or_stiffness.torque.x = F_(3);
00476         controller_state_publisher_->msg_.actual_pose.
00477           wrench_or_stiffness.torque.y = F_(4);
00478         controller_state_publisher_->msg_.actual_pose.
00479           wrench_or_stiffness.torque.z = F_(5);
00480         chain_.getEfforts(tau_act_);
00481         double eff_err = 0;
00482         for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++) {
00483           eff_err += (tau_(i) - tau_act_(i))*(tau_(i) - tau_act_(i));
00484           controller_state_publisher_->msg_.requested_joint_efforts[i] = 
00485             tau_(i);
00486           controller_state_publisher_->msg_.actual_joint_efforts[i] = 
00487             tau_act_(i);
00488         }
00489         controller_state_publisher_->msg_.effort_sq_error = eff_err;
00490         boost::shared_ptr<const EECartImpedData> desired_poses_ptr;
00491         desired_poses_box_.get(desired_poses_ptr);
00492         controller_state_publisher_->msg_.goal = desired_poses_ptr->traj;
00493         controller_state_publisher_->msg_.initial_point = last_point_;
00494         controller_state_publisher_->msg_.running_time =
00495           robot_state_->getTime() - desired_poses_ptr->starting_time;
00496         controller_state_publisher_->unlockAndPublish();
00497       }
00498     }
00499     updates_++;
00500 }
00501 
00502 void EECartImpedControlClass::stopping() {
00503   starting();
00504 }
00505 
00506 
00508 PLUGINLIB_DECLARE_CLASS(ee_cart_imped_control, EECartImpedControlPlugin,
00509                         ee_cart_imped_control_ns::EECartImpedControlClass,
00510                         pr2_controller_interface::Controller)
00511
ee_cart_imped_control.cpp
