trajectory.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018, University of Edinburgh
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #include <iostream>
31 #include <kdl/path.hpp>
32 #include <kdl/path_line.hpp>
33 #include <kdl/rotational_interpolation.hpp>
34 #include <kdl/rotational_interpolation_sa.hpp>
35 #include <kdl/trajectory.hpp>
36 #include <kdl/trajectory_segment.hpp>
37 #include <kdl/trajectory_stationary.hpp>
38 #include <kdl/velocityprofile.hpp>
39 #include <kdl/velocityprofile_spline.hpp>
40 #include <string>
41 
42 #include <exotica_core/tools.h>
44 
45 namespace exotica
46 {
47 Trajectory::Trajectory() : radius_(1.0), trajectory_(nullptr)
48 {
49 }
50 
51 Trajectory::Trajectory(const std::string& data)
52 {
53  std::istringstream ss(data);
54  ss >> radius_;
55  int n, m;
56  ss >> n;
57  ss >> m;
58  data_.resize(n, m);
59 
60  for (int i = 0; i < n; ++i)
61  {
62  for (int j = 0; j < m; ++j)
63  {
64  double val;
65  ss >> val;
66  data_(i, j) = val;
67  }
68  }
70 }
71 
73 {
74  ConstructFromData(data, radius);
75 }
76 
77 KDL::Frame Trajectory::GetPosition(double t)
78 {
79  return trajectory_->Pos(t);
80 }
81 
82 KDL::Twist Trajectory::GetVelocity(double t)
83 {
84  return trajectory_->Vel(t);
85 }
86 
87 KDL::Twist Trajectory::GetAcceleration(double t)
88 {
89  return trajectory_->Acc(t);
90 }
91 
93 {
94  return trajectory_->Duration();
95 }
96 
97 Eigen::MatrixXd Trajectory::GetData()
98 {
99  return data_;
100 }
101 
103 {
104  return radius_;
105 }
106 
107 std::string Trajectory::ToString()
108 {
109  std::ostringstream ss;
110  ss << radius_ << "\n";
111  ss << data_.rows() << " " << data_.cols() << "\n";
112  ss << data_;
113  return ss.str();
114 }
115 
117 {
118  if (!(data.cols() == 4 || data.cols() == 7 || data.cols() == 8) || data.rows() < 2) ThrowPretty("Invalid trajectory data size!\nNeeds to contain 4, 7, or 8 columns and at least 2 rows.");
119  trajectory_.reset(new KDL::Trajectory_Composite());
120  for (int i = 0; i < data.rows() - 1; ++i)
121  {
122  KDL::Frame f1 = GetFrame(data.row(i).tail(data.cols() - 1).transpose());
123  KDL::Frame f2 = GetFrame(data.row(i + 1).tail(data.cols() - 1).transpose());
124  double dt = data(i + 1, 0) - data(i, 0);
125  if (dt <= 0) ThrowPretty("Time indices must be monotonically increasing! " << i << " (" << dt << ")");
126  if (KDL::Equal(f1, f2, 1e-6))
127  {
128  trajectory_->Add(new KDL::Trajectory_Stationary(dt, f1));
129  }
130  else
131  {
132  KDL::RotationalInterpolation_SingleAxis* rot = new KDL::RotationalInterpolation_SingleAxis();
133  KDL::Path_Line* path = new KDL::Path_Line(f1, f2, rot, radius);
134  KDL::VelocityProfile_Spline* vel = new KDL::VelocityProfile_Spline();
135  KDL::Trajectory_Segment* seg = new KDL::Trajectory_Segment(path, vel, dt);
136  trajectory_->Add(seg);
137  }
138  }
139  data_ = data;
140  radius_ = radius;
141 }
142 } // namespace exotica
exotica::GetFrame
KDL::Frame GetFrame(Eigen::VectorXdRefConst val)
Definition: conversions.cpp:52
trajectory.h
generate_initializers.n
n
Definition: generate_initializers.py:637
exotica::Trajectory::ToString
std::string ToString()
Definition: trajectory.cpp:107
exotica
Definition: collision_scene.h:46
exotica::Trajectory::GetAcceleration
KDL::Twist GetAcceleration(double t)
Definition: trajectory.cpp:87
exotica::Trajectory::trajectory_
std::shared_ptr< KDL::Trajectory_Composite > trajectory_
Definition: trajectory.h:59
exotica::Trajectory::GetVelocity
KDL::Twist GetVelocity(double t)
Definition: trajectory.cpp:82
exotica::Trajectory::Trajectory
Trajectory()
Definition: trajectory.cpp:47
Eigen::MatrixXdRefConst
const typedef Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
Definition: conversions.h:54
exotica::Trajectory::radius_
double radius_
Definition: trajectory.h:57
exotica::Trajectory::GetDuration
double GetDuration()
Definition: trajectory.cpp:92
ThrowPretty
#define ThrowPretty(m)
Definition: exception.h:36
exotica::Trajectory::ConstructFromData
void ConstructFromData(Eigen::MatrixXdRefConst data, double radius)
Definition: trajectory.cpp:116
exotica::Trajectory::GetPosition
KDL::Frame GetPosition(double t)
Definition: trajectory.cpp:77
exotica::Trajectory::data_
Eigen::MatrixXd data_
Definition: trajectory.h:58
tools.h
exotica::Trajectory::GetRadius
double GetRadius()
Definition: trajectory.cpp:102
t
geometry_msgs::TransformStamped t
exotica::Trajectory::GetData
Eigen::MatrixXd GetData()
Definition: trajectory.cpp:97


exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Aug 2 2024 08:43:02