cob_simpletrajectories: cob_simpletrajectories.cpp Source File

00001 
00002 /****************************************************************
00003  *
00004  * Copyright (c) 2010
00005  *
00006  * Fraunhofer Institute for Manufacturing Engineering   
00007  * and Automation (IPA)
00008  *
00009  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00010  *
00011  * Project name: care-o-bot
00012  * ROS stack name: cob_apps
00013  * ROS package name: cob_simpletrajectories
00014  *                                                              
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *                      
00017  * Author: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de
00018  * Supervised by: Alexander Bubeck, email:alexander.bubeck@ipa.fhg.de
00019  *
00020  * Date of creation: June 2010
00021  * ToDo:
00022  *
00023  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00024  *
00025  * Redistribution and use in source and binary forms, with or without
00026  * modification, are permitted provided that the following conditions are met:
00027  *
00028  *     * Redistributions of source code must retain the above copyright
00029  *       notice, this list of conditions and the following disclaimer.
00030  *     * Redistributions in binary form must reproduce the above copyright
00031  *       notice, this list of conditions and the following disclaimer in the
00032  *       documentation and/or other materials provided with the distribution.
00033  *     * Neither the name of the Fraunhofer Institute for Manufacturing 
00034  *       Engineering and Automation (IPA) nor the names of its
00035  *       contributors may be used to endorse or promote products derived from
00036  *       this software without specific prior written permission.
00037  *
00038  * This program is free software: you can redistribute it and/or modify
00039  * it under the terms of the GNU Lesser General Public License LGPL as 
00040  * published by the Free Software Foundation, either version 3 of the 
00041  * License, or (at your option) any later version.
00042  * 
00043  * This program is distributed in the hope that it will be useful,
00044  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00045  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00046  * GNU Lesser General Public License LGPL for more details.
00047  * 
00048  * You should have received a copy of the GNU Lesser General Public 
00049  * License LGPL along with this program. 
00050  * If not, see <http://www.gnu.org/licenses/>.
00051  *
00052  ****************************************************************/
00053 
00054 // ROS includes
00055 #include <ros/ros.h>
00056 #include <cob_simpletrajectories/MoveRelLin.h>
00057 #include <cob_simpletrajectories/MoveDoorHandle.h>
00058 //#include <cob_mmcontroller/MoveTrajectory.h>
00059 #include <visualization_msgs/Marker.h>
00060 #include <pr2_controllers_msgs/JointTrajectoryControllerState.h>
00061 
00062 // KDL includes
00063 #include <kdl/chain.hpp>
00064 #include <kdl/chainfksolver.hpp>
00065 #include <kdl/chainfksolverpos_recursive.hpp>
00066 #include <kdl/chainiksolvervel_pinv.hpp>
00067 #include <kdl/chainiksolvervel_wdls.hpp>
00068 #include <kdl/chainiksolverpos_nr.hpp>
00069 #include <kdl/frames_io.hpp>
00070 
00071 #include <kdl/path_line.hpp>
00072 #include <kdl/path_circle.hpp>
00073 #include <kdl/frames.hpp>
00074 #include <kdl/rotational_interpolation_sa.hpp>
00075 #include <kdl/velocityprofile_trap.hpp>
00076 #include <kdl/trajectory_segment.hpp>
00077 
00078 #include <tf_conversions/tf_kdl.h>
00079 
00080 #define Time_Resolution 0.5
00081 
00082 class SimpleTrajectories
00083 {
00084 public:
00085   SimpleTrajectories();
00086   ros::NodeHandle nh_;
00087 private:
00088 
00089 
00090   ros::ServiceServer srvServer_MoveRelLin_;
00091   ros::ServiceServer srvServer_MoveDoorHandle_;
00092 //  ros::ServiceClient srvClient_MoveTrajectory;
00093   ros::Publisher marker_pub;
00094   ros::Subscriber topicSub_ControllerState_;
00095 
00096   std::vector<geometry_msgs::Point> TrajPoints;
00097   std::vector<geometry_msgs::Pose> TrajPoses;
00098 
00099   std::vector<double> m_CurrentConfig;
00100   std::vector<double> m_CurrentVels;
00101 
00102 
00103   KDL::Trajectory_Segment * m_SyncMM_Trajectory ;
00104 
00105   int moveRelLin(double x, double y, double z, double timeS);
00106   int moveDoorHandle(double radius, double movingAngle, double timeS, bool dir, double delay);
00107 
00108   bool srvCallback_MoveRelLin(cob_simpletrajectories::MoveRelLin::Request &req, cob_simpletrajectories::MoveRelLin::Response &res );
00109   bool srvCallback_MoveDoorHandle(cob_simpletrajectories::MoveDoorHandle::Request &req, cob_simpletrajectories::MoveDoorHandle::Response &res );
00110 
00111   void topicCallback_ControllerState(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg);
00112 
00113   KDL::Frame GlobalToKDL(KDL::Frame in);
00114   KDL::Frame KDLToGlobal(KDL::Frame in);
00115   
00116   KDL::Chain m_chain;
00117 
00118 };
00119 
00120 SimpleTrajectories::SimpleTrajectories()
00121 {
00122   srvServer_MoveRelLin_ = nh_.advertiseService("MoveRelLin", &SimpleTrajectories::srvCallback_MoveRelLin, this);
00123   srvServer_MoveDoorHandle_ = nh_.advertiseService("MoveDoorHandle", &SimpleTrajectories::srvCallback_MoveDoorHandle, this);
00124   marker_pub = nh_.advertise<visualization_msgs::Marker>("/visualization_marker", 10);
00125   topicSub_ControllerState_ = nh_.subscribe("controller_state", 1, &SimpleTrajectories::topicCallback_ControllerState, this);
00126 //  srvClient_MoveTrajectory  = nh_.serviceClient<cob_mmcontroller::MoveTrajectory>("MMController/MoveTrajectory");
00127 
00128   //dhParameterArm
00129 
00130   m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  -M_PI/2.0,  -0.250    , 0.0     )));
00131   m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  M_PI/2.0,  0.0    , 0.0     )));
00132   m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  -M_PI/2.0,  -0.408    , 0.0     )));
00133   m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  M_PI/2.0,  0.0    , 0.0     )));
00134   m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  -M_PI/2.0, -0.316    , 0.0     )));
00135   m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  M_PI/2.0,  0.0    , 0.0     )));
00136   m_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame().DH( 0.0,  M_PI,  -0.327    , 0.0     )));
00137 
00138   m_CurrentConfig.resize(m_chain.getNrOfJoints());
00139   m_CurrentVels.resize(m_chain.getNrOfJoints());
00140 
00141 }
00142 
00143 void SimpleTrajectories::topicCallback_ControllerState(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
00144 {
00145   m_CurrentConfig = msg->actual.positions;
00146   m_CurrentVels = msg->actual.velocities;
00147 
00148 }
00149 
00150 bool SimpleTrajectories::srvCallback_MoveRelLin(cob_simpletrajectories::MoveRelLin::Request &req,
00151                                                 cob_simpletrajectories::MoveRelLin::Response &res )
00152 {
00153   moveRelLin(req.x, req.y, req.z, req.time);
00154 
00155   visualization_msgs::Marker points;
00156   points.header.frame_id = "/base_link";
00157   points.header.stamp = ros::Time::now();
00158   points.ns = "Trajectory";
00159   points.action = visualization_msgs::Marker::ADD;
00160   points.pose.orientation.w = 1.0;
00161   points.id = 0;
00162   points.type = visualization_msgs::Marker::LINE_STRIP;
00163   points.scale.x = 0.01;
00164   points.scale.y = 0.01;
00165   points.scale.z = 0.01;
00166   points.color.g = 1.0f;
00167   points.color.a = 1.0;
00168   points.lifetime = ros::Duration();
00169   
00170   points.points = TrajPoints;
00171   marker_pub.publish(points);
00172 
00173   //test
00174   uint32_t shape = visualization_msgs::Marker::CUBE;
00175   visualization_msgs::Marker marker;
00176      marker.header.frame_id = "/base_link";
00177      marker.header.stamp = ros::Time::now();
00178      marker.ns = "basic_shapes";
00179      marker.id = 0;
00180      marker.type = shape;
00181      marker.action = visualization_msgs::Marker::ADD;
00182      marker.pose.position.x = 0;
00183      marker.pose.position.y = 0;
00184      marker.pose.position.z = 0;
00185      marker.pose.orientation.x = 0.0;
00186      marker.pose.orientation.y = 0.0;
00187      marker.pose.orientation.z = 0.0;
00188      marker.pose.orientation.w = 1.0;
00189      marker.scale.x = 1.0;
00190      marker.scale.y = 1.0;
00191      marker.scale.z = 1.0;
00192      marker.color.r = 0.0f;
00193      marker.color.g = 1.0f;
00194      marker.color.b = 0.0f;
00195      marker.color.a = 1.0;
00196      marker.lifetime = ros::Duration();
00197      //marker_pub.publish(marker);
00198 
00199 
00200 
00201   return true;
00202 }
00203 
00204 bool SimpleTrajectories::srvCallback_MoveDoorHandle(cob_simpletrajectories::MoveDoorHandle::Request &req, cob_simpletrajectories::MoveDoorHandle::Response &res )
00205 {
00206   moveDoorHandle(req.radius, req.movingAngle, req.time, req.direction, req.delay);
00207 
00208   visualization_msgs::Marker points;
00209   points.header.frame_id = "/base_link";
00210   points.header.stamp = ros::Time::now();
00211   points.ns = "Trajectory";
00212   points.action = visualization_msgs::Marker::ADD;
00213   points.pose.orientation.w = 1.0;
00214   points.id = 0;
00215   points.type = visualization_msgs::Marker::POINTS;
00216   points.scale.x = 0.01;
00217   points.scale.y = 0.01;
00218   points.color.g = 1.0f;
00219   points.color.a = 1.0;
00220 
00221   points.points = TrajPoints;
00222   marker_pub.publish(points);
00223 
00224 
00225 /*  cob_mmcontroller::MoveTrajectory srv;
00226   srv.request.header.stamp = ros::Time::now();
00227   srv.request.timeres_ms = Time_Resolution * 1000;
00228   srv.request.poses = TrajPoses;
00229   srvClient_MoveTrajectory .call(srv);
00230 */
00231   return true;
00232 }
00233 
00234 int SimpleTrajectories::moveRelLin(double x, double y, double z, double timeS)
00235 {
00236   double r,p,yaw;
00237 
00238 
00239   KDL::ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(m_chain);
00240 
00241   // read current position of Jointsvel
00242   KDL::Frame cartpos;
00243   unsigned int nj = m_chain.getNrOfJoints();
00244   KDL::JntArray jointpositions = KDL::JntArray(nj);
00245   
00246 
00247   for(int i = 0; i<7; i++)
00248     jointpositions(i) = m_CurrentConfig[i];
00249  
00250 
00251   fksolver.JntToCart(jointpositions,cartpos);
00252 
00253   KDL::Vector vRelPos(x, y, z);
00254   KDL::Frame frameStart = KDLToGlobal(cartpos);
00255   KDL::Frame frameEnd = GlobalToKDL(KDL::Frame(frameStart.M, frameStart.p + vRelPos));
00256 
00257   KDL::RotationalInterpolation_SingleAxis rotInt_sa;
00258   KDL::Path_Line * myPath = new KDL::Path_Line(cartpos, frameEnd, &rotInt_sa, 1);
00259   KDL::VelocityProfile_Trap * myVelProfile = new KDL::VelocityProfile_Trap(150,70);// Parameter: maxvel, maxacc
00260 
00261 
00262   double m_SyncMM_Trajectory_TIME = timeS;
00263   m_SyncMM_Trajectory = new KDL::Trajectory_Segment(myPath, myVelProfile, m_SyncMM_Trajectory_TIME); //duration
00264   
00265   double resolution = Time_Resolution;
00266   for (double i = 0.0; i < m_SyncMM_Trajectory_TIME; i+=resolution)
00267     {
00268       KDL::Frame pathpos = KDLToGlobal(m_SyncMM_Trajectory->Pos(i));
00269       geometry_msgs::Point p;
00270       p.x = pathpos.p.x();
00271       p.y = pathpos.p.y();
00272       p.z = pathpos.p.z();
00273 
00274       geometry_msgs::Pose pos;
00275       tf::PoseKDLToMsg(pathpos, pos);
00276       TrajPoints.push_back(p);
00277       TrajPoses.push_back(pos);
00278     }
00279 
00280 
00281   ROS_INFO("Trajectory generated");
00282   ROS_INFO("Length: %f   Duration: %f s", myPath->PathLength(), m_SyncMM_Trajectory->Duration());
00283   return 0;
00284 }
00285 
00286 int SimpleTrajectories::moveDoorHandle(double radius, double movingAngle, double timeS, bool dir, double delay)
00287 {
00288   double m_dDelay = delay;
00289 
00290 
00291   KDL::ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(m_chain);
00292 
00293   // read current position of Jointsvel
00294   KDL::Frame cartpos;
00295   unsigned int nj = m_chain.getNrOfJoints();
00296   KDL::JntArray jointpositions = KDL::JntArray(nj);
00297 
00298   for(int i = 0; i<7; i++)
00299     jointpositions(i) = m_CurrentConfig[i];
00300 
00301   fksolver.JntToCart(jointpositions,cartpos);
00302 
00303   KDL::Frame frameStart = KDLToGlobal(cartpos);
00304 
00305 
00306   double kreisteil = 0.4;
00307   //calculate Startpos
00308   KDL::Frame newStartPos(cartpos.M, KDL::Vector(cartpos.p.x(),cartpos.p.y(),cartpos.p.z()));
00309 
00310   KDL::Rotation endPos;
00311   double phi = movingAngle*3.1;
00312   double R[3][3];
00313   double achse_x = 0;
00314   double achse_y = 0;
00315   if (dir)
00316     {
00317       double Rx[3][3] = { {1, 0, 0}, {0, cos(phi), -sin(phi)}, {0, sin(phi), cos(phi)} };
00318       for (int i=0;  i<3; i++)
00319         {
00320           for(int j=0; j<3; j++)
00321             {
00322               R[i][j] = Rx[i][j];
00323             }
00324         }
00325       achse_x = movingAngle/fabs(movingAngle)*radius*2;
00326       achse_y = 0;
00327     }
00328   else
00329     {
00330       double Ry[3][3] = { {cos(phi), 0, sin(phi)}, {0, 1, 0}, {-sin(phi), 0, cos(phi)} };
00331       for (int i=0;  i<3; i++)
00332         {
00333           for(int j=0; j<3; j++)
00334             {
00335               R[i][j] = Ry[i][j];
00336             }
00337         }
00338       achse_y = movingAngle/fabs(movingAngle)*radius*2;
00339       achse_x = 0;
00340     }
00341 
00342   for (int i=0; i<3; i++)
00343     {
00344       for (int j=0; j<3; j++)
00345         {
00346           endPos.data[i*3+j] = 0;
00347           for (int k=0; k<3; k++)
00348             {
00349               endPos.data[i*3+j] += R[i][k] * cartpos.M.data[k*3+j];
00350             }
00351         }
00352     }
00353 
00354   KDL::RotationalInterpolation_SingleAxis rotInt_sa;
00355   KDL::Path_Circle * myCircPath = new KDL::Path_Circle(newStartPos, GlobalToKDL(KDL::Frame(frameStart.M, frameStart.p + KDL::Vector(0,0,-radius))).p, GlobalToKDL(KDL::Frame(frameStart.M, frameStart.p + KDL::Vector( achse_x, achse_y, 0))).p, endPos, M_PI*fabs(movingAngle), &rotInt_sa, 1);
00356   KDL::VelocityProfile_Trap * myVelProfile = new KDL::VelocityProfile_Trap(80,70);// Parameter: maxvel, maxacc
00357 
00358   double m_SyncMM_Trajectory_TIME = timeS;
00359   m_SyncMM_Trajectory = new KDL::Trajectory_Segment(myCircPath, myVelProfile, m_SyncMM_Trajectory_TIME); //duration
00360  
00361  double resolution = 0.5;
00362   for (double i = 0.0; i < m_SyncMM_Trajectory_TIME; i+=resolution)
00363     {
00364       KDL::Frame pathpos = KDLToGlobal(m_SyncMM_Trajectory->Pos(i));
00365       geometry_msgs::Point p;
00366       p.x = pathpos.p.x();
00367       p.y = pathpos.p.y();
00368       p.z = pathpos.p.z();
00369       TrajPoints.push_back(p);
00370     }
00371 
00372 
00373   ROS_INFO("Trajectory generated");
00374   ROS_INFO("Length: %f   Duration: %f s", myCircPath->PathLength(), m_SyncMM_Trajectory->Duration());
00375   return 0;
00376 }
00377 
00378 
00379 
00380 KDL::Frame SimpleTrajectories::KDLToGlobal(KDL::Frame in)
00381 {
00382   // Create Transformation Matrix from ArmBase to Robot Base
00383   /*  KDL::Rotation T_Rot_AB_RB(M4_ArmBase[0][0],M4_ArmBase[1][0],M4_ArmBase[2][0],
00384           M4_ArmBase[0][1],M4_ArmBase[1][1],M4_ArmBase[2][1],
00385           M4_ArmBase[0][2],M4_ArmBase[1][2],M4_ArmBase[2][2]);*/
00386   /*KDL::Rotation T_Rot_AB_RB(-0.683013,0.258819,-0.683013,
00387         0.183013,0.965926,0.183013,
00388         0.707107,0.00000,-0.707107);*/
00389   KDL::Rotation T_Rot_AB_RB(-0.683013,0.258819,-0.683013,
00390                             0.183013,0.965926,0.183013,
00391                             0.707107,0.00000,-0.707107);
00392   // Turn T_Rot 180° around x-axis to adapt KDL to COb specification:
00393   //T_Rot_AB_RB = T_Rot_AB_RB * KDL::Rotation::EulerZYX(0, 0, M_PI);
00394   // translational part of transformation
00395   //  KDL::Vector T_Trans(M4_ArmBase[3][0],M4_ArmBase[3][1],M4_ArmBase[3][2]);
00396   //  KDL::Vector T_Trans(0,-0.115,0.76);
00397   KDL::Vector T_Trans(0.045,-0.005,0.83637);
00398   // KDL::Vector T_Trans(0,-0.115,0.63);
00399   // create transformation frame
00400   KDL::Frame T_AB_RB(T_Rot_AB_RB, T_Trans);
00401   return T_AB_RB * in;
00402 }
00403 
00404 KDL::Frame SimpleTrajectories::GlobalToKDL(KDL::Frame in)
00405 {
00406 
00407   // Create Transformation Matrix from ArmBase to Robot Base
00408   /*  KDL::Rotation T_Rot_AB_RB(M4_ArmBase[0][0],M4_ArmBase[1][0],M4_ArmBase[2][0],
00409           M4_ArmBase[0][1],M4_ArmBase[1][1],M4_ArmBase[2][1],
00410           M4_ArmBase[0][2],M4_ArmBase[1][2],M4_ArmBase[2][2]);*/
00411   /*
00412   KDL::Rotation T_Rot_AB_RB(-0.683013,0.258819,-0.683013,
00413       0.183013,0.965926,0.183013,
00414       0.707107,0.00000,-0.707107);*/
00415   KDL::Rotation T_Rot_AB_RB(-0.683013,0.258819,-0.683013,
00416                             0.183013,0.965926,0.183013,
00417                             0.707107,0.00000,-0.707107);
00418   // Turn T_Rot 180° around x-axis to adapt KDL to COb specification:
00419   //T_Rot_AB_RB = T_Rot_AB_RB * KDL::Rotation::EulerZYX(0, 0, M_PI);
00420   // translational part of transformation
00421   //KDL::Vector T_Trans(0,0,0.83637);
00422   //  KDL::Vector T_Trans(0,-0.115,0.76);
00423   KDL::Vector T_Trans(0.045,-0.005,0.83637);
00424   // create transformation frame
00425   //  T_Trans.ReverseSign();
00426   KDL::Frame T_AB_RB(T_Rot_AB_RB, T_Trans);
00427   return T_AB_RB.Inverse() * in;
00428 }
00429 
00430 
00431 int main(int argc, char ** argv)
00432 {
00433   ros::init(argc, argv, "cob_simpletrajectory");
00434   SimpleTrajectories st;
00435   ros::Rate loop_rate(5); // Hz
00436   while(st.nh_.ok())
00437     {
00438       ros::spinOnce();
00439       usleep(200000);
00440     }
00441   
00442   return 0;
00443 }
00444 
00445 
00446