ee_cart_imped_control.hpp

Go to the documentation of this file.

#ifndef __EE_CART_IMPED_CONTROL_HPP__
#define __EE_CART_IMPED_CONTROL_HPP__

#include <pr2_controller_interface/controller.h>
#include <pr2_mechanism_model/chain.h>
#include <pr2_mechanism_model/robot.h>

#include <boost/scoped_ptr.hpp>
#include <kdl/chain.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/frames.hpp>
#include <kdl/jacobian.hpp>
#include <kdl/jntarray.hpp>

#include <realtime_tools/realtime_publisher.h>
#include <realtime_tools/realtime_box.h>

#include <ros/ros.h>
#include <ee_cart_imped_msgs/EECartImpedAction.h>
#include <ee_cart_imped_msgs/EECartImpedGoal.h>
#include <ee_cart_imped_msgs/StiffPoint.h>

//Doxygen doesn't like these comments for some reason...
#define MAX_STIFFNESS 1000.0

#define ACCEPTABLE_ROT_STIFFNESS 100.0

namespace ee_cart_imped_control_ns {

  class EECartImpedControlClass: public pr2_controller_interface::Controller {
  private:
    
    //(to get the time stamp)
    // Read-only after initialization
    pr2_mechanism_model::RobotState* robot_state_;
    
    pr2_mechanism_model::Chain chain_;
    //Read-only after initialization
    pr2_mechanism_model::Chain read_only_chain_;
    // Read-only after initialization
    KDL::Chain kdl_chain_;
    
    // Referenced only in update loop
    boost::scoped_ptr<KDL::ChainFkSolverPos>    jnt_to_pose_solver_;
    // Referenced only in update loop
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
    
    // The variables (which are declared as class variables
    // because they need to be pre-allocated, meaning we must
    // be very careful how we access them to avoid race conditions)
    
    // Referenced only in update loop
    KDL::JntArray  q_; 
    // Referenced only in update loop
    KDL::JntArrayVel  qdot_;
    // Referenced only in update loop
    KDL::JntArray  tau_, tau_act_;
    
    // Referenced only in update loop
    KDL::Frame     x_; 
    // Referenced only in update loop
    KDL::Frame     xd_;
    
    // Referenced only in update loop
    KDL::Twist     xerr_;
    // Referenced only in update loop
    KDL::Twist     xdot_;
    // Referenced only in update loop
    KDL::Wrench    F_;  
    // Referenced only in update loop
    KDL::Wrench    Fdes_;
    // Referenced only in update loop
    KDL::Jacobian  J_;         

    //Referenced only in update loop
    double last_goal_starting_time_;
    
    typedef std::vector<ee_cart_imped_msgs::StiffPoint> EECartImpedTraj;
    
    //This is a class so as to be able to lock and unlock this data
    //together easily
    class EECartImpedData {
    public:
      EECartImpedTraj traj;
      ee_cart_imped_msgs::StiffPoint initial_point;
      ros::Time starting_time;
      EECartImpedData() {}
    };

    //This needs to be in realtime boxes because
    //we reference it in the update loop and modify 
    //it it in the command callback
    realtime_tools::RealtimeBox<
      boost::shared_ptr<const EECartImpedData> > desired_poses_box_;
    
    //Referenced only in updated loop
    ee_cart_imped_msgs::StiffPoint last_point_;

    // Note the gains are incorrectly typed as a twist,
    // as there is no appropriate type!
    //Referenced only in update loop
    KDL::Twist     Kp_;
    //Referenced only in update loop
    KDL::Twist     Kd_;   
    
    //Modified in both command callback
    //and update loop, but it does not matter
    ros::Time last_time_;

    double linearlyInterpolate(double time, double startTime, 
                               double endTime, double startValue, 
                               double endValue);
    
    
    void commandCB(const ee_cart_imped_msgs::EECartImpedGoalConstPtr &msg);
    ros::NodeHandle node_;
    ros::Subscriber subscriber_;
    

    ee_cart_imped_msgs::StiffPoint sampleInterpolation();
    
    boost::scoped_ptr<
      realtime_tools::RealtimePublisher<
        ee_cart_imped_msgs::EECartImpedFeedback> > 
    controller_state_publisher_;
    
    //Referenced only in updates
    int updates_;
  public:
    bool init(pr2_mechanism_model::RobotState *robot,
              ros::NodeHandle &n);
    void starting();
    
    void update();

    void stopping();
    
  };
}

#endif //ee_cart_imped_control.hpp