hpp-bezier-com-traj 6.0.0
Multi contact trajectory generation for the COM using Bezier curves
Loading...
Searching...
No Matches
waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
Go to the documentation of this file.
1/*
2 * Copyright 2018, LAAS-CNRS
3 * Author: Pierre Fernbach
4 */
5
6#ifndef BEZIER_COM_TRAJ_c0_dc0_ddc0_ddc1_dc1_c1_H
7#define BEZIER_COM_TRAJ_c0_dc0_ddc0_ddc1_dc1_c1_H
8
9#include <hpp/bezier-com-traj/data.hh>
10
11namespace bezier_com_traj {
12namespace c0_dc0_ddc0_ddc1_dc1_c1 {
13
14static const ConstraintFlag flag =
15 INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS;
16
20
28inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
29 coefs_t wp;
30 double t2 = t * t;
31 double t3 = t2 * t;
32 double t4 = t3 * t;
33 double t5 = t4 * t;
34 double t6 = t5 * t;
35 // equation found with sympy
36 wp.first = -20.0 * t6 + 60.0 * t5 - 60.0 * t4 + 20.0 * t3;
37 wp.second = 1.0 * pi[0] * t6 - 6.0 * pi[0] * t5 + 15.0 * pi[0] * t4 -
38 20.0 * pi[0] * t3 + 15.0 * pi[0] * t2 - 6.0 * pi[0] * t +
39 1.0 * pi[0] - 6.0 * pi[1] * t6 + 30.0 * pi[1] * t5 -
40 60.0 * pi[1] * t4 + 60.0 * pi[1] * t3 - 30.0 * pi[1] * t2 +
41 6.0 * pi[1] * t + 15.0 * pi[2] * t6 - 60.0 * pi[2] * t5 +
42 90.0 * pi[2] * t4 - 60.0 * pi[2] * t3 + 15.0 * pi[2] * t2 +
43 15.0 * pi[4] * t6 - 30.0 * pi[4] * t5 + 15.0 * pi[4] * t4 -
44 6.0 * pi[5] * t6 + 6.0 * pi[5] * t5 + 1.0 * pi[6] * t6;
45 return wp;
46}
47
48inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
49 double T, double t) {
50 coefs_t wp;
51 double alpha = 1. / (T * T);
52 double t2 = t * t;
53 double t3 = t2 * t;
54 double t4 = t3 * t;
55 // equation found with sympy
56 wp.first = 1.0 * (-600.0 * t4 + 1200.0 * t3 - 720.0 * t2 + 120.0 * t) * alpha;
57 wp.second = 1.0 *
58 (30.0 * pi[0] * t4 - 120.0 * pi[0] * t3 + 180.0 * pi[0] * t2 -
59 120.0 * pi[0] * t + 30.0 * pi[0] - 180.0 * pi[1] * t4 +
60 600.0 * pi[1] * t3 - 720.0 * pi[1] * t2 + 360.0 * pi[1] * t -
61 60.0 * pi[1] + 450.0 * pi[2] * t4 - 1200.0 * pi[2] * t3 +
62 1080.0 * pi[2] * t2 - 360.0 * pi[2] * t + 30.0 * pi[2] +
63 450.0 * pi[4] * t4 - 600.0 * pi[4] * t3 + 180.0 * pi[4] * t2 -
64 180.0 * pi[5] * t4 + 120.0 * pi[5] * t3 + 30.0 * pi[6] * t4) *
65 alpha;
66 return wp;
67}
68
69inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
70 double T) {
71 // equation for constraint on initial and final position and velocity and
72 // initial acceleration(degree 5, 5 constant waypoint and one free (p3))
73 // first, compute the constant waypoints that only depend on pData :
74 double n = 6.;
75 std::vector<point_t> pi;
76 pi.push_back(pData.c0_); // p0
77 pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
78 pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
79 (2. * pData.dc0_ * T / n) + pData.c0_); // p2
80 pi.push_back(point_t::Zero()); // x
81 pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
82 (2 * pData.dc1_ * T / n) + pData.c1_);
83 pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
84 pi.push_back(pData.c1_); // p5
85 return pi;
86}
87
88inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
89 double T) {
90 bezier_wp_t::t_point_t wps;
91 const int DIM_POINT = 6;
92 const int DIM_VAR = 3;
93 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
94 std::vector<Matrix3> Cpi;
95 for (std::size_t i = 0; i < pi.size(); ++i) {
96 Cpi.push_back(skew(pi[i]));
97 }
98 const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
99 const Matrix3 Cg = skew(g);
100 const double T2 = T * T;
101 const double alpha = 1 / (T2);
102
103 // equation of waypoints for curve w found with sympy
104 waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
105 w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
106 w0.second.tail<3>() =
107 1.0 *
108 (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) *
109 alpha;
110 wps.push_back(w0);
111 waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
112 w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
113 w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
114 w1.second.head<3>() =
115 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
116 w1.second.tail<3>() = 1.0 *
117 (0.333333333333333 * Cg * T2 * pi[0] +
118 0.666666666666667 * Cg * T2 * pi[1] -
119 30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
120 alpha;
121 wps.push_back(w1);
122 waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
123 w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
124 w2.first.block<3, 3>(3, 0) =
125 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
126 w2.second.head<3>() =
127 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
128 w2.second.tail<3>() =
129 1.0 *
130 (0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
131 0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
132 20.0 * Cpi[1] * pi[2]) *
133 alpha;
134 wps.push_back(w2);
135 waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
136 w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
137 w3.first.block<3, 3>(3, 0) = 1.0 *
138 (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
139 14.2857142857143 * Cpi[2]) *
140 alpha;
141 w3.second.head<3>() = 1.0 *
142 (3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
143 14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
144 1.42857142857143 * pi[5]) *
145 alpha;
146 w3.second.tail<3>() =
147 1.0 *
148 (0.0119047619047619 * Cg * T2 * pi[0] +
149 0.214285714285714 * Cg * T2 * pi[1] +
150 0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
151 1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
152 alpha;
153 wps.push_back(w3);
154 waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
155 w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
156 w4.first.block<3, 3>(3, 0) =
157 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
158 w4.second.head<3>() = 1.0 *
159 (1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
160 3.57142857142857 * pi[2] + 5.0 * pi[4] +
161 4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
162 alpha;
163 w4.second.tail<3>() =
164 1.0 *
165 (0.0476190476190471 * Cg * T2 * pi[1] +
166 0.357142857142857 * Cg * T2 * pi[2] +
167 0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
168 0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
169 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
170 alpha;
171 wps.push_back(w4);
172 waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
173 w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
174 w5.first.block<3, 3>(3, 0) =
175 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
176 w5.second.head<3>() = 1.0 *
177 (0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
178 5.0 * pi[2] - 3.57142857142857 * pi[4] +
179 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
180 alpha;
181 w5.second.tail<3>() =
182 1.0 *
183 (+0.11904761904762 * Cg * T2 * pi[2] +
184 0.357142857142857 * Cg * T2 * pi[4] +
185 0.0476190476190476 * Cg * T2 * pi[5] -
186 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
187 1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
188 12.8571428571429 * Cpi[2] * pi[5]) *
189 alpha;
190 wps.push_back(w5);
191 waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
192 w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
193 w6.first.block<3, 3>(3, 0) = 1.0 *
194 (0.238095238095238 * Cg * T2 +
195 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
196 alpha;
197 w6.second.head<3>() = 1.0 *
198 (1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
199 14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
200 3.57142857142857 * pi[6]) *
201 alpha;
202 w6.second.tail<3>() =
203 1.0 *
204 (0.535714285714286 * Cg * T2 * pi[4] +
205 0.214285714285714 * Cg * T2 * pi[5] +
206 0.0119047619047619 * Cg * T2 * pi[6] -
207 1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
208 5.0 * Cpi[2] * pi[6]) *
209 alpha;
210 wps.push_back(w6);
211 waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
212 w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
213 w7.first.block<3, 3>(3, 0) =
214 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
215 w7.second.head<3>() =
216 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
217 w7.second.tail<3>() =
218 1.0 *
219 (0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
220 0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
221 20.0 * Cpi[4] * pi[5]) *
222 alpha;
223 wps.push_back(w7);
224 waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
225 w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
226 w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
227 w8.second.head<3>() =
228 1.0 *
229 (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
230 alpha;
231 w8.second.tail<3>() = 1.0 *
232 (0.666666666666667 * Cg * T2 * pi[5] +
233 0.333333333333333 * Cg * T2 * pi[6] -
234 20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
235 alpha;
236 wps.push_back(w8);
237 waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
238 w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
239 w9.second.tail<3>() =
240 1.0 *
241 (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *
242 alpha;
243 wps.push_back(w9);
244 return wps;
245}
246
247inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
248 coefs_t v;
249 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
250 // equation found with sympy
251 v.first = 0.;
252 v.second = (-6.0 * pi[5] + 6.0 * pi[6]) / T;
253 return v;
254}
255
256} // namespace c0_dc0_ddc0_ddc1_dc1_c1
257
258} // namespace bezier_com_traj
259
260#endif
END_ACC
END_ACC
Definition flags.hh:25
INIT_VEL
INIT_VEL
Definition flags.hh:21
END_VEL
END_VEL
Definition flags.hh:24
END_POS
END_POS
Definition flags.hh:23
INIT_ACC
INIT_ACC
Definition flags.hh:22
INIT_POS
INIT_POS
Definition flags.hh:20
bezier_com_traj::c0_dc0_ddc0_ddc1_dc1_c1::evaluateCurveAtTime
coefs_t evaluateCurveAtTime(const std::vector< point_t > &pi, double t)
evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi...
Definition waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:28
bezier_com_traj::c0_dc0_ddc0_ddc1_dc1_c1::computeFinalVelocityPoint
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:247
bezier_com_traj::c0_dc0_ddc0_ddc1_dc1_c1::computeWwaypoints
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:88
bezier_com_traj::c0_dc0_ddc0_ddc1_dc1_c1::computeConstantWaypoints
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:69
bezier_com_traj::c0_dc0_ddc0_ddc1_dc1_c1::evaluateAccelerationCurveAtTime
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh:48
bezier_com_traj
Definition common_solve_methods.hh:15
bezier_com_traj::w0
waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &p0X, const Matrix3 &, const Matrix3 &, const double alpha)
Definition solve_0_step.cpp:12
bezier_com_traj::skew
BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x)
skew symmetric matrix
Definition utils.cpp:62
bezier_com_traj::Matrix3
Eigen::Matrix< value_type, 3, 3 > Matrix3
Definition definitions.hh:17
bezier_com_traj::DIM_POINT
const int DIM_POINT
Definition solve_end_effector.hh:15
bezier_com_traj::Vector3
centroidal_dynamics::Vector3 Vector3
Definition definitions.hh:22
bezier_com_traj::w3
waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &, const double alpha)
Definition solve_0_step.cpp:45
bezier_com_traj::coefs_t
std::pair< double, point3_t > coefs_t
Definition definitions.hh:62
bezier_com_traj::w1
waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha)
Definition solve_0_step.cpp:23
bezier_com_traj::w4
waypoint6_t w4(point_t_tC, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &, const double alpha)
Definition solve_0_step.cpp:56
bezier_com_traj::w2
waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3 &, const Matrix3 &, const Matrix3 &gX, const double alpha)
Definition solve_0_step.cpp:34
bezier_com_traj::computeDistanceCostFunction
std::pair< MatrixXX, VectorX > computeDistanceCostFunction(size_t numPoints, const ProblemData &pData, double T, std::vector< point3_t > pts_path)
Definition solve_end_effector.hh:224
bezier_com_traj::initwp
T initwp()
bezier_com_traj::ProblemData
Defines all the inputs of the problem: Initial and terminal constraints, as well as selected cost fun...
Definition data.hh:92
bezier_com_traj::waypoint_t
Definition utils.hh:25