$search
00001 #ifndef __EE_CART_IMPED_CONTROL_HPP__ 00002 #define __EE_CART_IMPED_CONTROL_HPP__ 00003 00004 #include <pr2_controller_interface/controller.h> 00005 #include <pr2_mechanism_model/chain.h> 00006 #include <pr2_mechanism_model/robot.h> 00007 00008 #include <boost/scoped_ptr.hpp> 00009 #include <kdl/chain.hpp> 00010 #include <kdl/chainjnttojacsolver.hpp> 00011 #include <kdl/chainfksolverpos_recursive.hpp> 00012 #include <kdl/frames.hpp> 00013 #include <kdl/jacobian.hpp> 00014 #include <kdl/jntarray.hpp> 00015 00016 #include <realtime_tools/realtime_publisher.h> 00017 #include <realtime_tools/realtime_box.h> 00018 00019 #include <ros/ros.h> 00020 #include <ee_cart_imped_msgs/EECartImpedAction.h> 00021 #include <ee_cart_imped_msgs/EECartImpedGoal.h> 00022 #include <ee_cart_imped_msgs/StiffPoint.h> 00023 00024 //Doxygen doesn't like these comments for some reason... 00026 #define MAX_STIFFNESS 1000.0 00027 00029 #define ACCEPTABLE_ROT_STIFFNESS 100.0 00030 00031 namespace ee_cart_imped_control_ns { 00032 00041 class EECartImpedControlClass: public pr2_controller_interface::Controller { 00042 private: 00043 00045 //(to get the time stamp) 00046 // Read-only after initialization 00047 pr2_mechanism_model::RobotState* robot_state_; 00048 00050 pr2_mechanism_model::Chain chain_; 00052 //Read-only after initialization 00053 pr2_mechanism_model::Chain read_only_chain_; 00055 // Read-only after initialization 00056 KDL::Chain kdl_chain_; 00057 00059 // Referenced only in update loop 00060 boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_; 00062 // Referenced only in update loop 00063 boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_; 00064 00065 // The variables (which are declared as class variables 00066 // because they need to be pre-allocated, meaning we must 00067 // be very careful how we access them to avoid race conditions) 00068 00070 // Referenced only in update loop 00071 KDL::JntArray q_; 00073 // Referenced only in update loop 00074 KDL::JntArrayVel qdot_; 00076 // Referenced only in update loop 00077 KDL::JntArray tau_, tau_act_; 00078 00080 // Referenced only in update loop 00081 KDL::Frame x_; 00083 // Referenced only in update loop 00084 KDL::Frame xd_; 00085 00087 // Referenced only in update loop 00088 KDL::Twist xerr_; 00090 // Referenced only in update loop 00091 KDL::Twist xdot_; 00093 // Referenced only in update loop 00094 KDL::Wrench F_; 00096 // Referenced only in update loop 00097 KDL::Wrench Fdes_; 00099 // Referenced only in update loop 00100 KDL::Jacobian J_; 00101 00103 //Referenced only in update loop 00104 double last_goal_starting_time_; 00105 00106 typedef std::vector<ee_cart_imped_msgs::StiffPoint> EECartImpedTraj; 00107 00109 //This is a class so as to be able to lock and unlock this data 00110 //together easily 00111 class EECartImpedData { 00112 public: 00114 EECartImpedTraj traj; 00116 ee_cart_imped_msgs::StiffPoint initial_point; 00118 ros::Time starting_time; 00119 EECartImpedData() {} 00120 }; 00121 00123 //This needs to be in realtime boxes because 00124 //we reference it in the update loop and modify 00125 //it it in the command callback 00126 realtime_tools::RealtimeBox< 00127 boost::shared_ptr<const EECartImpedData> > desired_poses_box_; 00128 00130 //Referenced only in updated loop 00131 ee_cart_imped_msgs::StiffPoint last_point_; 00132 00133 // Note the gains are incorrectly typed as a twist, 00134 // as there is no appropriate type! 00136 //Referenced only in update loop 00137 KDL::Twist Kp_; 00139 //Referenced only in update loop 00140 KDL::Twist Kd_; 00141 00143 //Modified in both command callback 00144 //and update loop, but it does not matter 00145 ros::Time last_time_; 00146 00161 double linearlyInterpolate(double time, double startTime, 00162 double endTime, double startValue, 00163 double endValue); 00164 00165 00175 void commandCB(const ee_cart_imped_msgs::EECartImpedGoalConstPtr &msg); 00176 ros::NodeHandle node_; 00177 ros::Subscriber subscriber_; 00178 00179 00195 ee_cart_imped_msgs::StiffPoint sampleInterpolation(); 00196 00198 boost::scoped_ptr< 00199 realtime_tools::RealtimePublisher< 00200 ee_cart_imped_msgs::EECartImpedFeedback> > 00201 controller_state_publisher_; 00202 00204 //Referenced only in updates 00205 int updates_; 00206 public: 00220 bool init(pr2_mechanism_model::RobotState *robot, 00221 ros::NodeHandle &n); 00230 void starting(); 00231 00243 void update(); 00244 00252 void stopping(); 00253 00254 }; 00255 } 00256 00257 #endif //ee_cart_imped_control.hpp