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
00027
00028
00029 try {
00030
00031
00032
00033 Path_RoundedComposite* path = new Path_RoundedComposite(0.2,0.01,new RotationalInterpolation_SingleAxis());
00034
00035
00036
00037
00038
00039
00040
00041
00042
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
00051 path->Finish();
00052
00053
00054
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
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
00077
00078
00079 }
00080 of.close();
00081
00082
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