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_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_dc1_c1_H
7#define BEZIER_COM_TRAJ_c0_dc0_ddc0_dc1_c1_H
8
9#include <hpp/bezier-com-traj/data.hh>
10
11namespace bezier_com_traj {
12namespace c0_dc0_ddc0_dc1_c1 {
13
14static const ConstraintFlag flag =
15 INIT_POS | INIT_VEL | INIT_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 // equation found with sympy
35 wp.first = 10.0 * t5 - 20.0 * t4 + 10.0 * t3;
36 wp.second = -1.0 * pi[0] * t5 + 5.0 * pi[0] * t4 - 10.0 * pi[0] * t3 +
37 10.0 * pi[0] * t2 - 5.0 * pi[0] * t + 1.0 * pi[0] +
38 5.0 * pi[1] * t5 - 20.0 * pi[1] * t4 + 30.0 * pi[1] * t3 -
39 20.0 * pi[1] * t2 + 5.0 * pi[1] * t - 10.0 * pi[2] * t5 +
40 30.0 * pi[2] * t4 - 30.0 * pi[2] * t3 + 10.0 * pi[2] * t2 -
41 5.0 * pi[4] * t5 + 5.0 * pi[4] * t4 + 1.0 * pi[5] * t5;
42 return wp;
43}
44
45inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
46 double T, double t) {
47 coefs_t wp;
48 double alpha = 1. / (T * T);
49 double t2 = t * t;
50 double t3 = t2 * t;
51 // equation found with sympy
52 wp.first = (200.0 * t3 - 240.0 * t2 + 60.0 * t) * alpha;
53 wp.second = 1.0 *
54 (-20.0 * pi[0] * t3 + 60.0 * pi[0] * t2 - 60.0 * pi[0] * t +
55 20.0 * pi[0] + 100.0 * pi[1] * t3 - 240.0 * pi[1] * t2 +
56 180.0 * pi[1] * t - 40.0 * pi[1] - 200.0 * pi[2] * t3 +
57 360.0 * pi[2] * t2 - 180.0 * pi[2] * t + 20.0 * pi[2] -
58 100.0 * pi[4] * t3 + 60.0 * pi[4] * t2 + 20.0 * pi[5] * t3) *
59 alpha;
60 return wp;
61}
62
63inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
64 double T) {
65 // equation for constraint on initial and final position and velocity and
66 // initial acceleration(degree 5, 5 constant waypoint and one free (p3))
67 // first, compute the constant waypoints that only depend on pData :
68 double n = 5.;
69 std::vector<point_t> pi;
70 pi.push_back(pData.c0_); // p0
71 pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
72 pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
73 (2. * pData.dc0_ * T / n) + pData.c0_); // p2
74 pi.push_back(point_t::Zero()); // x
75 pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
76 pi.push_back(pData.c1_); // p5
77 return pi;
78}
79
80inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
81 double T) {
82 bezier_wp_t::t_point_t wps;
83 const int DIM_POINT = 6;
84 const int DIM_VAR = 3;
85 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
86 std::vector<Matrix3> Cpi;
87 for (std::size_t i = 0; i < pi.size(); ++i) {
88 Cpi.push_back(skew(pi[i]));
89 }
90 const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
91 const Matrix3 Cg = skew(g);
92 const double T2 = T * T;
93 const double alpha = 1 / (T2);
94
95 // equation of waypoints for curve w found with sympy
96 waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
97 w0.second.head<3>() = (20 * pi[0] - 40 * pi[1] + 20 * pi[2]) * alpha;
98 w0.second.tail<3>() =
99 1.0 *
100 (1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.0 * Cpi[0] * pi[2]) *
101 alpha;
102 wps.push_back(w0);
103 waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
104 w1.first.block<3, 3>(0, 0) = 8.57142857142857 * alpha * Matrix3::Identity();
105 w1.first.block<3, 3>(3, 0) = 8.57142857142857 * Cpi[0] * alpha;
106 w1.second.head<3>() = 1.0 *
107 (11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] -
108 5.71428571428572 * pi[2]) *
109 alpha;
110 w1.second.tail<3>() =
111 1.0 *
112 (0.285714285714286 * Cg * T2 * pi[0] +
113 0.714285714285714 * Cg * T2 * pi[1] - 20.0 * Cpi[0] * pi[2] +
114 14.2857142857143 * Cpi[1] * pi[2]) *
115 alpha;
116 wps.push_back(w1);
117 waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
118 w2.first.block<3, 3>(0, 0) = 5.71428571428571 * alpha * Matrix3::Identity();
119 w2.first.block<3, 3>(3, 0) =
120 1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
121 w2.second.head<3>() = 1.0 *
122 (5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] +
123 2.85714285714286 * pi[4]) *
124 alpha;
125 w2.second.tail<3>() =
126 1.0 *
127 (0.0476190476190479 * Cg * T2 * pi[0] +
128 0.476190476190476 * Cg * T2 * pi[1] +
129 0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] -
130 14.2857142857143 * Cpi[1] * pi[2]) *
131 alpha;
132 wps.push_back(w2);
133 waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
134 w3.first.block<3, 3>(0, 0) = -2.85714285714286 * alpha * Matrix3::Identity();
135 w3.first.block<3, 3>(3, 0) =
136 1.0 *
137 (0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] +
138 11.4285714285714 * Cpi[2]) *
139 alpha;
140 w3.second.head<3>() = 1.0 *
141 (2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] -
142 11.4285714285714 * pi[2] + 5.71428571428571 * pi[4] +
143 0.571428571428571 * pi[5]) *
144 alpha;
145 w3.second.tail<3>() =
146 1.0 *
147 (0.142857142857143 * Cg * T2 * pi[1] +
148 0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
149 0.571428571428571 * Cpi[0] * pi[5] + 8.57142857142857 * Cpi[1] * pi[4]) *
150 alpha;
151 wps.push_back(w3);
152 waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
153 w4.first.block<3, 3>(0, 0) = -11.4285714285714 * alpha * Matrix3::Identity();
154 w4.first.block<3, 3>(3, 0) =
155 1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
156 w4.second.head<3>() = 1.0 *
157 (0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] -
158 2.85714285714286 * pi[2] + 5.71428571428571 * pi[4] +
159 2.28571428571429 * pi[5]) *
160 alpha;
161 w4.second.tail<3>() =
162 1.0 *
163 (0.285714285714286 * Cg * T2 * pi[2] +
164 0.142857142857143 * Cg * T2 * pi[4] -
165 0.571428571428572 * Cpi[0] * pi[5] - 8.57142857142857 * Cpi[1] * pi[4] +
166 2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
167 alpha;
168 wps.push_back(w4);
169 waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
170 w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
171 w5.first.block<3, 3>(3, 0) =
172 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
173 w5.second.head<3>() = 1.0 *
174 (2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] +
175 5.71428571428571 * pi[5]) *
176 alpha;
177 w5.second.tail<3>() =
178 1.0 *
179 (0.476190476190476 * Cg * T2 * pi[4] +
180 0.0476190476190476 * Cg * T2 * pi[5] -
181 2.85714285714286 * Cpi[1] * pi[5] - 14.2857142857143 * Cpi[2] * pi[4] +
182 8.57142857142857 * Cpi[2] * pi[5]) *
183 alpha;
184 wps.push_back(w5);
185 waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
186 w6.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
187 w6.first.block<3, 3>(3, 0) =
188 1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
189 w6.second.head<3>() = 1.0 *
190 (8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] +
191 11.4285714285714 * pi[5]) *
192 alpha;
193 w6.second.tail<3>() = 1.0 *
194 (0.714285714285714 * Cg * T2 * pi[4] +
195 0.285714285714286 * Cg * T2 * pi[5] -
196 8.57142857142858 * Cpi[2] * pi[5]) *
197 alpha;
198 wps.push_back(w6);
199 waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
200 w7.first.block<3, 3>(0, 0) = 20 * alpha * Matrix3::Identity();
201 w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5]) * alpha;
202 w7.second.head<3>() = (-40 * pi[4] + 20 * pi[5]) * alpha;
203 w7.second.tail<3>() =
204 1.0 * (1.0 * Cg * T2 * pi[5] + 40.0 * Cpi[4] * pi[5]) * alpha;
205 wps.push_back(w7);
206 return wps;
207}
208
209inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
210 coefs_t v;
211 std::vector<point_t> pi = computeConstantWaypoints(pData, T);
212 // equation found with sympy
213 v.first = 0.;
214 v.second = (-5.0 * pi[4] + 5.0 * pi[5]) / T;
215 return v;
216}
217
218} // namespace c0_dc0_ddc0_dc1_c1
219} // namespace bezier_com_traj
220
221#endif
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_dc1_c1::computeConstantWaypoints
std::vector< point_t > computeConstantWaypoints(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_dc1_c1.hh:63
bezier_com_traj::c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint
coefs_t computeFinalVelocityPoint(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_dc1_c1.hh:209
bezier_com_traj::c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime
coefs_t evaluateAccelerationCurveAtTime(const std::vector< point_t > &pi, double T, double t)
Definition waypoints_c0_dc0_ddc0_dc1_c1.hh:45
bezier_com_traj::c0_dc0_ddc0_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_dc1_c1.hh:28
bezier_com_traj::c0_dc0_ddc0_dc1_c1::computeWwaypoints
bezier_wp_t::t_point_t computeWwaypoints(const ProblemData &pData, double T)
Definition waypoints_c0_dc0_ddc0_dc1_c1.hh:80
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