| Line |
Branch |
Exec |
Source |
| 1 |
|
|
/* |
| 2 |
|
|
* Copyright 2018, LAAS-CNRS |
| 3 |
|
|
* Author: Pierre Fernbach |
| 4 |
|
|
*/ |
| 5 |
|
|
|
| 6 |
|
|
#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H |
| 7 |
|
|
#define BEZIER_COM_TRAJ_c0_dc0_ddc0_c1_H |
| 8 |
|
|
|
| 9 |
|
|
#include <hpp/bezier-com-traj/data.hh> |
| 10 |
|
|
|
| 11 |
|
|
namespace bezier_com_traj { |
| 12 |
|
|
namespace c0_dc0_ddc0_c1 { |
| 13 |
|
|
|
| 14 |
|
|
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_POS; |
| 15 |
|
|
|
| 16 |
|
|
/// ### EQUATION FOR CONSTRAINts on initial position, velocity and acceleration, |
| 17 |
|
|
/// and only final position (degree = 4) |
| 18 |
|
|
/** |
| 19 |
|
|
* @brief evaluateCurveAtTime compute the expression of the point on the curve |
| 20 |
|
|
* at t, defined by the waypoint pi and one free waypoint (x) |
| 21 |
|
|
* @param pi constant waypoints of the curve, assume p0 p1 x p2 p3 |
| 22 |
|
|
* @param t param (normalized !) |
| 23 |
|
|
* @return the expression of the waypoint such that wp.first . x + wp.second = |
| 24 |
|
|
* point on curve |
| 25 |
|
|
*/ |
| 26 |
|
✗ |
inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) { |
| 27 |
|
✗ |
coefs_t wp; |
| 28 |
|
✗ |
double t2 = t * t; |
| 29 |
|
✗ |
double t3 = t2 * t; |
| 30 |
|
✗ |
double t4 = t3 * t; |
| 31 |
|
|
// equation found with sympy |
| 32 |
|
✗ |
wp.first = -4.0 * t4 + 4.0 * t3; |
| 33 |
|
✗ |
wp.second = 1.0 * pi[0] * t4 - 4.0 * pi[0] * t3 + 6.0 * pi[0] * t2 - |
| 34 |
|
✗ |
4.0 * pi[0] * t + 1.0 * pi[0] - 4.0 * pi[1] * t4 + |
| 35 |
|
✗ |
12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t + |
| 36 |
|
✗ |
6.0 * pi[2] * t4 - 12.0 * pi[2] * t3 + 6.0 * pi[2] * t2 + |
| 37 |
|
✗ |
1.0 * pi[4] * t4; |
| 38 |
|
✗ |
return wp; |
| 39 |
|
|
} |
| 40 |
|
|
|
| 41 |
|
✗ |
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, |
| 42 |
|
|
double T, double t) { |
| 43 |
|
✗ |
coefs_t wp; |
| 44 |
|
✗ |
double alpha = 1. / (T * T); |
| 45 |
|
✗ |
double t2 = t * t; |
| 46 |
|
|
// equation found with sympy |
| 47 |
|
✗ |
wp.first = (-48.0 * t2 + 24.0 * t) * alpha; |
| 48 |
|
|
wp.second = |
| 49 |
|
✗ |
(12.0 * pi[0] * t2 - 24.0 * pi[0] * t + 12.0 * pi[0] - 48.0 * pi[1] * t2 + |
| 50 |
|
✗ |
72.0 * pi[1] * t - 24.0 * pi[1] + 72.0 * pi[2] * t2 - 72.0 * pi[2] * t + |
| 51 |
|
✗ |
12.0 * pi[2] + 12.0 * pi[4] * t2) * |
| 52 |
|
✗ |
alpha; |
| 53 |
|
✗ |
return wp; |
| 54 |
|
|
} |
| 55 |
|
|
|
| 56 |
|
✗ |
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, |
| 57 |
|
|
double T) { |
| 58 |
|
|
// equation for constraint on initial position, velocity and acceleration, and |
| 59 |
|
|
// only final position (degree = 4)(degree 4, 4 constant waypoint and one free |
| 60 |
|
|
// (p3)) first, compute the constant waypoints that only depend on pData : |
| 61 |
|
✗ |
double n = 4.; |
| 62 |
|
✗ |
std::vector<point_t> pi; |
| 63 |
|
✗ |
pi.push_back(pData.c0_); // p0 |
| 64 |
|
✗ |
pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1 |
| 65 |
|
✗ |
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + |
| 66 |
|
✗ |
(2. * pData.dc0_ * T / n) + pData.c0_); // p2 |
| 67 |
|
✗ |
pi.push_back(point_t::Zero()); // x |
| 68 |
|
✗ |
pi.push_back(pData.c1_); // p4 |
| 69 |
|
✗ |
return pi; |
| 70 |
|
|
} |
| 71 |
|
|
|
| 72 |
|
✗ |
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, |
| 73 |
|
|
double T) { |
| 74 |
|
✗ |
bezier_wp_t::t_point_t wps; |
| 75 |
|
✗ |
const int DIM_POINT = 6; |
| 76 |
|
✗ |
const int DIM_VAR = 3; |
| 77 |
|
✗ |
std::vector<point_t> pi = computeConstantWaypoints(pData, T); |
| 78 |
|
✗ |
std::vector<Matrix3> Cpi; |
| 79 |
|
✗ |
for (std::size_t i = 0; i < pi.size(); ++i) { |
| 80 |
|
✗ |
Cpi.push_back(skew(pi[i])); |
| 81 |
|
|
} |
| 82 |
|
✗ |
const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity; |
| 83 |
|
✗ |
const Matrix3 Cg = skew(g); |
| 84 |
|
✗ |
const double T2 = T * T; |
| 85 |
|
✗ |
const double alpha = 1 / (T2); |
| 86 |
|
|
|
| 87 |
|
|
// equation of waypoints for curve w found with sympy |
| 88 |
|
✗ |
waypoint_t w0 = initwp(DIM_POINT, DIM_VAR); |
| 89 |
|
✗ |
w0.second.head<3>() = (12 * pi[0] - 24 * pi[1] + 12 * pi[2]) * alpha; |
| 90 |
|
✗ |
w0.second.tail<3>() = |
| 91 |
|
✗ |
1.0 * |
| 92 |
|
✗ |
(1.0 * Cg * T2 * pi[0] - 24.0 * Cpi[0] * pi[1] + 12.0 * Cpi[0] * pi[2]) * |
| 93 |
|
✗ |
alpha; |
| 94 |
|
✗ |
wps.push_back(w0); |
| 95 |
|
✗ |
waypoint_t w1 = initwp(DIM_POINT, DIM_VAR); |
| 96 |
|
✗ |
w1.first.block<3, 3>(0, 0) = 4.8 * alpha * Matrix3::Identity(); |
| 97 |
|
✗ |
w1.first.block<3, 3>(3, 0) = 4.8 * Cpi[0] * alpha; |
| 98 |
|
✗ |
w1.second.head<3>() = 1.0 * (7.2 * pi[0] - 9.6 * pi[1] - 2.4 * pi[2]) * alpha; |
| 99 |
|
✗ |
w1.second.tail<3>() = 1.0 * |
| 100 |
|
✗ |
(0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] - |
| 101 |
|
✗ |
12.0 * Cpi[0] * pi[2] + 9.6 * Cpi[1] * pi[2]) * |
| 102 |
|
✗ |
alpha; |
| 103 |
|
✗ |
wps.push_back(w1); |
| 104 |
|
✗ |
waypoint_t w2 = initwp(DIM_POINT, DIM_VAR); |
| 105 |
|
✗ |
w2.first.block<3, 3>(0, 0) = 4.8 * alpha * Matrix3::Identity(); |
| 106 |
|
✗ |
w2.first.block<3, 3>(3, 0) = 1.0 * (-4.8 * Cpi[0] + 9.6 * Cpi[1]) * alpha; |
| 107 |
|
✗ |
w2.second.head<3>() = 1.0 * (3.6 * pi[0] - 9.6 * pi[2] + 1.2 * pi[4]) * alpha; |
| 108 |
|
✗ |
w2.second.tail<3>() = 1.0 * |
| 109 |
|
✗ |
(0.4 * Cg * T2 * pi[1] + 0.6 * Cg * T2 * pi[2] + |
| 110 |
|
✗ |
1.2 * Cpi[0] * pi[4] - 9.6 * Cpi[1] * pi[2]) * |
| 111 |
|
✗ |
alpha; |
| 112 |
|
✗ |
wps.push_back(w2); |
| 113 |
|
✗ |
waypoint_t w3 = initwp(DIM_POINT, DIM_VAR); |
| 114 |
|
✗ |
w3.first.block<3, 3>(3, 0) = |
| 115 |
|
✗ |
1.0 * (0.4 * Cg * T2 - 9.6 * Cpi[1] + 9.6 * Cpi[2]) * alpha; |
| 116 |
|
✗ |
w3.second.head<3>() = |
| 117 |
|
✗ |
1.0 * (1.2 * pi[0] + 4.8 * pi[1] - 9.6 * pi[2] + 3.6 * pi[4]) * alpha; |
| 118 |
|
✗ |
w3.second.tail<3>() = |
| 119 |
|
✗ |
1.0 * |
| 120 |
|
✗ |
(0.6 * Cg * T2 * pi[2] - 1.2 * Cpi[0] * pi[4] + 4.8 * Cpi[1] * pi[4]) * |
| 121 |
|
✗ |
alpha; |
| 122 |
|
✗ |
wps.push_back(w3); |
| 123 |
|
✗ |
waypoint_t w4 = initwp(DIM_POINT, DIM_VAR); |
| 124 |
|
✗ |
w4.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity(); |
| 125 |
|
✗ |
w4.first.block<3, 3>(3, 0) = 1.0 * (0.8 * Cg * T2 - 9.6 * Cpi[2]) * alpha; |
| 126 |
|
✗ |
w4.second.head<3>() = 1.0 * (4.8 * pi[1] - 2.4 * pi[2] + 7.2 * pi[4]) * alpha; |
| 127 |
|
✗ |
w4.second.tail<3>() = |
| 128 |
|
✗ |
1.0 * |
| 129 |
|
✗ |
(0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4] + 12.0 * Cpi[2] * pi[4]) * |
| 130 |
|
✗ |
alpha; |
| 131 |
|
✗ |
wps.push_back(w4); |
| 132 |
|
✗ |
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR); |
| 133 |
|
✗ |
w5.first.block<3, 3>(0, 0) = -24 * alpha * Matrix3::Identity(); |
| 134 |
|
✗ |
w5.first.block<3, 3>(3, 0) = 1.0 * (-24.0 * Cpi[4]) * alpha; |
| 135 |
|
✗ |
w5.second.head<3>() = (12 * pi[2] + 12 * pi[4]) * alpha; |
| 136 |
|
✗ |
w5.second.tail<3>() = |
| 137 |
|
✗ |
1.0 * (1.0 * Cg * T2 * pi[4] - 12.0 * Cpi[2] * pi[4]) * alpha; |
| 138 |
|
✗ |
wps.push_back(w5); |
| 139 |
|
✗ |
return wps; |
| 140 |
|
|
} |
| 141 |
|
|
|
| 142 |
|
✗ |
inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) { |
| 143 |
|
✗ |
coefs_t v; |
| 144 |
|
|
// equation found with sympy |
| 145 |
|
✗ |
v.first = -4. / T; |
| 146 |
|
✗ |
v.second = 4. * pData.c1_ / T; |
| 147 |
|
✗ |
return v; |
| 148 |
|
|
} |
| 149 |
|
|
|
| 150 |
|
|
} // namespace c0_dc0_ddc0_c1 |
| 151 |
|
|
} // namespace bezier_com_traj |
| 152 |
|
|
|
| 153 |
|
|
#endif |
| 154 |
|
|
|