r2_impedance_controller.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, General Motors.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Darren Earl, Stephen Hart
00032  */
00033 
00034 #pragma once
00035 
00036 #include "ros/ros.h"
00037 #include <realtime_tools/realtime_publisher.h>
00038 #include <tf/message_filter.h>
00039 #include <tf/transform_datatypes.h>
00040 #include <tf/transform_listener.h>
00041 
00042 #include "kdl/tree.hpp"
00043 #include "kdl/chain.hpp"
00044 #include "kdl/jntarray.hpp"
00045 #include "kdl/jacobian.hpp"
00046 #include "kdl/chainjnttojacsolver.hpp"
00047 #include "kdl/chainfksolverpos_recursive.hpp"
00048 #include "kdl/chainfksolvervel_recursive.hpp"
00049 #include "kdl/chainidsolver_recursive_newton_euler.hpp"
00050 #include "treeidsolver_recursive_newton_euler.hpp"
00051 #include <vector>
00052 
00053 #include <Eigen/Geometry>
00054 #include <boost/thread/mutex.hpp>
00055 
00056 #include <message_filters/subscriber.h>
00057 #include <pr2_controller_interface/controller.h>
00058 #include <pr2_mechanism_model/chain.h>
00059 #include <realtime_tools/realtime_publisher.h>
00060 #include <algorithm>
00061 
00063 #include <sensor_msgs/JointState.h>
00064 #include <geometry_msgs/PoseStamped.h>
00065 #include <geometry_msgs/Twist.h>
00066 #include <r2_msgs/Gains.h>
00067 #include <r2_msgs/SetTipName.h>
00068 #include <r2_msgs/SetJointMode.h>
00069 #include <r2_msgs/Power.h>
00070 #include <r2_msgs/Servo.h>
00071 #include <r2_msgs/PoseTwistStamped.h>
00072 
00073 #include "TreeChain.h"
00074 #include "WholeBodyCalc.h"
00075 
00076 
00077 
00078 namespace r2_controller_ns {
00079 class R2ImpedanceController: public pr2_controller_interface::Controller{
00080         
00081         //ros messaging
00082         ros::NodeHandle node;
00083         tf::TransformListener tfListener;
00084         boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> >       left_tip_pose_publisher;
00085         boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist> >             left_pose_error_publisher;
00086         ros::Subscriber                                                                         joint_command_sub;
00087         ros::Subscriber                                                                         left_joint_command_sub;
00088         
00089         message_filters::Subscriber<geometry_msgs::PoseStamped>                                 left_pose_command_sub;
00090         boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PoseStamped> >                       left_pose_command_filter;
00091         message_filters::Subscriber<r2_msgs::PoseTwistStamped>                                  left_pose_vel_command_sub;
00092         boost::scoped_ptr<tf::MessageFilter<r2_msgs::PoseTwistStamped> >                        left_pose_vel_command_filter;
00093         
00094         boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::PoseStamped> >       right_tip_pose_publisher;
00095         boost::scoped_ptr<realtime_tools::RealtimePublisher<geometry_msgs::Twist> >             right_pose_error_publisher;
00096         ros::Subscriber                                                                         right_joint_command_sub;
00097         
00098         message_filters::Subscriber<geometry_msgs::PoseStamped>                                 right_pose_command_sub;
00099         boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PoseStamped> >                       right_pose_command_filter;
00100         message_filters::Subscriber<r2_msgs::PoseTwistStamped>                                  right_pose_vel_command_sub;
00101         boost::scoped_ptr<tf::MessageFilter<r2_msgs::PoseTwistStamped> >                        right_pose_vel_command_filter;
00102         
00103         
00104         
00105         ros::Subscriber neck_joint_command_sub;
00106         ros::Subscriber waist_joint_command_sub;
00107         
00108         ros::Subscriber set_gains_sub;
00109         boost::scoped_ptr<realtime_tools::RealtimePublisher<r2_msgs::Gains> > gains_publisher;
00110         
00111         ros::ServiceServer srv_set_joint_mode;
00112         ros::ServiceServer srv_set_tip_name;
00113         ros::ServiceServer srv_set_power;
00114         ros::ServiceServer srv_set_servo;
00115         public:
00116         class CtrlCalc{ 
00117         public:
00119                 std::string root_name;
00120         
00121                 //guess neck isn't used yet
00122                 KDL::Tree robot_tree;
00123                 TreeChain left;
00124                 TreeChain right;
00125                 TreeChain neck;
00126         
00128                 Eigen::Matrix<double,7,1> leftCmd;
00129                 Eigen::Matrix<double,7,1> rightCmd;
00130                 Eigen::Matrix<double,7,1> neckCmd;
00131                 KDL::Twist leftVelCmd;
00132                 KDL::Twist rightVelCmd;
00133                 KDL::Twist neckVelCmd;
00134                 
00135                 bool left_cart;         //< flag for left cartesian mode
00136                 bool left_cart_vel;     //< flag for using velocity in left cart mode
00137                 bool right_cart;        //< flag for right cartesian mode
00138                 bool right_cart_vel;//< flag for using velocity in right cart mode
00139                 bool neck_cart;         //< flag for neck cartesian mode
00140                 bool neck_cart_vel;     //< flag for neck velocity in neck cart mode
00141                 std::vector<int> joint_pos_control; //< flags for p control on joint pos
00142                 std::vector<int> joint_vel_control; //< flags for adjusting d control to joint vel
00143                 
00144                 WholeBodyCalc wbc; //< performs nullspace calculations
00145         
00146                 //gains
00147                 std::vector<double> D_high;             //< derivative gains 
00148                 std::vector<double> D_low;                      //< derivative gains in cart mode
00149                 std::vector<double> K_high;             //< proportional gains
00150                 std::vector<double> K_low;                      //< proportional gains in cart mode
00151                 std::vector<double> D;                          //< current proportional gain value
00152                 std::vector<double> K;                          //< current proportional gain value
00153                 std::vector<double> desired;            //< desired joint positions
00154                 std::vector<double> desiredVel; //< desired joint velocity
00155                 int jnt_size;                                           //< number of joints in tree
00156         
00157                 std::vector<double> cartK_left; //< cartesian gains left
00158                 std::vector<double> cartK_right;        //< cartesian gains right
00159                 std::vector<double> cartD_left; //< cartesian gains left
00160                 std::vector<double> cartD_right;        //< cartesian gains right
00161         
00162         
00164                 template<int _N>
00165                 class AvgV{
00166                         double data[_N];
00167                         mutable double sorted[_N];
00168                         mutable bool update;
00169                         static const int N = _N;
00170                         //static const bool EVEN = ((N+1)/2 == (N/2));
00171                         int idx;
00172                 public:
00173                         AvgV():update(false),idx(0){}
00174                         operator double()const{
00175                                 if( update ){
00176                                         update = false;
00177                                         for(int x=0; x<N; ++x)
00178                                                 sorted[x] = data[x];
00179                                         std::sort(sorted,sorted+N);
00180                                 }
00181                                 return sorted[N/2]; // if N==3, N/2 == 1; if N==4, N/2 == 2; both are fine
00182                         }
00183                         void operator=(double in){
00184                                 data[idx++] = in;
00185                                 if( idx >= N )
00186                                         idx =0;
00187                                 update = true;
00188                         }
00189                 };
00190                 std::vector< AvgV<3> >treeJntsAvg;
00191                 std::vector< AvgV<3> >treeJntsVelAvg;
00192         
00193                 std::vector<double> treeJnts;   //<conveniance variable, joint positions
00194                 std::vector<double> treeJntsVel;//<conveniance variable, joint velocity
00195                 std::vector<double> jntsUpperLimit;//< upper limit
00196                 std::vector<double> jntsLowerLimit;//< lower limit
00197                 std::vector<double> jntsCenterPoint;//< middle point
00198                 std::map< std::string, int> name2idx;   //< name to index lookup map
00199                 std::vector<std::string> idx2name;
00200         
00201                 KDL::JntArray torques;
00202                 
00203                 boost::scoped_ptr<KDL::TreeIdSolver> rne_calc;
00204                 
00205                 KDL::JntArray jointKCmd(const std::vector<double>& q);
00206                 KDL::JntArray jointDCmd(const std::vector<double>& qd);
00207                 
00209                 void activate( TreeChain& tc, bool& flag, Eigen::Matrix<double,7,1>& pose_cmd );
00211                 void reactivate();
00212                 
00213                 void init(double gravity[3]);
00214                 void calculate();
00215         };
00216         private:
00217         CtrlCalc cc;
00218         
00219         
00220         
00221         //joints have a unique index value which is preserved across these arrays
00222         std::vector<pr2_mechanism_model::JointState*> robotStateJoints; //< queries robot state and commands joints
00223 
00224         
00225         pr2_mechanism_model::RobotState* robot_state; //< 
00226         ros::Time time;
00227         
00228         // ros message functions
00229         void init_ros_msgs();
00230         void publish_msgs();
00231         void joint_left_command(const sensor_msgs::JointState::ConstPtr& msg );
00232         void joint_right_command(const sensor_msgs::JointState::ConstPtr& msg );
00233         void joint_neck_command(const sensor_msgs::JointState::ConstPtr& msg );
00234         void joint_waist_command(const sensor_msgs::JointState::ConstPtr& msg );
00235         void joint_command( const sensor_msgs::JointState::ConstPtr& msg );
00236         
00237         void joint_command_entry( const std::string& name, double value, std::vector<double>& desired );
00238         void joint_command_entry( const std::string& name, bool value, std::vector<int>& desired );
00239         
00240         void pose_left_command(const geometry_msgs::PoseStamped::ConstPtr& msg);
00241         void pose_right_command(const geometry_msgs::PoseStamped::ConstPtr& msg);
00242 
00243         void pose_vel_left_command(const r2_msgs::PoseTwistStamped::ConstPtr& msg );
00244         void pose_vel_right_command(const r2_msgs::PoseTwistStamped::ConstPtr& msg );
00245         void pose_vel_command_inner(    const r2_msgs::PoseTwistStamped::ConstPtr& msg,
00246                                         Eigen::Matrix<double,7,1>& cmd,
00247                                         KDL::Twist& velCmd,
00248                                         bool& cart_vel );
00249 
00250         KDL::Frame transformPoseMsg(const geometry_msgs::PoseStamped::ConstPtr& msg);
00251         void set_gains(const r2_msgs::Gains::ConstPtr& msg );
00252 
00253         // service functions
00254         bool set_joint_mode(r2_msgs::SetJointMode::Request  &req,  r2_msgs::SetJointMode::Response &res );
00255         bool set_tip_name(r2_msgs::SetTipName::Request &req,  r2_msgs::SetTipName::Response &res );
00256         bool set_power(r2_msgs::Power::Request &req,  r2_msgs::Power::Response &res );
00257         bool set_servo(r2_msgs::Servo::Request &req,  r2_msgs::Servo::Response &res );
00258         
00259         
00260         
00262         void load_params();
00263         
00265         std::vector<double> getGainParams(const std::vector<std::string>& param_names, const std::string& param_name);
00266         
00267 
00268         
00269         boost::mutex thread_mutex;
00270         
00271 public:
00272         
00273         
00274         
00275         bool init(pr2_mechanism_model::RobotState* robot_state, ros::NodeHandle& n );
00276         
00278         virtual void starting(){
00279                 time = robot_state->getTime();
00280         }
00282         virtual void update();
00283         };
00284 }


r2_controllers_gazebo
Author(s): Stephen Hart
autogenerated on Thu Jan 2 2014 11:31:58