1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016 CNRS |
3 |
|
|
// Authors: Pierre Fernbach |
4 |
|
|
// |
5 |
|
|
// This file is part of hpp-core |
6 |
|
|
// hpp-core is free software: you can redistribute it |
7 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
8 |
|
|
// License as published by the Free Software Foundation, either version |
9 |
|
|
// 3 of the License, or (at your option) any later version. |
10 |
|
|
// |
11 |
|
|
// hpp-core is distributed in the hope that it will be |
12 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
13 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
14 |
|
|
// General Lesser Public License for more details. You should have |
15 |
|
|
// received a copy of the GNU Lesser General Public License along with |
16 |
|
|
// hpp-core If not, see |
17 |
|
|
// <http://www.gnu.org/licenses/>. |
18 |
|
|
|
19 |
|
|
#include <hpp/core/config-projector.hh> |
20 |
|
|
#include <hpp/core/straight-path.hh> |
21 |
|
|
#include <hpp/pinocchio/configuration.hh> |
22 |
|
|
#include <hpp/pinocchio/device.hh> |
23 |
|
|
#include <hpp/pinocchio/joint.hh> |
24 |
|
|
#include <hpp/rbprm/planner/timed-parabola-path.hh> |
25 |
|
|
#include <hpp/util/debug.hh> |
26 |
|
|
|
27 |
|
|
// coefficient[3] = theta |
28 |
|
|
// coefficient[4] = alpha |
29 |
|
|
|
30 |
|
|
namespace hpp { |
31 |
|
|
namespace rbprm { |
32 |
|
|
using core::interval_t; |
33 |
|
|
using core::size_type; |
34 |
|
|
using core::value_type; |
35 |
|
|
using core::vector_t; |
36 |
|
|
using pinocchio::displayConfig; |
37 |
|
|
|
38 |
|
|
/// Constructor |
39 |
|
|
TimedParabolaPath::TimedParabolaPath(const core::DevicePtr_t& robot, |
40 |
|
|
core::ConfigurationIn_t init, |
41 |
|
|
core::ConfigurationIn_t end, |
42 |
|
|
ParabolaPathPtr_t parabolaPath) |
43 |
|
|
: parent_t(*parabolaPath), |
44 |
|
|
device_(robot), |
45 |
|
|
initial_(init), |
46 |
|
|
end_(end), |
47 |
|
|
parabolaPath_(parabolaPath), |
48 |
|
|
length_(computeTimedLength(parabolaPath)) { |
49 |
|
|
hppDout(notice, |
50 |
|
|
"timed path constructor : end = " << pinocchio::displayConfig(end_)); |
51 |
|
|
} |
52 |
|
|
|
53 |
|
|
/// Constructor |
54 |
|
|
TimedParabolaPath::TimedParabolaPath(const core::DevicePtr_t& robot, |
55 |
|
|
core::ConfigurationIn_t init, |
56 |
|
|
core::ConfigurationIn_t end, |
57 |
|
|
core::value_type length, |
58 |
|
|
core::vector_t coefficients) |
59 |
|
|
: parent_t(robot, init, end, length, coefficients), |
60 |
|
|
device_(robot), |
61 |
|
|
initial_(init), |
62 |
|
|
end_(end), |
63 |
|
|
parabolaPath_( |
64 |
|
|
ParabolaPath::create(robot, init, end, length, coefficients)), |
65 |
|
|
length_(computeTimedLength(parabolaPath_)) {} |
66 |
|
|
|
67 |
|
|
/// Constructor with velocities and ROMnames |
68 |
|
|
TimedParabolaPath::TimedParabolaPath(const core::DevicePtr_t& robot, |
69 |
|
|
core::ConfigurationIn_t init, |
70 |
|
|
core::ConfigurationIn_t end, |
71 |
|
|
core::value_type length, |
72 |
|
|
core::vector_t coefs, core::vector_t V0, |
73 |
|
|
core::vector_t Vimp, |
74 |
|
|
std::vector<std::string> initialROMnames, |
75 |
|
|
std::vector<std::string> endROMnames) |
76 |
|
|
: parent_t(robot, init, end, length, coefs, V0, Vimp, initialROMnames, |
77 |
|
|
endROMnames), |
78 |
|
|
device_(robot), |
79 |
|
|
initial_(init), |
80 |
|
|
end_(end), |
81 |
|
|
parabolaPath_(ParabolaPath::create(robot, init, end, length, coefs, V0, |
82 |
|
|
Vimp, initialROMnames, endROMnames)), |
83 |
|
|
length_(computeTimedLength(parabolaPath_)) {} |
84 |
|
|
|
85 |
|
|
/// Copy constructor |
86 |
|
|
TimedParabolaPath::TimedParabolaPath(const TimedParabolaPath& path) |
87 |
|
|
: parent_t(path), |
88 |
|
|
device_(path.device_), |
89 |
|
|
initial_(path.initial_), |
90 |
|
|
end_(path.end_), |
91 |
|
|
parabolaPath_(path.parabolaPath_), |
92 |
|
|
length_(path.length_) {} |
93 |
|
|
|
94 |
|
|
/// Extraction/Reversion of a sub-path |
95 |
|
|
/// \param subInterval interval of definition of the extract path |
96 |
|
|
/// If upper bound of subInterval is smaller than lower bound, |
97 |
|
|
/// result is reversed. |
98 |
|
|
core::PathPtr_t TimedParabolaPath::extract( |
99 |
|
|
const core::interval_t& /*subInterval*/) const { |
100 |
|
|
// TODO |
101 |
|
|
throw core::projection_error("Extract is not implemented for parabola paths"); |
102 |
|
|
} |
103 |
|
|
|
104 |
|
|
/// Reversion of a path |
105 |
|
|
/// \return a new path that is this one reversed. |
106 |
|
|
core::PathPtr_t TimedParabolaPath::reverse() const { |
107 |
|
|
hppDout(notice, " ~ reverse timed path parabola !!!!!!!!!!!!!!!!!!!!!!"); |
108 |
|
|
core::PathPtr_t reversePath = parabolaPath_->reverse(); |
109 |
|
|
ParabolaPathPtr_t paraReverse = |
110 |
|
|
dynamic_pointer_cast<ParabolaPath>(reversePath); |
111 |
|
|
return TimedParabolaPath::create(device_, end_, initial_, paraReverse); |
112 |
|
|
} |
113 |
|
|
|
114 |
|
|
double TimedParabolaPath::computeTimedLength(double x_theta, double v0, |
115 |
|
|
double alpha0) { |
116 |
|
|
return x_theta / (v0 * cos(alpha0)); |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
double TimedParabolaPath::computeTimedLength(ParabolaPathPtr_t parabolaPath) { |
120 |
|
|
const value_type X = parabolaPath->end()[0] - parabolaPath->initial()[0]; |
121 |
|
|
const value_type Y = parabolaPath->end()[1] - parabolaPath->initial()[1]; |
122 |
|
|
; |
123 |
|
|
// theta = coef[3] |
124 |
|
|
const value_type X_theta = X * cos(parabolaPath->coefficients()[3]) + |
125 |
|
|
Y * sin(parabolaPath->coefficients()[3]); |
126 |
|
|
return computeTimedLength(X_theta, parabolaPath->V0_.norm(), |
127 |
|
|
parabolaPath->coefficients()[4]); |
128 |
|
|
} |
129 |
|
|
|
130 |
|
|
bool TimedParabolaPath::impl_compute(core::ConfigurationOut_t result, |
131 |
|
|
value_type t) const { |
132 |
|
|
if (t == 0 || initial_(0) == end_(0)) { |
133 |
|
|
result = initial_; |
134 |
|
|
return true; |
135 |
|
|
} |
136 |
|
|
if (t >= length_) { |
137 |
|
|
result = end_; |
138 |
|
|
return true; |
139 |
|
|
} |
140 |
|
|
value_type v0 = parabolaPath_->V0_.norm(); |
141 |
|
|
|
142 |
|
|
/* hppDout(notice,"impl compute timed-path-parabola"); |
143 |
|
|
hppDout(notice,"lenght = "<<length_); |
144 |
|
|
hppDout(notice,"t = "<<t); |
145 |
|
|
*/ |
146 |
|
|
// compute u and call parabolaPath |
147 |
|
|
value_type u = t * v0 * cos(parabolaPath_->coefficients()[4]); |
148 |
|
|
/* hppDout(notice,"parabola-path length = "<<parabolaPath_->length()); |
149 |
|
|
hppDout(notice,"u = "<<u);*/ |
150 |
|
|
bool successPath; |
151 |
|
|
result = (*parabolaPath_)(u, successPath); |
152 |
|
|
if (successPath) { |
153 |
|
|
// TODO : compute extraDOF |
154 |
|
|
const size_type indexEcs = |
155 |
|
|
device_->configSize() - |
156 |
|
|
device_->extraConfigSpace().dimension(); // ecs index |
157 |
|
|
// velocity : |
158 |
|
|
vector_t vel = parabolaPath_->evaluateVelocity(u); |
159 |
|
|
result[indexEcs] = vel[0]; |
160 |
|
|
result[indexEcs + 1] = vel[1]; |
161 |
|
|
result[indexEcs + 2] = vel[2]; |
162 |
|
|
// acceleration : |
163 |
|
|
result[indexEcs + 3] = 0.; |
164 |
|
|
result[indexEcs + 4] = 0.; |
165 |
|
|
result[indexEcs + 5] = -9.81; // FIXME : retrieve it from somewhere |
166 |
|
|
return true; |
167 |
|
|
} else { |
168 |
|
|
return false; |
169 |
|
|
} |
170 |
|
|
} // impl_compute |
171 |
|
|
|
172 |
|
|
} // namespace rbprm |
173 |
|
|
} // namespace hpp |