orocos_kdl
trajectory_example.cpp
Go to the documentation of this file.
1 
11 #include <frames.hpp>
12 #include <frames_io.hpp>
13 #include <trajectory.hpp>
14 #include <trajectory_segment.hpp>
16 #include <trajectory_composite.hpp>
17 #include <trajectory_composite.hpp>
18 #include <velocityprofile_trap.hpp>
21 #include <utilities/error.h>
22 #include <utilities/utility.h>
23 #include <trajectory_composite.hpp>
24 
25 int main(int argc,char* argv[]) {
26  using namespace KDL;
27  // Create the trajectory:
28  // use try/catch to catch any exceptions thrown.
29  // NOTE: exceptions will become obsolete in a future version.
30  try {
31  // Path_RoundedComposite defines the geometric path along
32  // which the robot will move.
33  //
35  // The routines are now robust against segments that are parallel.
36  // When the routines are parallel, no rounding is needed, and no attempt is made
37  // add constructing a rounding arc.
38  // (It is still not possible when the segments are on top of each other)
39  // Note that you can only rotate in a deterministic way over an angle less then PI!
40  // With an angle == PI, you cannot predict over which side will be rotated.
41  // With an angle > PI, the routine will rotate over 2*PI-angle.
42  // If you need to rotate over a larger angle, you need to introduce intermediate points.
43  // So, there is a common use case for using parallel segments.
44  path->Add(Frame(Rotation::RPY(PI,0,0), Vector(-1,0,0)));
45  path->Add(Frame(Rotation::RPY(PI_2,0,0), Vector(-0.5,0,0)));
46  path->Add(Frame(Rotation::RPY(0,0,0), Vector(0,0,0)));
47  path->Add(Frame(Rotation::RPY(0.7,0.7,0.7), Vector(1,1,1)));
48  path->Add(Frame(Rotation::RPY(0,0.7,0), Vector(1.5,0.3,0)));
49  path->Add(Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0)));
50 
51  // always call Finish() at the end, otherwise the last segment will not be added.
52  path->Finish();
53 
54  // Trajectory defines a motion of the robot along a path.
55  // This defines a trapezoidal velocity profile.
56  VelocityProfile* velpref = new VelocityProfile_Trap(0.5,0.1);
57  velpref->SetProfile(0,path->PathLength());
58  Trajectory* traject = new Trajectory_Segment(path, velpref);
59 
60 
62  ctraject->Add(traject);
63  ctraject->Add(new Trajectory_Stationary(1.0,Frame(Rotation::RPY(0.7,0.7,0), Vector(1,1,0))));
64 
65  // use the trajectory
66  double dt=0.1;
67  std::ofstream of("./trajectory.dat");
68  for (double t=0.0; t <= traject->Duration(); t+= dt) {
69  Frame current_pose;
70  current_pose = traject->Pos(t);
71  for (int i=0;i<4;++i)
72  for (int j=0;j<4;++j)
73  of << current_pose(i,j) << "\t";
74  of << "\n";
75  // also velocities and accelerations are available !
76  //traject->Vel(t);
77  //traject->Acc(t);
78  }
79  of.close();
80 
81  // you can get some meta-info on the path:
82  for (int segmentnr=0; segmentnr < path->GetNrOfSegments(); segmentnr++) {
83  double starts,ends;
84  Path::IdentifierType pathtype;
85  if (segmentnr==0) {
86  starts = 0.0;
87  } else {
88  starts = path->GetLengthToEndOfSegment(segmentnr-1);
89  }
90  ends = path->GetLengthToEndOfSegment(segmentnr);
91  pathtype = path->GetSegment(segmentnr)->getIdentifier();
92  std::cout << "segment " << segmentnr << " runs from s="<<starts << " to s=" <<ends;
93  switch(pathtype) {
94  case Path::ID_CIRCLE:
95  std::cout << " circle";
96  break;
97  case Path::ID_LINE:
98  std::cout << " line ";
99  break;
100  default:
101  std::cout << " unknown ";
102  break;
103  }
104  std::cout << std::endl;
105  }
106  std::cout << " trajectory written to the ./trajectory.dat file " << std::endl;
107 
108  delete ctraject;
109  } catch(Error& error) {
110  std::cout <<"I encountered this error : " << error.Description() << std::endl;
111  std::cout << "with the following type " << error.GetType() << std::endl;
112  }
113 
114 }
frames.hpp
KDL::RotationalInterpolation_SingleAxis
Definition: rotational_interpolation_sa.hpp:101
KDL::Trajectory_Segment
Definition: trajectory_segment.hpp:100
KDL::Error::Description
virtual const char * Description() const
Definition: error.h:143
KDL::Path_RoundedComposite::PathLength
virtual double PathLength()
Definition: path_roundedcomposite.cpp:190
frames_io.hpp
trajectory_composite.hpp
KDL::Path_RoundedComposite::GetLengthToEndOfSegment
virtual double GetLengthToEndOfSegment(int i)
Definition: path_roundedcomposite.cpp:218
utility.h
KDL::Trajectory_Stationary
Definition: trajectory_stationary.hpp:41
velocityprofile_trap.hpp
KDL::Path_RoundedComposite::Add
void Add(const Frame &F_base_point)
Definition: path_roundedcomposite.cpp:108
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Trajectory_Composite::Add
virtual void Add(Trajectory *elem)
Definition: trajectory_composite.cpp:101
trajectory_stationary.hpp
KDL::VelocityProfile
Definition: velocityprofile.hpp:101
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
std::cout
KDL::Path::ID_LINE
@ ID_LINE
Definition: path.hpp:176
KDL::Trajectory_Composite
Definition: trajectory_composite.hpp:45
KDL::PI_2
const double PI_2
the value of pi/2
Definition: utility.cxx:17
std::ofstream
KDL::Error::GetType
virtual int GetType() const
Definition: error.h:145
trajectory_segment.hpp
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::Path::IdentifierType
IdentifierType
Definition: path.hpp:137
error.h
KDL::PI
const double PI
the value of pi
Definition: utility.cxx:16
std::ofstream::close
T close(T... args)
trajectory.hpp
KDL::Error
Definition: error.h:96
KDL::Path_RoundedComposite
Definition: path_roundedcomposite.hpp:96
KDL::Path::getIdentifier
virtual IdentifierType getIdentifier() const =0
KDL::Trajectory::Pos
virtual Frame Pos(double time) const =0
KDL::Path_RoundedComposite::Finish
void Finish()
Definition: path_roundedcomposite.cpp:177
path_roundedcomposite.hpp
KDL::Path_RoundedComposite::GetSegment
virtual Path * GetSegment(int i)
Definition: path_roundedcomposite.cpp:214
std::endl
T endl(T... args)
KDL::Path_RoundedComposite::GetNrOfSegments
virtual int GetNrOfSegments()
Definition: path_roundedcomposite.cpp:210
rotational_interpolation_sa.hpp
KDL::VelocityProfile_Trap
Definition: velocityprofile_trap.hpp:98
main
int main(int argc, char *argv[])
Definition: trajectory_example.cpp:25
KDL::Trajectory::Duration
virtual double Duration() const =0
KDL::Rotation::RPY
static Rotation RPY(double roll, double pitch, double yaw)
Definition: frames.cpp:262
KDL::VelocityProfile::SetProfile
virtual void SetProfile(double pos1, double pos2)=0
KDL::Trajectory
Definition: trajectory.hpp:129
KDL::Path::ID_CIRCLE
@ ID_CIRCLE
Definition: path.hpp:177