cob_mmcontroller: cob_mmcontroller.cpp Source File

00001 
00002 
00003 
00004 #include <ros/ros.h>
00005 #include <cob_srvs/Trigger.h>
00006 #include <trajectory_msgs/JointTrajectory.h>
00007 #include <geometry_msgs/Twist.h>
00008 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00009 #include <kdl/chain.hpp>
00010 #include <kdl/chainfksolver.hpp>
00011 #include <kdl/chainfksolverpos_recursive.hpp>
00012 #include <kdl/chainiksolvervel_pinv.hpp>
00013 #include <kdl/chainiksolvervel_wdls.hpp>
00014 #include <kdl/chainiksolverpos_nr.hpp>
00015 #include <kdl/frames_io.hpp>
00016 
00017 #include <kdl/path_line.hpp>
00018 #include <kdl/path_circle.hpp>
00019 #include <kdl/frames.hpp>
00020 #include <kdl/rotational_interpolation_sa.hpp>
00021 #include <kdl/trajectory_segment.hpp>
00022 
00023 #include <tf_conversions/tf_kdl.h>
00024 #include <tf/transform_listener.h>
00025 
00026 
00027 class cob_mmcontroller
00028 {
00029 private:
00030   ros::Subscriber topicSub_cartvel_;
00031   ros::Subscriber topicSub_ControllerState_;
00032   ros::Publisher topicPub_armcmd_;
00033 
00034 
00035   ros::ServiceServer srvServer_Init_;
00036   ros::ServiceServer srvServer_Pause_;
00037   ros::ServiceServer srvServer_Stop_;
00038   ros::ServiceServer srvServer_Trajectoy_;
00039 
00040   
00041   KDL::Chain m_chain;
00042   KDL::ChainFkSolverPos_recursive * fksolver;;
00043   KDL::ChainIkSolverVel_pinv * iksolver1v; //generalize pseudo inverse
00044   KDL::ChainIkSolverPos_NR * iksolver1;
00045   std::vector<double> m_CurrentConfig;
00046   std::vector<double> m_CurrentVels;
00047 
00048   KDL::Twist cmdTwist;
00049   bool m_bNewTwist;
00050 
00051   tf::TransformListener tflistener;
00052   tf::StampedTransform transform_arm_base;
00053 public: 
00054   ros::NodeHandle nh_;
00055 
00056 
00057   cob_mmcontroller();
00058   void resetTwist();
00059   void updateArmCommands();
00060 
00061   void topicCallback_CartVel(const geometry_msgs::Twist::ConstPtr& msg);
00062   void topicCallback_ControllerState(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg);
00063   bool srvCallback_Init(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res );
00064   bool srvCallback_Pause(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res );
00065   bool srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res );
00066  // bool srvCallback_Trajectory(cob_mmcontroller::MoveTrajectory::Request &req, cob_mmcontroller::MoveTrajectory::Response &res );
00067     
00068   bool m_bInitialized;
00069   
00070 };
00071 
00072 cob_mmcontroller::cob_mmcontroller()
00073 {
00074   topicPub_armcmd_ = nh_.advertise<trajectory_msgs::JointTrajectory>("command", 1);
00075   topicSub_cartvel_ = nh_.subscribe("cartvel_command", 1, &cob_mmcontroller::topicCallback_CartVel, this);
00076   topicSub_ControllerState_ = nh_.subscribe("controller_state", 1, &cob_mmcontroller::topicCallback_ControllerState, this);
00077  // srvServer_Trajectoy_ = nh_.advertiseService("MMController/MoveTrajectory", &cob_mmcontroller::srvCallback_Trajectory, this);
00078   srvServer_Init_ = nh_.advertiseService("MMController/Init", &cob_mmcontroller::srvCallback_Init, this);
00079   srvServer_Pause_ = nh_.advertiseService("MMController/Pause", &cob_mmcontroller::srvCallback_Pause, this);
00080   srvServer_Stop_ = nh_.advertiseService("MMController/Stop", &cob_mmcontroller::srvCallback_Stop, this);
00081   m_bInitialized = false;
00082   m_bNewTwist = false;
00083 }
00084 
00085 void cob_mmcontroller::resetTwist()
00086 {
00087         cmdTwist = KDL::Twist();
00088         m_bNewTwist = false;
00089 }
00090 void cob_mmcontroller::updateArmCommands()
00091 {
00092   if(m_bInitialized && m_bNewTwist)
00093     {
00094       unsigned int nj = m_chain.getNrOfJoints();
00095       KDL::JntArray jointpositions = KDL::JntArray(nj);
00096       KDL::JntArray qdot_out = KDL::JntArray(nj);
00097       for(unsigned int i = 0; i < nj; i++)
00098                 jointpositions(i) = m_CurrentConfig[i];
00099       iksolver1v->CartToJnt(jointpositions, cmdTwist, qdot_out);
00100       trajectory_msgs::JointTrajectory msg;
00101       msg.header.stamp = ros::Time::now();
00102       msg.points.resize(1);
00103       msg.points[0].velocities.resize(nj);
00104       //std::cerr << "JointPositions: " << jointpositions(0) << " " <<  jointpositions(1) << " " <<  jointpositions(2) << " " <<  jointpositions(3) << " " <<  jointpositions(4) << " " << jointpositions(5) << " " <<  jointpositions(6) << "\n";
00105       //std::cerr << "Twist " << cmdTwist.vel.x() << " " << cmdTwist.vel.y() << " " << cmdTwist.vel.z() << "\n";
00106           //std::cerr << "Vels: " << qdot_out(0) << " " <<  qdot_out(1) << " " <<  qdot_out(2) << " " <<  qdot_out(3) << " " <<  qdot_out(4) << " " <<  qdot_out(5) << " " <<  qdot_out(6) << "\n";
00107       for(unsigned int i = 0; i < nj; i++)
00108                 msg.points[0].velocities[i] = qdot_out(i);
00109       topicPub_armcmd_.publish(msg);
00110       resetTwist();
00111     }
00112                        
00113 }
00114 
00115 void cob_mmcontroller::topicCallback_CartVel(const geometry_msgs::Twist::ConstPtr& msg)
00116 {
00117         ROS_DEBUG("Received Twist");
00118 
00119         try{
00120                 tflistener.lookupTransform("arm_0_link", "base_link", ros::Time(0), transform_arm_base);
00121     }
00122         catch (tf::TransformException ex){
00123                 ROS_ERROR("%s",ex.what());
00124         }
00125         KDL::Twist t;
00126         tf::TwistMsgToKDL(*msg, t);
00127         KDL::Frame frm_arm_base;
00128         tf::TransformTFToKDL(transform_arm_base, frm_arm_base);
00129         KDL::Twist t_base(frm_arm_base * t.vel, frm_arm_base * t.rot);
00130         cmdTwist += t_base/1000;
00131         m_bNewTwist = true;
00132   
00133 }
00134 
00135 void cob_mmcontroller::topicCallback_ControllerState(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
00136 {
00137   m_CurrentConfig = msg->actual.positions;
00138   m_CurrentVels = msg->actual.velocities;
00139 
00140 }
00141 
00142 bool cob_mmcontroller::srvCallback_Init(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res )
00143 {
00144   if(!m_bInitialized)
00145         {
00146           ROS_INFO("Initializing MMController");
00147           m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  -M_PI/2.0,  -0.250    , 0.0     )));
00148           m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  M_PI/2.0,  0.0    , 0.0     )));
00149           m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  -M_PI/2.0,  -0.408    , 0.0     )));
00150           m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  M_PI/2.0,  0.0    , 0.0     )));
00151           m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  -M_PI/2.0, -0.316    , 0.0     )));
00152           m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  M_PI/2.0,  0.0    , 0.0     )));
00153           m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  M_PI,  -0.327    , 0.0     )));
00154 
00155           m_CurrentConfig.resize(m_chain.getNrOfJoints());
00156           m_CurrentVels.resize(m_chain.getNrOfJoints());
00157 
00158           ROS_INFO("Creating Kinematic Solvers");
00159           fksolver = new KDL::ChainFkSolverPos_recursive(m_chain);
00160           iksolver1v = new  KDL::ChainIkSolverVel_pinv(m_chain,1e-6,200); //generalize pseudo inverse
00161 
00162 
00163           m_bInitialized = true;
00164         }
00165   else
00166         {       
00167           ROS_WARN("Allready initialized");
00168         }
00169   return true;
00170 }
00171 
00172 bool cob_mmcontroller::srvCallback_Pause(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res )
00173 {
00174   return true;
00175 }
00176 
00177 bool cob_mmcontroller::srvCallback_Stop(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res )
00178 {
00179   return true;
00180 }
00181 /*
00182 bool cob_mmcontroller::srvCallback_Trajectory(cob_mmcontroller::MoveTrajectory::Request &req, cob_mmcontroller::MoveTrajectory::Response &res )
00183 {
00184         return true;
00185 }
00186 */
00187 int main(int argc, char ** argv)
00188 {
00189   ros::init(argc, argv, "cob_mmcontroller");
00190   cob_mmcontroller mmctrl;
00191   ros::Rate loop_rate(10); // Hz
00192 
00193   while(mmctrl.nh_.ok())
00194     {
00195           //mmctrl.resetTwist();
00196       mmctrl.updateArmCommands();
00197       ros::spinOnce();
00198       loop_rate.sleep();
00199     }
00200 
00201   return 0;
00202 }