trajectory_example.cpp
Go to the documentation of this file.
00001 
00011 #include <frames.hpp>
00012 #include <frames_io.hpp>
00013 #include <trajectory.hpp>
00014 #include <trajectory_segment.hpp>
00015 #include <trajectory_stationary.hpp>
00016 #include <trajectory_composite.hpp>
00017 #include <trajectory_composite.hpp>
00018 #include <velocityprofile_trap.hpp>
00019 #include <path_roundedcomposite.hpp>
00020 #include <rotational_interpolation_sa.hpp>
00021 #include <utilities/error.h>
00022 #include <trajectory_composite.hpp>
00023 
00024 int main(int argc,char* argv[]) {
00025         using namespace KDL;
00026         // Create the trajectory:
00027     // use try/catch to catch any exceptions thrown.
00028     // NOTE:  exceptions will become obsolete in a future version.
00029         try {
00030         // Path_RoundedComposite defines the geometric path along
00031         // which the robot will move.
00032                 //
00033                 Path_RoundedComposite* path = new Path_RoundedComposite(0.2,0.01,new RotationalInterpolation_SingleAxis());
00034                 // The routines are now robust against segments that are parallel.
00035                 // When the routines are parallel, no rounding is needed, and no attempt is made
00036                 // add constructing a rounding arc.
00037                 // (It is still not possible when the segments are on top of each other)
00038                 // Note that you can only rotate in a deterministic way over an angle less then M_PI!
00039                 // With an angle == M_PI, you cannot predict over which side will be rotated.
00040                 // With an angle > M_PI, the routine will rotate over 2*M_PI-angle.
00041                 // If you need to rotate over a larger angle, you need to introduce intermediate points.
00042                 // So, there is a common use case for using parallel segments.
00043                 path->Add(Frame(Rotation::RPY(M_PI,0,0), Vector(-1,0,0)));
00044                 path->Add(Frame(Rotation::RPY(M_PI/2,0,0), Vector(-0.5,0,0)));
00045                 path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0)));
00046                 path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1)));
00047                 path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0)));
00048                 path->Add(Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)));
00049 
00050                 // always call Finish() at the end, otherwise the last segment will not be added.
00051                 path->Finish();
00052 
00053         // Trajectory defines a motion of the robot along a path.
00054         // This defines a trapezoidal velocity profile.
00055                 VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1);
00056                 velpref->SetProfile(0,path->PathLength());  
00057                 Trajectory* traject = new Trajectory_Segment(path, velpref);
00058 
00059 
00060                 Trajectory_Composite* ctraject = new Trajectory_Composite();
00061                 ctraject->Add(traject);
00062                 ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))));
00063 
00064 
00065 
00066                 // use the trajectory
00067                 double dt=0.1;
00068                 std::ofstream of("./trajectory.dat");
00069                 for (double t=0.0; t <= traject->Duration(); t+= dt) {
00070                         Frame current_pose;
00071                         current_pose = traject->Pos(t);
00072                         for (int i=0;i<4;++i)
00073                                 for (int j=0;j<4;++j)
00074                                         of << current_pose(i,j) << "\t";
00075                         of << "\n";
00076                         // also velocities and accelerations are available !
00077                         //traject->Vel(t);
00078                         //traject->Acc(t);
00079                 }
00080                 of.close();
00081 
00082                 // you can get some meta-info on the path:
00083                 for (int segmentnr=0;  segmentnr < path->GetNrOfSegments(); segmentnr++) {
00084                         double starts,ends;
00085                         Path::IdentifierType pathtype;
00086                         if (segmentnr==0) {
00087                                 starts = 0.0;
00088                         } else {
00089                                 starts = path->GetLengthToEndOfSegment(segmentnr-1);
00090                         }
00091                         ends = path->GetLengthToEndOfSegment(segmentnr);
00092                         pathtype = path->GetSegment(segmentnr)->getIdentifier();
00093                         std::cout << "segment " << segmentnr << " runs from s="<<starts << " to s=" <<ends;
00094                         switch(pathtype) {
00095                                 case Path::ID_CIRCLE:
00096                                         std::cout << " circle";
00097                                         break;
00098                                 case Path::ID_LINE:
00099                                         std::cout << " line ";
00100                                         break;
00101                                 default:
00102                                         std::cout << " unknown ";
00103                                         break;
00104                         }
00105                         std::cout << std::endl;
00106                 }
00107         std::cout << " trajectory written to the ./trajectory.dat file " << std::endl;
00108 
00109         delete ctraject;
00110         } catch(Error& error) {
00111                 std::cout <<"I encountered this error : " << error.Description() << std::endl;
00112                 std::cout << "with the following type " << error.GetType() << std::endl;
00113         }
00114 
00115 }
00116 
00117 


orocos_kdl
Author(s):
autogenerated on Fri Jun 14 2019 19:33:23