Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • jchemin/hpp-bezier-com-traj
  • pfernbac/hpp-bezier-com-traj
  • gsaurel/hpp-bezier-com-traj
  • humanoid-path-planner/hpp-bezier-com-traj
4 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 1864 additions and 1044 deletions
include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
View file @ 626b8c65
......@@ -13,13 +13,15 @@ namespace c0_dc0_ddc0_c1 {
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_POS;
/// ### EQUATION FOR CONSTRAINts on initial position, velocity and acceleration, and only final position (degree = 4)
/// ### EQUATION FOR CONSTRAINts on initial position, velocity and acceleration,
/// and only final position (degree = 4)
/**
* @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one
* free waypoint (x)
* @brief evaluateCurveAtTime compute the expression of the point on the curve
* at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve, assume p0 p1 x p2 p3
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
coefs_t wp;
......@@ -28,39 +30,47 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
double t4 = t3 * t;
// equation found with sympy
wp.first = -4.0 * t4 + 4.0 * t3;
wp.second = 1.0 * pi[0] * t4 - 4.0 * pi[0] * t3 + 6.0 * pi[0] * t2 - 4.0 * pi[0] * t + 1.0 * pi[0] -
4.0 * pi[1] * t4 + 12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t + 6.0 * pi[2] * t4 -
12.0 * pi[2] * t3 + 6.0 * pi[2] * t2 + 1.0 * pi[4] * t4;
wp.second = 1.0 * pi[0] * t4 - 4.0 * pi[0] * t3 + 6.0 * pi[0] * t2 -
4.0 * pi[0] * t + 1.0 * pi[0] - 4.0 * pi[1] * t4 +
12.0 * pi[1] * t3 - 12.0 * pi[1] * t2 + 4.0 * pi[1] * t +
6.0 * pi[2] * t4 - 12.0 * pi[2] * t3 + 6.0 * pi[2] * t2 +
1.0 * pi[4] * t4;
return wp;
}
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
double T, double t) {
coefs_t wp;
double alpha = 1. / (T * T);
double t2 = t * t;
// equation found with sympy
wp.first = (-48.0 * t2 + 24.0 * t) * alpha;
wp.second = (12.0 * pi[0] * t2 - 24.0 * pi[0] * t + 12.0 * pi[0] - 48.0 * pi[1] * t2 + 72.0 * pi[1] * t -
24.0 * pi[1] + 72.0 * pi[2] * t2 - 72.0 * pi[2] * t + 12.0 * pi[2] + 12.0 * pi[4] * t2) *
alpha;
wp.second =
(12.0 * pi[0] * t2 - 24.0 * pi[0] * t + 12.0 * pi[0] - 48.0 * pi[1] * t2 +
72.0 * pi[1] * t - 24.0 * pi[1] + 72.0 * pi[2] * t2 - 72.0 * pi[2] * t +
12.0 * pi[2] + 12.0 * pi[4] * t2) *
alpha;
return wp;
}
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
// equation for constraint on initial position, velocity and acceleration, and only final position (degree =
// 4)(degree 4, 4 constant waypoint and one free (p3)) first, compute the constant waypoints that only depend on
// pData :
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
double T) {
// equation for constraint on initial position, velocity and acceleration, and
// only final position (degree = 4)(degree 4, 4 constant waypoint and one free
// (p3)) first, compute the constant waypoints that only depend on pData :
double n = 4.;
std::vector<point_t> pi;
pi.push_back(pData.c0_); // p0
pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_); // p2
pi.push_back(point_t::Zero()); // x
pi.push_back(pData.c1_); // p4
pi.push_back(pData.c0_); // p0
pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
(2. * pData.dc0_ * T / n) + pData.c0_); // p2
pi.push_back(point_t::Zero()); // x
pi.push_back(pData.c1_); // p4
return pi;
}
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
double T) {
bezier_wp_t::t_point_t wps;
const int DIM_POINT = 6;
const int DIM_VAR = 3;
......@@ -77,38 +87,54 @@ inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double
// equation of waypoints for curve w found with sympy
waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
w0.second.head<3>() = (12 * pi[0] - 24 * pi[1] + 12 * pi[2]) * alpha;
w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 24.0 * Cpi[0] * pi[1] + 12.0 * Cpi[0] * pi[2]) * alpha;
w0.second.tail<3>() =
1.0 *
(1.0 * Cg * T2 * pi[0] - 24.0 * Cpi[0] * pi[1] + 12.0 * Cpi[0] * pi[2]) *
alpha;
wps.push_back(w0);
waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
w1.first.block<3, 3>(0, 0) = 4.8 * alpha * Matrix3::Identity();
w1.first.block<3, 3>(3, 0) = 4.8 * Cpi[0] * alpha;
w1.second.head<3>() = 1.0 * (7.2 * pi[0] - 9.6 * pi[1] - 2.4 * pi[2]) * alpha;
w1.second.tail<3>() =
1.0 * (0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] - 12.0 * Cpi[0] * pi[2] + 9.6 * Cpi[1] * pi[2]) * alpha;
w1.second.tail<3>() = 1.0 *
(0.2 * Cg * T2 * pi[0] + 0.8 * Cg * T2 * pi[1] -
12.0 * Cpi[0] * pi[2] + 9.6 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w1);
waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
w2.first.block<3, 3>(0, 0) = 4.8 * alpha * Matrix3::Identity();
w2.first.block<3, 3>(3, 0) = 1.0 * (-4.8 * Cpi[0] + 9.6 * Cpi[1]) * alpha;
w2.second.head<3>() = 1.0 * (3.6 * pi[0] - 9.6 * pi[2] + 1.2 * pi[4]) * alpha;
w2.second.tail<3>() =
1.0 * (0.4 * Cg * T2 * pi[1] + 0.6 * Cg * T2 * pi[2] + 1.2 * Cpi[0] * pi[4] - 9.6 * Cpi[1] * pi[2]) * alpha;
w2.second.tail<3>() = 1.0 *
(0.4 * Cg * T2 * pi[1] + 0.6 * Cg * T2 * pi[2] +
1.2 * Cpi[0] * pi[4] - 9.6 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w2);
waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
w3.first.block<3, 3>(3, 0) = 1.0 * (0.4 * Cg * T2 - 9.6 * Cpi[1] + 9.6 * Cpi[2]) * alpha;
w3.second.head<3>() = 1.0 * (1.2 * pi[0] + 4.8 * pi[1] - 9.6 * pi[2] + 3.6 * pi[4]) * alpha;
w3.second.tail<3>() = 1.0 * (0.6 * Cg * T2 * pi[2] - 1.2 * Cpi[0] * pi[4] + 4.8 * Cpi[1] * pi[4]) * alpha;
w3.first.block<3, 3>(3, 0) =
1.0 * (0.4 * Cg * T2 - 9.6 * Cpi[1] + 9.6 * Cpi[2]) * alpha;
w3.second.head<3>() =
1.0 * (1.2 * pi[0] + 4.8 * pi[1] - 9.6 * pi[2] + 3.6 * pi[4]) * alpha;
w3.second.tail<3>() =
1.0 *
(0.6 * Cg * T2 * pi[2] - 1.2 * Cpi[0] * pi[4] + 4.8 * Cpi[1] * pi[4]) *
alpha;
wps.push_back(w3);
waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
w4.first.block<3, 3>(0, 0) = -9.6 * alpha * Matrix3::Identity();
w4.first.block<3, 3>(3, 0) = 1.0 * (0.8 * Cg * T2 - 9.6 * Cpi[2]) * alpha;
w4.second.head<3>() = 1.0 * (4.8 * pi[1] - 2.4 * pi[2] + 7.2 * pi[4]) * alpha;
w4.second.tail<3>() = 1.0 * (0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4] + 12.0 * Cpi[2] * pi[4]) * alpha;
w4.second.tail<3>() =
1.0 *
(0.2 * Cg * T2 * pi[4] - 4.8 * Cpi[1] * pi[4] + 12.0 * Cpi[2] * pi[4]) *
alpha;
wps.push_back(w4);
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
w5.first.block<3, 3>(0, 0) = -24 * alpha * Matrix3::Identity();
w5.first.block<3, 3>(3, 0) = 1.0 * (-24.0 * Cpi[4]) * alpha;
w5.second.head<3>() = (12 * pi[2] + 12 * pi[4]) * alpha;
w5.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[4] - 12.0 * Cpi[2] * pi[4]) * alpha;
w5.second.tail<3>() =
1.0 * (1.0 * Cg * T2 * pi[4] - 12.0 * Cpi[2] * pi[4]) * alpha;
wps.push_back(w5);
return wps;
}
......
include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
View file @ 626b8c65
......@@ -11,16 +11,19 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_dc1_c1 {
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_VEL | END_POS;
static const ConstraintFlag flag =
INIT_POS | INIT_VEL | INIT_ACC | END_VEL | END_POS;
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND INIT ACCELERATION (DEGREE = 5)
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
/// INIT ACCELERATION (DEGREE = 5)
///
/**
* @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one
* free waypoint (x)
* @brief evaluateCurveAtTime compute the expression of the point on the curve
* at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve, assume p0 p1 x p2 p3
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
coefs_t wp;
......@@ -30,14 +33,17 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
double t5 = t4 * t;
// equation found with sympy
wp.first = 10.0 * t5 - 20.0 * t4 + 10.0 * t3;
wp.second = -1.0 * pi[0] * t5 + 5.0 * pi[0] * t4 - 10.0 * pi[0] * t3 + 10.0 * pi[0] * t2 - 5.0 * pi[0] * t +
1.0 * pi[0] + 5.0 * pi[1] * t5 - 20.0 * pi[1] * t4 + 30.0 * pi[1] * t3 - 20.0 * pi[1] * t2 +
5.0 * pi[1] * t - 10.0 * pi[2] * t5 + 30.0 * pi[2] * t4 - 30.0 * pi[2] * t3 + 10.0 * pi[2] * t2 -
wp.second = -1.0 * pi[0] * t5 + 5.0 * pi[0] * t4 - 10.0 * pi[0] * t3 +
10.0 * pi[0] * t2 - 5.0 * pi[0] * t + 1.0 * pi[0] +
5.0 * pi[1] * t5 - 20.0 * pi[1] * t4 + 30.0 * pi[1] * t3 -
20.0 * pi[1] * t2 + 5.0 * pi[1] * t - 10.0 * pi[2] * t5 +
30.0 * pi[2] * t4 - 30.0 * pi[2] * t3 + 10.0 * pi[2] * t2 -
5.0 * pi[4] * t5 + 5.0 * pi[4] * t4 + 1.0 * pi[5] * t5;
return wp;
}
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
double T, double t) {
coefs_t wp;
double alpha = 1. / (T * T);
double t2 = t * t;
......@@ -45,28 +51,34 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, d
// equation found with sympy
wp.first = (200.0 * t3 - 240.0 * t2 + 60.0 * t) * alpha;
wp.second = 1.0 *
(-20.0 * pi[0] * t3 + 60.0 * pi[0] * t2 - 60.0 * pi[0] * t + 20.0 * pi[0] + 100.0 * pi[1] * t3 -
240.0 * pi[1] * t2 + 180.0 * pi[1] * t - 40.0 * pi[1] - 200.0 * pi[2] * t3 + 360.0 * pi[2] * t2 -
180.0 * pi[2] * t + 20.0 * pi[2] - 100.0 * pi[4] * t3 + 60.0 * pi[4] * t2 + 20.0 * pi[5] * t3) *
(-20.0 * pi[0] * t3 + 60.0 * pi[0] * t2 - 60.0 * pi[0] * t +
20.0 * pi[0] + 100.0 * pi[1] * t3 - 240.0 * pi[1] * t2 +
180.0 * pi[1] * t - 40.0 * pi[1] - 200.0 * pi[2] * t3 +
360.0 * pi[2] * t2 - 180.0 * pi[2] * t + 20.0 * pi[2] -
100.0 * pi[4] * t3 + 60.0 * pi[4] * t2 + 20.0 * pi[5] * t3) *
alpha;
return wp;
}
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
// equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
// waypoint and one free (p3)) first, compute the constant waypoints that only depend on pData :
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
double T) {
// equation for constraint on initial and final position and velocity and
// initial acceleration(degree 5, 5 constant waypoint and one free (p3))
// first, compute the constant waypoints that only depend on pData :
double n = 5.;
std::vector<point_t> pi;
pi.push_back(pData.c0_); // p0
pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_); // p2
pi.push_back(point_t::Zero()); // x
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
pi.push_back(pData.c1_); // p5
pi.push_back(pData.c0_); // p0
pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
(2. * pData.dc0_ * T / n) + pData.c0_); // p2
pi.push_back(point_t::Zero()); // x
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
pi.push_back(pData.c1_); // p5
return pi;
}
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
double T) {
bezier_wp_t::t_point_t wps;
const int DIM_POINT = 6;
const int DIM_VAR = 3;
......@@ -83,78 +95,113 @@ inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double
// equation of waypoints for curve w found with sympy
waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
w0.second.head<3>() = (20 * pi[0] - 40 * pi[1] + 20 * pi[2]) * alpha;
w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.0 * Cpi[0] * pi[2]) * alpha;
w0.second.tail<3>() =
1.0 *
(1.0 * Cg * T2 * pi[0] - 40.0 * Cpi[0] * pi[1] + 20.0 * Cpi[0] * pi[2]) *
alpha;
wps.push_back(w0);
waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
w1.first.block<3, 3>(0, 0) = 8.57142857142857 * alpha * Matrix3::Identity();
w1.first.block<3, 3>(3, 0) = 8.57142857142857 * Cpi[0] * alpha;
w1.second.head<3>() = 1.0 * (11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] - 5.71428571428572 * pi[2]) * alpha;
w1.second.tail<3>() = 1.0 *
(0.285714285714286 * Cg * T2 * pi[0] + 0.714285714285714 * Cg * T2 * pi[1] -
20.0 * Cpi[0] * pi[2] + 14.2857142857143 * Cpi[1] * pi[2]) *
w1.second.head<3>() = 1.0 *
(11.4285714285714 * pi[0] - 14.2857142857143 * pi[1] -
5.71428571428572 * pi[2]) *
alpha;
w1.second.tail<3>() =
1.0 *
(0.285714285714286 * Cg * T2 * pi[0] +
0.714285714285714 * Cg * T2 * pi[1] - 20.0 * Cpi[0] * pi[2] +
14.2857142857143 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w1);
waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
w2.first.block<3, 3>(0, 0) = 5.71428571428571 * alpha * Matrix3::Identity();
w2.first.block<3, 3>(3, 0) = 1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
w2.second.head<3>() = 1.0 * (5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] + 2.85714285714286 * pi[4]) * alpha;
w2.first.block<3, 3>(3, 0) =
1.0 * (-8.57142857142857 * Cpi[0] + 14.2857142857143 * Cpi[1]) * alpha;
w2.second.head<3>() = 1.0 *
(5.71428571428571 * pi[0] - 14.2857142857143 * pi[2] +
2.85714285714286 * pi[4]) *
alpha;
w2.second.tail<3>() =
1.0 *
(0.0476190476190479 * Cg * T2 * pi[0] + 0.476190476190476 * Cg * T2 * pi[1] +
0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] - 14.2857142857143 * Cpi[1] * pi[2]) *
(0.0476190476190479 * Cg * T2 * pi[0] +
0.476190476190476 * Cg * T2 * pi[1] +
0.476190476190476 * Cg * T2 * pi[2] + 2.85714285714286 * Cpi[0] * pi[4] -
14.2857142857143 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w2);
waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
w3.first.block<3, 3>(0, 0) = -2.85714285714286 * alpha * Matrix3::Identity();
w3.first.block<3, 3>(3, 0) =
1.0 * (0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] + 11.4285714285714 * Cpi[2]) * alpha;
1.0 *
(0.285714285714286 * Cg * T2 - 14.2857142857143 * Cpi[1] +
11.4285714285714 * Cpi[2]) *
alpha;
w3.second.head<3>() = 1.0 *
(2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] - 11.4285714285714 * pi[2] +
5.71428571428571 * pi[4] + 0.571428571428571 * pi[5]) *
(2.28571428571429 * pi[0] + 5.71428571428571 * pi[1] -
11.4285714285714 * pi[2] + 5.71428571428571 * pi[4] +
0.571428571428571 * pi[5]) *
alpha;
w3.second.tail<3>() =
1.0 *
(0.142857142857143 * Cg * T2 * pi[1] + 0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
(0.142857142857143 * Cg * T2 * pi[1] +
0.571428571428571 * Cg * T2 * pi[2] - 2.85714285714286 * Cpi[0] * pi[4] +
0.571428571428571 * Cpi[0] * pi[5] + 8.57142857142857 * Cpi[1] * pi[4]) *
alpha;
wps.push_back(w3);
waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
w4.first.block<3, 3>(0, 0) = -11.4285714285714 * alpha * Matrix3::Identity();
w4.first.block<3, 3>(3, 0) = 1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
w4.first.block<3, 3>(3, 0) =
1.0 * (0.571428571428571 * Cg * T2 - 11.4285714285714 * Cpi[2]) * alpha;
w4.second.head<3>() = 1.0 *
(0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] - 2.85714285714286 * pi[2] +
5.71428571428571 * pi[4] + 2.28571428571429 * pi[5]) *
(0.571428571428571 * pi[0] + 5.71428571428571 * pi[1] -
2.85714285714286 * pi[2] + 5.71428571428571 * pi[4] +
2.28571428571429 * pi[5]) *
alpha;
w4.second.tail<3>() =
1.0 *
(0.285714285714286 * Cg * T2 * pi[2] + 0.142857142857143 * Cg * T2 * pi[4] - 0.571428571428572 * Cpi[0] * pi[5] -
8.57142857142857 * Cpi[1] * pi[4] + 2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
(0.285714285714286 * Cg * T2 * pi[2] +
0.142857142857143 * Cg * T2 * pi[4] -
0.571428571428572 * Cpi[0] * pi[5] - 8.57142857142857 * Cpi[1] * pi[4] +
2.85714285714286 * Cpi[1] * pi[5] + 14.2857142857143 * Cpi[2] * pi[4]) *
alpha;
wps.push_back(w4);
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
w5.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
w5.second.head<3>() = 1.0 * (2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] + 5.71428571428571 * pi[5]) * alpha;
w5.first.block<3, 3>(3, 0) =
1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
w5.second.head<3>() = 1.0 *
(2.85714285714286 * pi[1] + 5.71428571428571 * pi[2] +
5.71428571428571 * pi[5]) *
alpha;
w5.second.tail<3>() =
1.0 *
(0.476190476190476 * Cg * T2 * pi[4] + 0.0476190476190476 * Cg * T2 * pi[5] - 2.85714285714286 * Cpi[1] * pi[5] -
14.2857142857143 * Cpi[2] * pi[4] + 8.57142857142857 * Cpi[2] * pi[5]) *
(0.476190476190476 * Cg * T2 * pi[4] +
0.0476190476190476 * Cg * T2 * pi[5] -
2.85714285714286 * Cpi[1] * pi[5] - 14.2857142857143 * Cpi[2] * pi[4] +
8.57142857142857 * Cpi[2] * pi[5]) *
alpha;
wps.push_back(w5);
waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
w6.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
w6.first.block<3, 3>(3, 0) = 1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
w6.second.head<3>() = 1.0 * (8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] + 11.4285714285714 * pi[5]) * alpha;
w6.second.tail<3>() =
1.0 *
(0.714285714285714 * Cg * T2 * pi[4] + 0.285714285714286 * Cg * T2 * pi[5] - 8.57142857142858 * Cpi[2] * pi[5]) *
alpha;
w6.first.block<3, 3>(3, 0) =
1.0 * (14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
w6.second.head<3>() = 1.0 *
(8.57142857142857 * pi[2] - 14.2857142857143 * pi[4] +
11.4285714285714 * pi[5]) *
alpha;
w6.second.tail<3>() = 1.0 *
(0.714285714285714 * Cg * T2 * pi[4] +
0.285714285714286 * Cg * T2 * pi[5] -
8.57142857142858 * Cpi[2] * pi[5]) *
alpha;
wps.push_back(w6);
waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
w7.first.block<3, 3>(0, 0) = 20 * alpha * Matrix3::Identity();
w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5]) * alpha;
w7.second.head<3>() = (-40 * pi[4] + 20 * pi[5]) * alpha;
w7.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[5] + 40.0 * Cpi[4] * pi[5]) * alpha;
w7.second.tail<3>() =
1.0 * (1.0 * Cg * T2 * pi[5] + 40.0 * Cpi[4] * pi[5]) * alpha;
wps.push_back(w7);
return wps;
}
......
include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
View file @ 626b8c65
......@@ -11,16 +11,19 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_ddc1_dc1_c1 {
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS;
static const ConstraintFlag flag =
INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS;
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION (DEGREE = 6)
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
/// ACCELERATION (DEGREE = 6)
///
/**
* @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one
* free waypoint (x)
* @brief evaluateCurveAtTime compute the expression of the point on the curve
* at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve, assume p0 p1 p2 x p3 p4 p5
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
coefs_t wp;
......@@ -31,15 +34,19 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
double t6 = t5 * t;
// equation found with sympy
wp.first = -20.0 * t6 + 60.0 * t5 - 60.0 * t4 + 20.0 * t3;
wp.second = 1.0 * pi[0] * t6 - 6.0 * pi[0] * t5 + 15.0 * pi[0] * t4 - 20.0 * pi[0] * t3 + 15.0 * pi[0] * t2 -
6.0 * pi[0] * t + 1.0 * pi[0] - 6.0 * pi[1] * t6 + 30.0 * pi[1] * t5 - 60.0 * pi[1] * t4 +
60.0 * pi[1] * t3 - 30.0 * pi[1] * t2 + 6.0 * pi[1] * t + 15.0 * pi[2] * t6 - 60.0 * pi[2] * t5 +
90.0 * pi[2] * t4 - 60.0 * pi[2] * t3 + 15.0 * pi[2] * t2 + 15.0 * pi[4] * t6 - 30.0 * pi[4] * t5 +
15.0 * pi[4] * t4 - 6.0 * pi[5] * t6 + 6.0 * pi[5] * t5 + 1.0 * pi[6] * t6;
wp.second = 1.0 * pi[0] * t6 - 6.0 * pi[0] * t5 + 15.0 * pi[0] * t4 -
20.0 * pi[0] * t3 + 15.0 * pi[0] * t2 - 6.0 * pi[0] * t +
1.0 * pi[0] - 6.0 * pi[1] * t6 + 30.0 * pi[1] * t5 -
60.0 * pi[1] * t4 + 60.0 * pi[1] * t3 - 30.0 * pi[1] * t2 +
6.0 * pi[1] * t + 15.0 * pi[2] * t6 - 60.0 * pi[2] * t5 +
90.0 * pi[2] * t4 - 60.0 * pi[2] * t3 + 15.0 * pi[2] * t2 +
15.0 * pi[4] * t6 - 30.0 * pi[4] * t5 + 15.0 * pi[4] * t4 -
6.0 * pi[5] * t6 + 6.0 * pi[5] * t5 + 1.0 * pi[6] * t6;
return wp;
}
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
double T, double t) {
coefs_t wp;
double alpha = 1. / (T * T);
double t2 = t * t;
......@@ -48,31 +55,38 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, d
// equation found with sympy
wp.first = 1.0 * (-600.0 * t4 + 1200.0 * t3 - 720.0 * t2 + 120.0 * t) * alpha;
wp.second = 1.0 *
(30.0 * pi[0] * t4 - 120.0 * pi[0] * t3 + 180.0 * pi[0] * t2 - 120.0 * pi[0] * t + 30.0 * pi[0] -
180.0 * pi[1] * t4 + 600.0 * pi[1] * t3 - 720.0 * pi[1] * t2 + 360.0 * pi[1] * t - 60.0 * pi[1] +
450.0 * pi[2] * t4 - 1200.0 * pi[2] * t3 + 1080.0 * pi[2] * t2 - 360.0 * pi[2] * t + 30.0 * pi[2] +
450.0 * pi[4] * t4 - 600.0 * pi[4] * t3 + 180.0 * pi[4] * t2 - 180.0 * pi[5] * t4 + 120.0 * pi[5] * t3 +
30.0 * pi[6] * t4) *
(30.0 * pi[0] * t4 - 120.0 * pi[0] * t3 + 180.0 * pi[0] * t2 -
120.0 * pi[0] * t + 30.0 * pi[0] - 180.0 * pi[1] * t4 +
600.0 * pi[1] * t3 - 720.0 * pi[1] * t2 + 360.0 * pi[1] * t -
60.0 * pi[1] + 450.0 * pi[2] * t4 - 1200.0 * pi[2] * t3 +
1080.0 * pi[2] * t2 - 360.0 * pi[2] * t + 30.0 * pi[2] +
450.0 * pi[4] * t4 - 600.0 * pi[4] * t3 + 180.0 * pi[4] * t2 -
180.0 * pi[5] * t4 + 120.0 * pi[5] * t3 + 30.0 * pi[6] * t4) *
alpha;
return wp;
}
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
// equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
// waypoint and one free (p3)) first, compute the constant waypoints that only depend on pData :
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
double T) {
// equation for constraint on initial and final position and velocity and
// initial acceleration(degree 5, 5 constant waypoint and one free (p3))
// first, compute the constant waypoints that only depend on pData :
double n = 6.;
std::vector<point_t> pi;
pi.push_back(pData.c0_); // p0
pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2. * pData.dc0_ * T / n) + pData.c0_); // p2
pi.push_back(point_t::Zero()); // x
pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_);
pi.push_back(pData.c0_); // p0
pi.push_back((pData.dc0_ * T / n) + pData.c0_); // p1
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
(2. * pData.dc0_ * T / n) + pData.c0_); // p2
pi.push_back(point_t::Zero()); // x
pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
(2 * pData.dc1_ * T / n) + pData.c1_);
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
pi.push_back(pData.c1_); // p5
return pi;
}
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
double T) {
bezier_wp_t::t_point_t wps;
const int DIM_POINT = 6;
const int DIM_VAR = 3;
......@@ -89,101 +103,143 @@ inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double
// equation of waypoints for curve w found with sympy
waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) * alpha;
w0.second.tail<3>() =
1.0 *
(1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) *
alpha;
wps.push_back(w0);
waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
w1.second.head<3>() = 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
w1.second.head<3>() =
1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
w1.second.tail<3>() = 1.0 *
(0.333333333333333 * Cg * T2 * pi[0] + 0.666666666666667 * Cg * T2 * pi[1] -
(0.333333333333333 * Cg * T2 * pi[0] +
0.666666666666667 * Cg * T2 * pi[1] -
30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w1);
waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
w2.first.block<3, 3>(3, 0) = 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
w2.second.head<3>() = 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
w2.second.tail<3>() = 1.0 *
(0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] - 20.0 * Cpi[1] * pi[2]) *
alpha;
w2.first.block<3, 3>(3, 0) =
1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
w2.second.head<3>() =
1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
w2.second.tail<3>() =
1.0 *
(0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
20.0 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w2);
waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
w3.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] + 14.2857142857143 * Cpi[2]) * alpha;
w3.first.block<3, 3>(3, 0) = 1.0 *
(0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
14.2857142857143 * Cpi[2]) *
alpha;
w3.second.head<3>() = 1.0 *
(3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] - 14.2857142857143 * pi[2] +
7.85714285714286 * pi[4] + 1.42857142857143 * pi[5]) *
alpha;
w3.second.tail<3>() = 1.0 *
(0.0119047619047619 * Cg * T2 * pi[0] + 0.214285714285714 * Cg * T2 * pi[1] +
0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
(3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
1.42857142857143 * pi[5]) *
alpha;
w3.second.tail<3>() =
1.0 *
(0.0119047619047619 * Cg * T2 * pi[0] +
0.214285714285714 * Cg * T2 * pi[1] +
0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
alpha;
wps.push_back(w3);
waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
w4.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
w4.first.block<3, 3>(3, 0) =
1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
w4.second.head<3>() = 1.0 *
(1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] - 3.57142857142857 * pi[2] + 5.0 * pi[4] +
(1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
3.57142857142857 * pi[2] + 5.0 * pi[4] +
4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
alpha;
w4.second.tail<3>() =
1.0 *
(0.0476190476190471 * Cg * T2 * pi[1] + 0.357142857142857 * Cg * T2 * pi[2] +
0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] + 0.238095238095238 * Cpi[0] * pi[6] -
12.8571428571429 * Cpi[1] * pi[4] + 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
(0.0476190476190471 * Cg * T2 * pi[1] +
0.357142857142857 * Cg * T2 * pi[2] +
0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
alpha;
wps.push_back(w4);
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
w5.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
w5.first.block<3, 3>(3, 0) =
1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
w5.second.head<3>() = 1.0 *
(0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] + 5.0 * pi[2] -
3.57142857142857 * pi[4] + 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
(0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
5.0 * pi[2] - 3.57142857142857 * pi[4] +
7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
alpha;
w5.second.tail<3>() =
1.0 *
(+0.11904761904762 * Cg * T2 * pi[2] + 0.357142857142857 * Cg * T2 * pi[4] +
0.0476190476190476 * Cg * T2 * pi[5] - 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] + 12.8571428571429 * Cpi[2] * pi[5]) *
(+0.11904761904762 * Cg * T2 * pi[2] +
0.357142857142857 * Cg * T2 * pi[4] +
0.0476190476190476 * Cg * T2 * pi[5] -
0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
12.8571428571429 * Cpi[2] * pi[5]) *
alpha;
wps.push_back(w5);
waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
w6.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 + 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
w6.first.block<3, 3>(3, 0) = 1.0 *
(0.238095238095238 * Cg * T2 +
14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
alpha;
w6.second.head<3>() = 1.0 *
(1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] - 14.2857142857143 * pi[4] +
7.14285714285715 * pi[5] + 3.57142857142857 * pi[6]) *
alpha;
w6.second.tail<3>() = 1.0 *
(0.535714285714286 * Cg * T2 * pi[4] + 0.214285714285714 * Cg * T2 * pi[5] +
0.0119047619047619 * Cg * T2 * pi[6] - 1.42857142857143 * Cpi[1] * pi[6] -
12.8571428571429 * Cpi[2] * pi[5] + 5.0 * Cpi[2] * pi[6]) *
(1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
3.57142857142857 * pi[6]) *
alpha;
w6.second.tail<3>() =
1.0 *
(0.535714285714286 * Cg * T2 * pi[4] +
0.214285714285714 * Cg * T2 * pi[5] +
0.0119047619047619 * Cg * T2 * pi[6] -
1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
5.0 * Cpi[2] * pi[6]) *
alpha;
wps.push_back(w6);
waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
w7.second.head<3>() = 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
w7.second.tail<3>() = 1.0 *
(0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] + 20.0 * Cpi[4] * pi[5]) *
alpha;
w7.first.block<3, 3>(3, 0) =
1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
w7.second.head<3>() =
1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
w7.second.tail<3>() =
1.0 *
(0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
20.0 * Cpi[4] * pi[5]) *
alpha;
wps.push_back(w7);
waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
w8.second.head<3>() = 1.0 * (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) * alpha;
w8.second.head<3>() =
1.0 *
(-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
alpha;
w8.second.tail<3>() = 1.0 *
(0.666666666666667 * Cg * T2 * pi[5] + 0.333333333333333 * Cg * T2 * pi[6] -
(0.666666666666667 * Cg * T2 * pi[5] +
0.333333333333333 * Cg * T2 * pi[6] -
20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
alpha;
wps.push_back(w8);
waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
w9.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) * alpha;
w9.second.tail<3>() =
1.0 *
(1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *
alpha;
wps.push_back(w9);
return wps;
}
......
include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
View file @ 626b8c65
......@@ -11,17 +11,21 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_j0_j1_ddc1_dc1_c1 {
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK;
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
END_VEL | END_POS | INIT_JERK | END_JERK;
static const size_t DIM_VAR = 3;
static const size_t DIM_POINT = 3;
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION AND JERK (DEGREE = 8)
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
/// ACCELERATION AND JERK (DEGREE = 8)
///
/**
* @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one
* free waypoint (x)
* @param pi constant waypoints of the curve, assume pi[8] pi[1] pi[2] pi[3] x pi[4] pi[5] pi[6] pi[7]
* @brief evaluateCurveAtTime compute the expression of the point on the curve
* at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve, assume pi[8] pi[1] pi[2] pi[3] x
* pi[4] pi[5] pi[6] pi[7]
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
coefs_t wp;
......@@ -34,19 +38,25 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double t) {
const double t8 = t7 * t;
// equation found with sympy
wp.first = 70.0 * t8 - 280.0 * t7 + 420.0 * t6 - 280.0 * t5 + 70.0 * t4;
wp.second = 1.0 * pi[8] * t8 - 8.0 * pi[8] * t7 + 28.0 * pi[8] * t6 - 56.0 * pi[8] * t5 + 70.0 * pi[8] * t4 -
56.0 * pi[8] * t3 + 28.0 * pi[8] * t2 - 8.0 * pi[8] * t + 1.0 * pi[8] - 8.0 * pi[1] * t8 +
56.0 * pi[1] * t7 - 168.0 * pi[1] * t6 + 280.0 * pi[1] * t5 - 280.0 * pi[1] * t4 + 168.0 * pi[1] * t3 -
56.0 * pi[1] * t2 + 8.0 * pi[1] * t + 28.0 * pi[2] * t8 - 168.0 * pi[2] * t7 + 420.0 * pi[2] * t6 -
560.0 * pi[2] * t5 + 420.0 * pi[2] * t4 - 168.0 * pi[2] * t3 + 28.0 * pi[2] * t2 -
56.0 * pi[3] * pow(t, 8) + 280.0 * pi[3] * t7 - 560.0 * pi[3] * t6 + 560.0 * pi[3] * t5 -
280.0 * pi[3] * t4 + 56.0 * pi[3] * t3 - 56.0 * pi[5] * t8 + 168.0 * pi[5] * t7 - 168.0 * pi[5] * t6 +
56.0 * pi[5] * pow(t, 5) + 28.0 * pi[6] * t8 - 56.0 * pi[6] * t7 + 28.0 * pi[6] * t6 - 8.0 * pi[7] * t8 +
8.0 * pi[7] * t7 + 1.0 * pi[8] * t8;
wp.second = 1.0 * pi[8] * t8 - 8.0 * pi[8] * t7 + 28.0 * pi[8] * t6 -
56.0 * pi[8] * t5 + 70.0 * pi[8] * t4 - 56.0 * pi[8] * t3 +
28.0 * pi[8] * t2 - 8.0 * pi[8] * t + 1.0 * pi[8] -
8.0 * pi[1] * t8 + 56.0 * pi[1] * t7 - 168.0 * pi[1] * t6 +
280.0 * pi[1] * t5 - 280.0 * pi[1] * t4 + 168.0 * pi[1] * t3 -
56.0 * pi[1] * t2 + 8.0 * pi[1] * t + 28.0 * pi[2] * t8 -
168.0 * pi[2] * t7 + 420.0 * pi[2] * t6 - 560.0 * pi[2] * t5 +
420.0 * pi[2] * t4 - 168.0 * pi[2] * t3 + 28.0 * pi[2] * t2 -
56.0 * pi[3] * pow(t, 8) + 280.0 * pi[3] * t7 -
560.0 * pi[3] * t6 + 560.0 * pi[3] * t5 - 280.0 * pi[3] * t4 +
56.0 * pi[3] * t3 - 56.0 * pi[5] * t8 + 168.0 * pi[5] * t7 -
168.0 * pi[5] * t6 + 56.0 * pi[5] * pow(t, 5) +
28.0 * pi[6] * t8 - 56.0 * pi[6] * t7 + 28.0 * pi[6] * t6 -
8.0 * pi[7] * t8 + 8.0 * pi[7] * t7 + 1.0 * pi[8] * t8;
return wp;
}
inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi,
double T, double t) {
coefs_t wp;
const double alpha = 1. / (T);
const double t2 = t * t;
......@@ -56,21 +66,28 @@ inline coefs_t evaluateVelocityCurveAtTime(const std::vector<point_t>& pi, doubl
const double t6 = t5 * t;
const double t7 = t6 * t;
// equation found with sympy
wp.first = (560.0 * t7 - 1960.0 * t6 + 2520.0 * t5 - 1400.0 * t4 + 280.0 * t3) * alpha;
wp.second =
(8.0 * pi[8] * t7 - 56.0 * pi[8] * t6 + 168.0 * pi[8] * t5 - 280.0 * pi[8] * t4 + 280.0 * pi[8] * t3 -
168.0 * pi[8] * t2 + 56.0 * pi[8] * t - 8.0 * pi[8] - 64.0 * pi[1] * t7 + 392.0 * pi[1] * t6 -
1008.0 * pi[1] * t5 + 1400.0 * pi[1] * t4 - 1120.0 * pi[1] * t3 + 504.0 * pi[1] * t2 - 112.0 * pi[1] * t +
8.0 * pi[1] + 224.0 * pi[2] * t7 - 1176.0 * pi[2] * t6 + 2520.0 * pi[2] * t5 - 2800.0 * pi[2] * t4 +
1680.0 * pi[2] * t3 - 504.0 * pi[2] * t2 + 56.0 * pi[2] * t - 448.0 * pi[3] * t7 + 1960.0 * pi[3] * t6 -
3360.0 * pi[3] * t5 + 2800.0 * pi[3] * t4 - 1120.0 * pi[3] * t3 + 168.0 * pi[3] * t2 - 448.0 * pi[5] * t7 +
1176.0 * pi[5] * t6 - 1008.0 * pi[5] * t5 + 280.0 * pi[5] * t4 + 224.0 * pi[6] * t7 - 392.0 * pi[6] * t6 +
168.0 * pi[6] * t5 - 64.0 * pi[7] * t7 + 56.0 * pi[7] * t6 + 8.0 * pi[8] * t7) *
wp.first =
(560.0 * t7 - 1960.0 * t6 + 2520.0 * t5 - 1400.0 * t4 + 280.0 * t3) *
alpha;
wp.second = (8.0 * pi[8] * t7 - 56.0 * pi[8] * t6 + 168.0 * pi[8] * t5 -
280.0 * pi[8] * t4 + 280.0 * pi[8] * t3 - 168.0 * pi[8] * t2 +
56.0 * pi[8] * t - 8.0 * pi[8] - 64.0 * pi[1] * t7 +
392.0 * pi[1] * t6 - 1008.0 * pi[1] * t5 + 1400.0 * pi[1] * t4 -
1120.0 * pi[1] * t3 + 504.0 * pi[1] * t2 - 112.0 * pi[1] * t +
8.0 * pi[1] + 224.0 * pi[2] * t7 - 1176.0 * pi[2] * t6 +
2520.0 * pi[2] * t5 - 2800.0 * pi[2] * t4 + 1680.0 * pi[2] * t3 -
504.0 * pi[2] * t2 + 56.0 * pi[2] * t - 448.0 * pi[3] * t7 +
1960.0 * pi[3] * t6 - 3360.0 * pi[3] * t5 + 2800.0 * pi[3] * t4 -
1120.0 * pi[3] * t3 + 168.0 * pi[3] * t2 - 448.0 * pi[5] * t7 +
1176.0 * pi[5] * t6 - 1008.0 * pi[5] * t5 + 280.0 * pi[5] * t4 +
224.0 * pi[6] * t7 - 392.0 * pi[6] * t6 + 168.0 * pi[6] * t5 -
64.0 * pi[7] * t7 + 56.0 * pi[7] * t6 + 8.0 * pi[8] * t7) *
alpha;
return wp;
}
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,
double T, double t) {
coefs_t wp;
const double alpha = 1. / (T * T);
const double t2 = t * t;
......@@ -79,21 +96,28 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi, d
const double t5 = t4 * t;
const double t6 = t5 * t;
// equation found with sympy
wp.first = ((3920.0 * t6 - 11760.0 * t5 + 12600.0 * t4 - 5600.0 * t3 + 840.0 * t2)) * alpha;
wp.second =
(56.0 * pi[8] * t6 - 336.0 * pi[8] * t5 + 840.0 * pi[8] * t4 - 1120.0 * pi[8] * t3 + 840.0 * pi[8] * t2 -
336.0 * pi[8] * t + 56.0 * pi[8] - 448.0 * pi[1] * t6 + 2352.0 * pi[1] * t5 - 5040.0 * pi[1] * t4 +
5600.0 * pi[1] * t3 - 3360.0 * pi[1] * t2 + 1008.0 * pi[1] * t - 112.0 * pi[1] + 1568.0 * pi[2] * t6 -
7056.0 * pi[2] * t5 + 12600.0 * pi[2] * t4 - 11200.0 * pi[2] * t3 + 5040.0 * pi[2] * t2 - 1008.0 * pi[2] * t +
56.0 * pi[2] - 3136.0 * pi[3] * t6 + 11760.0 * pi[3] * t5 - 16800.0 * pi[3] * t4 + 11200.0 * pi[3] * t3 -
3360.0 * pi[3] * t2 + 336.0 * pi[3] * t - 3136.0 * pi[5] * t6 + 7056.0 * pi[5] * t5 - 5040.0 * pi[5] * t4 +
1120.0 * pi[5] * t3 + 1568.0 * pi[6] * t6 - 2352.0 * pi[6] * t5 + 840.0 * pi[6] * t4 - 448.0 * pi[7] * t6 +
336.0 * pi[7] * t5 + 56.0 * pi[8] * t6) *
wp.first =
((3920.0 * t6 - 11760.0 * t5 + 12600.0 * t4 - 5600.0 * t3 + 840.0 * t2)) *
alpha;
wp.second = (56.0 * pi[8] * t6 - 336.0 * pi[8] * t5 + 840.0 * pi[8] * t4 -
1120.0 * pi[8] * t3 + 840.0 * pi[8] * t2 - 336.0 * pi[8] * t +
56.0 * pi[8] - 448.0 * pi[1] * t6 + 2352.0 * pi[1] * t5 -
5040.0 * pi[1] * t4 + 5600.0 * pi[1] * t3 - 3360.0 * pi[1] * t2 +
1008.0 * pi[1] * t - 112.0 * pi[1] + 1568.0 * pi[2] * t6 -
7056.0 * pi[2] * t5 + 12600.0 * pi[2] * t4 -
11200.0 * pi[2] * t3 + 5040.0 * pi[2] * t2 - 1008.0 * pi[2] * t +
56.0 * pi[2] - 3136.0 * pi[3] * t6 + 11760.0 * pi[3] * t5 -
16800.0 * pi[3] * t4 + 11200.0 * pi[3] * t3 -
3360.0 * pi[3] * t2 + 336.0 * pi[3] * t - 3136.0 * pi[5] * t6 +
7056.0 * pi[5] * t5 - 5040.0 * pi[5] * t4 + 1120.0 * pi[5] * t3 +
1568.0 * pi[6] * t6 - 2352.0 * pi[6] * t5 + 840.0 * pi[6] * t4 -
448.0 * pi[7] * t6 + 336.0 * pi[7] * t5 + 56.0 * pi[8] * t6) *
alpha;
return wp;
}
inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi, double T, double t) {
inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi, double T,
double t) {
coefs_t wp;
const double alpha = 1. / (T * T * T);
const double t2 = t * t;
......@@ -101,34 +125,44 @@ inline coefs_t evaluateJerkCurveAtTime(const std::vector<point_t>& pi, double T,
const double t4 = t3 * t;
const double t5 = t4 * t;
// equation found with sympy
wp.first = (23520.0 * t5 - 58800.0 * t4 + 50400.0 * t3 - 16800.0 * t2 + 1680.0 * t) * alpha;
wp.first =
(23520.0 * t5 - 58800.0 * t4 + 50400.0 * t3 - 16800.0 * t2 + 1680.0 * t) *
alpha;
wp.second =
1.0 *
(336.0 * pi[0] * t5 - 1680.0 * pi[0] * t4 + 3360.0 * pi[0] * t3 - 3360.0 * pi[0] * t2 + 1680.0 * pi[0] * t -
336.0 * pi[0] - 2688.0 * pi[1] * t5 + 11760.0 * pi[1] * t4 - 20160.0 * pi[1] * t3 + 16800.0 * pi[1] * t2 -
6720.0 * pi[1] * t + 1008.0 * pi[1] + 9408.0 * pi[2] * t5 - 35280.0 * pi[2] * t4 + 50400.0 * pi[2] * t3 -
33600.0 * pi[2] * t2 + 10080.0 * pi[2] * t - 1008.0 * pi[2] - 18816.0 * pi[3] * t5 + 58800.0 * pi[3] * t4 -
67200.0 * pi[3] * t3 + 33600.0 * pi[3] * t2 - 6720.0 * pi[3] * t + 336.0 * pi[3] - 18816.0 * pi[5] * t5 +
35280.0 * pi[5] * t4 - 20160.0 * pi[5] * t3 + 3360.0 * pi[5] * t2 + 9408.0 * pi[6] * t5 - 11760.0 * pi[6] * t4 +
3360.0 * pi[6] * t3 - 2688.0 * pi[7] * t5 + 1680.0 * pi[7] * t4 + 336.0 * pi[8] * t5) *
(336.0 * pi[0] * t5 - 1680.0 * pi[0] * t4 + 3360.0 * pi[0] * t3 -
3360.0 * pi[0] * t2 + 1680.0 * pi[0] * t - 336.0 * pi[0] -
2688.0 * pi[1] * t5 + 11760.0 * pi[1] * t4 - 20160.0 * pi[1] * t3 +
16800.0 * pi[1] * t2 - 6720.0 * pi[1] * t + 1008.0 * pi[1] +
9408.0 * pi[2] * t5 - 35280.0 * pi[2] * t4 + 50400.0 * pi[2] * t3 -
33600.0 * pi[2] * t2 + 10080.0 * pi[2] * t - 1008.0 * pi[2] -
18816.0 * pi[3] * t5 + 58800.0 * pi[3] * t4 - 67200.0 * pi[3] * t3 +
33600.0 * pi[3] * t2 - 6720.0 * pi[3] * t + 336.0 * pi[3] -
18816.0 * pi[5] * t5 + 35280.0 * pi[5] * t4 - 20160.0 * pi[5] * t3 +
3360.0 * pi[5] * t2 + 9408.0 * pi[6] * t5 - 11760.0 * pi[6] * t4 +
3360.0 * pi[6] * t3 - 2688.0 * pi[7] * t5 + 1680.0 * pi[7] * t4 +
336.0 * pi[8] * t5) *
alpha;
return wp;
}
inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, double t) {
inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
double t) {
coefs_t coef = evaluateCurveAtTime(pi, t);
waypoint_t wp;
wp.first = Matrix3::Identity() * coef.first;
wp.second = coef.second;
return wp;
}
inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateVelocityCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
coefs_t coef = evaluateVelocityCurveAtTime(pi, T, t);
waypoint_t wp;
wp.first = Matrix3::Identity() * coef.first;
wp.second = coef.second;
return wp;
}
inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateAccelerationCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
coefs_t coef = evaluateAccelerationCurveAtTime(pi, T, t);
waypoint_t wp;
wp.first = Matrix3::Identity() * coef.first;
......@@ -136,7 +170,8 @@ inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<poin
return wp;
}
inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateJerkCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
coefs_t coef = evaluateJerkCurveAtTime(pi, T, t);
waypoint_t wp;
wp.first = Matrix3::Identity() * coef.first;
......@@ -144,27 +179,34 @@ inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi
return wp;
}
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
// equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
// waypoint and one free (pi[3])) first, compute the constant waypoints that only depend on pData :
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
double T) {
// equation for constraint on initial and final position and velocity and
// initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
// first, compute the constant waypoints that only depend on pData :
double n = 8.;
std::vector<point_t> pi;
pi.push_back(pData.c0_);
pi.push_back((pData.dc0_ * T / n) + pData.c0_);
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2 * pData.dc0_ * T / n) +
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
(2 * pData.dc0_ * T / n) +
pData.c0_); // * T because derivation make a T appear
pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
(3 * pData.ddc0_ * T * T / (n * (n - 1))) +
(3 * pData.dc0_ * T / n) + pData.c0_);
pi.push_back(point_t::Zero());
pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
(3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
(3 * pData.ddc1_ * T * T / (n * (n - 1))) -
(3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
(2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back(pData.c1_);
return pi;
}
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
double T) {
bezier_wp_t::t_point_t wps;
const int DIM_POINT = 6;
const int DIM_VAR = 3;
......@@ -181,107 +223,150 @@ inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double
// equation of waypoints for curve w found with sympy
waypoint_t w0 = initwp(DIM_POINT, DIM_VAR);
w0.second.head<3>() = (30 * pi[0] - 60 * pi[1] + 30 * pi[2]) * alpha;
w0.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) * alpha;
w0.second.tail<3>() =
1.0 *
(1.0 * Cg * T2 * pi[0] - 60.0 * Cpi[0] * pi[1] + 30.0 * Cpi[0] * pi[2]) *
alpha;
wps.push_back(w0);
waypoint_t w1 = initwp(DIM_POINT, DIM_VAR);
w1.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
w1.first.block<3, 3>(3, 0) = 13.3333333333333 * Cpi[0] * alpha;
w1.second.head<3>() = 1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
w1.second.head<3>() =
1.0 * (16.6666666666667 * pi[0] - 20.0 * pi[1] - 10.0 * pi[2]) * alpha;
w1.second.tail<3>() = 1.0 *
(0.333333333333333 * Cg * T2 * pi[0] + 0.666666666666667 * Cg * T2 * pi[1] -
(0.333333333333333 * Cg * T2 * pi[0] +
0.666666666666667 * Cg * T2 * pi[1] -
30.0 * Cpi[0] * pi[2] + 20.0 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w1);
waypoint_t w2 = initwp(DIM_POINT, DIM_VAR);
w2.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
w2.first.block<3, 3>(3, 0) = 1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
w2.second.head<3>() = 1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
w2.second.tail<3>() = 1.0 *
(0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] - 20.0 * Cpi[1] * pi[2]) *
alpha;
w2.first.block<3, 3>(3, 0) =
1.0 * (-13.3333333333333 * Cpi[0] + 20.0 * Cpi[1]) * alpha;
w2.second.head<3>() =
1.0 * (8.33333333333333 * pi[0] - 20.0 * pi[2] + 5.0 * pi[4]) * alpha;
w2.second.tail<3>() =
1.0 *
(0.0833333333333334 * Cg * T2 * pi[0] + 0.5 * Cg * T2 * pi[1] +
0.416666666666667 * Cg * T2 * pi[2] + 5.0 * Cpi[0] * pi[4] -
20.0 * Cpi[1] * pi[2]) *
alpha;
wps.push_back(w2);
waypoint_t w3 = initwp(DIM_POINT, DIM_VAR);
w3.first.block<3, 3>(0, 0) = -5.71428571428572 * alpha * Matrix3::Identity();
w3.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] + 14.2857142857143 * Cpi[2]) * alpha;
w3.first.block<3, 3>(3, 0) = 1.0 *
(0.238095238095238 * Cg * T2 - 20.0 * Cpi[1] +
14.2857142857143 * Cpi[2]) *
alpha;
w3.second.head<3>() = 1.0 *
(3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] - 14.2857142857143 * pi[2] +
7.85714285714286 * pi[4] + 1.42857142857143 * pi[5]) *
alpha;
w3.second.tail<3>() = 1.0 *
(0.0119047619047619 * Cg * T2 * pi[0] + 0.214285714285714 * Cg * T2 * pi[1] +
0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
(3.57142857142857 * pi[0] + 7.14285714285714 * pi[1] -
14.2857142857143 * pi[2] + 7.85714285714286 * pi[4] +
1.42857142857143 * pi[5]) *
alpha;
w3.second.tail<3>() =
1.0 *
(0.0119047619047619 * Cg * T2 * pi[0] +
0.214285714285714 * Cg * T2 * pi[1] +
0.535714285714286 * Cg * T2 * pi[2] - 5.0 * Cpi[0] * pi[4] +
1.42857142857143 * Cpi[0] * pi[5] + 12.8571428571429 * Cpi[1] * pi[4]) *
alpha;
wps.push_back(w3);
waypoint_t w4 = initwp(DIM_POINT, DIM_VAR);
w4.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
w4.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
w4.first.block<3, 3>(3, 0) =
1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[2]) * alpha;
w4.second.head<3>() = 1.0 *
(1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] - 3.57142857142857 * pi[2] + 5.0 * pi[4] +
(1.19047619047619 * pi[0] + 7.14285714285714 * pi[1] -
3.57142857142857 * pi[2] + 5.0 * pi[4] +
4.28571428571429 * pi[5] + 0.238095238095238 * pi[6]) *
alpha;
w4.second.tail<3>() =
1.0 *
(0.0476190476190471 * Cg * T2 * pi[1] + 0.357142857142857 * Cg * T2 * pi[2] +
0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] + 0.238095238095238 * Cpi[0] * pi[6] -
12.8571428571429 * Cpi[1] * pi[4] + 5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
(0.0476190476190471 * Cg * T2 * pi[1] +
0.357142857142857 * Cg * T2 * pi[2] +
0.119047619047619 * Cg * T2 * pi[4] - 1.42857142857143 * Cpi[0] * pi[5] +
0.238095238095238 * Cpi[0] * pi[6] - 12.8571428571429 * Cpi[1] * pi[4] +
5.71428571428571 * Cpi[1] * pi[5] + 17.8571428571429 * Cpi[2] * pi[4]) *
alpha;
wps.push_back(w4);
waypoint_t w5 = initwp(DIM_POINT, DIM_VAR);
w5.first.block<3, 3>(0, 0) = -14.2857142857143 * alpha * Matrix3::Identity();
w5.first.block<3, 3>(3, 0) = 1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
w5.first.block<3, 3>(3, 0) =
1.0 * (0.476190476190476 * Cg * T2 - 14.2857142857143 * Cpi[4]) * alpha;
w5.second.head<3>() = 1.0 *
(0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] + 5.0 * pi[2] -
3.57142857142857 * pi[4] + 7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
(0.238095238095238 * pi[0] + 4.28571428571429 * pi[1] +
5.0 * pi[2] - 3.57142857142857 * pi[4] +
7.14285714285714 * pi[5] + 1.19047619047619 * pi[6]) *
alpha;
w5.second.tail<3>() =
1.0 *
(+0.11904761904762 * Cg * T2 * pi[2] + 0.357142857142857 * Cg * T2 * pi[4] +
0.0476190476190476 * Cg * T2 * pi[5] - 0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] + 12.8571428571429 * Cpi[2] * pi[5]) *
(+0.11904761904762 * Cg * T2 * pi[2] +
0.357142857142857 * Cg * T2 * pi[4] +
0.0476190476190476 * Cg * T2 * pi[5] -
0.238095238095238 * Cpi[0] * pi[6] - 5.71428571428572 * Cpi[1] * pi[5] +
1.42857142857143 * Cpi[1] * pi[6] - 17.8571428571429 * Cpi[2] * pi[4] +
12.8571428571429 * Cpi[2] * pi[5]) *
alpha;
wps.push_back(w5);
waypoint_t w6 = initwp(DIM_POINT, DIM_VAR);
w6.first.block<3, 3>(0, 0) = -5.71428571428571 * alpha * Matrix3::Identity();
w6.first.block<3, 3>(3, 0) = 1.0 * (0.238095238095238 * Cg * T2 + 14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) * alpha;
w6.first.block<3, 3>(3, 0) = 1.0 *
(0.238095238095238 * Cg * T2 +
14.2857142857143 * Cpi[4] - 20.0 * Cpi[5]) *
alpha;
w6.second.head<3>() = 1.0 *
(1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] - 14.2857142857143 * pi[4] +
7.14285714285715 * pi[5] + 3.57142857142857 * pi[6]) *
alpha;
w6.second.tail<3>() = 1.0 *
(0.535714285714286 * Cg * T2 * pi[4] + 0.214285714285714 * Cg * T2 * pi[5] +
0.0119047619047619 * Cg * T2 * pi[6] - 1.42857142857143 * Cpi[1] * pi[6] -
12.8571428571429 * Cpi[2] * pi[5] + 5.0 * Cpi[2] * pi[6]) *
(1.42857142857143 * pi[1] + 7.85714285714286 * pi[2] -
14.2857142857143 * pi[4] + 7.14285714285715 * pi[5] +
3.57142857142857 * pi[6]) *
alpha;
w6.second.tail<3>() =
1.0 *
(0.535714285714286 * Cg * T2 * pi[4] +
0.214285714285714 * Cg * T2 * pi[5] +
0.0119047619047619 * Cg * T2 * pi[6] -
1.42857142857143 * Cpi[1] * pi[6] - 12.8571428571429 * Cpi[2] * pi[5] +
5.0 * Cpi[2] * pi[6]) *
alpha;
wps.push_back(w6);
waypoint_t w7 = initwp(DIM_POINT, DIM_VAR);
w7.first.block<3, 3>(0, 0) = 6.66666666666667 * alpha * Matrix3::Identity();
w7.first.block<3, 3>(3, 0) = 1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
w7.second.head<3>() = 1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
w7.second.tail<3>() = 1.0 *
(0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] + 20.0 * Cpi[4] * pi[5]) *
alpha;
w7.first.block<3, 3>(3, 0) =
1.0 * (20.0 * Cpi[5] - 13.3333333333333 * Cpi[6]) * alpha;
w7.second.head<3>() =
1.0 * (5.0 * pi[2] - 20.0 * pi[4] + 8.33333333333333 * pi[6]) * alpha;
w7.second.tail<3>() =
1.0 *
(0.416666666666667 * Cg * T2 * pi[4] + 0.5 * Cg * T2 * pi[5] +
0.0833333333333333 * Cg * T2 * pi[6] - 5.0 * Cpi[2] * pi[6] +
20.0 * Cpi[4] * pi[5]) *
alpha;
wps.push_back(w7);
waypoint_t w8 = initwp(DIM_POINT, DIM_VAR);
w8.first.block<3, 3>(0, 0) = 13.3333333333333 * alpha * Matrix3::Identity();
w8.first.block<3, 3>(3, 0) = 1.0 * (13.3333333333333 * Cpi[6]) * alpha;
w8.second.head<3>() = 1.0 * (-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) * alpha;
w8.second.head<3>() =
1.0 *
(-9.99999999999999 * pi[4] - 20.0 * pi[5] + 16.6666666666667 * pi[6]) *
alpha;
w8.second.tail<3>() = 1.0 *
(0.666666666666667 * Cg * T2 * pi[5] + 0.333333333333333 * Cg * T2 * pi[6] -
(0.666666666666667 * Cg * T2 * pi[5] +
0.333333333333333 * Cg * T2 * pi[6] -
20.0 * Cpi[4] * pi[5] + 30.0 * Cpi[4] * pi[6]) *
alpha;
wps.push_back(w8);
waypoint_t w9 = initwp(DIM_POINT, DIM_VAR);
w9.second.head<3>() = (30 * pi[4] - 60 * pi[5] + 30 * pi[6]) * alpha;
w9.second.tail<3>() = 1.0 * (1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) * alpha;
w9.second.tail<3>() =
1.0 *
(1.0 * Cg * T2 * pi[6] - 30.0 * Cpi[4] * pi[6] + 60.0 * Cpi[5] * pi[6]) *
alpha;
wps.push_back(w9);
return wps;
}
std::vector<waypoint_t> computeVelocityWaypoints(
const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -326,7 +411,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(
}
std::vector<waypoint_t> computeAccelerationWaypoints(
const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -368,8 +454,9 @@ std::vector<waypoint_t> computeAccelerationWaypoints(
return wps;
}
std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
std::vector<waypoint_t> computeJerkWaypoints(
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -419,12 +506,14 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
}
inline std::pair<MatrixXX, VectorX> computeVelocityCost(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
MatrixXX H = MatrixXX::Zero(3, 3);
VectorX g = VectorX::Zero(3);
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
g = (-7.8321678321748 * pi[0] - 7.83216783237586 * pi[1] + 9.13752913728184 * pi[3] + 9.13752913758454 * pi[5] -
g = (-7.8321678321748 * pi[0] - 7.83216783237586 * pi[1] +
9.13752913728184 * pi[3] + 9.13752913758454 * pi[5] -
7.83216783216697 * pi[7] - 7.83216783216777 * pi[8]) /
(2 * T);
H = Matrix3::Identity() * 6.52680652684107 / (T);
......
include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
View file @ 626b8c65
......@@ -11,22 +11,26 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1 {
static const ConstraintFlag flag =
INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK | THREE_FREE_VAR;
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
END_VEL | END_POS | INIT_JERK | END_JERK |
THREE_FREE_VAR;
static const size_t DIM_VAR = 9;
static const size_t DIM_POINT = 3;
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION AND JERK AND 3 variables in
/// the middle (DEGREE = 10)
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
/// ACCELERATION AND JERK AND 3 variables in the middle (DEGREE = 10)
///
/**
* @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one
* free waypoint (x)
* @param pi constant waypoints of the curve, assume pi[8] pi[1] pi[2] pi[3] x0 x1 x2 pi[4] pi[5] pi[6] pi[7]
* @brief evaluateCurveAtTime compute the expression of the point on the curve
* at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve, assume pi[8] pi[1] pi[2] pi[3] x0
* x1 x2 pi[4] pi[5] pi[6] pi[7]
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
// TODO
inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, double t) {
inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double t2 = t * t;
const double t3 = t2 * t;
......@@ -39,28 +43,37 @@ inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, do
const double t10 = t9 * t;
// equation found with sympy
wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * (t4 * 210.0 - t5 * 1260.0 + t6 * 3150.0 - 4200.0 * t7 +
3150.0 * t8 - 1260.0 * t9 + 210.0 * t10); // x0
wp.first.block<3, 3>(0, 0) =
Matrix3::Identity() *
(t4 * 210.0 - t5 * 1260.0 + t6 * 3150.0 - 4200.0 * t7 + 3150.0 * t8 -
1260.0 * t9 + 210.0 * t10); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * (252.0 * t5 - 1260.0 * t6 + 2520.0 * t7 - 2520.0 * t8 + 1260.0 * t9 - 252.0 * t10); // x1
Matrix3::Identity() * (252.0 * t5 - 1260.0 * t6 + 2520.0 * t7 -
2520.0 * t8 + 1260.0 * t9 - 252.0 * t10); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * (210.0 * t6 - 840.0 * t7 + 1260.0 * t8 - 840.0 * t9 + 210.0 * t10); // x2
wp.second = 1.0 * pi[0] + t * (-10.0 * pi[0] + 10.0 * pi[1]) + t2 * (45.0 * pi[0] - 90.0 * pi[1] + 45.0 * pi[2]) +
t3 * (-120.0 * pi[0] + 360.0 * pi[1] - 360.0 * pi[2] + 120.0 * pi[3]) +
t4 * (210.0 * pi[0] - 840.0 * pi[1] + 1260.0 * pi[2] - 840.0 * pi[3]) +
t5 * (-252.0 * pi[0] + 1260.0 * pi[1] - 2520.0 * pi[2] + 2520.0 * pi[3]) +
t6 * (210.0 * pi[0] - 1260.0 * pi[1] + 3150.0 * pi[2] - 4200.0 * pi[3]) +
t7 * (-120.0 * pi[0] + 840.0 * pi[1] - 2520.0 * pi[2] + 4200.0 * pi[3] + 120.0 * pi[7]) +
t8 * (45.0 * pi[0] - 360.0 * pi[1] + 1260.0 * pi[2] - 2520.0 * pi[3] - 360.0 * pi[7] + 45.0 * pi[8]) +
t9 * (-10.0 * pi[0] + 90.0 * pi[1] - 360.0 * pi[2] + 840.0 * pi[3] + 360.0 * pi[7] - 90.0 * pi[8] +
10.0 * pi[9]) +
t10 * (1.0 * pi[0] + 1.0 * pi[10] - 10.0 * pi[1] + 45.0 * pi[2] - 120.0 * pi[3] - 120.0 * pi[7] +
45.0 * pi[8] - 10.0 * pi[9]);
Matrix3::Identity() *
(210.0 * t6 - 840.0 * t7 + 1260.0 * t8 - 840.0 * t9 + 210.0 * t10); // x2
wp.second =
1.0 * pi[0] + t * (-10.0 * pi[0] + 10.0 * pi[1]) +
t2 * (45.0 * pi[0] - 90.0 * pi[1] + 45.0 * pi[2]) +
t3 * (-120.0 * pi[0] + 360.0 * pi[1] - 360.0 * pi[2] + 120.0 * pi[3]) +
t4 * (210.0 * pi[0] - 840.0 * pi[1] + 1260.0 * pi[2] - 840.0 * pi[3]) +
t5 * (-252.0 * pi[0] + 1260.0 * pi[1] - 2520.0 * pi[2] + 2520.0 * pi[3]) +
t6 * (210.0 * pi[0] - 1260.0 * pi[1] + 3150.0 * pi[2] - 4200.0 * pi[3]) +
t7 * (-120.0 * pi[0] + 840.0 * pi[1] - 2520.0 * pi[2] + 4200.0 * pi[3] +
120.0 * pi[7]) +
t8 * (45.0 * pi[0] - 360.0 * pi[1] + 1260.0 * pi[2] - 2520.0 * pi[3] -
360.0 * pi[7] + 45.0 * pi[8]) +
t9 * (-10.0 * pi[0] + 90.0 * pi[1] - 360.0 * pi[2] + 840.0 * pi[3] +
360.0 * pi[7] - 90.0 * pi[8] + 10.0 * pi[9]) +
t10 * (1.0 * pi[0] + 1.0 * pi[10] - 10.0 * pi[1] + 45.0 * pi[2] -
120.0 * pi[3] - 120.0 * pi[7] + 45.0 * pi[8] - 10.0 * pi[9]);
return wp;
}
// TODO
inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateVelocityCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double alpha = 1. / (T);
const double t2 = t * t;
......@@ -72,32 +85,46 @@ inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>
const double t8 = t7 * t;
const double t9 = t8 * t;
// equation found with sympy
wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
(1.0 * (2100.0 * t9 - 11340.0 * t8 + 25200.0 * t7 - 29400.0 * t6 + 18900.0 * t5 -
6300.0 * t4 + 840.0 * t3)); // x0
wp.first.block<3, 3>(0, 0) =
Matrix3::Identity() * alpha *
(1.0 * (2100.0 * t9 - 11340.0 * t8 + 25200.0 * t7 - 29400.0 * t6 +
18900.0 * t5 - 6300.0 * t4 + 840.0 * t3)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
(1.0 * (-2520.0 * t9 + 11340.0 * t8 - 20160.0 * t7 + 17640.0 * t6 - 7560.0 * t5 + 1260.0 * t4)); // x1
wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * alpha *
(1.0 * (2100.0 * t9 - 7560.0 * t8 + 10080.0 * t7 - 5880.0 * t6 + 1260.0 * t5)); // x2
wp.second = (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) + t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] + 360.0 * pi[3])) +
t3 * (1.0 * (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] + 3240.0 * pi[7] -
810.0 * pi[8] + 90.0 * pi[9])) +
t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] - 3360.0 * pi[3])) +
t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] + 450.0 * pi[2] - 1200.0 * pi[3] -
1200.0 * pi[7] + 450.0 * pi[8] - 100.0 * pi[9])) +
t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] + 12600.0 * pi[3])) +
t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] - 25200.0 * pi[3])) +
t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] + 29400.0 * pi[3] + 840.0 * pi[7])) +
t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] - 20160.0 * pi[3] - 2880.0 * pi[7] +
360.0 * pi[8]))) *
alpha;
(1.0 * (-2520.0 * t9 + 11340.0 * t8 - 20160.0 * t7 + 17640.0 * t6 -
7560.0 * t5 + 1260.0 * t4)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
(1.0 * (2100.0 * t9 - 7560.0 * t8 + 10080.0 * t7 - 5880.0 * t6 +
1260.0 * t5)); // x2
wp.second =
(1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) +
t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] +
360.0 * pi[3])) +
t3 * (1.0 *
(-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] +
3240.0 * pi[7] - 810.0 * pi[8] + 90.0 * pi[9])) +
t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] -
3360.0 * pi[3])) +
t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] +
450.0 * pi[2] - 1200.0 * pi[3] - 1200.0 * pi[7] +
450.0 * pi[8] - 100.0 * pi[9])) +
t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] +
12600.0 * pi[3])) +
t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] -
25200.0 * pi[3])) +
t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] +
29400.0 * pi[3] + 840.0 * pi[7])) +
t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] -
20160.0 * pi[3] - 2880.0 * pi[7] + 360.0 * pi[8]))) *
alpha;
return wp;
}
// TODO
inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateAccelerationCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double alpha = 1. / (T * T);
const double t2 = t * t;
......@@ -108,34 +135,45 @@ inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<poin
const double t7 = t6 * t;
const double t8 = t7 * t;
// equation found with sympy
wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
(1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 + 94500.0 * t4 -
25200.0 * t3 + 2520.0 * t2)); // x0
wp.first.block<3, 3>(0, 0) =
Matrix3::Identity() * alpha *
(1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 +
94500.0 * t4 - 25200.0 * t3 + 2520.0 * t2)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
(1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 - 37800.0 * t4 + 5040.0 * t3)); // x1
(1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 -
37800.0 * t4 + 5040.0 * t3)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
(1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 + 6300.0 * t4)); // x2
(1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 +
6300.0 * t4)); // x2
wp.second =
(1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3])) +
t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] - 10080.0 * pi[3])) +
t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] + 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] +
720.0 * pi[3])) +
t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] -
10080.0 * pi[3])) +
t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] +
4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
4050.0 * pi[8] - 900.0 * pi[9])) +
t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] + 50400.0 * pi[3])) +
t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] - 126000.0 * pi[3])) +
t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] + 176400.0 * pi[3] + 5040.0 * pi[7])) +
t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] - 141120.0 * pi[3] - 20160.0 * pi[7] +
2520.0 * pi[8])) +
t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] + 60480.0 * pi[3] + 25920.0 * pi[7] -
6480.0 * pi[8] + 720.0 * pi[9]))) *
t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] +
50400.0 * pi[3])) +
t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] -
126000.0 * pi[3])) +
t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] +
176400.0 * pi[3] + 5040.0 * pi[7])) +
t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] -
141120.0 * pi[3] - 20160.0 * pi[7] + 2520.0 * pi[8])) +
t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] +
60480.0 * pi[3] + 25920.0 * pi[7] - 6480.0 * pi[8] +
720.0 * pi[9]))) *
alpha;
return wp;
}
// TODO
inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateJerkCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double alpha = 1. / (T * T * T);
const double t2 = t * t;
......@@ -145,73 +183,91 @@ inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi
const double t6 = t5 * t;
const double t7 = t6 * t;
// equation found with sympy
wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
(1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 + 378000.0 * t3 -
75600.0 * t2 + 5040.0 * t)); // x0
wp.first.block<3, 3>(0, 0) =
Matrix3::Identity() * alpha *
(1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 +
378000.0 * t3 - 75600.0 * t2 + 5040.0 * t)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
(1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 - 151200.0 * t3 + 15120.0 * t2)); // x1
(1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 -
151200.0 * t3 + 15120.0 * t2)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
(1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 + 25200.0 * t3)); // x2
(1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 +
25200.0 * t3)); // x2
wp.second =
(1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] - 20160.0 * pi[3])) +
t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] + 151200.0 * pi[3])) +
t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] - 504000.0 * pi[3])) +
t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] + 882000.0 * pi[3] + 25200.0 * pi[7])) +
t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] - 846720.0 * pi[3] - 120960.0 * pi[7] +
15120.0 * pi[8])) +
t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] + 423360.0 * pi[3] + 181440.0 * pi[7] -
45360.0 * pi[8] + 5040.0 * pi[9])) +
t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] + 32400.0 * pi[2] - 86400.0 * pi[3] -
86400.0 * pi[7] + 32400.0 * pi[8] - 7200.0 * pi[9]))) *
(1.0 *
(-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] -
20160.0 * pi[3])) +
t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] +
151200.0 * pi[3])) +
t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] -
504000.0 * pi[3])) +
t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] +
882000.0 * pi[3] + 25200.0 * pi[7])) +
t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] -
846720.0 * pi[3] - 120960.0 * pi[7] + 15120.0 * pi[8])) +
t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] +
423360.0 * pi[3] + 181440.0 * pi[7] - 45360.0 * pi[8] +
5040.0 * pi[9])) +
t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] +
32400.0 * pi[2] - 86400.0 * pi[3] - 86400.0 * pi[7] +
32400.0 * pi[8] - 7200.0 * pi[9]))) *
alpha;
return wp;
}
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
// equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
// waypoint and one free (pi[3])) first, compute the constant waypoints that only depend on pData :
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
double T) {
// equation for constraint on initial and final position and velocity and
// initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
// first, compute the constant waypoints that only depend on pData :
double n = 10.;
std::vector<point_t> pi;
pi.push_back(pData.c0_);
pi.push_back((pData.dc0_ * T / n) + pData.c0_);
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2 * pData.dc0_ * T / n) +
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
(2 * pData.dc0_ * T / n) +
pData.c0_); // * T because derivation make a T appear
pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
(3 * pData.ddc0_ * T * T / (n * (n - 1))) +
(3 * pData.dc0_ * T / n) + pData.c0_);
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
(3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
(3 * pData.ddc1_ * T * T / (n * (n - 1))) -
(3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
(2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back(pData.c1_);
return pi;
}
// TODO
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
double T) {
bezier_wp_t::t_point_t wps;
//const int DIM_POINT = 6;
//const int DIM_VAR = 9;
// const int DIM_POINT = 6;
// const int DIM_VAR = 9;
std::vector<point_t> pi = computeConstantWaypoints(pData, T);
std::vector<Matrix3> Cpi;
for (std::size_t i = 0; i < pi.size(); ++i) {
Cpi.push_back(skew(pi[i]));
}
//const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
//const Matrix3 Cg = skew(g);
//const double T2 = T * T;
//const double alpha = 1 / (T2);
// const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
// const Matrix3 Cg = skew(g);
// const double T2 = T * T;
// const double alpha = 1 / (T2);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
return wps;
}
std::vector<waypoint_t> computeVelocityWaypoints(
const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -267,7 +323,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(
}
std::vector<waypoint_t> computeAccelerationWaypoints(
const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -323,8 +380,9 @@ std::vector<waypoint_t> computeAccelerationWaypoints(
return wps;
}
std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
std::vector<waypoint_t> computeJerkWaypoints(
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -391,33 +449,43 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
}
inline std::pair<MatrixXX, VectorX> computeVelocityCost(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
MatrixXX H = MatrixXX::Zero(9, 9);
VectorX g = VectorX::Zero(9);
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
g.segment<3>(0) =
(-12.352941184069 * pi[0] - 2.03619909502433 * pi[10] - 10.5882353430148 * pi[1] + 1.2217194516605 * pi[2] +
12.2171947000329 * pi[3] - 4.66474701697538 * pi[7] - 7.21925133730399 * pi[8] - 5.42986425333795 * pi[9]) /
(2 * T); // x0
g.segment<3>(3) =
(-5.29411764601331 * pi[0] - 5.29411764705762 * pi[10] - 8.95927605247282 * pi[1] - 6.10859723220821 * pi[2] +
2.2213080007358 * pi[3] + 2.22130810120924 * pi[7] - 6.10859728485633 * pi[8] - 8.95927601808432 * pi[9]) /
(2 * T); // x1
g.segment<3>(6) =
(-2.03619909557297 * pi[0] - 12.3529411764706 * pi[10] - 5.42986425052241 * pi[1] - 7.21925133714926 * pi[2] -
4.66474700749421 * pi[3] + 12.2171945706055 * pi[7] + 1.22171945695766 * pi[8] - 10.5882352941172 * pi[9]) /
(2 * T); // x2
H.block<3, 3>(0, 0) = Matrix3::Identity() * 7.77457833646806 / (T); // x0^2
H.block<3, 3>(3, 3) = Matrix3::Identity() * 7.25627312788583 / (T); // x1^2
H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.77457836216558 / (T); // x2^2
H.block<3, 3>(0, 3) = Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
H.block<3, 3>(3, 0) = Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
H.block<3, 3>(0, 6) = Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
H.block<3, 3>(6, 0) = Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
H.block<3, 3>(3, 6) = Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
H.block<3, 3>(6, 3) = Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
g.segment<3>(0) = (-12.352941184069 * pi[0] - 2.03619909502433 * pi[10] -
10.5882353430148 * pi[1] + 1.2217194516605 * pi[2] +
12.2171947000329 * pi[3] - 4.66474701697538 * pi[7] -
7.21925133730399 * pi[8] - 5.42986425333795 * pi[9]) /
(2 * T); // x0
g.segment<3>(3) = (-5.29411764601331 * pi[0] - 5.29411764705762 * pi[10] -
8.95927605247282 * pi[1] - 6.10859723220821 * pi[2] +
2.2213080007358 * pi[3] + 2.22130810120924 * pi[7] -
6.10859728485633 * pi[8] - 8.95927601808432 * pi[9]) /
(2 * T); // x1
g.segment<3>(6) = (-2.03619909557297 * pi[0] - 12.3529411764706 * pi[10] -
5.42986425052241 * pi[1] - 7.21925133714926 * pi[2] -
4.66474700749421 * pi[3] + 12.2171945706055 * pi[7] +
1.22171945695766 * pi[8] - 10.5882352941172 * pi[9]) /
(2 * T); // x2
H.block<3, 3>(0, 0) = Matrix3::Identity() * 7.77457833646806 / (T); // x0^2
H.block<3, 3>(3, 3) = Matrix3::Identity() * 7.25627312788583 / (T); // x1^2
H.block<3, 3>(6, 6) = Matrix3::Identity() * 7.77457836216558 / (T); // x2^2
H.block<3, 3>(0, 3) =
Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
H.block<3, 3>(3, 0) =
Matrix3::Identity() * 10.8844097406652 / (2 * T); // x0*x1 / 2
H.block<3, 3>(0, 6) =
Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
H.block<3, 3>(6, 0) =
Matrix3::Identity() * 2.41875768460934 / (2 * T); // x0*x2 / 2
H.block<3, 3>(3, 6) =
Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
H.block<3, 3>(6, 3) =
Matrix3::Identity() * 10.8844097036619 / (2 * T); // x1*x2 / 2
double norm = H.norm();
H /= norm;
......
include/hpp/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
View file @ 626b8c65
......@@ -11,21 +11,25 @@
namespace bezier_com_traj {
namespace c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1 {
static const ConstraintFlag flag =
INIT_POS | INIT_VEL | INIT_ACC | END_ACC | END_VEL | END_POS | INIT_JERK | END_JERK | FIVE_FREE_VAR;
static const ConstraintFlag flag = INIT_POS | INIT_VEL | INIT_ACC | END_ACC |
END_VEL | END_POS | INIT_JERK | END_JERK |
FIVE_FREE_VAR;
static const size_t DIM_VAR = 3 * 5;
static const size_t DIM_POINT = 3;
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND ACCELERATION AND JERK AND 5 variables in
/// the middle (DEGREE = 10)
/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY AND
/// ACCELERATION AND JERK AND 5 variables in the middle (DEGREE = 10)
///
/**
* @brief evaluateCurveAtTime compute the expression of the point on the curve at t, defined by the waypoint pi and one
* free waypoint (x)
* @param pi constant waypoints of the curve, assume pi[8] pi[1] pi[2] pi[3] x0 x1 x2 x3 x4 pi[4] pi[5] pi[6] pi[7]
* @brief evaluateCurveAtTime compute the expression of the point on the curve
* at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve, assume pi[8] pi[1] pi[2] pi[3] x0
* x1 x2 x3 x4 pi[4] pi[5] pi[6] pi[7]
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, double t) {
inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi,
double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
const double t2 = t * t;
const double t3 = t2 * t;
......@@ -41,36 +45,50 @@ inline waypoint_t evaluateCurveWaypointAtTime(const std::vector<point_t>& pi, do
// equation found with sympy
wp.first.block<3, 3>(0, 0) =
Matrix3::Identity() * (+495.0 * t4 - 3960.0 * t5 + 13860.0 * t6 - 27720.0 * t7 + 34650.0 * t8 - 27720.0 * t9 +
13860.0 * t10 - 3960.0 * t11 + 495.0 * t12); // x0
Matrix3::Identity() *
(+495.0 * t4 - 3960.0 * t5 + 13860.0 * t6 - 27720.0 * t7 + 34650.0 * t8 -
27720.0 * t9 + 13860.0 * t10 - 3960.0 * t11 + 495.0 * t12); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * (+792.0 * t5 - 5544.0 * t6 + 16632.0 * t7 - 27720.0 * t8 + 27720.0 * t9 - 16632.0 * t10 +
5544.0 * t11 - 792.0 * t12); // x1
wp.first.block<3, 3>(0, 6) = Matrix3::Identity() * (+924.0 * t6 - 5544.0 * t7 + 13860.0 * t8 - 18480.0 * t9 +
13860.0 * t10 - 5544.0 * t11 + 924.0 * t12); // x2
wp.first.block<3, 3>(0, 9) = Matrix3::Identity() * (+792.0 * t7 - 3960.0 * t8 + 7920.0 * t9 - 7920.0 * t10 +
3960.0 * t11 - 792.0 * t12); // x3
Matrix3::Identity() *
(+792.0 * t5 - 5544.0 * t6 + 16632.0 * t7 - 27720.0 * t8 + 27720.0 * t9 -
16632.0 * t10 + 5544.0 * t11 - 792.0 * t12); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() *
(+924.0 * t6 - 5544.0 * t7 + 13860.0 * t8 - 18480.0 * t9 + 13860.0 * t10 -
5544.0 * t11 + 924.0 * t12); // x2
wp.first.block<3, 3>(0, 9) =
Matrix3::Identity() * (+792.0 * t7 - 3960.0 * t8 + 7920.0 * t9 -
7920.0 * t10 + 3960.0 * t11 - 792.0 * t12); // x3
wp.first.block<3, 3>(0, 12) =
Matrix3::Identity() * (+495.0 * t8 - 1980.0 * t9 + 2970.0 * t10 - 1980.0 * t11 + 495.0 * t12); // x4
wp.second = 1.0 * pi[0] + t * (-12.0 * pi[0] + 12.0 * pi[1]) + t2 * (66.0 * pi[0] - 132.0 * pi[1] + 66.0 * pi[2]) +
t3 * (-220.0 * pi[0] + 660.0 * pi[1] - 660.0 * pi[2] + 220.0 * pi[3]) +
t4 * (495.0 * pi[0] - 1980.0 * pi[1] + 2970.0 * pi[2] - 1980.0 * pi[3]) +
t5 * (-792.0 * pi[0] + 3960.0 * pi[1] - 7920.0 * pi[2] + 7920.0 * pi[3]) +
t6 * (924.0 * pi[0] - 5544.0 * pi[1] + 13860.0 * pi[2] - 18480.0 * pi[3]) +
t7 * (-792.0 * pi[0] + 5544.0 * pi[1] - 16632.0 * pi[2] + 27720.0 * pi[3]) +
t8 * (495.0 * pi[0] - 3960.0 * pi[1] + 13860.0 * pi[2] - 27720.0 * pi[3]) +
t9 * (-220.0 * pi[0] + 1980.0 * pi[1] - 7920.0 * pi[2] + 18480.0 * pi[3] + 220.0 * pi[9]) +
t10 * (66.0 * pi[0] + 66.0 * pi[10] - 660.0 * pi[1] + 2970.0 * pi[2] - 7920.0 * pi[3] - 660.0 * pi[9]) +
t11 * (-12.0 * pi[0] - 132.0 * pi[10] + 12.0 * pi[11] + 132.0 * pi[1] - 660.0 * pi[2] + 1980.0 * pi[3] +
660.0 * pi[9]) +
t12 * (1.0 * pi[0] + 66.0 * pi[10] - 12.0 * pi[11] + 1.0 * pi[12] - 12.0 * pi[1] + 66.0 * pi[2] -
220.0 * pi[3] - 220.0 * pi[9]);
Matrix3::Identity() * (+495.0 * t8 - 1980.0 * t9 + 2970.0 * t10 -
1980.0 * t11 + 495.0 * t12); // x4
wp.second =
1.0 * pi[0] + t * (-12.0 * pi[0] + 12.0 * pi[1]) +
t2 * (66.0 * pi[0] - 132.0 * pi[1] + 66.0 * pi[2]) +
t3 * (-220.0 * pi[0] + 660.0 * pi[1] - 660.0 * pi[2] + 220.0 * pi[3]) +
t4 * (495.0 * pi[0] - 1980.0 * pi[1] + 2970.0 * pi[2] - 1980.0 * pi[3]) +
t5 * (-792.0 * pi[0] + 3960.0 * pi[1] - 7920.0 * pi[2] + 7920.0 * pi[3]) +
t6 *
(924.0 * pi[0] - 5544.0 * pi[1] + 13860.0 * pi[2] - 18480.0 * pi[3]) +
t7 * (-792.0 * pi[0] + 5544.0 * pi[1] - 16632.0 * pi[2] +
27720.0 * pi[3]) +
t8 *
(495.0 * pi[0] - 3960.0 * pi[1] + 13860.0 * pi[2] - 27720.0 * pi[3]) +
t9 * (-220.0 * pi[0] + 1980.0 * pi[1] - 7920.0 * pi[2] + 18480.0 * pi[3] +
220.0 * pi[9]) +
t10 * (66.0 * pi[0] + 66.0 * pi[10] - 660.0 * pi[1] + 2970.0 * pi[2] -
7920.0 * pi[3] - 660.0 * pi[9]) +
t11 * (-12.0 * pi[0] - 132.0 * pi[10] + 12.0 * pi[11] + 132.0 * pi[1] -
660.0 * pi[2] + 1980.0 * pi[3] + 660.0 * pi[9]) +
t12 * (1.0 * pi[0] + 66.0 * pi[10] - 12.0 * pi[11] + 1.0 * pi[12] -
12.0 * pi[1] + 66.0 * pi[2] - 220.0 * pi[3] - 220.0 * pi[9]);
return wp;
}
// TODO
inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateVelocityCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
......@@ -91,24 +109,34 @@ inline waypoint_t evaluateVelocityCurveWaypointAtTime(const std::vector<point_t>
wp.first.block<3, 3>(0, 9) = Matrix3::Identity() * alpha; // x3
wp.first.block<3, 3>(0, 12) = Matrix3::Identity() * alpha; // x4
wp.second = (1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) + t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] + 360.0 * pi[3])) +
t3 * (1.0 * (-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] + 3240.0 * pi[7] -
810.0 * pi[8] + 90.0 * pi[9])) +
t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] - 3360.0 * pi[3])) +
t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] + 450.0 * pi[2] - 1200.0 * pi[3] -
1200.0 * pi[7] + 450.0 * pi[8] - 100.0 * pi[9])) +
t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] + 12600.0 * pi[3])) +
t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] - 25200.0 * pi[3])) +
t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] + 29400.0 * pi[3] + 840.0 * pi[7])) +
t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] - 20160.0 * pi[3] - 2880.0 * pi[7] +
360.0 * pi[8]))) *
alpha;
wp.second =
(1.0 * (-10.0 * pi[0] + 10.0 * pi[1]) +
t * (1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2])) +
t2 * (1.0 * (-360.0 * pi[0] + 1080.0 * pi[1] - 1080.0 * pi[2] +
360.0 * pi[3])) +
t3 * (1.0 *
(-90.0 * pi[0] + 810.0 * pi[1] - 3240.0 * pi[2] + 7560.0 * pi[3] +
3240.0 * pi[7] - 810.0 * pi[8] + 90.0 * pi[9])) +
t4 * (1.0 * (840.0 * pi[0] - 3360.0 * pi[1] + 5040.0 * pi[2] -
3360.0 * pi[3])) +
t5 * (1.0 * (10.0 * pi[0] + 10.0 * pi[10] - 100.0 * pi[1] +
450.0 * pi[2] - 1200.0 * pi[3] - 1200.0 * pi[7] +
450.0 * pi[8] - 100.0 * pi[9])) +
t6 * (1.0 * (-1260.0 * pi[0] + 6300.0 * pi[1] - 12600.0 * pi[2] +
12600.0 * pi[3])) +
t7 * (1.0 * (1260.0 * pi[0] - 7560.0 * pi[1] + 18900.0 * pi[2] -
25200.0 * pi[3])) +
t8 * (1.0 * (-840.0 * pi[0] + 5880.0 * pi[1] - 17640.0 * pi[2] +
29400.0 * pi[3] + 840.0 * pi[7])) +
t9 * (1.0 * (360.0 * pi[0] - 2880.0 * pi[1] + 10080.0 * pi[2] -
20160.0 * pi[3] - 2880.0 * pi[7] + 360.0 * pi[8]))) *
alpha;
return wp;
}
// TODO
inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateAccelerationCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
......@@ -121,34 +149,45 @@ inline waypoint_t evaluateAccelerationCurveWaypointAtTime(const std::vector<poin
const double t7 = t6 * t;
const double t8 = t7 * t;
// equation found with sympy
wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
(1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 + 94500.0 * t4 -
25200.0 * t3 + 2520.0 * t2)); // x0
wp.first.block<3, 3>(0, 0) =
Matrix3::Identity() * alpha *
(1.0 * (18900.0 * t8 - 90720.0 * t7 + 176400.0 * t6 - 176400.0 * t5 +
94500.0 * t4 - 25200.0 * t3 + 2520.0 * t2)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
(1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 - 37800.0 * t4 + 5040.0 * t3)); // x1
(1.0 * (-22680.0 * t8 + 90720.0 * t7 - 141120.0 * t6 + 105840.0 * t5 -
37800.0 * t4 + 5040.0 * t3)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
(1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 + 6300.0 * t4)); // x2
(1.0 * (18900.0 * t8 - 60480.0 * t7 + 70560.0 * t6 - 35280.0 * t5 +
6300.0 * t4)); // x2
wp.second =
(1.0 * (90.0 * pi[0] - 180.0 * pi[1] + 90.0 * pi[2]) +
t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3])) +
t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] - 10080.0 * pi[3])) +
t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] + 4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
t * (1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] +
720.0 * pi[3])) +
t2 * (1.0 * (2520.0 * pi[0] - 10080.0 * pi[1] + 15120.0 * pi[2] -
10080.0 * pi[3])) +
t3 * (1.0 * (90.0 * pi[0] + 90.0 * pi[10] - 900.0 * pi[1] +
4050.0 * pi[2] - 10800.0 * pi[3] - 10800.0 * pi[7] +
4050.0 * pi[8] - 900.0 * pi[9])) +
t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] + 50400.0 * pi[3])) +
t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] - 126000.0 * pi[3])) +
t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] + 176400.0 * pi[3] + 5040.0 * pi[7])) +
t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] - 141120.0 * pi[3] - 20160.0 * pi[7] +
2520.0 * pi[8])) +
t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] + 60480.0 * pi[3] + 25920.0 * pi[7] -
6480.0 * pi[8] + 720.0 * pi[9]))) *
t4 * (1.0 * (-5040.0 * pi[0] + 25200.0 * pi[1] - 50400.0 * pi[2] +
50400.0 * pi[3])) +
t5 * (1.0 * (6300.0 * pi[0] - 37800.0 * pi[1] + 94500.0 * pi[2] -
126000.0 * pi[3])) +
t6 * (1.0 * (-5040.0 * pi[0] + 35280.0 * pi[1] - 105840.0 * pi[2] +
176400.0 * pi[3] + 5040.0 * pi[7])) +
t7 * (1.0 * (2520.0 * pi[0] - 20160.0 * pi[1] + 70560.0 * pi[2] -
141120.0 * pi[3] - 20160.0 * pi[7] + 2520.0 * pi[8])) +
t8 * (1.0 * (-720.0 * pi[0] + 6480.0 * pi[1] - 25920.0 * pi[2] +
60480.0 * pi[3] + 25920.0 * pi[7] - 6480.0 * pi[8] +
720.0 * pi[9]))) *
alpha;
return wp;
}
// TODO
inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi, double T, double t) {
inline waypoint_t evaluateJerkCurveWaypointAtTime(
const std::vector<point_t>& pi, double T, double t) {
waypoint_t wp = initwp(DIM_POINT, DIM_VAR);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
......@@ -160,75 +199,93 @@ inline waypoint_t evaluateJerkCurveWaypointAtTime(const std::vector<point_t>& pi
const double t6 = t5 * t;
const double t7 = t6 * t;
// equation found with sympy
wp.first.block<3, 3>(0, 0) = Matrix3::Identity() * alpha *
(1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 + 378000.0 * t3 -
75600.0 * t2 + 5040.0 * t)); // x0
wp.first.block<3, 3>(0, 0) =
Matrix3::Identity() * alpha *
(1.0 * (151200.0 * t7 - 635040.0 * t6 + 1058400.0 * t5 - 882000.0 * t4 +
378000.0 * t3 - 75600.0 * t2 + 5040.0 * t)); // x0
wp.first.block<3, 3>(0, 3) =
Matrix3::Identity() * alpha *
(1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 - 151200.0 * t3 + 15120.0 * t2)); // x1
(1.0 * (-181440.0 * t7 + 635040.0 * t6 - 846720.0 * t5 + 529200.0 * t4 -
151200.0 * t3 + 15120.0 * t2)); // x1
wp.first.block<3, 3>(0, 6) =
Matrix3::Identity() * alpha *
(1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 + 25200.0 * t3)); // x2
(1.0 * (151200.0 * t7 - 423360.0 * t6 + 423360.0 * t5 - 176400.0 * t4 +
25200.0 * t3)); // x2
wp.second =
(1.0 * (-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] - 20160.0 * pi[3])) +
t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] + 151200.0 * pi[3])) +
t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] - 504000.0 * pi[3])) +
t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] + 882000.0 * pi[3] + 25200.0 * pi[7])) +
t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] - 846720.0 * pi[3] - 120960.0 * pi[7] +
15120.0 * pi[8])) +
t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] + 423360.0 * pi[3] + 181440.0 * pi[7] -
45360.0 * pi[8] + 5040.0 * pi[9])) +
t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] + 32400.0 * pi[2] - 86400.0 * pi[3] -
86400.0 * pi[7] + 32400.0 * pi[8] - 7200.0 * pi[9]))) *
(1.0 *
(-720.0 * pi[0] + 2160.0 * pi[1] - 2160.0 * pi[2] + 720.0 * pi[3]) +
t * (1.0 * (5040.0 * pi[0] - 20160.0 * pi[1] + 30240.0 * pi[2] -
20160.0 * pi[3])) +
t2 * (1.0 * (-15120.0 * pi[0] + 75600.0 * pi[1] - 151200.0 * pi[2] +
151200.0 * pi[3])) +
t3 * (1.0 * (25200.0 * pi[0] - 151200.0 * pi[1] + 378000.0 * pi[2] -
504000.0 * pi[3])) +
t4 * (1.0 * (-25200.0 * pi[0] + 176400.0 * pi[1] - 529200.0 * pi[2] +
882000.0 * pi[3] + 25200.0 * pi[7])) +
t5 * (1.0 * (15120.0 * pi[0] - 120960.0 * pi[1] + 423360.0 * pi[2] -
846720.0 * pi[3] - 120960.0 * pi[7] + 15120.0 * pi[8])) +
t6 * (1.0 * (-5040.0 * pi[0] + 45360.0 * pi[1] - 181440.0 * pi[2] +
423360.0 * pi[3] + 181440.0 * pi[7] - 45360.0 * pi[8] +
5040.0 * pi[9])) +
t7 * (1.0 * (720.0 * pi[0] + 720.0 * pi[10] - 7200.0 * pi[1] +
32400.0 * pi[2] - 86400.0 * pi[3] - 86400.0 * pi[7] +
32400.0 * pi[8] - 7200.0 * pi[9]))) *
alpha;
return wp;
}
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T) {
// equation for constraint on initial and final position and velocity and initial acceleration(degree 5, 5 constant
// waypoint and one free (pi[3])) first, compute the constant waypoints that only depend on pData :
inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
double T) {
// equation for constraint on initial and final position and velocity and
// initial acceleration(degree 5, 5 constant waypoint and one free (pi[3]))
// first, compute the constant waypoints that only depend on pData :
double n = 12.;
std::vector<point_t> pi;
pi.push_back(pData.c0_);
pi.push_back((pData.dc0_ * T / n) + pData.c0_);
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) + (2 * pData.dc0_ * T / n) +
pi.push_back((pData.ddc0_ * T * T / (n * (n - 1))) +
(2 * pData.dc0_ * T / n) +
pData.c0_); // * T because derivation make a T appear
pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc0_ * T * T / (n * (n - 1))) +
pi.push_back((pData.j0_ * T * T * T / (n * (n - 1) * (n - 2))) +
(3 * pData.ddc0_ * T * T / (n * (n - 1))) +
(3 * pData.dc0_ * T / n) + pData.c0_);
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back(point_t::Zero());
pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) + (3 * pData.ddc1_ * T * T / (n * (n - 1))) -
(3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) - (2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back((-pData.j1_ * T * T * T / (n * (n - 1) * (n - 2))) +
(3 * pData.ddc1_ * T * T / (n * (n - 1))) -
(3 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((pData.ddc1_ * T * T / (n * (n - 1))) -
(2 * pData.dc1_ * T / n) + pData.c1_); // * T ??
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // * T ?
pi.push_back(pData.c1_);
return pi;
}
// TODO
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData, double T) {
inline bezier_wp_t::t_point_t computeWwaypoints(const ProblemData& pData,
double T) {
bezier_wp_t::t_point_t wps;
//const int DIM_POINT = 6;
//const int DIM_VAR = 15;
// const int DIM_POINT = 6;
// const int DIM_VAR = 15;
std::vector<point_t> pi = computeConstantWaypoints(pData, T);
std::vector<Matrix3> Cpi;
for (std::size_t i = 0; i < pi.size(); ++i) {
Cpi.push_back(skew(pi[i]));
}
//const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
//const Matrix3 Cg = skew(g);
//const double T2 = T * T;
//const double alpha = 1 / (T2);
// const Vector3 g = pData.contacts_.front().contactPhase_->m_gravity;
// const Matrix3 Cg = skew(g);
// const double T2 = T * T;
// const double alpha = 1 / (T2);
std::cout << "NOT IMPLEMENTED YET" << std::endl;
return wps;
}
std::vector<waypoint_t> computeVelocityWaypoints(
const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -294,7 +351,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(
}
std::vector<waypoint_t> computeAccelerationWaypoints(
const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -362,8 +420,9 @@ std::vector<waypoint_t> computeAccelerationWaypoints(
return wps;
}
std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
std::vector<waypoint_t> computeJerkWaypoints(
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
std::vector<waypoint_t> wps;
......@@ -445,31 +504,37 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T) {
// TODO
inline std::pair<MatrixXX, VectorX> computeVelocityCost(
const ProblemData& pData, double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>()) {
MatrixXX H = MatrixXX::Zero(DIM_VAR, DIM_VAR);
VectorX g = VectorX::Zero(DIM_VAR);
if (pi.size() == 0) pi = computeConstantWaypoints(pData, T);
g.segment<3>(0) = ((-17.8646615739593 * pi[0] - 4.24835843773412 * pi[10] - 1.80981866649436 * pi[11] -
0.408668730537654 * pi[12] - 13.8947369836412 * pi[1] + 2.56878303943036 * pi[2] +
g.segment<3>(0) = ((-17.8646615739593 * pi[0] - 4.24835843773412 * pi[10] -
1.80981866649436 * pi[11] - 0.408668730537654 * pi[12] -
13.8947369836412 * pi[1] + 2.56878303943036 * pi[2] +
16.0548432515434 * pi[3] - 6.66893486967885 * pi[9])) /
(2 * T); // x0
g.segment<3>(3) =
((-7.93984965058761 * pi[0] - 7.0641309185535 * pi[10] - 4.08668730511085 * pi[11] - 1.22600619206665 * pi[12] -
12.1432972410894 * pi[1] - 7.06413670827152 * pi[2] + 3.85315840674136 * pi[3] - 7.50872663647158 * pi[9])) /
(2 * T); // x1
g.segment<3>(6) =
(-3.26934980716442 * pi[0] - 8.9907120599184 * pi[10] - 7.76470588258269 * pi[11] - 3.26934984520124 * pi[12] -
7.76470597007188 * pi[1] - 8.99071211730055 * pi[2] - 4.49535589801788 * pi[3] - 4.49535607858364 * pi[9]) /
(2 * T); // x2
g.segment<3>(9) =
(-1.22600620726636 * pi[0] - 7.06413092270385 * pi[10] - 12.1432994250704 * pi[11] - 7.93984962409094 * pi[12] -
4.08668774398579 * pi[1] - 7.0641311269266 * pi[2] - 7.50872489092664 * pi[3] + 3.85316232209763 * pi[9]) /
(2 * T); // x3
g.segment<3>(12) =
(-0.408668732514974 * pi[0] + 2.56877487851457 * pi[10] - 13.8947368423667 * pi[11] - 17.8646616541281 * pi[12] -
1.80981880873492 * pi[1] - 4.2483587965255 * pi[2] - 6.66893350792178 * pi[3] + 16.0548429731073 * pi[9]) /
(2 * T); // x4
g.segment<3>(3) = ((-7.93984965058761 * pi[0] - 7.0641309185535 * pi[10] -
4.08668730511085 * pi[11] - 1.22600619206665 * pi[12] -
12.1432972410894 * pi[1] - 7.06413670827152 * pi[2] +
3.85315840674136 * pi[3] - 7.50872663647158 * pi[9])) /
(2 * T); // x1
g.segment<3>(6) = (-3.26934980716442 * pi[0] - 8.9907120599184 * pi[10] -
7.76470588258269 * pi[11] - 3.26934984520124 * pi[12] -
7.76470597007188 * pi[1] - 8.99071211730055 * pi[2] -
4.49535589801788 * pi[3] - 4.49535607858364 * pi[9]) /
(2 * T); // x2
g.segment<3>(9) = (-1.22600620726636 * pi[0] - 7.06413092270385 * pi[10] -
12.1432994250704 * pi[11] - 7.93984962409094 * pi[12] -
4.08668774398579 * pi[1] - 7.0641311269266 * pi[2] -
7.50872489092664 * pi[3] + 3.85316232209763 * pi[9]) /
(2 * T); // x3
g.segment<3>(12) = (-0.408668732514974 * pi[0] + 2.56877487851457 * pi[10] -
13.8947368423667 * pi[11] - 17.8646616541281 * pi[12] -
1.80981880873492 * pi[1] - 4.2483587965255 * pi[2] -
6.66893350792178 * pi[3] + 16.0548429731073 * pi[9]) /
(2 * T); // x4
H.block<3, 3>(0, 0) = Matrix3::Identity() * 9.63290527229048 / (T); // x0^2
H.block<3, 3>(3, 3) = Matrix3::Identity() * 8.29911962311903 / (T); // x1^2
......@@ -477,29 +542,49 @@ inline std::pair<MatrixXX, VectorX> computeVelocityCost(
H.block<3, 3>(9, 9) = Matrix3::Identity() * 8.29911871865983 / (T); // x3^2
H.block<3, 3>(12, 12) = Matrix3::Identity() * 9.63290582796267 / (T); // x4^2
H.block<3, 3>(0, 3) = Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
H.block<3, 3>(3, 0) = Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
H.block<3, 3>(0, 6) = Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
H.block<3, 3>(6, 0) = Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
H.block<3, 3>(0, 9) = Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
H.block<3, 3>(9, 0) = Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
H.block<3, 3>(0, 12) = Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
H.block<3, 3>(12, 0) = Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
H.block<3, 3>(3, 6) = Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
H.block<3, 3>(6, 3) = Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
H.block<3, 3>(3, 9) = Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
H.block<3, 3>(9, 3) = Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
H.block<3, 3>(3, 12) = Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
H.block<3, 3>(12, 3) = Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
H.block<3, 3>(6, 9) = Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
H.block<3, 3>(9, 6) = Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
H.block<3, 3>(6, 12) = Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
H.block<3, 3>(12, 6) = Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
H.block<3, 3>(9, 12) = Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
H.block<3, 3>(12, 9) = Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
H.block<3, 3>(0, 3) =
Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
H.block<3, 3>(3, 0) =
Matrix3::Identity() * 13.4860690009623 / (2 * T); // x0*x1 /2
H.block<3, 3>(0, 6) =
Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
H.block<3, 3>(6, 0) =
Matrix3::Identity() * 4.14955180440231 / (2 * T); // x0*x2 /2
H.block<3, 3>(0, 9) =
Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
H.block<3, 3>(9, 0) =
Matrix3::Identity() * -3.55676093144659 / (2 * T); // x0*x3 /2
H.block<3, 3>(0, 12) =
Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
H.block<3, 3>(12, 0) =
Matrix3::Identity() * -7.07311260219052 / (2 * T); // x0*x4 /2
H.block<3, 3>(3, 6) =
Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
H.block<3, 3>(6, 3) =
Matrix3::Identity() * 12.4486856197374 / (2 * T); // x1*x2 /2
H.block<3, 3>(3, 9) =
Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
H.block<3, 3>(9, 3) =
Matrix3::Identity() * 4.20345048607838 / (2 * T); // x1*x3 /2
H.block<3, 3>(3, 12) =
Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
H.block<3, 3>(12, 3) =
Matrix3::Identity() * -3.55676456195318 / (2 * T); // x1*x4 /2
H.block<3, 3>(6, 9) =
Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
H.block<3, 3>(9, 6) =
Matrix3::Identity() * 12.448679688301 / (2 * T); // x2*x3 /2
H.block<3, 3>(6, 12) =
Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
H.block<3, 3>(12, 6) =
Matrix3::Identity() * 4.149559164651 / (2 * T); // x2*x4 /2
H.block<3, 3>(9, 12) =
Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
H.block<3, 3>(12, 9) =
Matrix3::Identity() * 13.4860680294621 / (2 * T); // x3*x4 /2
double norm = H.norm();
H /= norm;
......
include/hpp/bezier-com-traj/waypoints/waypoints_definition.hh
View file @ 626b8c65
......@@ -14,37 +14,48 @@ namespace bezier_com_traj {
* depending on the options set in ProblemData.constraints
*/
/** @brief evaluateCurveAtTime compute the expression of the point on the curve c at t,
* defined by the waypoint pi and one free waypoint (x)
/** @brief evaluateCurveAtTime compute the expression of the point on the curve
* c at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
coefs_t evaluateCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double t);
coefs_t evaluateCurveAtTime(const ProblemData& pData,
const std::vector<point_t>& pi, double t);
/** @brief evaluateVelocityCurveAtTime compute the expression of the point on the curve dc at t,
* defined by the waypoint pi and one free waypoint (x)
/** @brief evaluateVelocityCurveAtTime compute the expression of the point on
* the curve dc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
coefs_t evaluateVelocityCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double T, double t);
coefs_t evaluateVelocityCurveAtTime(const ProblemData& pData,
const std::vector<point_t>& pi, double T,
double t);
/** @brief evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t,
* defined by the waypoint pi and one free waypoint (x)
/** @brief evaluateAccelerationCurveAtTime compute the expression of the point
* on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double T, double t);
coefs_t evaluateAccelerationCurveAtTime(const ProblemData& pData,
const std::vector<point_t>& pi,
double T, double t);
/** @brief evaluateAccelerationCurveAtTime compute the expression of the point on the curve ddc at t,
* defined by the waypoint pi and one free waypoint (x)
/** @brief evaluateAccelerationCurveAtTime compute the expression of the point
* on the curve ddc at t, defined by the waypoint pi and one free waypoint (x)
* @param pi constant waypoints of the curve
* @param t param (normalized !)
* @return the expression of the waypoint such that wp.first . x + wp.second = point on curve
* @return the expression of the waypoint such that wp.first . x + wp.second =
* point on curve
*/
coefs_t evaluateJerkCurveAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double T, double t);
coefs_t evaluateJerkCurveAtTime(const ProblemData& pData,
const std::vector<point_t>& pi, double T,
double t);
/**
* @brief computeConstantWaypoints compute the constant waypoints of c(t)
......@@ -53,16 +64,18 @@ coefs_t evaluateJerkCurveAtTime(const ProblemData& pData, const std::vector<poin
* @param T
* @return
*/
std::vector<point_t> computeConstantWaypoints(const ProblemData& pData, double T);
std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,
double T);
/**
* @brief computeConstantWaypointsSymbolic compute the constant waypoints of c(t)
* defined by the constraints on initial and final states
* @brief computeConstantWaypointsSymbolic compute the constant waypoints of
* c(t) defined by the constraints on initial and final states
* @param pData
* @param T
* @return the waypoints expressed as a polynom of the free waypoint
*/
bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(const ProblemData& pData, double T);
bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(
const ProblemData& pData, double T);
/**
* @brief computeWwaypoints compute the constant waypoints of dc(t)
......@@ -71,8 +84,9 @@ bezier_wp_t::t_point_t computeConstantWaypointsSymbolic(const ProblemData& pData
* @param T
* @return
*/
std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
std::vector<waypoint_t> computeVelocityWaypoints(
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
/**
* @brief computeWwaypoints compute the constant waypoints of ddc(t)
......@@ -82,7 +96,8 @@ std::vector<waypoint_t> computeVelocityWaypoints(const ProblemData& pData, const
* @return
*/
std::vector<waypoint_t> computeAccelerationWaypoints(
const ProblemData& pData, const double T, std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
/**
* @brief computeWwaypoints compute the constant waypoints of dddc(t)
......@@ -91,15 +106,23 @@ std::vector<waypoint_t> computeAccelerationWaypoints(
* @param T
* @return
*/
std::vector<waypoint_t> computeJerkWaypoints(const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData, const std::vector<point_t>& pi, double t);
waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData, const double T,
const std::vector<point_t>& pi, double t);
waypoint_t evaluateAccelerationCurveWaypointAtTime(const ProblemData& pData, const double T,
const std::vector<point_t>& pi, double t);
waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData, const double T, const std::vector<point_t>& pi,
std::vector<waypoint_t> computeJerkWaypoints(
const ProblemData& pData, const double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
waypoint_t evaluateCurveWaypointAtTime(const ProblemData& pData,
const std::vector<point_t>& pi,
double t);
waypoint_t evaluateVelocityCurveWaypointAtTime(const ProblemData& pData,
const double T,
const std::vector<point_t>& pi,
double t);
waypoint_t evaluateAccelerationCurveWaypointAtTime(
const ProblemData& pData, const double T, const std::vector<point_t>& pi,
double t);
waypoint_t evaluateJerkCurveWaypointAtTime(const ProblemData& pData,
const double T,
const std::vector<point_t>& pi,
double t);
/**
......@@ -115,8 +138,9 @@ coefs_t computeFinalVelocityPoint(const ProblemData& pData, double T);
int dimVar(const ProblemData& pData);
std::pair<MatrixXX, VectorX> computeVelocityCost(const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
std::pair<MatrixXX, VectorX> computeVelocityCost(
const ProblemData& pData, double T,
std::vector<bezier_t::point_t> pi = std::vector<bezier_t::point_t>());
} // namespace bezier_com_traj
......
pyproject.toml 0 → 100644
View file @ 626b8c65
[tool.black]
exclude = "cmake"
python/bezier_com_traj.cpp
View file @ 626b8c65
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
#include <hpp/bezier-com-traj/solve_end_effector.hh>
#include <eigenpy/memory.hpp>
#include <eigenpy/eigenpy.hpp>
#include <boost/python.hpp>
#include <boost/python/exception_translator.hpp>
#include <eigenpy/eigenpy.hpp>
#include <eigenpy/memory.hpp>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/solve_end_effector.hh>
#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ContactData)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ProblemData)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ResultData)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ResultDataCOMTraj)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(
bezier_com_traj::ResultDataCOMTraj)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::bezier_t)
namespace bezier_com_traj {
using namespace boost::python;
typedef double real;
ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq, const Vector3& com, const Vector3& dCom,
const Vector3& l0, const bool useAngMomentum, const double timeDuration,
ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq,
const Vector3& com,
const Vector3& dCom, const Vector3& l0,
const bool useAngMomentum,
const double timeDuration,
const double timeStep) {
bezier_com_traj::ContactData data;
data.contactPhase_ = eq;
......@@ -34,11 +37,11 @@ ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq, c
return new ResultDataCOMTraj(res);
}
ResultDataCOMTraj* zeroStepCapturabilityWithKinConstraints(centroidal_dynamics::Equilibrium* eq, const Vector3& com,
const Vector3& dCom, const Vector3& l0,
const bool useAngMomentum, const double timeDuration,
const double timeStep, const MatrixXX& Kin,
const MatrixXX& kin) {
ResultDataCOMTraj* zeroStepCapturabilityWithKinConstraints(
centroidal_dynamics::Equilibrium* eq, const Vector3& com,
const Vector3& dCom, const Vector3& l0, const bool useAngMomentum,
const double timeDuration, const double timeStep, const MatrixXX& Kin,
const MatrixXX& kin) {
bezier_com_traj::ContactData data;
data.Kin_ = Kin;
data.kin_ = kin;
......@@ -56,7 +59,9 @@ ResultDataCOMTraj* zeroStepCapturabilityWithKinConstraints(centroidal_dynamics::
}
struct res_data_exception : std::exception {
char const* what() const throw() { return "attributes not accessible for false resData"; }
char const* what() const throw() {
return "attributes not accessible for false resData";
}
};
void translate(const res_data_exception& e) {
......@@ -65,7 +70,9 @@ void translate(const res_data_exception& e) {
}
struct contact_data_exception : std::exception {
char const* what() const throw() { return "attribute not defined yet for ContactData"; }
char const* what() const throw() {
return "attribute not defined yet for ContactData";
}
};
void translateContactData(const contact_data_exception& e) {
......@@ -83,9 +90,13 @@ double get_costD(const ResultData& res) { return res.cost_; }
bool get_succD(const ResultData& res) { return res.success_; }
bezier_t* getC_of_t(const ResultDataCOMTraj& res) { return new bezier_t(res.c_of_t_); }
bezier_t* getC_of_t(const ResultDataCOMTraj& res) {
return new bezier_t(res.c_of_t_);
}
bezier_t* getDL_of_t(const ResultDataCOMTraj& res) { return new bezier_t(res.dL_of_t_); }
bezier_t* getDL_of_t(const ResultDataCOMTraj& res) {
return new bezier_t(res.dL_of_t_);
}
VectorX get_x(const ResultDataCOMTraj& res) {
if (res.x.size() > 0) return res.x;
......@@ -104,7 +115,9 @@ centroidal_dynamics::Equilibrium* getContactPhase_(const ContactData& data) {
throw contact_data_exception();
}
void setContactPhase_(ContactData& data, centroidal_dynamics::Equilibrium* eq) { data.contactPhase_ = eq; }
void setContactPhase_(ContactData& data, centroidal_dynamics::Equilibrium* eq) {
data.contactPhase_ = eq;
}
boost::python::tuple get_Ang(const ContactData& res) {
if (res.ang_.size() == 0) {
......@@ -124,7 +137,8 @@ boost::python::tuple get_Kin(const ContactData& res) {
void set_Kin(ContactData& res, const MatrixX3& val, const VectorX& val2) {
if (val2.size() != val.rows()) {
std::cout << " Kinematic inequality matrix sizes do not match " << std::endl;
std::cout << " Kinematic inequality matrix sizes do not match "
<< std::endl;
throw contact_data_exception();
}
res.Kin_ = val;
......@@ -144,14 +158,22 @@ void set_Ang(ContactData& res, const MatrixX3& val, const VectorX& val2) {
/** BEGIN Constraints**/
int get_Flag(const Constraints& res) { return (int)res.flag_; }
bool get_ConstrainAcc(const Constraints& res) { return res.constraintAcceleration_; }
bool get_ConstrainAcc(const Constraints& res) {
return res.constraintAcceleration_;
}
double get_MaxAcc(const Constraints& res) { return res.maxAcceleration_; }
double get_ReduceH(const Constraints& res) { return res.reduce_h_; }
void set_Flag(Constraints& res, const int val) { res.flag_ = (ConstraintFlag)val; }
void set_ConstrainAcc(Constraints& res, const bool val) { res.constraintAcceleration_ = val; }
void set_Flag(Constraints& res, const int val) {
res.flag_ = (ConstraintFlag)val;
}
void set_ConstrainAcc(Constraints& res, const bool val) {
res.constraintAcceleration_ = val;
}
void set_MaxAcc(Constraints& res, const double val) { res.maxAcceleration_ = val; }
void set_MaxAcc(Constraints& res, const double val) {
res.maxAcceleration_ = val;
}
void set_ReduceH(Constraints& res, const double val) { res.reduce_h_ = val; }
......@@ -181,24 +203,42 @@ void set_dc1_(ProblemData& res, const point_t& val) { res.dc1_ = val; }
void set_ddc1_(ProblemData& res, const point_t& val) { res.ddc1_ = val; }
bool get_useAngularMomentum_(const ProblemData& res) { return res.useAngularMomentum_; }
void set_useAngularMomentum_(ProblemData& res, const bool val) { res.useAngularMomentum_ = val; }
bool get_useAngularMomentum_(const ProblemData& res) {
return res.useAngularMomentum_;
}
void set_useAngularMomentum_(ProblemData& res, const bool val) {
res.useAngularMomentum_ = val;
}
CostFunction get_costFunction_(const ProblemData& res) { return res.costFunction_; }
CostFunction get_costFunction_(const ProblemData& res) {
return res.costFunction_;
}
void set_costFunction_(ProblemData& res, const CostFunction val) { res.costFunction_ = val; }
void set_costFunction_(ProblemData& res, const CostFunction val) {
res.costFunction_ = val;
}
GIWCRepresentation get_GIWC_representation_(const ProblemData& res) { return res.representation_; }
GIWCRepresentation get_GIWC_representation_(const ProblemData& res) {
return res.representation_;
}
void set_GIWC_representation_(ProblemData& res, const GIWCRepresentation val) { res.representation_ = val; }
void set_GIWC_representation_(ProblemData& res, const GIWCRepresentation val) {
res.representation_ = val;
}
Constraints* get_constraints_(ProblemData& res) { return &res.constraints_; }
void set_constraints_(ProblemData& res, const Constraints& val) { res.constraints_ = val; }
void set_constraints_(ProblemData& res, const Constraints& val) {
res.constraints_ = val;
}
std::vector<ContactData> get_contacts_(const ProblemData& res) { return res.contacts_; }
std::vector<ContactData> get_contacts_(const ProblemData& res) {
return res.contacts_;
}
void addContact(ProblemData& res, const ContactData& val) { res.contacts_.push_back(ContactData(val)); }
void addContact(ProblemData& res, const ContactData& val) {
res.contacts_.push_back(ContactData(val));
}
void clearContacts(ProblemData& res) { res.contacts_.clear(); }
......@@ -206,13 +246,16 @@ void clearContacts(ProblemData& res) { res.contacts_.clear(); }
/** BEGIN computeCOMTraj **/
ResultDataCOMTraj* computeCOMTrajPointer(const ProblemData& pData, const VectorX& Ts, const double timeStep) {
ResultDataCOMTraj* computeCOMTrajPointer(const ProblemData& pData,
const VectorX& Ts,
const double timeStep) {
ResultDataCOMTraj res = computeCOMTraj(pData, Ts, timeStep);
return new ResultDataCOMTraj(res);
}
ResultDataCOMTraj* computeCOMTrajPointerChooseSolver(const ProblemData& pData, const VectorX& Ts,
const double timeStep, const solvers::SolverType solver) {
ResultDataCOMTraj* computeCOMTrajPointerChooseSolver(
const ProblemData& pData, const VectorX& Ts, const double timeStep,
const solvers::SolverType solver) {
ResultDataCOMTraj res = computeCOMTraj(pData, Ts, timeStep, solver);
return new ResultDataCOMTraj(res);
}
......@@ -224,7 +267,7 @@ struct DummyPath {
point3_t operator()(double /*u*/) const { return point3_t::Zero(); }
};
typedef std::pair<MatrixXX,VectorX> linear_points_t;
typedef std::pair<MatrixXX, VectorX> linear_points_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
struct MatrixVector {
......@@ -233,26 +276,32 @@ struct MatrixVector {
VectorX b() { return res.second; }
};
ResultDataCOMTraj* computeEndEffector(const ProblemData& pData, const double time) {
ResultDataCOMTraj res = solveEndEffector<DummyPath>(pData, DummyPath(), time, 0);
ResultDataCOMTraj* computeEndEffector(const ProblemData& pData,
const double time) {
ResultDataCOMTraj res =
solveEndEffector<DummyPath>(pData, DummyPath(), time, 0);
return new ResultDataCOMTraj(res);
}
MatrixVector* computeEndEffectorConstraintsPython(const ProblemData& pData, const double time) {
MatrixVector* computeEndEffectorConstraintsPython(const ProblemData& pData,
const double time) {
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time);
MatrixVector* res = new MatrixVector();
res->res = computeEndEffectorConstraints(pData, time, pi);
return res;
}
MatrixVector* computeEndEffectorVelocityCostPython(const ProblemData& pData, const double time) {
MatrixVector* computeEndEffectorVelocityCostPython(const ProblemData& pData,
const double time) {
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time);
MatrixVector* res = new MatrixVector();
res->res = computeVelocityCost(pData, time, pi);
return res;
}
MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData, const double time, const int numPoints,
MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData,
const double time,
const int numPoints,
point_list_t pts_l) {
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time);
// transform the matrice 3xN in a std::vector<point3_t> of size N :
......@@ -265,12 +314,14 @@ MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData, con
return res;
}
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> computeEndEffectorConstantWaypoints(const ProblemData& pData,
const double time) {
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>
computeEndEffectorConstantWaypoints(const ProblemData& pData,
const double time) {
std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time);
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res(3, pi.size());
int col = 0;
for (std::vector<bezier_t::point_t>::const_iterator cit = pi.begin(); cit != pi.end(); ++cit, ++col)
for (std::vector<bezier_t::point_t>::const_iterator cit = pi.begin();
cit != pi.end(); ++cit, ++col)
res.block<3, 1>(0, col) = *cit;
return res;
}
......@@ -294,8 +345,12 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
eigenpy::exposeQuaternion();*/
class_<ResultDataCOMTraj>("ResultDataCOMTraj", init<>())
.add_property("c_of_t", make_function(&getC_of_t, return_value_policy<manage_new_object>()))
.add_property("dL_of_t", make_function(&getDL_of_t, return_value_policy<manage_new_object>()))
.add_property(
"c_of_t",
make_function(&getC_of_t, return_value_policy<manage_new_object>()))
.add_property(
"dL_of_t",
make_function(&getDL_of_t, return_value_policy<manage_new_object>()))
.add_property("success", &get_succ)
.add_property("cost", &get_cost)
.add_property("x", &get_x);
......@@ -305,10 +360,15 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
.add_property("cost", &get_costD)
.add_property("x", &get_xD);
class_<ContactData>("ContactData", init<centroidal_dynamics::Equilibrium*>()[with_custodian_and_ward<1, 2>()])
.add_property("contactPhase_",
make_function(&getContactPhase_, return_value_policy<reference_existing_object>()),
&setContactPhase_)
class_<ContactData>(
"ContactData",
init<
centroidal_dynamics::Equilibrium*>()[with_custodian_and_ward<1, 2>()])
.add_property(
"contactPhase_",
make_function(&getContactPhase_,
return_value_policy<reference_existing_object>()),
&setContactPhase_)
.add_property("Kin_", &get_Kin)
.add_property("Ang_", &get_Ang)
.def("setKinematicConstraints", &set_Kin)
......@@ -321,17 +381,23 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
.add_property("c1_", &get_c1_, &set_c1_)
.add_property("dc1_", &get_dc1_, &set_dc1_)
.add_property("ddc1_", &get_ddc1_, &set_ddc1_)
.add_property("useAngularMomentum_", &get_useAngularMomentum_, &set_useAngularMomentum_)
.add_property("useAngularMomentum_", &get_useAngularMomentum_,
&set_useAngularMomentum_)
.add_property("costFunction_", &get_costFunction_, &set_costFunction_)
.add_property("GIWCrepresentation_", &get_GIWC_representation_, &set_GIWC_representation_)
.add_property("constraints_", make_function(&get_constraints_, return_value_policy<reference_existing_object>()),
&set_constraints_)
.add_property("GIWCrepresentation_", &get_GIWC_representation_,
&set_GIWC_representation_)
.add_property(
"constraints_",
make_function(&get_constraints_,
return_value_policy<reference_existing_object>()),
&set_constraints_)
.def("clearContacts", clearContacts)
.def("addContact", addContact);
class_<Constraints>("Constraints", init<>())
.add_property("flag_", &get_Flag, &set_Flag)
.add_property("constrainAcceleration_", &get_ConstrainAcc, &set_ConstrainAcc)
.add_property("constrainAcceleration_", &get_ConstrainAcc,
&set_ConstrainAcc)
.add_property("maxAcceleration_", &get_MaxAcc, &set_MaxAcc)
.add_property("reduce_h_", &get_ReduceH, &set_ReduceH);
......@@ -377,17 +443,24 @@ BOOST_PYTHON_MODULE(hpp_bezier_com_traj) {
.def_readonly("A", &MatrixVector::A)
.def_readonly("b", &MatrixVector::b);
def("zeroStepCapturability", &zeroStepCapturability, return_value_policy<manage_new_object>());
def("zeroStepCapturability", &zeroStepCapturabilityWithKinConstraints, return_value_policy<manage_new_object>());
def("computeCOMTraj", &computeCOMTrajPointer, return_value_policy<manage_new_object>());
def("computeCOMTraj", &computeCOMTrajPointerChooseSolver, return_value_policy<manage_new_object>());
def("computeEndEffector", &computeEndEffector, return_value_policy<manage_new_object>());
def("computeEndEffectorConstraints", &computeEndEffectorConstraintsPython, return_value_policy<manage_new_object>());
def("zeroStepCapturability", &zeroStepCapturability,
return_value_policy<manage_new_object>());
def("zeroStepCapturability", &zeroStepCapturabilityWithKinConstraints,
return_value_policy<manage_new_object>());
def("computeCOMTraj", &computeCOMTrajPointer,
return_value_policy<manage_new_object>());
def("computeCOMTraj", &computeCOMTrajPointerChooseSolver,
return_value_policy<manage_new_object>());
def("computeEndEffector", &computeEndEffector,
return_value_policy<manage_new_object>());
def("computeEndEffectorConstraints", &computeEndEffectorConstraintsPython,
return_value_policy<manage_new_object>());
def("computeEndEffectorVelocityCost", &computeEndEffectorVelocityCostPython,
return_value_policy<manage_new_object>());
def("computeEndEffectorDistanceCost", &computeEndEffectorDistanceCostPython,
return_value_policy<manage_new_object>());
def("computeEndEffectorConstantWaypoints", &computeEndEffectorConstantWaypoints);
def("computeEndEffectorConstantWaypoints",
&computeEndEffectorConstantWaypoints);
}
} // namespace bezier_com_traj
python/test/binding_tests.py
View file @ 626b8c65
......@@ -2,27 +2,34 @@ import ndcurves # noqa - necessary to register ndcurves::bezier_curve
import numpy as np
from numpy import array
from hpp_centroidal_dynamics import Equilibrium, EquilibriumAlgorithm, SolverLP
from hpp_bezier_com_traj import (SOLVER_QUADPROG, ConstraintFlag, Constraints, ContactData, ProblemData,
computeCOMTraj, zeroStepCapturability)
from hpp_bezier_com_traj import (
SOLVER_QUADPROG,
ConstraintFlag,
Constraints,
ContactData,
ProblemData,
computeCOMTraj,
zeroStepCapturability,
)
# testing constructors
eq = Equilibrium("test", 54., 4)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
eq = Equilibrium("test", 54.0, 4)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, False)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
# whether useWarmStart is enable (True by default)
previous = eq.useWarmStart()
# enable warm start in solver (only for QPOases)
eq.setUseWarmStart(False)
assert (previous != eq.useWarmStart())
assert previous != eq.useWarmStart()
# access solver name
assert (eq.getName() == "test")
assert eq.getName() == "test"
z = array([0., 0., 1.])
z = array([0.0, 0.0, 1.0])
P = array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]])
N = array([z for _ in range(4)])
......@@ -31,16 +38,16 @@ eq.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
# eq.setNewContacts(P,N,0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
# setting up optimization problem
c0 = array([0., 0., 1.])
c0 = array([0.0, 0.0, 1.0])
# dc0 = array(np.random.uniform(-1, 1, size=3));
dc0 = array([0.1, 0., 0.])
l0 = array([0., 0., 0.])
dc0 = array([0.1, 0.0, 0.0])
l0 = array([0.0, 0.0, 0.0])
T = 1.2
tstep = -1.
tstep = -1.0
a = zeroStepCapturability(eq, c0, dc0, l0, False, T, tstep)
assert (a.success)
assert a.success
a.c_of_t(0)
a.dL_of_t(T)
......@@ -57,13 +64,14 @@ kin = 10 * np.ones(3)
# a = zeroStepCapturability(eq, c0, dc0, l0, True, T, tstep, Kin, matrix(kin))
# testing contactData
cData = ContactData(Equilibrium("test", 54., 4))
cData = ContactData(Equilibrium("test", 54.0, 4))
ceq = cData.contactPhase_
ceq.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
assert cData.contactPhase_.getAlgorithm(
) == EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP, "modifying ceq should modify cData.contactPhase_"
assert (
cData.contactPhase_.getAlgorithm() == EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP
), "modifying ceq should modify cData.contactPhase_"
Id = array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
Id = array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
excep = False
try:
......@@ -71,12 +79,12 @@ try:
except RuntimeError:
excep = True
assert excep, "[ERROR] No kin assigned should have raised exception"
cData.setKinematicConstraints(Id, array([0., 0., 1.]))
cData.setKinematicConstraints(Id, array([0.0, 0.0, 1.0]))
cData.Kin_
excep = False
try:
cData.setKinematicConstraints(Id, array([0., 0., 0., 1.]))
cData.setKinematicConstraints(Id, array([0.0, 0.0, 0.0, 1.0]))
except RuntimeError:
excep = True
assert excep, "[ERROR] Miss matching matrix and vector should raise an error"
......@@ -87,12 +95,12 @@ try:
except RuntimeError:
excep = True
assert excep, "[ERROR] No Ang_ assigned should have raised exception"
cData.setAngularConstraints(Id, array([0., 0., 1.]))
cData.setAngularConstraints(Id, array([0.0, 0.0, 1.0]))
cData.Ang_
excep = False
try:
cData.setAngularConstraints(Id, array([0., 0., 0., 1.]))
cData.setAngularConstraints(Id, array([0.0, 0.0, 0.0, 1.0]))
except RuntimeError:
excep = True
assert excep, "[ERROR] Missmatching matrix and vector should raise an error"
......@@ -103,20 +111,26 @@ old = c.constrainAcceleration_
c.constrainAcceleration_ = not old
assert c.constrainAcceleration_ != old
old = c.flag_
assert c.flag_ == ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL | ConstraintFlag.END_VEL | ConstraintFlag.END_POS
assert (
c.flag_
== ConstraintFlag.INIT_POS
| ConstraintFlag.INIT_VEL
| ConstraintFlag.END_VEL
| ConstraintFlag.END_POS
)
c.flag_ = ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL
assert c.flag_ != old
old = c.maxAcceleration_
c.maxAcceleration_ = .235
c.maxAcceleration_ = 0.235
assert c.maxAcceleration_ != old
old = c.reduce_h_
c.reduce_h_ = .235
c.reduce_h_ = 0.235
assert c.reduce_h_ != old
# testing problem data
c = ProblemData()
nv = array([0., 0., 10.])
nv = array([0.0, 0.0, 10.0])
old = c.c0_
c.c0_ = nv
assert (c.c0_ != old).any()
......@@ -145,12 +159,16 @@ c.flag_ = ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL
assert pD.constraints_.flag_ != old
pD = ProblemData()
pD.constraints_.flag_ = ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL | ConstraintFlag.END_VEL
pD.constraints_.flag_ = (
ConstraintFlag.INIT_POS | ConstraintFlag.INIT_VEL | ConstraintFlag.END_VEL
)
def initContactData(pD):
cData = ContactData(Equilibrium("test", 54., 4))
cData.contactPhase_.setNewContacts(P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
cData = ContactData(Equilibrium("test", 54.0, 4))
cData.contactPhase_.setNewContacts(
P, N, 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP
)
pD.addContact(cData)
......
python/test/compare_pin_inv_dyn.py
View file @ 626b8c65
......@@ -15,9 +15,15 @@ from numpy import cross as X
from numpy import matrix, zeros
from centroidal_dynamics import Equilibrium, EquilibriumAlgorithm
from pinocchio_inv_dyn.multi_contact.bezier.bezier_0_step_capturability import (BezierZeroStepCapturability,
compute_CWC)
from pinocchio_inv_dyn.multi_contact.utils import (check_static_eq, find_static_equilibrium_com, generate_contacts)
from pinocchio_inv_dyn.multi_contact.bezier.bezier_0_step_capturability import (
BezierZeroStepCapturability,
compute_CWC,
)
from pinocchio_inv_dyn.multi_contact.utils import (
check_static_eq,
find_static_equilibrium_com,
generate_contacts,
)
from curves import bezier
__EPS = 1e-5
......@@ -27,23 +33,27 @@ np.set_printoptions(precision=2, suppress=True, linewidth=100)
#####################
# EQUILIBRIUM CHECK #
#####################
def compute_w(c, ddc, dL=array([0., 0., 0.]), m=54., g_vec=array([0., 0., -9.81])):
def compute_w(
c, ddc, dL=array([0.0, 0.0, 0.0]), m=54.0, g_vec=array([0.0, 0.0, -9.81])
):
w1 = m * (ddc - g_vec)
return array(w1.tolist() + (X(c, w1) + dL).tolist())
def is_stable(H,
c=array([0., 0., 0.]),
ddc=array([0., 0., 0.]),
dL=array([0., 0., 0.]),
m=54.,
g_vec=array([0., 0., -9.81]),
robustness=0.):
def is_stable(
H,
c=array([0.0, 0.0, 0.0]),
ddc=array([0.0, 0.0, 0.0]),
dL=array([0.0, 0.0, 0.0]),
m=54.0,
g_vec=array([0.0, 0.0, -9.81]),
robustness=0.0,
):
w = compute_w(c, ddc, dL, m, g_vec)
res = np.max(H.dot(w))
if res > -robustness:
print("offset ", res, dL)
w = compute_w(c, ddc, array([0., 0., 0.]), m, g_vec)
w = compute_w(c, ddc, array([0.0, 0.0, 0.0]), m, g_vec)
print("offset2 ", np.max(H.dot(w)))
return res <= -robustness
......@@ -65,25 +75,38 @@ def __check_trajectory(p0, p1, p2, p3, T, H, mass, g, time_step=0.1, dL=allZeros
return asarray(c_t(t / T)).flatten()
def ddc_tT(t):
return 1. / (T * T) * asarray(ddc_t(t / T)).flatten()
return 1.0 / (T * T) * asarray(ddc_t(t / T)).flatten()
def dL_tT(t):
return 1. / (T) * asarray(dL(t / T)).flatten()
return 1.0 / (T) * asarray(dL(t / T)).flatten()
for i in range(resolution):
t = T * float(i) / float(resolution)
if not (is_stable(H, c=c_tT(t), ddc=ddc_tT(t), dL=dL_tT(t), m=mass, g_vec=g, robustness=-0.00001)):
if not (
is_stable(
H,
c=c_tT(t),
ddc=ddc_tT(t),
dL=dL_tT(t),
m=mass,
g_vec=g,
robustness=-0.00001,
)
):
if t > 0.1:
raise ValueError("trajectory is not stale ! at ", t)
else:
print(
is_stable(H,
c=c_tT(t),
ddc=ddc_tT(t),
dL=asarray(dL(t / T)).flatten(),
m=mass,
g_vec=g,
robustness=-0.00001))
is_stable(
H,
c=c_tT(t),
ddc=ddc_tT(t),
dL=asarray(dL(t / T)).flatten(),
m=mass,
g_vec=g,
robustness=-0.00001,
)
)
print("failed at 0")
......@@ -92,7 +115,7 @@ def __check_trajectory(p0, p1, p2, p3, T, H, mass, g, time_step=0.1, dL=allZeros
###################
def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0):
def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver="qpoases", verb=0):
mu = 0.5
# friction coefficient
......@@ -114,9 +137,19 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
Y_MARG = 0.07
succeeded = False
while (not succeeded):
p, N = generate_contacts(N_CONTACTS, lx, ly, mu, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, MIN_CONTACT_DISTANCE, False)
while not succeeded:
p, N = generate_contacts(
N_CONTACTS,
lx,
ly,
mu,
CONTACT_POINT_LOWER_BOUNDS,
CONTACT_POINT_UPPER_BOUNDS,
RPY_LOWER_BOUNDS,
RPY_UPPER_BOUNDS,
MIN_CONTACT_DISTANCE,
False,
)
X_LB = np.min(p[:, 0] - X_MARG)
X_UB = np.max(p[:, 0] + X_MARG)
Y_LB = np.min(p[:, 1] - Y_MARG)
......@@ -126,7 +159,9 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
# (H,h) = compute_GIWC(p, N, mu, False);
H = -compute_CWC(p, N, mass, mu)
h = zeros(H.shape[0])
(succeeded, c0) = find_static_equilibrium_com(mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h)
(succeeded, c0) = find_static_equilibrium_com(
mass, [X_LB, Y_LB, Z_LB], [X_UB, Y_UB, Z_UB], H, h
)
dc0 = np.random.uniform(-1, 1, size=3)
......@@ -136,56 +171,67 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
ineq_kin = zeros(3)
ineq_kin[2] = -Z_MIN
bezierSolver = BezierZeroStepCapturability("ss",
c0,
dc0,
p,
N,
mu,
g_vector,
mass,
verb=verb,
solver=solver,
kinematic_constraints=[Ineq_kin, ineq_kin])
bezierSolver = BezierZeroStepCapturability(
"ss",
c0,
dc0,
p,
N,
mu,
g_vector,
mass,
verb=verb,
solver=solver,
kinematic_constraints=[Ineq_kin, ineq_kin],
)
eqCpp = Equilibrium("dyn_eq2", mass, 4)
eqCpp.setNewContacts(asmatrix(p), asmatrix(N), mu, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
# bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver,
eqCpp.setNewContacts(
asmatrix(p), asmatrix(N), mu, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP
)
# bezierSolver = BezierZeroStepCapturability("ss", c0, dc0, p, N, mu, g_vector,
# mass, verb=verb, solver=solver,
# kinematic_constraints = None);
# stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass, verb=verb, solver=solver);
window_times = [1] + [0.1 * i for i in range(1, 10)] + [0.1 * i for i in range(11, 21)] # try nominal time first
# window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)] #try nominal time first
# stabilitySolver = StabilityCriterion("ss", c0, dc0, p, N, mu, g_vector, mass,
# verb=verb, solver=solver);
window_times = (
[1] + [0.1 * i for i in range(1, 10)] + [0.1 * i for i in range(11, 21)]
) # try nominal time first
# try nominal time first
# window_times = [0.2*i for i in range(1,5)] + [0.2*i for i in range(6,11)]
# window_times = [1]+ [0.4*i for i in range(1,4)] #try nominal time first
# window_times = [1]+ [0.4*i for i in range(3,6)] #try nominal time first
# window_times = [0.7]
found = False
time_step_check = -0.2
for i, el in enumerate(window_times):
if (found):
if found:
break
res = bezierSolver.can_I_stop(T=el, time_step=time_step_check)
if (res.is_stable):
if res.is_stable:
found = True
# print("continuous found at ", el)
__check_trajectory(bezierSolver._p0,
bezierSolver._p1,
res.c,
res.c,
el,
bezierSolver._H,
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(matrix([p_i.tolist() for p_i in res.wpsdL]).transpose()))
__check_trajectory(
bezierSolver._p0,
bezierSolver._p1,
res.c,
res.c,
el,
bezierSolver._H,
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(matrix([p_i.tolist() for p_i in res.wpsdL]).transpose()),
)
if i != 0:
print("continuous Failed to stop at 1, but managed to stop at ", el)
found = False
time_step_check = 0.05
for i, el in enumerate(window_times):
if (found):
if found:
break
res2 = bezierSolver.can_I_stop(T=el, time_step=time_step_check, l0=zeros(3))
if (res2.is_stable):
if res2.is_stable:
found = True
# print("ang_momentum found at ", el)
__check_trajectory(
......@@ -195,11 +241,13 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
res2.c,
el,
bezierSolver._H,
# bezierSolver._mass, bezierSolver._g, time_step = time_step_check, dL = res2.dL_of_t)
# bezierSolver._mass, bezierSolver._g, time_step =
# time_step_check, dL = res2.dL_of_t)
bezierSolver._mass,
bezierSolver._g,
time_step=time_step_check,
dL=bezier(matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose()))
dL=bezier(matrix([p_i.tolist() for p_i in res2.wpsdL]).transpose()),
)
if i != 0:
print("ang_momentum Failed to stop at 1, but managed to stop at ", el)
# res2 = None
......@@ -208,8 +256,8 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
# except Exception as e:
# pass
if (res2.is_stable != res.is_stable):
if (res.is_stable):
if res2.is_stable != res.is_stable:
if res.is_stable:
print("continuous won")
else:
print("ang_momentum won")
......@@ -218,11 +266,12 @@ def test_continuous_cpp_vs_continuous_py(N_CONTACTS=2, solver='qpoases', verb=0)
if __name__ == "__main__":
g_vector = np.array([0., 0., -9.81])
g_vector = np.array([0.0, 0.0, -9.81])
mass = 75
# mass of the robot
from matplotlib import rcParams
rcParams.update({'font.size': 11})
rcParams.update({"font.size": 11})
mine_won = 0
mine_lose = 0
total_stop = 0
......@@ -236,23 +285,26 @@ if __name__ == "__main__":
import matplotlib.pyplot as plt
def __plot_3d_points(ax, points, c='b'):
def __plot_3d_points(ax, points, c="b"):
xs = [point[0] for point in points]
ys = [point[1] for point in points]
zs = [point[2] for point in points]
ax.scatter(xs[:1], ys[:1], zs[:1], c='r')
ax.scatter(xs[:1], ys[:1], zs[:1], c="r")
ax.scatter(xs[1:-1], ys[1:-1], zs[1:-1], c=c)
ax.scatter(xs[-1:], ys[-1:], zs[-1:], c='g')
ax.set_xlabel('X Label', fontsize=11)
ax.set_ylabel('Y Label', fontsize=11)
ax.set_zlabel('Z Label', fontsize=11)
ax.scatter(xs[-1:], ys[-1:], zs[-1:], c="g")
ax.set_xlabel("X Label", fontsize=11)
ax.set_ylabel("Y Label", fontsize=11)
ax.set_zlabel("Z Label", fontsize=11)
def plot_support_polygon(H, h, p, N, ax, c0):
from pinocchio_inv_dyn.multi_contact.utils import compute_support_polygon
# (H,h) = compute_GIWC(p, N, mu);
global mass
global g_vector
(B_sp, b_sp) = compute_support_polygon(H, h, mass, g_vector, eliminate_redundancies=False)
(B_sp, b_sp) = compute_support_polygon(
H, h, mass, g_vector, eliminate_redundancies=False
)
X_MIN = np.min(p[:, 0])
X_MAX = np.max(p[:, 0])
X_MIN -= 0.5 * (X_MAX - X_MIN)
......@@ -264,12 +316,27 @@ if __name__ == "__main__":
num_steps = 50
dx = (X_MAX - X_MIN) / float(num_steps)
dy = (Y_MAX - Y_MIN) / float(num_steps)
# points = [(X_MIN + dx * i, Y_MAX + dy * j, 0.) for i in range(num_steps+1) for j in range(num_steps+1) if
# check_static_eq(H, h, mass, array([X_MIN + dx * i, Y_MAX + dy * j,0.]), g_vector) ]
# points = [c0]+[[X_MIN + dx * i, Y_MIN + dy * j, -0.5] for i in range(num_steps+1) for j in
# range(num_steps+1) if check_static_eq(H, h, mass, [X_MIN + dx * i, Y_MAX + dy * j,0.], g_vector)]
points = [c0] + [[X_MIN + dx * i, Y_MIN + dy * j, 0] for i in range(num_steps + 1)
for j in range(num_steps + 1)]
# points = [
# (X_MIN + dx * i, Y_MAX + dy * j, 0.0)
# for i in range(num_steps + 1)
# for j in range(num_steps + 1)
# if check_static_eq(
# H, h, mass, array([X_MIN + dx * i, Y_MAX + dy * j, 0.0]), g_vector
# )
# ]
# points = [c0] + [
# [X_MIN + dx * i, Y_MIN + dy * j, -0.5]
# for i in range(num_steps + 1)
# for j in range(num_steps + 1)
# if check_static_eq(
# H, h, mass, [X_MIN + dx * i, Y_MAX + dy * j, 0.0], g_vector
# )
# ]
points = [c0] + [
[X_MIN + dx * i, Y_MIN + dy * j, 0]
for i in range(num_steps + 1)
for j in range(num_steps + 1)
]
pts2 = []
for pt in points:
if check_static_eq(H, h, mass, pt, g_vector):
......@@ -279,8 +346,10 @@ if __name__ == "__main__":
__plot_3d_points(ax, p, c="r")
# for i in range(num_steps):
# for j in range(num_steps):
# plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX], ax=ax, color='b', lw=4, is_3d=False);
# plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX], ax=ax, color='b', lw=4, is_3d=False);
# plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX],
# ax=ax, color='b', lw=4, is_3d=False);
# plot_inequalities(B_sp, b_sp, [X_MIN,X_MAX], [Y_MIN,Y_MAX],
# ax=ax, color='b', lw=4, is_3d=False);
# plt.show();
def plot_win_curve(n=-1, num_pts=20):
......@@ -288,7 +357,22 @@ if __name__ == "__main__":
if n > len(curves_when_i_win) - 1 or n < 0:
print("n bigger than num curves or equal to -1, plotting last curve")
n = len(curves_when_i_win) - 1
c0, dc0, c_end, dc_end, t_max, c_of_t, dc_of_t, ddc_of_t, H, h, p, N, dl_of_t, L_of_t = curves_when_i_win[n]
(
c0,
dc0,
c_end,
dc_end,
t_max,
c_of_t,
dc_of_t,
ddc_of_t,
H,
h,
p,
N,
dl_of_t,
L_of_t,
) = curves_when_i_win[n]
print("c0 ", c0)
print("Is c0 stable ? ", check_static_eq(H, h, mass, c0, g_vector))
print("Is end stable ? ", check_static_eq(H, h, mass, c_of_t(t_max), g_vector))
......@@ -296,7 +380,7 @@ if __name__ == "__main__":
w = np.zeros(6)
w[2] = -mass * 9.81
w[3:] = mass * np.cross(c_of_t(t_max), g_vector)
print('max ', np.max(np.dot(H, w) - h))
print("max ", np.max(np.dot(H, w) - h))
X_MIN = np.min(p[:, 0])
X_MAX = np.max(p[:, 0])
......@@ -311,30 +395,36 @@ if __name__ == "__main__":
delta = t_max / float(num_pts)
num_pts += 1
fig = plt.figure()
ax = fig.add_subplot(221, projection='3d')
ax = fig.add_subplot(221, projection="3d")
# ax = fig.add_subplot(221)
__plot_3d_points(ax, [c_of_t(i * delta) for i in range(num_pts)])
__plot_3d_points(ax, [c0 + (c_end - c0) * i * delta for i in range(num_pts)], c="y")
__plot_3d_points(
ax, [c0 + (c_end - c0) * i * delta for i in range(num_pts)], c="y"
)
plot_support_polygon(H, h, p, N, ax, c0)
ax = fig.add_subplot(222, projection='3d')
ax = fig.add_subplot(222, projection="3d")
__plot_3d_points(ax, [dc_of_t(i * delta) for i in range(num_pts)])
__plot_3d_points(ax, [dc0 + (dc_end - dc0) * i * delta for i in range(num_pts)], c="y")
ax = fig.add_subplot(223, projection='3d')
__plot_3d_points(
ax, [dc0 + (dc_end - dc0) * i * delta for i in range(num_pts)], c="y"
)
ax = fig.add_subplot(223, projection="3d")
# __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)])
# ax = fig.add_subplot(224, projection='3d')
__plot_3d_points(ax, [L_of_t(i * delta) for i in range(num_pts)], c="y")
# __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)], c = "y")
ax = fig.add_subplot(224, projection='3d')
ax = fig.add_subplot(224, projection="3d")
__plot_3d_points(ax, [dl_of_t(i * delta) for i in range(num_pts)])
# ax = fig.add_subplot(121, projection='3d')
# __plot_3d_points(ax, [ddc_of_t(i * delta) for i in range(num_pts)])
# ax = fig.add_subplot(122, projection='3d')
# __plot_3d_points(ax, [-dc0* i * delta for i in range(num_pts)])
# print("cross product ", X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )))
# print("cross product ", X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )
# / norm(X(-dc0,ddc_of_t(0.5) - ddc_of_t(0) )))
# print("init acceleration ", ddc_of_t(0))
print("init velocity ", dc_of_t(0))
print("end velocity ", dc_of_t(t_max))
# print("cross product ", X(-dc0,ddc_of_t(t_max) - ddc_of_t(0) ) / norm(X(-dc0,ddc_of_t(t_max) - ddc_of_t(0))))
# print("cross product ", X(-dc0,ddc_of_t(t_max) - ddc_of_t(0) )
# / norm(X(-dc0,ddc_of_t(t_max) - ddc_of_t(0))))
# plt.show()
......@@ -347,21 +437,47 @@ if __name__ == "__main__":
plot_win_curve(i, num_pts)
plt.show()
num_tested = 0.
num_tested = 0.0
for i in range(1000):
num_tested = i - 1
mine, theirs, r_mine, r_theirs, c0, dc0, H, h, p, N = test_continuous_cpp_vs_continuous_py()
# mine, theirs, r_mine, r_theirs, c0, dc0, H,h, p, N = test_momentum_cpp_vs_momentum_py()
(
mine,
theirs,
r_mine,
r_theirs,
c0,
dc0,
H,
h,
p,
N,
) = test_continuous_cpp_vs_continuous_py()
# mine, theirs, r_mine, r_theirs, c0, dc0, H,h, p, N = \
# test_momentum_cpp_vs_momentum_py()
# print("H test", H.shape)
if (mine != theirs):
if mine != theirs:
total_disagree += 1
if (mine):
if mine:
# times_disagree +=[r_mine.t]
# raise ValueError (" BITE ME " )
margin_i_win_he_lose += [r_theirs.dc]
curves_when_i_win += [
(c0[:], dc0[:], r_theirs.c[:], r_theirs.dc[:], r_mine.t, r_mine.c_of_t, r_mine.dc_of_t,
r_mine.ddc_of_t, H[:], h[:], p[:], N, r_mine.dL_of_t, r_mine.L_of_t)
(
c0[:],
dc0[:],
r_theirs.c[:],
r_theirs.dc[:],
r_mine.t,
r_mine.c_of_t,
r_mine.dc_of_t,
r_mine.ddc_of_t,
H[:],
h[:],
p[:],
N,
r_mine.dL_of_t,
r_mine.L_of_t,
)
]
# print("margin when he lost: ", norm(r_theirs.dc))
# else:
......@@ -370,20 +486,25 @@ if __name__ == "__main__":
mine_won += 1
else:
mine_lose += 1
elif (mine or theirs):
elif mine or theirs:
total_stop += 1
# times_agree_stop+=[r_mine.t]
margin_he_wins_i_lost += [r_theirs.ddc_min]
# margin_i_win_he_lose+=[r_theirs.dc]
# curves_when_i_win+=[(c0[:], dc0[:], r_theirs.c[:], r_theirs.dc[:], r_mine.t, r_mine.c_of_t,
# curves_when_i_win+=[(c0[:], dc0[:], r_theirs.c[:],
# r_theirs.dc[:], r_mine.t, r_mine.c_of_t,
# r_mine.dc_of_t, r_mine.ddc_of_t, H[:], h[:], p[:], N)]
# print("margin when he wins: ", r_theirs.ddc_min)
else:
total_not_stop += 1
print("% of stops", 100. * float(total_stop) / num_tested, total_stop)
print("% of total_disagree", 100. * float(total_disagree) / num_tested, total_disagree)
print("% of stops", 100.0 * float(total_stop) / num_tested, total_stop)
print(
"% of total_disagree",
100.0 * float(total_disagree) / num_tested,
total_disagree,
)
if total_disagree > 0:
print("% of wins", 100. * float(mine_won) / total_disagree)
print("% of lose", 100. * float(mine_lose) / total_disagree)
print("% of wins", 100.0 * float(mine_won) / total_disagree)
print("% of lose", 100.0 * float(mine_lose) / total_disagree)
setup.cfg 0 → 100644
View file @ 626b8c65
[flake8]
exclude = cmake
max-line-length = 88
ignore = E226, E704, E24, E121, W504, E126, E123, W503, E203
src/common_solve_methods.cpp
View file @ 626b8c65
......@@ -5,8 +5,9 @@
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
namespace bezier_com_traj {
std::vector<waypoint6_t> ComputeDiscretizedWaypoints(const std::vector<waypoint6_t>& wps,
const std::vector<ndcurves::Bern<double> >& berns, int numSteps) {
std::vector<waypoint6_t> ComputeDiscretizedWaypoints(
const std::vector<waypoint6_t>& wps,
const std::vector<ndcurves::Bern<double> >& berns, int numSteps) {
double dt = 1. / double(numSteps);
std::vector<waypoint6_t> res;
for (int i = 0; i < numSteps + 1; ++i) {
......@@ -21,27 +22,30 @@ std::vector<waypoint6_t> ComputeDiscretizedWaypoints(const std::vector<waypoint6
return res;
}
MatrixXX initMatrixA(const int dimH, const std::vector<waypoint6_t>& wps, const long int extraConstraintSize,
MatrixXX initMatrixA(const int dimH, const std::vector<waypoint6_t>& wps,
const long int extraConstraintSize,
const bool useAngMomentum) {
int dimPb = useAngMomentum ? 6 : 3;
return MatrixXX::Zero(dimH * wps.size() + extraConstraintSize, dimPb);
}
MatrixXX initMatrixD(const int colG, const std::vector<waypoint6_t>& wps, const long int extraConstraintSize,
MatrixXX initMatrixD(const int colG, const std::vector<waypoint6_t>& wps,
const long int extraConstraintSize,
const bool useAngMomentum) {
int dimPb = useAngMomentum ? 6 : 3;
return MatrixXX::Zero(6 + extraConstraintSize, wps.size() * colG + dimPb);
}
void addKinematic(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Kin, Cref_vectorX kin,
const long int otherConstraintIndex) {
void addKinematic(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Kin,
Cref_vectorX kin, const long int otherConstraintIndex) {
int dimKin = (int)(kin.rows());
if (dimKin == 0) return;
A.block(A.rows() - dimKin - otherConstraintIndex, 0, dimKin, 3) = Kin;
b.segment(A.rows() - dimKin - otherConstraintIndex, dimKin) = kin;
}
void addAngularMomentum(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Ang, Cref_vectorX ang) {
void addAngularMomentum(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Ang,
Cref_vectorX ang) {
int dimAng = (int)(ang.rows());
if (dimAng > 0) {
A.block(A.rows() - dimAng, 3, dimAng, 3) = Ang;
......@@ -50,13 +54,15 @@ void addAngularMomentum(Ref_matrixXX A, Ref_vectorX b, Cref_matrixX3 Ang, Cref_v
}
int removeZeroRows(Ref_matrixXX& A, Ref_vectorX& b) {
Eigen::Matrix<bool, Eigen::Dynamic, 1> empty = (A.array() == 0).rowwise().all();
Eigen::Matrix<bool, Eigen::Dynamic, 1> empty =
(A.array() == 0).rowwise().all();
size_t last = A.rows() - 1;
for (size_t i = 0; i < last + 1;) {
if (empty(i)) {
assert(A.row(i).norm() == 0);
if (b(i) < 0) {
// std::cout << "empty row for A while correponding b is negative. Problem is infeasible" << b(i) << std::endl;
// std::cout << "empty row for A while correponding b is negative.
// Problem is infeasible" << b(i) << std::endl;
return -1;
}
A.row(i).swap(A.row(last));
......@@ -79,16 +85,17 @@ int Normalize(Ref_matrixXX A, Ref_vectorX b) {
return zeroindex;
}
std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData& cData,
const std::vector<waypoint6_t>& wps,
const std::vector<waypoint6_t>& wpL,
const bool useAngMomentum, bool& fail) {
std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
const ContactData& cData, const std::vector<waypoint6_t>& wps,
const std::vector<waypoint6_t>& wpL, const bool useAngMomentum,
bool& fail) {
MatrixXX A;
VectorX b;
// gravity vector
const point_t& g = cData.contactPhase_->m_gravity;
// compute GIWC
assert(cData.contactPhase_->getAlgorithm() == centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
assert(cData.contactPhase_->getAlgorithm() ==
centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
centroidal_dynamics::MatrixXX Hrow;
VectorX h;
cData.contactPhase_->getPolytopeInequalities(Hrow, h);
......@@ -105,7 +112,8 @@ std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData
bc.head(3) = g; // constant part of Aub, Aubi = mH * (bc - wsi)
int i = 0;
std::vector<waypoint6_t>::const_iterator wpLcit = wpL.begin();
for (std::vector<waypoint6_t>::const_iterator wpcit = wps.begin(); wpcit != wps.end(); ++wpcit) {
for (std::vector<waypoint6_t>::const_iterator wpcit = wps.begin();
wpcit != wps.end(); ++wpcit) {
A.block(i * dimH, 0, dimH, 3) = mH * wpcit->first;
b.segment(i * dimH, dimH) = mH * (bc - wpcit->second);
if (useAngMomentum) {
......@@ -132,28 +140,35 @@ std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData
return std::make_pair(A, b);
}
ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D, Cref_vectorX d, Cref_matrixXX H, Cref_vectorX g,
Cref_vectorX initGuess, Cref_vectorX minBounds, Cref_vectorX maxBounds,
const solvers::SolverType solver) {
return solvers::solve(A, ci0, D, d, H, g, initGuess, minBounds, maxBounds, solver);
ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX D,
Cref_vectorX d, Cref_matrixXX H, Cref_vectorX g,
Cref_vectorX initGuess, Cref_vectorX minBounds,
Cref_vectorX maxBounds, const solvers::SolverType solver) {
return solvers::solve(A, ci0, D, d, H, g, initGuess, minBounds, maxBounds,
solver);
}
ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H, Cref_vectorX g, Cref_vectorX initGuess,
ResultData solve(Cref_matrixXX A, Cref_vectorX b, Cref_matrixXX H,
Cref_vectorX g, Cref_vectorX initGuess,
const solvers::SolverType solver) {
MatrixXX D = MatrixXX::Zero(0, A.cols());
VectorX d = VectorX::Zero(0);
return solvers::solve(A, b, D, d, H, g, initGuess, d, d, solver);
}
ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Hg, const VectorX& init,
ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
const std::pair<MatrixXX, VectorX>& Hg, const VectorX& init,
const solvers::SolverType solver) {
return solve(Ab.first, Ab.second, Hg.first, Hg.second, init, solver);
}
ResultData solve(const std::pair<MatrixXX, VectorX>& Ab, const std::pair<MatrixXX, VectorX>& Dd,
const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds, Cref_vectorX maxBounds,
const VectorX& init, const solvers::SolverType solver) {
return solve(Ab.first, Ab.second, Dd.first, Dd.second, Hg.first, Hg.second, init, minBounds, maxBounds, solver);
ResultData solve(const std::pair<MatrixXX, VectorX>& Ab,
const std::pair<MatrixXX, VectorX>& Dd,
const std::pair<MatrixXX, VectorX>& Hg, Cref_vectorX minBounds,
Cref_vectorX maxBounds, const VectorX& init,
const solvers::SolverType solver) {
return solve(Ab.first, Ab.second, Dd.first, Dd.second, Hg.first, Hg.second,
init, minBounds, maxBounds, solver);
}
} // namespace bezier_com_traj
src/computeCOMTraj.cpp
View file @ 626b8c65
......@@ -3,17 +3,14 @@
* Author: Pierre Fernbach
*/
#include <hpp/bezier-com-traj/solve.hh>
#include <algorithm>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/bezier-com-traj/cost/costfunction_definition.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <limits>
#include <algorithm>
static const int dimVarX = 3;
static const int numRowsForce = 6;
......@@ -21,11 +18,14 @@ static const int numRowsForce = 6;
namespace bezier_com_traj {
typedef std::pair<double, point3_t> coefs_t;
bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData, double T, const T_time& timeArray) {
bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData,
double T,
const T_time& timeArray) {
bezier_wp_t::t_point_t wps = computeWwaypoints(pData, T);
bezier_wp_t::t_point_t res;
const int DIM_VAR = (int)dimVar(pData);
std::vector<ndcurves::Bern<double> > berns = ComputeBersteinPolynoms((int)wps.size() - 1);
std::vector<ndcurves::Bern<double> > berns =
ComputeBersteinPolynoms((int)wps.size() - 1);
double t, b;
for (CIT_time cit = timeArray.begin(); cit != timeArray.end(); ++cit) {
waypoint_t w = initwp(6, DIM_VAR);
......@@ -40,23 +40,28 @@ bezier_wp_t::t_point_t computeDiscretizedWwaypoints(const ProblemData& pData, do
return res;
}
std::pair<MatrixXX, VectorX> dynamicStabilityConstraints_cross(const MatrixXX& mH, const VectorX& h, const Vector3& g,
const coefs_t& c, const coefs_t& ddc) {
std::pair<MatrixXX, VectorX> dynamicStabilityConstraints_cross(
const MatrixXX& mH, const VectorX& h, const Vector3& g, const coefs_t& c,
const coefs_t& ddc) {
Matrix3 S_hat;
int dimH = (int)(mH.rows());
MatrixXX A(dimH, 4);
VectorX b(dimH);
S_hat = skew(c.second * ddc.first - ddc.second * c.first + g * c.first);
A.block(0, 0, dimH, 3) = mH.block(0, 3, dimH, 3) * S_hat + mH.block(0, 0, dimH, 3) * ddc.first;
A.block(0, 0, dimH, 3) =
mH.block(0, 3, dimH, 3) * S_hat + mH.block(0, 0, dimH, 3) * ddc.first;
b = h + mH.block(0, 0, dimH, 3) * (g - ddc.second) +
mH.block(0, 3, dimH, 3) * (c.second.cross(g) - c.second.cross(ddc.second));
mH.block(0, 3, dimH, 3) *
(c.second.cross(g) - c.second.cross(ddc.second));
Normalize(A, b);
// add 1 for the slack variable :
A.block(0, 3, dimH, 1) = VectorX::Ones(dimH);
return std::make_pair(A, b);
}
std::pair<MatrixXX, VectorX> dynamicStabilityConstraints(const MatrixXX& mH, const VectorX& h, const Vector3& g,
std::pair<MatrixXX, VectorX> dynamicStabilityConstraints(const MatrixXX& mH,
const VectorX& h,
const Vector3& g,
const waypoint_t& w) {
int dimH = (int)(mH.rows());
MatrixXX A(dimH, dimVarX);
......@@ -69,12 +74,14 @@ std::pair<MatrixXX, VectorX> dynamicStabilityConstraints(const MatrixXX& mH, con
return std::make_pair(A, b);
}
std::vector<int> stepIdPerPhase(const T_time& timeArray) // const int pointsPerPhase)
std::vector<int> stepIdPerPhase(
const T_time& timeArray) // const int pointsPerPhase)
{
std::vector<int> res;
int i = 0;
CIT_time cit = timeArray.begin();
for (CIT_time cit2 = timeArray.begin() + 1; cit2 != timeArray.end(); ++cit, ++cit2, ++i) {
for (CIT_time cit2 = timeArray.begin() + 1; cit2 != timeArray.end();
++cit, ++cit2, ++i) {
if (cit2->second != cit->second) {
res.push_back(i);
}
......@@ -83,7 +90,8 @@ std::vector<int> stepIdPerPhase(const T_time& timeArray) // const int pointsPer
return res;
}
long int computeNumIneq(const ProblemData& pData, const VectorX& Ts, const std::vector<int>& phaseSwitch) {
long int computeNumIneq(const ProblemData& pData, const VectorX& Ts,
const std::vector<int>& phaseSwitch) {
const size_t numPoints = phaseSwitch.back() + 1;
long int num_stab_ineq = 0;
long int num_kin_ineq = 0;
......@@ -93,44 +101,56 @@ long int computeNumIneq(const ProblemData& pData, const VectorX& Ts, const std::
VectorX h;
for (int i = 0; i < Ts.size(); ++i) {
pData.contacts_[i].contactPhase_->getPolytopeInequalities(Hrow, h);
numStepForPhase = phaseSwitch[i] + 1 - numStepsCumulated; // pointsPerPhase;
numStepForPhase =
phaseSwitch[i] + 1 - numStepsCumulated; // pointsPerPhase;
numStepsCumulated = phaseSwitch[i] + 1;
if (i > 0) ++numStepForPhase; // because at the switch point between phases we add the constraints of both phases.
if (i > 0)
++numStepForPhase; // because at the switch point between phases we add
// the constraints of both phases.
num_stab_ineq += Hrow.rows() * numStepForPhase;
num_kin_ineq += pData.contacts_[i].kin_.rows() * numStepForPhase;
}
long int res = num_stab_ineq + num_kin_ineq;
if (pData.constraints_.constraintAcceleration_)
res += 2 * 3 *
(numPoints); // upper and lower bound on acceleration for each discretized waypoint (exept the first one)
res +=
2 * 3 * (numPoints); // upper and lower bound on acceleration for each
// discretized waypoint (exept the first one)
return res;
}
void updateH(const ProblemData& pData, const ContactData& phase, MatrixXX& mH, VectorX& h, int& dimH) {
void updateH(const ProblemData& pData, const ContactData& phase, MatrixXX& mH,
VectorX& h, int& dimH) {
VectorX hrow;
centroidal_dynamics::MatrixXX Hrow;
centroidal_dynamics::LP_status status = phase.contactPhase_->getPolytopeInequalities(Hrow, hrow);
centroidal_dynamics::LP_status status =
phase.contactPhase_->getPolytopeInequalities(Hrow, hrow);
assert(status == centroidal_dynamics::LP_STATUS_OPTIMAL &&
"Error in centroidal dynamics lib while computing inequalities");
mH = -Hrow * phase.contactPhase_->m_mass;
mH.rowwise().normalize();
h = hrow;
dimH = (int)(mH.rows());
if (pData.constraints_.reduce_h_ > 0) h -= VectorX::Ones(h.rows()) * pData.constraints_.reduce_h_;
if (pData.constraints_.reduce_h_ > 0)
h -= VectorX::Ones(h.rows()) * pData.constraints_.reduce_h_;
}
void assignStabilityConstraintsForTimeStep(MatrixXX& mH, VectorX& h, const waypoint_t& wp_w, const int dimH,
long int& id_rows, MatrixXX& A, VectorX& b, const Vector3& g) {
std::pair<MatrixXX, VectorX> Ab_stab = dynamicStabilityConstraints(mH, h, g, wp_w);
void assignStabilityConstraintsForTimeStep(MatrixXX& mH, VectorX& h,
const waypoint_t& wp_w,
const int dimH, long int& id_rows,
MatrixXX& A, VectorX& b,
const Vector3& g) {
std::pair<MatrixXX, VectorX> Ab_stab =
dynamicStabilityConstraints(mH, h, g, wp_w);
A.block(id_rows, 0, dimH, dimVarX) = Ab_stab.first;
b.segment(id_rows, dimH) = Ab_stab.second;
id_rows += dimH;
}
// mG is -G
void assignStabilityConstraintsForTimeStepForce(const waypoint_t& wp_w, const long int rowIdx, long int& id_cols,
const centroidal_dynamics::Matrix6X mG, MatrixXX& D, VectorX& d,
const double mass, const Vector3& g) {
void assignStabilityConstraintsForTimeStepForce(
const waypoint_t& wp_w, const long int rowIdx, long int& id_cols,
const centroidal_dynamics::Matrix6X mG, MatrixXX& D, VectorX& d,
const double mass, const Vector3& g) {
D.block(rowIdx, id_cols, numRowsForce, mG.cols()) = mG;
id_cols += mG.cols();
D.block(rowIdx, 0, numRowsForce, dimVarX) = mass * wp_w.first;
......@@ -139,19 +159,26 @@ void assignStabilityConstraintsForTimeStepForce(const waypoint_t& wp_w, const lo
d.segment(rowIdx, numRowsForce) = mass * (g_ - wp_w.second);
}
void switchContactPhase(const ProblemData& pData, MatrixXX& A, VectorX& b, MatrixXX& mH, VectorX& h,
const waypoint_t& wp_w, const ContactData& phase, long int& id_rows, int& dimH) {
void switchContactPhase(const ProblemData& pData, MatrixXX& A, VectorX& b,
MatrixXX& mH, VectorX& h, const waypoint_t& wp_w,
const ContactData& phase, long int& id_rows,
int& dimH) {
updateH(pData, phase, mH, h, dimH);
// the current waypoint must have the constraints of both phases. So we add it again :
// the current waypoint must have the constraints of both phases. So we add it
// again :
// TODO : filter for redunbdant constraints ...
// add stability constraints :
assignStabilityConstraintsForTimeStep(mH, h, wp_w, dimH, id_rows, A, b, phase.contactPhase_->m_gravity);
assignStabilityConstraintsForTimeStep(mH, h, wp_w, dimH, id_rows, A, b,
phase.contactPhase_->m_gravity);
}
long int assignStabilityConstraints(const ProblemData& pData, MatrixXX& A, VectorX& b, const T_time& timeArray,
const double t_total, const std::vector<int>& stepIdForPhase) {
long int assignStabilityConstraints(const ProblemData& pData, MatrixXX& A,
VectorX& b, const T_time& timeArray,
const double t_total,
const std::vector<int>& stepIdForPhase) {
long int id_rows = 0;
bezier_wp_t::t_point_t wps_w = computeDiscretizedWwaypoints(pData, t_total, timeArray);
bezier_wp_t::t_point_t wps_w =
computeDiscretizedWwaypoints(pData, t_total, timeArray);
std::size_t id_phase = 0;
const Vector3& g = pData.contacts_[id_phase].contactPhase_->m_gravity;
......@@ -166,23 +193,29 @@ long int assignStabilityConstraints(const ProblemData& pData, MatrixXX& A, Vecto
// we don't consider the first point, because it's independant of x.
for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
// add stability constraints :
assignStabilityConstraintsForTimeStep(mH, h, wps_w[id_step], dimH, id_rows, A, b, g);
assignStabilityConstraintsForTimeStep(mH, h, wps_w[id_step], dimH, id_rows,
A, b, g);
// check if we are going to switch phases :
for (std::vector<int>::const_iterator it_switch = stepIdForPhase.begin(); it_switch != (stepIdForPhase.end() - 1);
++it_switch) {
for (std::vector<int>::const_iterator it_switch = stepIdForPhase.begin();
it_switch != (stepIdForPhase.end() - 1); ++it_switch) {
if ((int)id_step == (*it_switch)) {
id_phase++;
switchContactPhase(pData, A, b, mH, h, wps_w[id_step], pData.contacts_[id_phase], id_rows, dimH);
switchContactPhase(pData, A, b, mH, h, wps_w[id_step],
pData.contacts_[id_phase], id_rows, dimH);
}
}
}
return id_rows;
}
void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX& b, const T_time& timeArray,
const double t_total, const std::vector<int>& stepIdForPhase, long int& id_rows) {
void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A,
VectorX& b, const T_time& timeArray,
const double t_total,
const std::vector<int>& stepIdForPhase,
long int& id_rows) {
std::size_t id_phase = 0;
std::vector<coefs_t> wps_c = computeDiscretizedWaypoints<point_t>(pData, t_total, timeArray);
std::vector<coefs_t> wps_c =
computeDiscretizedWaypoints<point_t>(pData, t_total, timeArray);
long int current_size;
id_phase = 0;
// assign the Kinematics constraints for each discretized waypoints :
......@@ -190,12 +223,15 @@ void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX&
for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
// add constraints for wp id_step, on current phase :
// add kinematics constraints :
// constraint are of the shape A c <= b . But here c(x) = Fx + s so : AFx <= b - As
// constraint are of the shape A c <= b . But here c(x) = Fx + s so : AFx <=
// b - As
current_size = (int)pData.contacts_[id_phase].kin_.rows();
if (current_size > 0) {
A.block(id_rows, 0, current_size, 3) = (pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
A.block(id_rows, 0, current_size, 3) =
(pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
b.segment(id_rows, current_size) =
pData.contacts_[id_phase].kin_ - (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
pData.contacts_[id_phase].kin_ -
(pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
id_rows += current_size;
}
......@@ -204,13 +240,16 @@ void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX&
if (id_step == (std::size_t)stepIdForPhase[i]) {
// switch to phase i
id_phase = i + 1;
// the current waypoint must have the constraints of both phases. So we add it again :
// the current waypoint must have the constraints of both phases. So we
// add it again :
// TODO : filter for redunbdant constraints ...
current_size = pData.contacts_[id_phase].kin_.rows();
if (current_size > 0) {
A.block(id_rows, 0, current_size, 3) = (pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
A.block(id_rows, 0, current_size, 3) =
(pData.contacts_[id_phase].Kin_ * wps_c[id_step].first);
b.segment(id_rows, current_size) =
pData.contacts_[id_phase].kin_ - (pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
pData.contacts_[id_phase].kin_ -
(pData.contacts_[id_phase].Kin_ * wps_c[id_step].second);
id_rows += current_size;
}
}
......@@ -218,63 +257,82 @@ void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX&
}
}
void assignAccelerationConstraints(const ProblemData& pData, MatrixXX& A, VectorX& b, const T_time& timeArray,
void assignAccelerationConstraints(const ProblemData& pData, MatrixXX& A,
VectorX& b, const T_time& timeArray,
const double t_total, long int& id_rows) {
if (pData.constraints_
.constraintAcceleration_) { // assign the acceleration constraints for each discretized waypoints :
std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints<point_t>(pData, t_total, timeArray);
.constraintAcceleration_) { // assign the acceleration constraints
// for each discretized waypoints :
std::vector<coefs_t> wps_ddc =
computeDiscretizedAccelerationWaypoints<point_t>(pData, t_total,
timeArray);
Vector3 acc_bounds = Vector3::Ones() * pData.constraints_.maxAcceleration_;
for (std::size_t id_step = 0; id_step < timeArray.size(); ++id_step) {
A.block(id_rows, 0, 3, 3) = Matrix3::Identity() * wps_ddc[id_step].first; // upper
A.block(id_rows, 0, 3, 3) =
Matrix3::Identity() * wps_ddc[id_step].first; // upper
b.segment(id_rows, 3) = acc_bounds - wps_ddc[id_step].second;
A.block(id_rows + 3, 0, 3, 3) = -Matrix3::Identity() * wps_ddc[id_step].first; // lower
A.block(id_rows + 3, 0, 3, 3) =
-Matrix3::Identity() * wps_ddc[id_step].first; // lower
b.segment(id_rows + 3, 3) = acc_bounds + wps_ddc[id_step].second;
id_rows += 6;
}
}
}
std::pair<MatrixXX, VectorX> computeConstraintsOneStep(const ProblemData& pData, const VectorX& Ts,
const double t_total, const T_time& timeArray) {
std::pair<MatrixXX, VectorX> computeConstraintsOneStep(
const ProblemData& pData, const VectorX& Ts, const double t_total,
const T_time& timeArray) {
// Compute all the discretized wayPoint
std::vector<int> stepIdForPhase = stepIdPerPhase(timeArray);
// stepIdForPhase[i] is the id of the last step of phase i / first step of phase i+1 (overlap)
// stepIdForPhase[i] is the id of the last step of phase i / first step of
// phase i+1 (overlap)
// init constraints matrix :
long int num_ineq = computeNumIneq(pData, Ts, stepIdForPhase);
MatrixXX A = MatrixXX::Zero(num_ineq, dimVarX);
VectorX b(num_ineq);
// assign dynamic and kinematic constraints per timestep, including additional acceleration
// constraints.
long int id_rows = assignStabilityConstraints(pData, A, b, timeArray, t_total, stepIdForPhase);
assignKinematicConstraints(pData, A, b, timeArray, t_total, stepIdForPhase, id_rows);
// assign dynamic and kinematic constraints per timestep, including additional
// acceleration constraints.
long int id_rows = assignStabilityConstraints(pData, A, b, timeArray, t_total,
stepIdForPhase);
assignKinematicConstraints(pData, A, b, timeArray, t_total, stepIdForPhase,
id_rows);
assignAccelerationConstraints(pData, A, b, timeArray, t_total, id_rows);
assert(id_rows == (A.rows()) && "The constraints matrices were not fully filled.");
assert(id_rows == (A.rows()) &&
"The constraints matrices were not fully filled.");
return std::make_pair(A, b);
}
void computeFinalAcceleration(ResultDataCOMTraj& res) { res.ddc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 2); }
void computeFinalAcceleration(ResultDataCOMTraj& res) {
res.ddc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 2);
}
void computeFinalVelocity(ResultDataCOMTraj& res) { res.dc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 1); }
void computeFinalVelocity(ResultDataCOMTraj& res) {
res.dc1_ = res.c_of_t_.derivate(res.c_of_t_.max(), 1);
}
std::pair<MatrixXX, VectorX> genCostFunction(const ProblemData& pData, const VectorX& Ts, const double T,
const T_time& timeArray, const long int& dim) {
std::pair<MatrixXX, VectorX> genCostFunction(const ProblemData& pData,
const VectorX& Ts, const double T,
const T_time& timeArray,
const long int& dim) {
MatrixXX H = MatrixXX::Identity(dim, dim) * 1e-6;
VectorX g = VectorX::Zero(dim);
cost::genCostFunction(pData, Ts, T, timeArray, H, g);
return std::make_pair(H, g);
}
ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData, const double T) {
ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData,
const double T) {
ResultDataCOMTraj res;
if (resQp.success_) {
res.success_ = true;
res.x = resQp.x.head<3>();
res.cost_ = resQp.cost_;
std::vector<Vector3> pis = computeConstantWaypoints(pData, T);
res.c_of_t_ = computeBezierCurve<bezier_t, point_t>(pData.constraints_.flag_, T, pis, res.x);
res.c_of_t_ = computeBezierCurve<bezier_t, point_t>(
pData.constraints_.flag_, T, pis, res.x);
computeFinalVelocity(res);
computeFinalAcceleration(res);
res.dL_of_t_ = bezier_t::zero(3, T);
......@@ -283,21 +341,25 @@ ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData, const doub
}
/**
* @brief computeNumIneqContinuous compute the number of inequalitie required by all the phases
* @brief computeNumIneqContinuous compute the number of inequalitie required by
* all the phases
* @param pData
* @param Ts
* @param degree
* @param w_degree //FIXME : cannot use 2n+3 because for capturability the degree doesn't correspond (cf
* waypoints_c0_dc0_dc1 )
* @param useDD whether double description or force formulation is used to compute dynamic constraints)
* @param w_degree //FIXME : cannot use 2n+3 because for capturability the
* degree doesn't correspond (cf waypoints_c0_dc0_dc1 )
* @param useDD whether double description or force formulation is used to
* compute dynamic constraints)
* @return
*/
long int computeNumIneqContinuous(const ProblemData& pData, const VectorX& Ts, const int degree, const int w_degree,
long int computeNumIneqContinuous(const ProblemData& pData, const VectorX& Ts,
const int degree, const int w_degree,
const bool useDD) {
long int num_ineq = 0;
centroidal_dynamics::MatrixXX Hrow;
VectorX h;
for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin(); it != pData.contacts_.end(); ++it) {
for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin();
it != pData.contacts_.end(); ++it) {
// kinematics :
num_ineq += it->kin_.rows() * (degree + 1);
// stability :
......@@ -312,28 +374,34 @@ long int computeNumIneqContinuous(const ProblemData& pData, const VectorX& Ts, c
}
/**
* @brief computeNumEqContinuous compute the number of equalities required by all the phases
* @brief computeNumEqContinuous compute the number of equalities required by
* all the phases
* @param pData
* @param w_degree //FIXME : cannot use 2n+3 because for capturability the degree doesn't correspond (cf
* waypoints_c0_dc0_dc1 )
* @param useForce whether double description or force formulation is used to compute dynamic constraints)
* @param w_degree //FIXME : cannot use 2n+3 because for capturability the
* degree doesn't correspond (cf waypoints_c0_dc0_dc1 )
* @param useForce whether double description or force formulation is used to
* compute dynamic constraints)
* @param forceVarDim reference value that stores the total force variable size
* @return
*/
long int computeNumEqContinuous(const ProblemData& pData, const int w_degree, const bool useForce,
long int& forceVarDim) {
long int computeNumEqContinuous(const ProblemData& pData, const int w_degree,
const bool useForce, long int& forceVarDim) {
long int numEq = 0;
if (useForce) {
long int cSize = 0;
int currentDegree; // remove redundant equalities depending on more constraining problem
int currentDegree; // remove redundant equalities depending on more
// constraining problem
std::vector<ContactData>::const_iterator it2 = pData.contacts_.begin();
++it2;
for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin(); it != pData.contacts_.end();
++it, ++it2) {
for (std::vector<ContactData>::const_iterator it = pData.contacts_.begin();
it != pData.contacts_.end(); ++it, ++it2) {
currentDegree = w_degree + 1;
if (cSize != 0 && it->contactPhase_->m_G_centr.cols() > cSize) currentDegree -= 1;
if (cSize != 0 && it->contactPhase_->m_G_centr.cols() > cSize)
currentDegree -= 1;
cSize = it->contactPhase_->m_G_centr.cols();
if (it2 != pData.contacts_.end() && it2->contactPhase_->m_G_centr.cols() <= cSize) currentDegree -= 1;
if (it2 != pData.contacts_.end() &&
it2->contactPhase_->m_G_centr.cols() <= cSize)
currentDegree -= 1;
forceVarDim += cSize * (currentDegree);
numEq += (numRowsForce) * (currentDegree);
}
......@@ -341,13 +409,16 @@ long int computeNumEqContinuous(const ProblemData& pData, const int w_degree, co
return numEq;
}
std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pData, const VectorX& Ts,
std::pair<MatrixXX, VectorX>& Dd, VectorX& minBounds,
VectorX& /*maxBounds*/) {
std::pair<MatrixXX, VectorX> computeConstraintsContinuous(
const ProblemData& pData, const VectorX& Ts,
std::pair<MatrixXX, VectorX>& Dd, VectorX& minBounds,
VectorX& /*maxBounds*/) {
// determine whether to use force or double description
// based on equilibrium value
bool useDD = pData.representation_ == DOUBLE_DESCRIPTION; //!(pData.contacts_.begin()->contactPhase_->getAlgorithm()
//!== centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
bool useDD =
pData.representation_ ==
DOUBLE_DESCRIPTION; //!(pData.contacts_.begin()->contactPhase_->getAlgorithm()
//!== centroidal_dynamics::EQUILIBRIUM_ALGORITHM_PP);
double T = Ts.sum();
......@@ -359,9 +430,11 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
bezier_wp_t w(wps_w.begin(), wps_w.end(), 0., T);
// for each splitted curves : add the constraints for each waypoints
const long int num_ineq = computeNumIneqContinuous(pData, Ts, (int)c.degree_, (int)w.degree_, useDD);
const long int num_ineq = computeNumIneqContinuous(pData, Ts, (int)c.degree_,
(int)w.degree_, useDD);
long int forceVarDim = 0;
const long int num_eq = computeNumEqContinuous(pData, (int)w.degree_, !useDD, forceVarDim);
const long int num_eq =
computeNumEqContinuous(pData, (int)w.degree_, !useDD, forceVarDim);
long int totalVarDim = dimVarX + forceVarDim;
long int id_rows = 0;
long int id_cols = dimVarX; // start after x variable
......@@ -383,7 +456,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
long int cSize = 0;
long int rowIdx = 0;
// for each phases, split the curve and compute the waypoints of the splitted curves
// for each phases, split the curve and compute the waypoints of the splitted
// curves
for (size_t id_phase = 0; id_phase < (size_t)Ts.size(); ++id_phase) {
bezier_wp_t cs = c.extract(current_t, Ts[id_phase] + current_t);
bezier_wp_t ddcs = ddc.extract(current_t, Ts[id_phase] + current_t);
......@@ -394,7 +468,8 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
// add kinematics constraints :
size_kin = (int)phase.kin_.rows();
for (bezier_wp_t::cit_point_t wpit = cs.waypoints().begin(); wpit != cs.waypoints().end(); ++wpit) {
for (bezier_wp_t::cit_point_t wpit = cs.waypoints().begin();
wpit != cs.waypoints().end(); ++wpit) {
A.block(id_rows, 0, size_kin, 3) = (phase.Kin_ * wpit->first);
b.segment(id_rows, size_kin) = phase.kin_ - (phase.Kin_ * wpit->second);
id_rows += size_kin;
......@@ -402,22 +477,28 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
// add stability constraints
if (useDD) {
updateH(pData, phase, mH, h, dimH);
for (bezier_wp_t::cit_point_t wpit = ws.waypoints().begin(); wpit != ws.waypoints().end(); ++wpit)
assignStabilityConstraintsForTimeStep(mH, h, *wpit, dimH, id_rows, A, b, phase.contactPhase_->m_gravity);
for (bezier_wp_t::cit_point_t wpit = ws.waypoints().begin();
wpit != ws.waypoints().end(); ++wpit)
assignStabilityConstraintsForTimeStep(mH, h, *wpit, dimH, id_rows, A, b,
phase.contactPhase_->m_gravity);
} else {
bezier_wp_t::cit_point_t start = ws.waypoints().begin();
bezier_wp_t::cit_point_t stop = ws.waypoints().end();
if (id_phase + 1 < (size_t)Ts.size() &&
pData.contacts_[id_phase + 1].contactPhase_->m_G_centr.cols() <= phase.contactPhase_->m_G_centr.cols())
pData.contacts_[id_phase + 1].contactPhase_->m_G_centr.cols() <=
phase.contactPhase_->m_G_centr.cols())
--stop;
if (cSize > 0 && cSize < phase.contactPhase_->m_G_centr.cols()) ++start;
cSize = phase.contactPhase_->m_G_centr.cols();
for (bezier_wp_t::cit_point_t wpit = start; wpit != stop; ++wpit, rowIdx += 6)
assignStabilityConstraintsForTimeStepForce(*wpit, rowIdx, id_cols, phase.contactPhase_->m_G_centr, D, d,
phase.contactPhase_->m_mass, phase.contactPhase_->m_gravity);
for (bezier_wp_t::cit_point_t wpit = start; wpit != stop;
++wpit, rowIdx += 6)
assignStabilityConstraintsForTimeStepForce(
*wpit, rowIdx, id_cols, phase.contactPhase_->m_G_centr, D, d,
phase.contactPhase_->m_mass, phase.contactPhase_->m_gravity);
}
// add acceleration constraints :
for (bezier_wp_t::cit_point_t wpit = ddcs.waypoints().begin(); wpit != ddcs.waypoints().end(); ++wpit) {
for (bezier_wp_t::cit_point_t wpit = ddcs.waypoints().begin();
wpit != ddcs.waypoints().end(); ++wpit) {
A.block(id_rows, 0, 3, 3) = wpit->first; // upper
b.segment(id_rows, 3) = acc_bounds - wpit->second;
A.block(id_rows + 3, 0, 3, 3) = -wpit->first; // lower
......@@ -429,27 +510,37 @@ std::pair<MatrixXX, VectorX> computeConstraintsContinuous(const ProblemData& pDa
// add positive constraints on forces
// A.block(id_rows,3,forceVarDim,forceVarDim)=MatrixXX::Identity(forceVarDim,forceVarDim)*(-1);
// id_rows += forceVarDim;
minBounds = VectorX::Zero(A.cols()); // * (-std::numeric_limits<double>::infinity());
minBounds = VectorX::Zero(
A.cols()); // * (-std::numeric_limits<double>::infinity());
minBounds.head<3>() = VectorX::Ones(3) * (solvers::UNBOUNDED_DOWN);
minBounds.tail(forceVarDim) = VectorX::Zero(forceVarDim);
}
assert(id_rows == (A.rows()) && "The inequality constraints matrices were not fully filled.");
assert(id_rows == (b.rows()) && "The inequality constraints matrices were not fully filled.");
assert(id_cols == (D.cols()) && "The equality constraints matrices were not fully filled.");
assert(rowIdx == (d.rows()) && "The equality constraints matrices were not fully filled.");
assert(id_rows == (A.rows()) &&
"The inequality constraints matrices were not fully filled.");
assert(id_rows == (b.rows()) &&
"The inequality constraints matrices were not fully filled.");
assert(id_cols == (D.cols()) &&
"The equality constraints matrices were not fully filled.");
assert(rowIdx == (d.rows()) &&
"The equality constraints matrices were not fully filled.");
// int out = Normalize(D,d);
return std::make_pair(A, b);
}
ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData, const VectorX& Ts,
ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData,
const VectorX& Ts,
const unsigned int pointsPerPhase) {
assert(pData.representation_ == DOUBLE_DESCRIPTION);
if (Ts.size() != (int)pData.contacts_.size())
throw std::runtime_error("Time phase vector has different size than the number of contact phases");
throw std::runtime_error(
"Time phase vector has different size than the number of contact "
"phases");
double T = Ts.sum();
T_time timeArray = computeDiscretizedTimeFixed(Ts, pointsPerPhase);
std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData, Ts, T, timeArray);
std::pair<MatrixXX, VectorX> Hg = genCostFunction(pData, Ts, T, timeArray, dimVarX);
std::pair<MatrixXX, VectorX> Ab =
computeConstraintsOneStep(pData, Ts, T, timeArray);
std::pair<MatrixXX, VectorX> Hg =
genCostFunction(pData, Ts, T, timeArray, dimVarX);
VectorX x = VectorX::Zero(dimVarX);
ResultData resQp = solve(Ab, Hg, x);
#if PRINT_QHULL_INEQ
......@@ -458,10 +549,13 @@ ResultDataCOMTraj computeCOMTrajFixedSize(const ProblemData& pData, const Vector
return genTraj(resQp, pData, T);
}
ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts, const double timeStep,
ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts,
const double timeStep,
const solvers::SolverType solver) {
if (Ts.size() != (int)pData.contacts_.size())
throw std::runtime_error("Time phase vector has different size than the number of contact phases");
throw std::runtime_error(
"Time phase vector has different size than the number of contact "
"phases");
double T = Ts.sum();
T_time timeArray;
std::pair<MatrixXX, VectorX> Ab;
......@@ -476,10 +570,12 @@ ResultDataCOMTraj computeCOMTraj(const ProblemData& pData, const VectorX& Ts, co
maxBounds = VectorX::Zero(0);
} else { // continuous
Ab = computeConstraintsContinuous(pData, Ts, Dd, minBounds, maxBounds);
timeArray = computeDiscretizedTimeFixed(Ts, 7); // FIXME : hardcoded value for discretization for cost function in
// case of continuous formulation for the constraints
timeArray = computeDiscretizedTimeFixed(
Ts, 7); // FIXME : hardcoded value for discretization for cost function
// in case of continuous formulation for the constraints
}
std::pair<MatrixXX, VectorX> Hg = genCostFunction(pData, Ts, T, timeArray, Ab.first.cols());
std::pair<MatrixXX, VectorX> Hg =
genCostFunction(pData, Ts, T, timeArray, Ab.first.cols());
VectorX x = VectorX::Zero(Ab.first.cols());
ResultData resQp = solve(Ab, Dd, Hg, minBounds, maxBounds, x, solver);
#if PRINT_QHULL_INEQ
......
src/costfunction_definition.cpp
View file @ 626b8c65
......@@ -3,52 +3,65 @@
* Author: Steve Tonneau
*/
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/cost/costfunction_definition.hh>
#include <hpp/bezier-com-traj/waypoints/waypoints_definition.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
namespace bezier_com_traj {
namespace cost {
// cost : min distance between x and midPoint :
void computeCostMidPoint(const ProblemData& pData, const VectorX& /*Ts*/, const double /*T*/,
const T_time& /*timeArray*/, MatrixXX& H, VectorX& g) {
void computeCostMidPoint(const ProblemData& pData, const VectorX& /*Ts*/,
const double /*T*/, const T_time& /*timeArray*/,
MatrixXX& H, VectorX& g) {
// cost : x' H x + 2 x g'
Vector3 midPoint = (pData.c0_ + pData.c1_) / 2.; // todo : replace it with point found by planning ??
Vector3 midPoint = (pData.c0_ + pData.c1_) /
2.; // todo : replace it with point found by planning ??
H.block<3, 3>(0, 0) = Matrix3::Identity();
g.head<3>() = -midPoint;
}
// cost : min distance between end velocity and the one computed by planning
void computeCostEndVelocity(const ProblemData& pData, const VectorX& /*Ts*/, const double T,
const T_time& /*timeArray*/, MatrixXX& H, VectorX& g) {
void computeCostEndVelocity(const ProblemData& pData, const VectorX& /*Ts*/,
const double T, const T_time& /*timeArray*/,
MatrixXX& H, VectorX& g) {
if (pData.constraints_.flag_ && END_VEL)
throw std::runtime_error("Can't use computeCostEndVelocity as cost function when end velocity is a contraint");
throw std::runtime_error(
"Can't use computeCostEndVelocity as cost function when end velocity "
"is a contraint");
coefs_t v = computeFinalVelocityPoint(pData, T);
H.block<3, 3>(0, 0) = Matrix3::Identity() * v.first * v.first;
g.head<3>() = v.first * (v.second - pData.dc1_);
}
// TODO this is temporary.The acceleration integral can be computed analitcally
void computeCostMinAcceleration(const ProblemData& pData, const VectorX& Ts, const double /*T*/,
const T_time& timeArray, MatrixXX& H, VectorX& g) {
void computeCostMinAcceleration(const ProblemData& pData, const VectorX& Ts,
const double /*T*/, const T_time& timeArray,
MatrixXX& H, VectorX& g) {
double t_total = 0.;
for (int i = 0; i < Ts.size(); ++i) t_total += Ts[i];
std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints<point3_t>(pData, t_total, timeArray);
std::vector<coefs_t> wps_ddc =
computeDiscretizedAccelerationWaypoints<point3_t>(pData, t_total,
timeArray);
// cost : x' H x + 2 x g'
H.block<3, 3>(0, 0) = Matrix3::Zero();
g.head<3>() = Vector3::Zero();
for (size_t i = 0; i < wps_ddc.size(); ++i) {
H.block<3, 3>(0, 0) += Matrix3::Identity() * wps_ddc[i].first * wps_ddc[i].first;
H.block<3, 3>(0, 0) +=
Matrix3::Identity() * wps_ddc[i].first * wps_ddc[i].first;
g.head<3>() += wps_ddc[i].first * wps_ddc[i].second;
}
}
typedef void (*costCompute)(const ProblemData&, const VectorX&, const double, const T_time&, MatrixXX&, VectorX&);
typedef void (*costCompute)(const ProblemData&, const VectorX&, const double,
const T_time&, MatrixXX&, VectorX&);
typedef std::map<CostFunction, costCompute> T_costCompute;
static const T_costCompute costs = boost::assign::map_list_of(ACCELERATION, computeCostMinAcceleration)(
DISTANCE_TRAVELED, computeCostMidPoint)(TARGET_END_VELOCITY, computeCostEndVelocity);
static const T_costCompute costs =
boost::assign::map_list_of(ACCELERATION, computeCostMinAcceleration)(
DISTANCE_TRAVELED, computeCostMidPoint)(TARGET_END_VELOCITY,
computeCostEndVelocity);
void genCostFunction(const ProblemData& pData, const VectorX& Ts, const double T, const T_time& timeArray, MatrixXX& H,
void genCostFunction(const ProblemData& pData, const VectorX& Ts,
const double T, const T_time& timeArray, MatrixXX& H,
VectorX& g) {
T_costCompute::const_iterator cit = costs.find(pData.costFunction_);
if (cit != costs.end())
......
src/eiquadprog-fast.cpp
View file @ 626b8c65
......@@ -16,6 +16,7 @@
//
#include "hpp/bezier-com-traj/solver/eiquadprog-fast.hpp"
#include <iostream>
namespace tsid {
namespace solvers {
......@@ -38,7 +39,8 @@ inline Scalar distance(Scalar a, Scalar b) {
EiquadprogFast::EiquadprogFast() {
m_maxIter = DEFAULT_MAX_ITER;
q = 0; // size of the active set A (containing the indices of the active constraints)
q = 0; // size of the active set A (containing the indices of the active
// constraints)
is_inverse_provided_ = false;
m_nVars = 0;
m_nEqCon = 0;
......@@ -72,7 +74,8 @@ void EiquadprogFast::reset(int nVars, int nEqCon, int nIneqCon) {
#endif
}
bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm) {
bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d,
int& iq, double& R_norm) {
long int nVars = J.rows();
#ifdef TRACE_SOLVER
std::cout << "Add constraint " << iq << '/';
......@@ -90,13 +93,12 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int&
decreasing j */
for (j = d.size() - 1; j >= iq + 1; j--) {
/* The Givens rotation is done with the matrix (cc cs, cs -cc).
If cc is one, then element (j) of d is zero compared with element
(j - 1). Hence we don't have to do anything.
If cc is zero, then we just have to switch column (j) and column (j - 1)
of J. Since we only switch columns in J, we have to be careful how we
update d depending on the sign of gs.
Otherwise we have to apply the Givens rotation to these columns.
The i - 1 element of d has to be updated to h. */
If cc is one, then element (j) of d is zero compared with
element (j - 1). Hence we don't have to do anything. If cc is zero, then
we just have to switch column (j) and column (j - 1) of J. Since we only
switch columns in J, we have to be careful how we update d depending on
the sign of gs. Otherwise we have to apply the Givens rotation to these
columns. The i - 1 element of d has to be updated to h. */
cc = d(j - 1);
ss = d(j);
h = distance(cc, ss);
......@@ -113,7 +115,8 @@ bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int&
xny = ss / (1.0 + cc);
// #define OPTIMIZE_ADD_CONSTRAINT
#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than the original
#ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than
// the original
T1 = J.col(j - 1);
cc_ss(0) = cc;
cc_ss(1) = ss;
......@@ -146,7 +149,8 @@ into column iq - 1 of R
return true;
}
void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u, int nEqCon, int& iq,
void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
VectorXd& u, int nEqCon, int& iq,
int l) {
const long int nVars = R.rows();
#ifdef TRACE_SOLVER
......@@ -223,14 +227,15 @@ void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) {
// std::cout << name << std::endl << x << std::endl;
}
EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
VectorXd& x) {
EiquadprogFast_status EiquadprogFast::solve_quadprog(
const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE,
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) {
const int nVars = (int)g0.size();
const int nEqCon = (int)ce0.size();
const int nIneqCon = (int)ci0.size();
if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon) reset(nVars, nEqCon, nIneqCon);
if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon)
reset(nVars, nEqCon, nIneqCon);
assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars);
assert(g0.size() == m_nVars);
......@@ -272,7 +277,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
R.setZero(nVars, nVars);
R_norm = 1.0;
/* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
/* compute the inverse of the factorized matrix Hess^-1, this is the initial
* value for H */
// m_J = L^-T
if (!is_inverse_provided_) {
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
......@@ -293,9 +299,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
/* c1 * c2 is an estimate for cond(Hess) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 x
* this is a feasible point in the dual space
* x = Hess^-1 * g0
* Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0
* x this is a feasible point in the dual space x = Hess^-1 * g0
*/
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
if (is_inverse_provided_) {
......@@ -336,10 +341,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
print_vector("d", d, nVars);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
/* compute full step length t2: i.e., the minimum step in primal space s.t.
the contraint becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
......@@ -366,7 +372,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
for (i = 0; i < nIneqCon; i++) iai(i) = i;
#ifdef USE_WARM_START
// DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
// DEBUG_STREAM("Gonna warm start using previous active
// set:\n"<<A.transpose()<<"\n")
for (i = nEqCon; i < q; i++) {
iai(i - nEqCon) = -1;
ip = A(i);
......@@ -375,10 +382,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog(const MatrixXd& Hess, const
update_z(z, m_J, d, iq);
update_r(R, r, d, iq);
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
/* compute full step length t2: i.e., the minimum step in primal space s.t.
the contraint becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
else
DEBUG_STREAM("[WARM START] z=0\n")
......@@ -445,10 +453,12 @@ l1:
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
if (std::abs(psi) <= nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
if (std::abs(psi) <=
nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
/* numerically there are not infeasibilities anymore */
q = iq;
// DEBUG_STREAM("Optimal active set:\n"<<A.head(iq).transpose()<<"\n\n")
// DEBUG_STREAM("Optimal active
// set:\n"<<A.head(iq).transpose()<<"\n\n")
return EIQUADPROG_FAST_OPTIMAL;
}
......@@ -459,7 +469,8 @@ l1:
l2: /* Step 2: check for feasibility and determine a new S-pair */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
// find constraint with highest violation (what about normalizing constraints?)
// find constraint with highest violation (what about normalizing
// constraints?)
for (i = 0; i < nIneqCon; i++) {
if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
ss = s(i);
......@@ -489,7 +500,8 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
l2a: /* Step 2a: determine step direction */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
/* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
/* compute z = H np: the step direction in the primal space (through m_J, see
* the paper) */
compute_d(d, m_J, np);
// update_z(z, m_J, d, iq);
if (iq >= nVars) {
......@@ -498,7 +510,8 @@ l2a: /* Step 2a: determine step direction */
} else {
update_z(z, m_J, d, iq);
}
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
/* compute N* np (if q > 0): the negative of the step direction in the dual
* space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cout << "Step direction z" << std::endl;
......@@ -513,7 +526,8 @@ l2a: /* Step 2a: determine step direction */
/* Step 2b: compute step length */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
/* Compute t1: partial step length (maximum step in dual space without
* violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */
// l: index of constraint to drop (maybe)
......@@ -524,8 +538,10 @@ l2a: /* Step 2a: determine step direction */
l = A(k);
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
/* Compute t2: full step length (minimum step in primal space such that the
* constraint ip becomes feasible */
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
......@@ -533,7 +549,8 @@ l2a: /* Step 2a: determine step direction */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
<< ") ";
#endif
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
......@@ -635,14 +652,15 @@ double trace_sparse(const EiquadprogFast::SpMat& Hess) {
return sum;
}
EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast::SpMat& Hess, const VectorXd& g0,
const MatrixXd& CE, const VectorXd& ce0,
const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) {
EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(
const EiquadprogFast::SpMat& Hess, const VectorXd& g0, const MatrixXd& CE,
const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) {
const int nVars = (int)g0.size();
const int nEqCon = (int)ce0.size();
const int nIneqCon = (int)ci0.size();
if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon) reset(nVars, nEqCon, nIneqCon);
if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon)
reset(nVars, nEqCon, nIneqCon);
assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars);
assert(g0.size() == m_nVars);
......@@ -681,7 +699,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
R.setZero(nVars, nVars);
R_norm = 1.0;
/* compute the inverse of the factorized matrix Hess^-1, this is the initial value for H */
/* compute the inverse of the factorized matrix Hess^-1, this is the initial
* value for H */
// m_J = L^-T
if (!is_inverse_provided_) {
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE);
......@@ -702,9 +721,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
/* c1 * c2 is an estimate for cond(Hess) */
/*
* Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 x
* this is a feasible point in the dual space
* x = Hess^-1 * g0
* Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0
* x this is a feasible point in the dual space x = Hess^-1 * g0
*/
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM);
if (is_inverse_provided_) {
......@@ -745,10 +763,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
print_vector("d", d, nVars);
#endif
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
/* compute full step length t2: i.e., the minimum step in primal space s.t.
the contraint becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
x += t2 * z;
......@@ -775,7 +794,8 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
for (i = 0; i < nIneqCon; i++) iai(i) = i;
#ifdef USE_WARM_START
// DEBUG_STREAM("Gonna warm start using previous active set:\n"<<A.transpose()<<"\n")
// DEBUG_STREAM("Gonna warm start using previous active
// set:\n"<<A.transpose()<<"\n")
for (i = nEqCon; i < q; i++) {
iai(i - nEqCon) = -1;
ip = A(i);
......@@ -784,10 +804,11 @@ EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse(const EiquadprogFast
update_z(z, m_J, d, iq);
update_r(R, r, d, iq);
/* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint
becomes feasible */
/* compute full step length t2: i.e., the minimum step in primal space s.t.
the contraint becomes feasible */
t2 = 0.0;
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = (-np.dot(x) - ci0(ip)) / z.dot(np);
else
DEBUG_STREAM("[WARM START] z=0\n")
......@@ -854,10 +875,12 @@ l1:
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1);
if (std::abs(psi) <= nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
if (std::abs(psi) <=
nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
/* numerically there are not infeasibilities anymore */
q = iq;
// DEBUG_STREAM("Optimal active set:\n"<<A.head(iq).transpose()<<"\n\n")
// DEBUG_STREAM("Optimal active
// set:\n"<<A.head(iq).transpose()<<"\n\n")
return EIQUADPROG_FAST_OPTIMAL;
}
......@@ -868,7 +891,8 @@ l1:
l2: /* Step 2: check for feasibility and determine a new S-pair */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2);
// find constraint with highest violation (what about normalizing constraints?)
// find constraint with highest violation (what about normalizing
// constraints?)
for (i = 0; i < nIneqCon; i++) {
if (s(i) < ss && iai(i) != -1 && iaexcl(i)) {
ss = s(i);
......@@ -898,7 +922,8 @@ l2: /* Step 2: check for feasibility and determine a new S-pair */
l2a: /* Step 2a: determine step direction */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A);
/* compute z = H np: the step direction in the primal space (through m_J, see the paper) */
/* compute z = H np: the step direction in the primal space (through m_J, see
* the paper) */
compute_d(d, m_J, np);
// update_z(z, m_J, d, iq);
if (iq >= nVars) {
......@@ -907,7 +932,8 @@ l2a: /* Step 2a: determine step direction */
} else {
update_z(z, m_J, d, iq);
}
/* compute N* np (if q > 0): the negative of the step direction in the dual space */
/* compute N* np (if q > 0): the negative of the step direction in the dual
* space */
update_r(R, r, d, iq);
#ifdef TRACE_SOLVER
std::cout << "Step direction z" << std::endl;
......@@ -922,7 +948,8 @@ l2a: /* Step 2a: determine step direction */
/* Step 2b: compute step length */
START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
l = 0;
/* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
/* Compute t1: partial step length (maximum step in dual space without
* violating dual feasibility */
t1 = inf; /* +inf */
/* find the index l s.t. it reaches the minimum of u+(x) / r */
// l: index of constraint to drop (maybe)
......@@ -933,8 +960,10 @@ l2a: /* Step 2a: determine step direction */
l = A(k);
}
}
/* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
/* Compute t2: full step length (minimum step in primal space such that the
* constraint ip becomes feasible */
if (std::abs(z.dot(z)) >
std::numeric_limits<double>::epsilon()) // i.e. z != 0
t2 = -s(ip) / z.dot(np);
else
t2 = inf; /* +inf */
......@@ -942,7 +971,8 @@ l2a: /* Step 2a: determine step direction */
/* the step is chosen as the minimum of t1 and t2 */
t = std::min(t1, t2);
#ifdef TRACE_SOLVER
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
<< ") ";
#endif
STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B);
......
src/glpk-wrapper.cpp
View file @ 626b8c65
......@@ -16,7 +16,9 @@
//
#include "hpp/bezier-com-traj/solver/glpk-wrapper.hpp"
#include <glpk.h>
#include <iostream>
namespace solvers {
......@@ -34,20 +36,23 @@ int getType(const VectorXd& mib, const VectorXd& mab, const int i) {
return type;
}
int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0,
solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds, VectorXd& x, double& cost) {
int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
const MatrixXd& CI, const VectorXd& ci0,
solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds,
VectorXd& x, double& cost) {
glp_smcp opts;
glp_init_smcp(&opts);
opts.msg_lev = GLP_MSG_OFF;
glp_prob* lp;
int ia[1 + 20000]; // Row indices of each element
int ja[1 + 20000]; // column indices of each element
double ar[1 + 20000]; // numerical values of corresponding elements
lp = glp_create_prob(); // creates a problem object
glp_set_prob_name(lp, "sample"); // assigns a symbolic name to the problem object
glp_set_obj_dir(lp, GLP_MIN); // calls the routine glp_set_obj_dir to set the
// omptimization direction flag,
// where GLP_MAX means maximization
int ia[1 + 20000]; // Row indices of each element
int ja[1 + 20000]; // column indices of each element
double ar[1 + 20000]; // numerical values of corresponding elements
lp = glp_create_prob(); // creates a problem object
glp_set_prob_name(lp,
"sample"); // assigns a symbolic name to the problem object
glp_set_obj_dir(lp, GLP_MIN); // calls the routine glp_set_obj_dir to set the
// omptimization direction flag,
// where GLP_MAX means maximization
// ROWS
const int numEqConstraints = (int)(CE.rows());
......@@ -62,7 +67,8 @@ int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const
glp_set_row_bnds(lp, idrow, GLP_UP, 0.0, ci0(i));
for (int j = 0; j < xsize; ++j, ++idcol) {
if (CI(i, j) != 0.) {
ia[idConsMat] = idrow, ja[idConsMat] = idcol, ar[idConsMat] = CI(i, j); /* a[1,1] = 1 */
ia[idConsMat] = idrow, ja[idConsMat] = idcol,
ar[idConsMat] = CI(i, j); /* a[1,1] = 1 */
++idConsMat;
}
}
......@@ -72,7 +78,8 @@ int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const
glp_set_row_bnds(lp, idrow, GLP_FX, ce0(i), ce0(i));
for (int j = 0; j < xsize; ++j, ++idcol) {
if (CE(i, j) != 0.) {
ia[idConsMat] = idrow, ja[idConsMat] = idcol, ar[idConsMat] = CE(i, j); /* a[1,1] = 1 */
ia[idConsMat] = idrow, ja[idConsMat] = idcol,
ar[idConsMat] = CE(i, j); /* a[1,1] = 1 */
++idConsMat;
}
}
......@@ -81,8 +88,10 @@ int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const
// COLUMNS
glp_add_cols(lp, xsize);
VectorXd miB = minBounds.size() > 0 ? minBounds : VectorXd::Ones(xsize) * UNBOUNDED_DOWN;
VectorXd maB = maxBounds.size() > 0 ? maxBounds : VectorXd::Ones(xsize) * UNBOUNDED_UP;
VectorXd miB =
minBounds.size() > 0 ? minBounds : VectorXd::Ones(xsize) * UNBOUNDED_DOWN;
VectorXd maB =
maxBounds.size() > 0 ? maxBounds : VectorXd::Ones(xsize) * UNBOUNDED_UP;
for (int i = 0; i < xsize; ++i, ++idcol) {
glp_set_col_bnds(lp, idcol, getType(miB, maB, i), miB(i), maB(i));
glp_set_obj_coef(lp, idcol, g0(i));
......@@ -92,11 +101,13 @@ int solveglpk(const VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const
int res = glp_simplex(lp, &opts);
res = glp_get_status(lp);
if (res == GLP_OPT) {
cost = glp_get_obj_val(lp); // obtains a computed value of the objective function
cost = glp_get_obj_val(
lp); // obtains a computed value of the objective function
idrow = 1;
for (int i = 0; i < xsize; ++i, ++idrow) x(i) = glp_get_col_prim(lp, idrow);
}
glp_delete_prob(lp); // calls the routine glp_delete_prob, which frees all the memory
glp_delete_prob(
lp); // calls the routine glp_delete_prob, which frees all the memory
glp_free_env();
return res;
}
......
src/solve_0_step.cpp
View file @ 626b8c65
......@@ -3,14 +3,15 @@
* Author: Steve Tonneau
*/
#include <hpp/bezier-com-traj/common_solve_methods.hh>
#include <hpp/bezier-com-traj/solve.hh>
#include <hpp/bezier-com-traj/utils.hh>
#include <hpp/bezier-com-traj/common_solve_methods.hh>
using namespace bezier_com_traj;
namespace bezier_com_traj {
waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X, const Matrix3& /*p1X*/,
const Matrix3& /*gX*/, const double alpha) {
waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X,
const Matrix3& /*p1X*/, const Matrix3& /*gX*/,
const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3, 3>(0, 0) = 6 * alpha * Matrix3::Identity();
w.first.block<3, 3>(3, 0) = 6 * alpha * p0X;
......@@ -19,7 +20,8 @@ waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X, c
return w;
}
waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/,
const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
const Matrix3& gX, const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3, 3>(0, 0) = 3 * alpha * Matrix3::Identity();
......@@ -29,7 +31,8 @@ waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/, const Matrix3& /*
return w;
}
waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g,
const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
const Matrix3& gX, const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
// w.first.block<3,3>(0,0) = 0;
......@@ -39,7 +42,8 @@ waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*
return w;
}
waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g,
const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
const Matrix3& /*gX*/, const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3, 3>(0, 0) = -3 * alpha * Matrix3::Identity();
......@@ -49,7 +53,8 @@ waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*
return w;
}
waypoint6_t w4(point_t_tC /*p0*/, point_t_tC p1, point_t_tC g, const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
waypoint6_t w4(point_t_tC /*p0*/, point_t_tC p1, point_t_tC g,
const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
const Matrix3& /*gX*/, const double alpha) {
waypoint6_t w = initwp<waypoint6_t>();
w.first.block<3, 3>(0, 0) = -6 * alpha * Matrix3::Identity();
......@@ -103,9 +108,12 @@ waypoint6_t u4(point_t_tC /*l0*/, const double /*alpha*/) {
return w;
}
int computeNumSteps(const double T, const double timeStep) { return timeStep > 0. ? int(T / timeStep) : -1; }
int computeNumSteps(const double T, const double timeStep) {
return timeStep > 0. ? int(T / timeStep) : -1;
}
std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, point_t_tC g, const double T,
std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0,
point_t_tC g, const double T,
const double timeStep) {
int numSteps = computeNumSteps(T, timeStep);
static const double n = 3.; // degree
......@@ -127,7 +135,8 @@ std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0, poin
return wps;
}
std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(point_t_tC l0, const double T, const double timeStep) {
std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(
point_t_tC l0, const double T, const double timeStep) {
int numSteps = computeNumSteps(T, timeStep);
double alpha = 1. / (T);
std::vector<waypoint6_t> wps;
......@@ -145,23 +154,28 @@ std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(point_t_tC l0, const
/* compute the inequality methods that determine the 6D bezier curve w(t)
as a function of a variable waypoint for the 3D COM trajectory.
The initial curve is of degree 3 (init pos and velocity, 0 velocity constraints + one free variable).
The 6d curve is of degree 2*n-2 = 4, thus 5 control points are to be computed.
Each control point produces a 6 * 3 inequality matrix wix, and a 6 *1 column right member wsi.
Premultiplying it by H gives mH w_xi * x <= mH_wsi where m is the mass
Stacking all of these results in a big inequality matrix A and a column vector x that determines the constraints
On the 6d curves, Ain x <= Aub
The initial curve is of degree 3 (init pos and velocity, 0 velocity constraints
+ one free variable). The 6d curve is of degree 2*n-2 = 4, thus 5 control points
are to be computed. Each control point produces a 6 * 3 inequality matrix wix,
and a 6 *1 column right member wsi. Premultiplying it by H gives mH w_xi * x <=
mH_wsi where m is the mass Stacking all of these results in a big inequality
matrix A and a column vector x that determines the constraints On the 6d curves,
Ain x <= Aub
*/
std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(const ContactData& cData, point_t_tC c0, point_t_tC dc0,
point_t_tC l0, const bool useAngMomentum,
const double T, const double timeStep, bool& fail) {
std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
const ContactData& cData, point_t_tC c0, point_t_tC dc0, point_t_tC l0,
const bool useAngMomentum, const double T, const double timeStep,
bool& fail) {
std::vector<waypoint6_t> wps, wpL;
wps = ComputeAllWaypoints(c0, dc0, cData.contactPhase_->m_gravity, T, timeStep);
wps =
ComputeAllWaypoints(c0, dc0, cData.contactPhase_->m_gravity, T, timeStep);
if (useAngMomentum) wpL = ComputeAllWaypointsAngularMomentum(l0, T, timeStep);
return compute6dControlPointInequalities(cData, wps, wpL, useAngMomentum, fail);
return compute6dControlPointInequalities(cData, wps, wpL, useAngMomentum,
fail);
}
std::pair<MatrixXX, VectorX> computeCostFunction(point_t_tC p0, point_t_tC l0, const bool useAngMomentum) {
std::pair<MatrixXX, VectorX> computeCostFunction(point_t_tC p0, point_t_tC l0,
const bool useAngMomentum) {
int dimPb = useAngMomentum ? 6 : 3;
std::pair<MatrixXX, VectorX> res;
res.first = MatrixXX::Zero(dimPb, dimPb);
......@@ -185,12 +199,14 @@ std::pair<MatrixXX, VectorX> computeCostFunction(point_t_tC p0, point_t_tC l0, c
void computeRealCost(const ProblemData& pData, ResultData& resData) {
if (pData.useAngularMomentum_) {
Vector3 xL = resData.x.tail(3);
resData.cost_ = (1. / 5.) * (9. * pData.l0_.dot(pData.l0_) - 9. * pData.l0_.dot(xL) + 6. * xL.dot(xL));
resData.cost_ = (1. / 5.) * (9. * pData.l0_.dot(pData.l0_) -
9. * pData.l0_.dot(xL) + 6. * xL.dot(xL));
} else
resData.cost_ = (pData.c0_ - resData.x).norm();
}
void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts, ResultDataCOMTraj& res) {
void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts,
ResultDataCOMTraj& res) {
std::vector<Vector3> wps;
wps.push_back(pData.c0_);
wps.push_back(pData.dc0_ * Ts[0] / 3 + pData.c0_);
......@@ -199,7 +215,8 @@ void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts, Resu
res.c_of_t_ = bezier_t(wps.begin(), wps.end(), 0., Ts[0]);
}
void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts, ResultDataCOMTraj& res) {
void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts,
ResultDataCOMTraj& res) {
if (pData.useAngularMomentum_) {
std::vector<Vector3> wps;
wps.push_back(3 * (res.x.tail(3) - pData.l0_));
......@@ -211,17 +228,20 @@ void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts, Res
}
// no angular momentum for now
ResultDataCOMTraj solve0step(const ProblemData& pData, const std::vector<double>& Ts, const double timeStep) {
ResultDataCOMTraj solve0step(const ProblemData& pData,
const std::vector<double>& Ts,
const double timeStep) {
assert(pData.representation_ == DOUBLE_DESCRIPTION);
assert(pData.contacts_.size() == 1);
assert(Ts.size() == pData.contacts_.size());
bool fail = true;
std::pair<MatrixXX, VectorX> Ab =
compute6dControlPointInequalities(pData.contacts_.front(), pData.c0_, pData.dc0_, pData.l0_,
pData.useAngularMomentum_, Ts.front(), timeStep, fail);
std::pair<MatrixXX, VectorX> Ab = compute6dControlPointInequalities(
pData.contacts_.front(), pData.c0_, pData.dc0_, pData.l0_,
pData.useAngularMomentum_, Ts.front(), timeStep, fail);
ResultDataCOMTraj res;
if (fail) return res;
std::pair<MatrixXX, VectorX> Hg = computeCostFunction(pData.c0_, pData.l0_, pData.useAngularMomentum_);
std::pair<MatrixXX, VectorX> Hg =
computeCostFunction(pData.c0_, pData.l0_, pData.useAngularMomentum_);
int dimPb = pData.useAngularMomentum_ ? 6 : 3;
VectorX init = VectorX(dimPb);
init.head(3) = pData.c0_;
......
src/solver-abstract.cpp
View file @ 626b8c65
......@@ -17,12 +17,12 @@
#include "hpp/bezier-com-traj/solver/solver-abstract.hpp"
#ifdef USE_GLPK_SOLVER
#include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
#include <glpk.h>
#endif
#include <hpp/bezier-com-traj/solver/eiquadprog-fast.hpp>
#include <hpp/bezier-com-traj/solver/glpk-wrapper.hpp>
#endif
#include <Eigen/Sparse>
#include <hpp/bezier-com-traj/solver/eiquadprog-fast.hpp>
#include <stdexcept>
namespace solvers {
......@@ -38,37 +38,41 @@ typedef Eigen::SparseVector<double> SpVec;
typedef Eigen::SparseVector<int> SpVeci;
namespace {
void addConstraintMinBoundQuadProg(solvers::Cref_vectorX minBounds, std::pair<MatrixXd, VectorXd>& data) {
void addConstraintMinBoundQuadProg(solvers::Cref_vectorX minBounds,
std::pair<MatrixXd, VectorXd>& data) {
if (minBounds.size() == 0) return;
MatrixXd& res = data.first;
VectorXd& resv = data.second;
MatrixXd D(res.rows() + res.cols(), res.cols());
VectorXd d(resv.rows() + res.cols());
D.block(0, 0, res.rows(), res.cols()) = res;
D.block(res.rows(), 0, res.cols(), res.cols()) = (-1.) * MatrixXd::Identity(res.cols(), res.cols());
D.block(res.rows(), 0, res.cols(), res.cols()) =
(-1.) * MatrixXd::Identity(res.cols(), res.cols());
d.head(resv.size()) = resv;
d.tail(res.cols()) = -minBounds;
data.first = D;
data.second = d;
}
void addConstraintMaxBoundQuadProg(solvers::Cref_vectorX maxBounds, std::pair<MatrixXd, VectorXd>& data) {
void addConstraintMaxBoundQuadProg(solvers::Cref_vectorX maxBounds,
std::pair<MatrixXd, VectorXd>& data) {
if (maxBounds.size() == 0) return;
MatrixXd& res = data.first;
VectorXd& resv = data.second;
MatrixXd D(res.rows() + res.cols() - 3, res.cols());
VectorXd d(resv.rows() + res.cols());
D.block(0, 0, res.rows(), res.cols()) = res;
D.block(res.rows(), 0, res.cols(), res.cols()) = MatrixXd::Identity(res.cols(), res.cols());
D.block(res.rows(), 0, res.cols(), res.cols()) =
MatrixXd::Identity(res.cols(), res.cols());
d.head(resv.size()) = resv;
d.tail(res.cols()) = maxBounds;
data.first = D;
data.second = d;
}
std::pair<MatrixXd, VectorXd> addBoundaryConstraintsQuadProg(solvers::Cref_vectorX minBounds,
solvers::Cref_vectorX maxBounds, const MatrixXd& CI,
const VectorXd& ci0) {
std::pair<MatrixXd, VectorXd> addBoundaryConstraintsQuadProg(
solvers::Cref_vectorX minBounds, solvers::Cref_vectorX maxBounds,
const MatrixXd& CI, const VectorXd& ci0) {
std::pair<MatrixXd, VectorXd> data;
data.first = CI;
data.second = ci0;
......@@ -78,8 +82,9 @@ std::pair<MatrixXd, VectorXd> addBoundaryConstraintsQuadProg(solvers::Cref_vecto
}
} // namespace
ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const VectorXd& d, const MatrixXd& Hess,
const VectorXd& g, const VectorXd& initGuess, solvers::Cref_vectorX minBounds,
ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D,
const VectorXd& d, const MatrixXd& Hess, const VectorXd& g,
const VectorXd& initGuess, solvers::Cref_vectorX minBounds,
solvers::Cref_vectorX maxBounds, const SolverType solver) {
assert(!(is_nan(A)));
assert(!(is_nan(b)));
......@@ -101,12 +106,15 @@ ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const
// case SOLVER_QUADPROG_SPARSE:
{
assert(!(is_nan(Hess)));
std::pair<MatrixXd, VectorXd> CIp = addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
std::pair<MatrixXd, VectorXd> CIp =
addBoundaryConstraintsQuadProg(minBounds, maxBounds, A, b);
VectorXd ce0 = -d;
tsid::solvers::EiquadprogFast QPsolver = tsid::solvers::EiquadprogFast();
tsid::solvers::EiquadprogFast QPsolver =
tsid::solvers::EiquadprogFast();
tsid::solvers::EiquadprogFast_status status;
// if(solver == SOLVER_QUADPROG)
status = QPsolver.solve_quadprog(Hess, g, D, ce0, -CIp.first, CIp.second, res.x);
status = QPsolver.solve_quadprog(Hess, g, D, ce0, -CIp.first,
CIp.second, res.x);
/* else
{
SpMat Hsp = Hess.sparseView();
......@@ -118,7 +126,8 @@ ResultData solve(const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const
}
#ifdef USE_GLPK_SOLVER
case SOLVER_GLPK: {
res.success_ = (solvers::solveglpk(g, D, d, A, b, minBounds, maxBounds, res.x, res.cost_) == GLP_OPT);
res.success_ = (solvers::solveglpk(g, D, d, A, b, minBounds, maxBounds,
res.x, res.cost_) == GLP_OPT);
return res;
}
#endif
......
src/utils.cpp
View file @ 626b8c65
......@@ -14,20 +14,26 @@ waypoint_t initwp(const size_t rows, const size_t cols) {
}
waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2) {
if (w1.second.rows() != w2.second.rows() || w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
if (w1.second.rows() != w2.second.rows() ||
w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
throw std::runtime_error("You cannot add waypoint_t of different size.");
return waypoint_t(w1.first + w2.first, w1.second + w2.second);
}
waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2) {
if (w1.second.rows() != w2.second.rows() || w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
if (w1.second.rows() != w2.second.rows() ||
w1.first.rows() != w2.first.rows() || w1.first.cols() != w2.first.cols())
throw std::runtime_error("You cannot add waypoint_t of different size.");
return waypoint_t(w1.first - w2.first, w1.second - w2.second);
}
waypoint_t operator*(const double k, const waypoint_t& w) { return waypoint_t(k * w.first, k * w.second); }
waypoint_t operator*(const double k, const waypoint_t& w) {
return waypoint_t(k * w.first, k * w.second);
}
waypoint_t operator*(const waypoint_t& w, const double k) { return waypoint_t(k * w.first, k * w.second); }
waypoint_t operator*(const waypoint_t& w, const double k) {
return waypoint_t(k * w.first, k * w.second);
}
template <>
waypoint9_t initwp<waypoint9_t>() {
......@@ -64,13 +70,16 @@ Matrix3 skew(point_t_tC x) {
return res;
}
std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(const unsigned int degree) {
std::vector<ndcurves::Bern<double> > ComputeBersteinPolynoms(
const unsigned int degree) {
std::vector<ndcurves::Bern<double> > res;
for (unsigned int i = 0; i <= (unsigned int)degree; ++i) res.push_back(ndcurves::Bern<double>(degree, i));
for (unsigned int i = 0; i <= (unsigned int)degree; ++i)
res.push_back(ndcurves::Bern<double>(degree, i));
return res;
}
T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned int pointsPerPhase) {
T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings,
const unsigned int pointsPerPhase) {
T_time timeArray;
double t = 0;
double t_total = phaseTimings.sum();
......@@ -83,17 +92,20 @@ T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings, const unsigned i
}
}
timeArray.pop_back();
timeArray.push_back(std::make_pair(t_total, phaseTimings.size() - 1)); // avoid numerical errors
timeArray.push_back(std::make_pair(
t_total, phaseTimings.size() - 1)); // avoid numerical errors
return timeArray;
}
T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep) {
T_time computeDiscretizedTime(const VectorX& phaseTimings,
const double timeStep) {
T_time timeArray;
double t = 0;
double currentTiming = 0.;
for (int i = 0; i < phaseTimings.size(); ++i) {
assert(timeStep * 2 <= phaseTimings[i] &&
"Time step too high: should allow to contain at least 2 points per phase");
"Time step too high: should allow to contain at least 2 points per "
"phase");
t = currentTiming;
currentTiming += phaseTimings[i];
while (t < currentTiming) {
......@@ -105,20 +117,21 @@ T_time computeDiscretizedTime(const VectorX& phaseTimings, const double timeStep
return timeArray;
}
void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint, const std::string& fileName,
bool clipZ) {
void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
const std::string& fileName, bool clipZ) {
std::ofstream file;
using std::endl;
std::string path("/local/fernbac/bench_iros18/constraints_obj/");
path.append(fileName);
file.open(path.c_str(), std::ios::out | std::ios::trunc);
file << "3 1" << endl;
file << "\t " << intPoint[0] << "\t" << intPoint[1] << "\t" << intPoint[2] << endl;
file << "\t " << intPoint[0] << "\t" << intPoint[1] << "\t" << intPoint[2]
<< endl;
file << "4" << endl;
clipZ ? file << Ab.first.rows() + 2 << endl : file << Ab.first.rows() << endl;
for (int i = 0; i < Ab.first.rows(); ++i) {
file << "\t" << Ab.first(i, 0) << "\t" << Ab.first(i, 1) << "\t" << Ab.first(i, 2) << "\t" << -Ab.second[i] - 0.001
<< endl;
file << "\t" << Ab.first(i, 0) << "\t" << Ab.first(i, 1) << "\t"
<< Ab.first(i, 2) << "\t" << -Ab.second[i] - 0.001 << endl;
}
if (clipZ) {
file << "\t" << 0 << "\t" << 0 << "\t" << 1. << "\t" << -3. << endl;
......