From 832fa50762412e8405ad41b1b2f9db15ebe971e9 Mon Sep 17 00:00:00 2001
From: t steve <pro@stevetonneau.fr>
Date: Thu, 29 Mar 2018 16:48:02 +0200
Subject: [PATCH] zero step capturability now equivalent in pierre and steve s
formulation. Keeping both at the moment for retrocompatibility
---
CMakeLists.txt | 2 +-
include/bezier-com-traj/solve.hh | 12 ++
include/bezier-com-traj/utils.hh | 30 ++--
.../waypoints/waypoints_c0_dc0_c1.hh | 13 --
.../waypoints/waypoints_c0_dc0_dc1.hh | 135 ++++++++++++++++++
.../waypoints/waypoints_c0_dc0_dc1_c1.hh | 16 ---
.../waypoints/waypoints_c0_dc0_ddc0_c1.hh | 17 ---
.../waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh | 18 ---
.../waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh | 16 ---
.../waypoints/waypoints_definition.hh | 2 -
src/CMakeLists.txt | 2 +-
src/common_solve_methods.cpp | 1 -
src/costfunction_definition.cpp | 2 +
src/solve_transition.cpp | 36 +++--
src/waypoints_definition.cpp | 28 +---
tests/test-transition.cc | 15 +-
tests/test-zero-step-capturability.cpp | 32 +++--
17 files changed, 231 insertions(+), 146 deletions(-)
create mode 100644 include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 317715e..0da4284 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,7 +63,7 @@ SET(${PROJECT_NAME}_HEADERS
include/bezier-com-traj/cost/costfunction_definition.hh
include/bezier-com-traj/waypoints/waypoints_definition.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
- include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+ include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
diff --git a/include/bezier-com-traj/solve.hh b/include/bezier-com-traj/solve.hh
index 6160594..6837b7e 100644
--- a/include/bezier-com-traj/solve.hh
+++ b/include/bezier-com-traj/solve.hh
@@ -39,6 +39,18 @@ namespace bezier_com_traj
const Vector3& init_guess,const int pointsPerPhase = 3,
const double feasability_treshold = 0.);
+
+ /**
+ * @brief solveOnestep Tries to solve the one step problem : Given two or three contact phases,
+ * an initial and final com position and velocity,
+ * try to compute the CoM trajectory (as a Bezier curve) that connect them
+ * @param pData problem Data. Should contain only two contact phases.
+ * @param Ts timelength of each contact phase. Should be the same legnth as pData.contacts
+ * @param timeStep time step used by the discretization
+ * @return ResultData a struct containing the resulting trajectory, if success is true.
+ */
+ BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj solveTransition(const ProblemData& pData, const VectorX& Ts, const double timeStep = -1);
+
} // end namespace bezier_com_traj
#endif
diff --git a/include/bezier-com-traj/utils.hh b/include/bezier-com-traj/utils.hh
index ac21f1c..45494f6 100644
--- a/include/bezier-com-traj/utils.hh
+++ b/include/bezier-com-traj/utils.hh
@@ -98,19 +98,27 @@ Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag, const dou
}
wps.push_back(x);
i++;
- if(flag & END_ACC){
- assert(flag & END_VEL && "You cannot constrain final acceleration if final velocity is not constrained.");
- wps.push_back(pi[i]);
- i++;
- }
- if(flag & END_VEL){
- assert(flag & END_POS && "You cannot constrain final velocity if final position is not constrained.");
- wps.push_back(pi[i]);
+ if(flag & (END_VEL) && !(flag & (END_POS) ))
+ {
+ wps.push_back(x);
i++;
}
- if(flag & END_POS){
- wps.push_back(pi[i]);
- i++;
+ else
+ {
+ if(flag & END_ACC){
+ assert(flag & END_VEL && "You cannot constrain final acceleration if final velocity is not constrained.");
+ wps.push_back(pi[i]);
+ i++;
+ }
+ if(flag & END_VEL){
+ assert(flag & END_POS && "You cannot constrain final velocity if final position is not constrained.");
+ wps.push_back(pi[i]);
+ i++;
+ }
+ if(flag & END_POS){
+ wps.push_back(pi[i]);
+ i++;
+ }
}
return Bezier (wps.begin(), wps.end(),T);
}
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
index 3982e23..cef126d 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
@@ -27,8 +27,6 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
// equation found with sympy
wp.first = -3.0*t3 + 3.0*t2;
wp.second = -1.0*pi[0]*t3 + 3.0*pi[0]*t2 - 3.0*pi[0]*t + 1.0*pi[0] + 3.0*pi[1]*t3 - 6.0*pi[1]*t2 + 3.0*pi[1]*t + 1.0*pi[3]*t3;
- // std::cout<<"wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -38,8 +36,6 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,do
// equation found with sympy
wp.first = (-18.0*t + 6.0)*alpha;
wp.second = (-6.0*pi[0]*t + 6.0*pi[0] + 18.0*pi[1]*t - 12.0*pi[1] + 6.0*pi[3]*t)*alpha;
- // std::cout<<"acc_wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -104,15 +100,6 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-inline coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = -12./(T*T);
- v.second = (-6.*pi[1] + 6.*pi[3])/ (T*T);
- return v;
-}
-
}
}
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
new file mode 100644
index 0000000..91ab7e0
--- /dev/null
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
@@ -0,0 +1,135 @@
+/*
+ * Copyright 2018, LAAS-CNRS
+ * Author: Pierre Fernbach
+ */
+
+#ifndef BEZIER_COM_TRAJ_C0DC0D1_H
+#define BEZIER_COM_TRAJ_C0DC0D1_H
+
+#include <bezier-com-traj/data.hh>
+
+namespace bezier_com_traj{
+namespace c0_dc0_dc1{
+
+static const ConstraintFlag flag =INIT_POS | INIT_VEL | END_VEL;
+
+/// ### EQUATION FOR CONSTRAINTS ON INIT AND FINAL POSITION AND VELOCITY (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)
+ * @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
+ */
+inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
+ coefs_t wp;
+ double t2 = t*t;
+ double t3 = t2*t;
+ // equation found with sympy
+ wp.first = -2.0*t3 + 3.0*t2;
+ wp.second = -1.0*pi[0]*t3 + 3.0*pi[0]*t2 - 3.0*pi[0]*t + 1.0*pi[0] + 3.0*pi[1]*t3 - 6.0*pi[1]*t2 + 3.0*pi[1]*t;
+ return wp;
+}
+
+inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,double T,double t){
+ coefs_t wp;
+ double alpha = 1./(T*T);
+ // equation found with sympy
+ wp.first = (-12.0*t + 6.0)*alpha;
+ wp.second = (-6.0*pi[0]*t + 6.0*pi[0] + 18.0*pi[1]*t - 12.0*pi[1])*alpha;
+ return wp;
+}
+
+
+inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T){
+ // equation for constraint on initial and final position and velocity (degree 3, 2 constant waypoints and two free (p2 = p3))
+ // first, compute the constant waypoints that only depend on pData :
+ if(pData.dc1_.norm() != 0.)
+ throw std::runtime_error("Capturability not implemented for spped different than 0");
+ std::vector<point_t> pi;
+ pi.push_back(pData.c0_); //p0
+ pi.push_back((pData.dc0_ * T / 3. )+ pData.c0_); // p1
+ pi.push_back(point_t::Zero()); // p2 = x
+ pi.push_back(point_t::Zero()); // p3 = x
+ return pi;
+}
+
+inline std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T){
+ std::vector<waypoint6_t> wps;
+ std::vector<point_t> pi = c0_dc0_dc1::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);
+ // equation of waypoints for curve w found with sympy
+ /*waypoint6_t w0 = initwp<waypoint6_t>();
+ w0.first.block<3,3>(0,0) = 6*alpha*Matrix3::Identity();
+ w0.first.block<3,3>(3,0) = 6.0*Cpi[0]*alpha;
+ w0.second.head<3>() = (6*pi[0] - 12*pi[1])*alpha;
+ w0.second.tail<3>() = 1.0*(1.0*Cg*T2*pi[0] - 12.0*Cpi[0]*pi[1])*alpha;
+ wps.push_back(w0);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ w1.first.block<3,3>(0,0) = 2.0*alpha*Matrix3::Identity();
+ w1.first.block<3,3>(3,0) = 1.0*(-4.0*Cpi[0] + 6.0*Cpi[1])*alpha;
+ w1.second.head<3>() = 1.0*(4.0*pi[0] - 6.0*pi[1])*alpha;
+ w1.second.tail<3>() = 1.0*Cg*pi[1];
+ wps.push_back(w1);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ w2.first.block<3,3>(0,0) = -2.0*alpha*Matrix3::Identity();
+ w2.first.block<3,3>(3,0) = 1.0*(1.0*Cg* - 2.0*Cpi[0])*alpha;
+ w2.second.head<3>() = 2.0*pi[0]*alpha;
+ wps.push_back(w2);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ w3.first.block<3,3>(0,0) = -6*alpha*Matrix3::Identity();
+ w3.first.block<3,3>(3,0) = 1.0*(1.0*Cg* - 6.0*Cpi[1])*alpha;
+ w3.second.head<3>() = 6*pi[1]*alpha;
+ wps.push_back(w3);
+ return wps;*/
+
+ waypoint6_t w0 = initwp<waypoint6_t>();
+ w0.first.block<3,3>(0,0) = 6*alpha*Matrix3::Identity();
+ w0.first.block<3,3>(3,0) = 6.0*Cpi[0]*alpha;
+ w0.second.head<3>() = (6*pi[0] - 12*pi[1])*alpha;
+ w0.second.tail<3>() = (-Cpi[0])*(12.0*pi[1]*alpha + g);
+ wps.push_back(w0);
+ waypoint6_t w1 = initwp<waypoint6_t>();
+ w1.first.block<3,3>(0,0) = 3*alpha*Matrix3::Identity();
+ w1.first.block<3,3>(3,0) = skew(1.5 * (3*pi[1] - pi[0]))*alpha;
+ w1.second.head<3>() = 1.5 *alpha* (3* pi[0] - 5*pi[1]);
+ w1.second.tail<3>() = (3*alpha*pi[0]).cross(-pi[1]) + 0.25 * (Cg * (3*pi[1] + pi[0]));
+ wps.push_back(w1);
+ waypoint6_t w2 = initwp<waypoint6_t>();
+ w2.first.block<3,3>(0,0) = 0*alpha*Matrix3::Identity();
+ w2.first.block<3,3>(3,0) = skew(0.5*g - 3*alpha* pi[0] + 3*alpha*pi[1]);
+ w2.second.head<3>() = 3*alpha*(pi[0] - pi[1]);
+ w2.second.tail<3>() = 0.5 * Cg*pi[1];
+ wps.push_back(w2);
+ waypoint6_t w3 = initwp<waypoint6_t>();
+ w3.first.block<3,3>(0,0) = -3*alpha*Matrix3::Identity();
+ w3.first.block<3,3>(3,0) = skew(g - 1.5 *alpha* (pi[1] + pi[0]));
+ w3.second.head<3>() = 1.5*alpha * (pi[1] + pi[0]);
+ wps.push_back(w3);
+ waypoint6_t w4 = initwp<waypoint6_t>();
+ w4.first.block<3,3>(0,0) = -6*alpha * Matrix3::Identity();
+ w4.first.block<3,3>(3,0) = skew(g - 6*alpha* pi[1]);
+ w4.second.head<3>() = 6*pi[1]*alpha;
+ wps.push_back(w4);
+ return wps;
+}
+
+inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
+ coefs_t v;
+ std::vector<point_t> pi = c0_dc0_dc1::computeConstantWaypoints(pData,T);
+ // equation found with sympy
+ v.first = 0.;
+ v.second = point3_t::Zero();
+ return v;
+}
+
+
+}
+}
+
+#endif
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
index b7b6b09..c67f383 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
@@ -27,8 +27,6 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double t){
// equation found with sympy
wp.first = ( 6.0*t4 - 12.0*t3 + 6.0*t2);
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 - 4.0*pi[3]*t4 + 4.0*pi[3]*t3 + 1.0*pi[4]*t4;
- // std::cout<<"wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -38,8 +36,6 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,do
// equation found with sympy
wp.first = (72.0*t*t - 72.0*t + 12.0)*alpha;
wp.second = (12.0*pi[0]*t*t - 24.0*pi[0]*t + 12.0*pi[0] - 48.0*pi[1]*t*t + 72.0*pi[1]*t - 24.0*pi[1] - 48.0*pi[3]*t*t + 24.0*pi[3]*t + 12.0*pi[4]*t*t)*alpha;
- // std::cout<<"acc_wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -54,9 +50,6 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
pi.push_back(point_t::Zero()); // p2 = x
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p3
pi.push_back(pData.c1_); // p4
- /* for(int i = 0 ; i < pi.size() ; ++i){
- std::cout<<" p"<<i<<" = "<<pi[i].transpose()<<std::endl;
- }*/
return pi;
}
@@ -120,15 +113,6 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-inline coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 12./(T*T);
- v.second = (-24.0*pi[3] + 12.*pi[4])/ (T*T);
- return v;
-}
-
}
}
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
index db93c04..13b914e 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
@@ -29,8 +29,6 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double 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;
- //std::cout<<"wp at t = "<<t<<std::endl;
- //std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -41,8 +39,6 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,do
// 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;
- //std::cout<<"acc_wp at t = "<<t<<std::endl;
- //std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -57,10 +53,6 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
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
- /*std::cout<<"fixed waypoints : "<<std::endl;
- for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
- std::cout<<" pi = "<<*pit<<std::endl;
- }*/
return pi;
}
@@ -121,15 +113,6 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-inline coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = -24./(T*T);
- v.second = (12.0*pi[2] + 12.*pi[4])/ (T*T);
- return v;
-}
-
}
}
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
index f4d9bf0..b86bb91 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
@@ -31,8 +31,6 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi, double 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 - 5.0*pi[4]*t5 + 5.0*pi[4]*t4 + 1.0*pi[5]*t5;
- // std::cout<<"wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -44,8 +42,6 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,do
// 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)*alpha;
- // std::cout<<"acc_wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -61,10 +57,6 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
pi.push_back(point_t::Zero()); // x
pi.push_back((-pData.dc1_ * T / n) + pData.c1_); // p4
pi.push_back(pData.c1_); // p5
- /*std::cout<<"fixed waypoints : "<<std::endl;
- for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
- std::cout<<" pi = "<<*pit<<std::endl;
- }*/
return pi;
}
@@ -139,16 +131,6 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-inline coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 20./(T*T);
- v.second = (-40.0*pi[4] + 20.*pi[5])/ (T*T);
- return v;
-}
-
-
}
}
diff --git a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
index 0da45f3..31c11e8 100644
--- a/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
@@ -32,8 +32,6 @@ inline coefs_t evaluateCurveAtTime(const std::vector<point_t>& pi,double 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;
- // std::cout<<"wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -46,8 +44,6 @@ inline coefs_t evaluateAccelerationCurveAtTime(const std::vector<point_t>& pi,do
// 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)*alpha;
- // std::cout<<"acc_wp at t = "<<t<<std::endl;
- // std::cout<<" first : "<<wp.first<<" ; second : "<<wp.second.transpose()<<std::endl;
return wp;
}
@@ -64,10 +60,6 @@ inline std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,do
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
- /*std::cout<<"fixed waypoints : "<<std::endl;
- for(std::vector<point_t>::const_iterator pit = pi.begin() ; pit != pi.end() ; ++pit){
- std::cout<<" pi = "<<*pit<<std::endl;
- }*/
return pi;
}
@@ -152,14 +144,6 @@ inline coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T){
return v;
}
-inline coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T){
- coefs_t v;
- std::vector<point_t> pi = computeConstantWaypoints(pData,T);
- // equation found with sympy
- v.first = 0.;
- v.second = (-30.0*pi[4] - 60.*pi[5] + 30.*pi[6])/ (T*T);
- return v;
-}
}
}
diff --git a/include/bezier-com-traj/waypoints/waypoints_definition.hh b/include/bezier-com-traj/waypoints/waypoints_definition.hh
index 89ac1e2..614a142 100644
--- a/include/bezier-com-traj/waypoints/waypoints_definition.hh
+++ b/include/bezier-com-traj/waypoints/waypoints_definition.hh
@@ -50,8 +50,6 @@ std::vector<point_t> computeConstantWaypoints(const ProblemData& pData,double T)
*/
std::vector<waypoint6_t> computeWwaypoints(const ProblemData& pData,double T);
-coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T);
-
coefs_t computeFinalVelocityPoint(const ProblemData& pData,double T);
}
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ab90b9b..7d12ead 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -22,7 +22,7 @@ SET(${LIBRARY_NAME}_SOURCES
${INCLUDE_DIR}/bezier-com-traj/cost/costfunction_definition.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_definition.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh
- ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
+ ${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh
${INCLUDE_DIR}/bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
diff --git a/src/common_solve_methods.cpp b/src/common_solve_methods.cpp
index d473dd7..e030a35 100644
--- a/src/common_solve_methods.cpp
+++ b/src/common_solve_methods.cpp
@@ -169,7 +169,6 @@ ResultData solve(Cref_matrixXX A, Cref_vectorX ci0, Cref_matrixXX H, Cref_vector
ResultData res;
res.success_ = (status == tsid::solvers::EIQUADPROG_FAST_OPTIMAL );
res.x = x;
- //std::cout<<"quad_prog status : "<<status<<std::endl;
if(res.success_)
{
assert (!(is_nan(x)));
diff --git a/src/costfunction_definition.cpp b/src/costfunction_definition.cpp
index 20a70cf..57e0d70 100644
--- a/src/costfunction_definition.cpp
+++ b/src/costfunction_definition.cpp
@@ -24,6 +24,8 @@ void computeCostMidPoint(const ProblemData& pData,const VectorX& /*Ts*/, const d
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");
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_);
diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index 2e41b11..b12d821 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -102,12 +102,10 @@ long int computeNumIneq(const ProblemData& pData, const VectorX& Ts, const std::
{
pData.contacts_[i].contactPhase_->getPolytopeInequalities(Hrow,h);
numStepForPhase = phaseSwitch[i]+1 - numStepsCumulated; // pointsPerPhase;
- numStepsCumulated= phaseSwitch[i]+1;
+ numStepsCumulated = phaseSwitch[i]+1;
if(i > 0 )
++numStepForPhase; // because at the switch point between phases we add the constraints of both phases.
num_stab_ineq += Hrow.rows() * numStepForPhase;
- if(i == Ts.size()-1)
- --numStepForPhase; // we don't consider kinematics constraints for the last point (because it's independant of x)
num_kin_ineq += pData.contacts_[i].kin_.rows() * numStepForPhase;
}
long int res = num_stab_ineq + num_kin_ineq;
@@ -202,9 +200,9 @@ void assignKinematicConstraints(const ProblemData& pData, MatrixXX& A, VectorX&
// 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
- if(id_step != timeArray.size()-1)
+ current_size = (int)phase.kin_.rows();
+ if(current_size > 0)
{ // we don't consider kinematics constraints for the last point (because it's independant of x)
- current_size = (int)phase.kin_.rows();
A.block(id_rows,0,current_size,3) = (phase.Kin_ * wps_c[id_step].first);
b.segment(id_rows,current_size) = phase.kin_ - (phase.Kin_*wps_c[id_step].second);
id_rows += current_size;
@@ -297,20 +295,14 @@ ResultDataCOMTraj genTraj(ResultData resQp, const ProblemData& pData, const doub
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(T);
}
return res;
}
-double computeTotalTime(const VectorX& Ts)
-{
- double T = 0;
- for(int i = 0 ; i < Ts.size() ; ++i)
- T+=Ts[i];
- return T;
-}
-
ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const Vector3& init_guess,
- const int pointsPerPhase, const double /*feasability_treshold*/){
+ const int pointsPerPhase, const double /*feasability_treshold*/)
+{
assert(Ts.size() == pData.contacts_.size());
double T = Ts.sum();
T_time timeArray = computeDiscretizedTime(Ts,pointsPerPhase);
@@ -324,5 +316,21 @@ ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const
return genTraj(resQp, pData, T);
}
+ResultDataCOMTraj solveTransition(const ProblemData& pData, const VectorX& Ts,
+ const double timeStep)
+{
+ assert(Ts.size() == pData.contacts_.size());
+ double T = Ts.sum();
+ T_time timeArray = computeDiscretizedTime(Ts,timeStep);
+ std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData,Ts,T,timeArray);
+ std::pair<MatrixXX, VectorX> Hg = genCostFunction(pData,Ts,T,timeArray);
+ VectorX x = VectorX::Zero(numCol);
+ ResultData resQp = solve(Ab,Hg, x);
+#if QHULL
+ if (resQp.success_) printQHullFile(Ab,resQp.x, "bezier_wp.txt");
+#endif
+ return genTraj(resQp, pData, T);
+}
+
} // namespace
diff --git a/src/waypoints_definition.cpp b/src/waypoints_definition.cpp
index f3eb445..dfa681b 100644
--- a/src/waypoints_definition.cpp
+++ b/src/waypoints_definition.cpp
@@ -9,6 +9,7 @@
#include <bezier-com-traj/data.hh>
#include <bezier-com-traj/waypoints/waypoints_definition.hh>
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_c1.hh>
+#include <bezier-com-traj/waypoints/waypoints_c0_dc0_dc1.hh>
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_dc1_c1.hh>
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_c1.hh>
#include <bezier-com-traj/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh>
@@ -29,6 +30,7 @@ typedef std::map<ConstraintFlag,evalCurveAtTime > T_evalCurveAtTime;
typedef T_evalCurveAtTime::const_iterator CIT_evalCurveAtTime;
static const T_evalCurveAtTime evalCurveAtTimes = boost::assign::map_list_of
(c0_dc0_c1::flag , c0_dc0_c1::evaluateCurveAtTime)
+ (c0_dc0_dc1::flag , c0_dc0_dc1::evaluateCurveAtTime)
(c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::evaluateCurveAtTime)
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::evaluateCurveAtTime)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::evaluateCurveAtTime)
@@ -57,6 +59,7 @@ typedef std::map<ConstraintFlag,evalAccCurveAtTime > T_evalAccCurveAtTime;
typedef T_evalAccCurveAtTime::const_iterator CIT_evalAccCurveAtTime;
static const T_evalAccCurveAtTime evalAccCurveAtTimes = boost::assign::map_list_of
(c0_dc0_c1::flag , c0_dc0_c1::evaluateAccelerationCurveAtTime)
+ (c0_dc0_dc1::flag , c0_dc0_dc1::evaluateAccelerationCurveAtTime)
(c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::evaluateAccelerationCurveAtTime)
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::evaluateAccelerationCurveAtTime)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::evaluateAccelerationCurveAtTime)
@@ -84,6 +87,7 @@ typedef std::map<ConstraintFlag,compConsWp > T_compConsWp;
typedef T_compConsWp::const_iterator CIT_compConsWp;
static const T_compConsWp compConsWps = boost::assign::map_list_of
(c0_dc0_c1::flag , c0_dc0_c1::computeConstantWaypoints)
+ (c0_dc0_dc1::flag , c0_dc0_dc1::computeConstantWaypoints)
(c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeConstantWaypoints)
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeConstantWaypoints)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeConstantWaypoints)
@@ -111,6 +115,7 @@ typedef std::map<ConstraintFlag,compWp > T_compWp;
typedef T_compWp::const_iterator CIT_compWp;
static const T_compWp compWps = boost::assign::map_list_of
(c0_dc0_c1::flag , c0_dc0_c1::computeWwaypoints)
+ (c0_dc0_dc1::flag , c0_dc0_dc1::computeWwaypoints)
(c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeWwaypoints)
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeWwaypoints)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeWwaypoints)
@@ -134,33 +139,12 @@ static const T_compWp compWps = boost::assign::map_list_of
}
}
-typedef coefs_t (*compFinalAccP) (const ProblemData& pData,double T);
-typedef std::map<ConstraintFlag,compFinalAccP > T_compFinalAccP;
-typedef T_compFinalAccP::const_iterator CIT_compFinalAccP;
-static const T_compFinalAccP compFinalAccPs = boost::assign::map_list_of
- (c0_dc0_c1::flag , c0_dc0_c1::computeFinalAccelerationPoint)
- (c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeFinalAccelerationPoint)
- (c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeFinalAccelerationPoint)
- (c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeFinalAccelerationPoint)
- (c0_dc0_ddc0_ddc1_dc1_c1::flag , c0_dc0_ddc0_ddc1_dc1_c1::computeFinalAccelerationPoint);
-
- coefs_t computeFinalAccelerationPoint(const ProblemData& pData,double T)
-{
- CIT_compFinalAccP cit = compFinalAccPs.find(pData.constraints_.flag_);
- if(cit != compFinalAccPs.end())
- return cit->second(pData,T);
- else
- {
- std::cout<<"Current constraints set are not implemented"<<std::endl;
- throw std::runtime_error("Current constraints set are not implemented");
- }
-}
-
typedef coefs_t (*compFinalVelP) (const ProblemData& pData,double T);
typedef std::map<ConstraintFlag,compFinalVelP > T_compFinalVelP;
typedef T_compFinalVelP::const_iterator CIT_compFinalVelP;
static const T_compFinalVelP compFinalVelPs = boost::assign::map_list_of
(c0_dc0_c1::flag , c0_dc0_c1::computeFinalVelocityPoint)
+ (c0_dc0_dc1::flag , c0_dc0_dc1::computeFinalVelocityPoint)
(c0_dc0_dc1_c1::flag , c0_dc0_dc1_c1::computeFinalVelocityPoint)
(c0_dc0_ddc0_c1::flag , c0_dc0_ddc0_c1::computeFinalVelocityPoint)
(c0_dc0_ddc0_dc1_c1::flag , c0_dc0_ddc0_dc1_c1::computeFinalVelocityPoint)
diff --git a/tests/test-transition.cc b/tests/test-transition.cc
index a7ef10b..0950c90 100644
--- a/tests/test-transition.cc
+++ b/tests/test-transition.cc
@@ -198,7 +198,7 @@ bezier_com_traj::ProblemData gen_problem_data_flat(){
return pData;
}
-
+/*
BOOST_AUTO_TEST_CASE(quasi_static){
// compute kinematic constraints for the right foot :
@@ -228,10 +228,17 @@ BOOST_AUTO_TEST_CASE(transition){
VectorX Ts(3);
Ts<<0.6,0.6,0.6;
check_transition(pData,Ts);
-}
+}*/
+BOOST_AUTO_TEST_CASE(transition_noc1){
+ bezier_com_traj::ProblemData pData = gen_problem_data_flat();
+ pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::END_VEL;
+ VectorX Ts(3);
+ Ts<<0.6,0.6,0.6;
+ check_transition(pData,Ts);
+}
-BOOST_AUTO_TEST_CASE(transition_noDc1){
+/*BOOST_AUTO_TEST_CASE(transition_noDc1){
bezier_com_traj::ProblemData pData = gen_problem_data_flat();
pData.constraints_.flag_ ^= bezier_com_traj::END_VEL;
VectorX Ts(3);
@@ -678,6 +685,6 @@ BOOST_AUTO_TEST_CASE(transition_2){
check_transition(pData,Ts);
-}
+}*/
BOOST_AUTO_TEST_SUITE_END()
diff --git a/tests/test-zero-step-capturability.cpp b/tests/test-zero-step-capturability.cpp
index fc3d141..466e085 100644
--- a/tests/test-zero-step-capturability.cpp
+++ b/tests/test-zero-step-capturability.cpp
@@ -7,6 +7,7 @@
#include <iostream>
#include <centroidal-dynamics-lib/centroidal_dynamics.hh>
#include <bezier-com-traj/solve.hh>
+#include <bezier-com-traj/data.hh>
#include <math.h>
using namespace centroidal_dynamics;
@@ -117,9 +118,9 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
const bezier_com_traj::bezier_t c_of_t = resData.c_of_t_;
const bezier_com_traj::bezier_t dL_of_t = resData.dL_of_t_;
bezier_com_traj::bezier_t ddc_of_t = c_of_t.compute_derivate(2);
- for (int i = 0; i < num_steps; ++i)
+ for (int i = 1; i < num_steps; ++i)
{
- double dt = double(i) / float(num_steps) * T;
+ double dt = double(i) / double(num_steps) * T;
c_of_t(dt);
dL_of_t(dt);
Vector6 w = computew(eq, c_of_t(dt),ddc_of_t(dt),dL_of_t(dt));
@@ -128,7 +129,7 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
if(res(j)>0.0001)
{
std::cout << "check trajectory failed for solver " << solver << " " << res(j) << "; iteration " << dt<< "; numsteps " << num_steps << " c_of_t(dt) " << (c_of_t)(dt).transpose() << " dc_of_t(dt) " << c_of_t.derivate(dt,1).transpose() << " dL_of_t(dt) " << (dL_of_t)(dt).transpose() << std::endl;
- std::cout << "c0 " << c0.transpose() << std::endl;
+ /*std::cout << "c0 " << c0.transpose() << std::endl;
std::cout << "H row " << H.row(j) << std::endl;
std::cout << "w " << w << std::endl;
std::cout << "j " << j << std::endl;
@@ -138,7 +139,7 @@ bool checkTrajectory(const Vector3& c0, const std::string& solver, const Equilib
std::cout << "T c" << c_of_t.T_ << std::endl;
std::cout << "T " << T << std::endl;
std::cout << "T ddc " << ddc_of_t.T_ << std::endl;
- std::cout << "1 / T * T " << 1/ (T * T) << std::endl;
+ std::cout << "1 / T * T " << 1/ (T * T) << std::endl;*/
return false;
}
}
@@ -198,12 +199,14 @@ int main()
{
bezier_com_traj::ContactData data;
data.contactPhase_ = &solver_PP;
- bezier_com_traj::ProblemData pData;
+ bezier_com_traj::ProblemData pData;
+ pData.constraints_.flag_ = bezier_com_traj::INIT_POS | bezier_com_traj::INIT_VEL | bezier_com_traj::END_VEL;
pData.c0_ = c0;
pData.dc0_ << fRandom(-1.,1.) , fRandom(-1.,1.) , fRandom(-1.,1.);
//pData.dc0_ *= 1;
pData.l0_ = Vector3(0.,0.,0.);
- pData.contacts_.push_back(data);
+ pData.contacts_.push_back(data);
+ pData.costFunction_ = bezier_com_traj::DISTANCE_TRAVELED;
double T;
for (int k = -1; k < 2; ++k)
//for (int k = 0; k < 1; ++k)
@@ -229,17 +232,26 @@ int main()
succCont = false;
}
// try discretize
- bezier_com_traj::ResultDataCOMTraj rData0 = bezier_com_traj::solve0step(pData,Ts,DISCRETIZATION_STEP);
+ VectorX t2(1); t2 << Ts[0];
+ bezier_com_traj::ResultDataCOMTraj rData0 = bezier_com_traj::solveTransition(pData,t2,DISCRETIZATION_STEP);
if(rData0.success_)
{
+ bezier_com_traj::ResultDataCOMTraj rDatatest = bezier_com_traj::solve0step(pData,Ts,DISCRETIZATION_STEP);
+ //bezier_com_traj::ResultDataCOMTraj rDatatest = bezier_com_traj::solveOnestep(pData,t2,tg,(int)(round(T / DISCRETIZATION_STEP)));
+ assert(rDatatest.success_);
assert ((rData0.c_of_t_(0.) - c0).norm() < 0.0001);
+ assert ((rData0.c_of_t_.compute_derivate(1)(0.) - pData.dc0_).norm() < 0.0001);
succDisc = true;
succDiscretize += 1;
std::string solverName("discretize");
- checkTrajectory(c0, solverName, &solver_PP,rData0,T, int(T / DISCRETIZATION_STEP));
+ checkTrajectory(c0, solverName, &solver_PP,rData0,T, (int)(round(T / DISCRETIZATION_STEP)));
+ //checkTrajectory(c0, solverName, &solver_PP,rData0,T, (int)(T / DISCRETIZATION_STEP));
}
else
{
+ VectorX t2(1); t2 << Ts[0];
+ bezier_com_traj::ResultDataCOMTraj rDatatest = bezier_com_traj::solveTransition(pData,t2,DISCRETIZATION_STEP);
+ assert(! rDatatest.success_);
if(succCont)
std::cout << "error: Solver discretize failed while a solution was found for the continuous case" << std::endl;
succDisc = false;
@@ -273,8 +285,8 @@ int main()
}
else
{
- //if(succCont || succDisc ||succdLbool)
- // std::cout << "error: Solver discretize with angular momentum failed while a solution was found for another case" << std::endl;
+ if(succCont || succDisc ||succdLbool)
+ std::cout << "error: Solver discretize with angular momentum failed while a solution was found for another case" << std::endl;
succDiscdL = false;
}
pData.contacts_.front().Kin_ = Eigen::Matrix3d::Identity();
--
GitLab