diff --git a/CMakeLists.txt b/CMakeLists.txt
index 96be7edddb93315f60d14b67cb07b74c870aacbb..16afcf8b28d1689789b8eeca74831e9284cafda4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -75,10 +75,10 @@ add_project_dependency(pinocchio REQUIRED)
TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC pinocchio::pinocchio)
# Find parametric curves library and headers
-add_project_dependency(curves REQUIRED)
+# add_project_dependency(curves REQUIRED)
# Link parametric curves library
-target_link_libraries(${PROJECT_NAME} PUBLIC curves::curves)
+# target_link_libraries(${PROJECT_NAME} PUBLIC curves::curves)
# Find OSQP library and headers
find_package(osqp REQUIRED)
diff --git a/include/example-adder/Planner.hpp b/include/example-adder/Planner.hpp
index 00fdcc4d8d6ea412e138a3a8e9cffafc1e93d2bf..a70678bda48b9b804d8ea22789ab93778380a0d0 100644
--- a/include/example-adder/Planner.hpp
+++ b/include/example-adder/Planner.hpp
@@ -7,11 +7,7 @@
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Dense>
-
#include "pinocchio/math/rpy.hpp"
-#include "curves/fwd.h"
-#include "curves/bezier_curve.h"
-// #include "curves/helpers/effector_spline.h"
#define N0_gait 20
@@ -126,15 +122,6 @@ class Planner {
Eigen::Matrix<double, 6, 4>::Zero(); // Storage variable for the trajectory generator
Eigen::Matrix<double, 11, 4> res_gen = Eigen::Matrix<double, 11, 4>::Zero();
- /*curves::bezier_t::curve_constraints_t constrts;
- constrts = curves::bezier_t::curve_constraints_t(3);*/
-
- std::vector<std::vector<curves::point3_t>> pr_feet;
- std::vector<curves::bezier_t::num_t> T_min;
- std::vector<curves::bezier_t::num_t> T_max;
- std::vector<curves::bezier_t> c_feet;
- // curves::bezier_t::curve_constraints<point3_t> constraints;
- curves::bezier_t::curve_constraints_t constraints;
public:
Planner();
diff --git a/src/Planner.cpp b/src/Planner.cpp
index b57418a80298f12a4ce08d94e570b0dec0fc1bfc..c32856ca96d4c9e0b2d5fc5a42fcb1215ad76821 100644
--- a/src/Planner.cpp
+++ b/src/Planner.cpp
@@ -35,22 +35,6 @@ Planner::Planner(double dt_in, double dt_tsid_in, double T_gait_in, double T_mpc
mgoals.row(0) << fsteps_in.block(0, 0, 1, 4);
mgoals.row(3) << fsteps_in.block(1, 0, 1, 4);
- constraints.init_vel = curves::point3_t(0.0, 0.0, 0.0);
- constraints.init_acc = curves::point3_t(0.0, 0.0, 0.0);
- constraints.end_vel = curves::point3_t(0.0, 0.0, 0.0);
- constraints.end_acc = curves::point3_t(0.0, 0.0, 0.0);
-
- std::vector<curves::point3_t> params;
- params.push_back(curves::point3_t(0.0, 0.0, 0.0));
- params.push_back(curves::point3_t(0.0, 0.0, 0.0));
- params.push_back(curves::point3_t(0.0, 0.0, 0.0));
- for (int i = 0; i < 4; i++) {
- pr_feet.push_back(params);
- T_min.push_back(curves::bezier_t::num_t(0.0));
- T_max.push_back(curves::bezier_t::num_t(0.5 * T_mpc));
- c_feet.push_back(curves::bezier_t(pr_feet[i].begin(), pr_feet[i].end(), constraints, T_min[i], T_max[i]));
- }
-
for (int i = 0; i < 4; i++) {
myTrajGen.push_back(TrajGen(max_height_feet, t_lock_before_touchdown, shoulders(0, i), shoulders(1, i)));
}
@@ -623,8 +607,6 @@ int Planner::update_trajectory_generator(int k, double h_estim) {
for (int i = 0; i < feet.size(); i++) {
int i_foot = feet[i];
- // c_feet[i_foot] = curves::bezier_t(pr_feet[i].begin(), pr_feet[i].end(), constraints, T_min[i], T_max[i]));
-
// Get desired 3D position, velocity and acceleration
if ((t0s[i] == 0.000) || (k == 0)) {
/*std::cout << "PASS 1 ";
@@ -637,13 +619,6 @@ int Planner::update_trajectory_generator(int k, double h_estim) {
mgoals.col(i_foot) << res_gen.block(0, i_foot, 6, 1);
- /*pr_feet[i_foot][0] = curves::point3_t(mgoals(0, i_foot), mgoals(3, i_foot), 0.0);
- pr_feet[i_foot][2] = curves::point3_t(footsteps_target(0, i_foot), footsteps_target(1, i_foot), 0.0);
- pr_feet[i_foot][1] = curves::point3_t(((pr_feet[i_foot][0])(0) + (pr_feet[i_foot][2])(0)) * 0.5,
- ((pr_feet[i_foot][0])(1) + (pr_feet[i_foot][2])(1)) * 0.5,
- max_height_feet);
- T_min[i_foot] = k * dt_tsid;
- T_max[i_foot] = T_min[i_foot] + t_swing[i_foot];*/
} else {
// std::cout << "PASS 2 ";
@@ -655,35 +630,8 @@ int Planner::update_trajectory_generator(int k, double h_estim) {
mgoals.col(i_foot) << res_gen.block(0, i_foot, 6, 1);
- /*T_min[i_foot] = k * dt_tsid;
- pr_feet[i_foot][0] = curves::point3_t(mgoals(0, i_foot), mgoals(3, i_foot), 0.0);
- pr_feet[i_foot][2] = curves::point3_t(footsteps_target(0, i_foot), footsteps_target(1, i_foot), 0.0);
- pr_feet[i_foot][1] = curves::point3_t(((pr_feet[i_foot][0])(0) + (pr_feet[i_foot][2])(0)) * 0.5,
- ((pr_feet[i_foot][0])(1) + (pr_feet[i_foot][2])(1)) * 0.5,
- max_height_feet);
- T_max[i_foot] = T_min[i_foot] + t_swing[i_foot];*/
}
- /*std::cout << "---- " << std::endl;
- std::cout << "Processing feet " << i_foot << std::endl;
- std::cout << "Tmin/max: " << T_min[i_foot] << " / " << T_max[i_foot] << std::endl;
- std::cout << "t_swing: " << t_swing[0] << " / " << t_swing[1] << " / " << t_swing[2] << " / " << t_swing[3] <<
- std::endl; std::cout << "start: " << (pr_feet[i_foot][0])(0) << " / " << (pr_feet[i_foot][0])(1) << " / "<<
- (pr_feet[i_foot][0])(2) << std::endl; std::cout << "end : " << (pr_feet[i_foot][2])(0) << " / " <<
- (pr_feet[i_foot][2])(1) << " / "<< (pr_feet[i_foot][2])(2) << std::endl; std::cout << "mgoals:" << mgoals(0,
- i_foot) << " / " << mgoals(3, i_foot) << std::endl; std::cout << "target:" << footsteps_target(0, i_foot) << " / "
- << footsteps_target(1, i_foot) << std::endl;
-
-
- c_feet[i_foot] = curves::bezier_t(pr_feet[i_foot].begin(), pr_feet[i_foot].end(), constraints, T_min[i_foot],
- T_max[i_foot]);
- // Store desired position, velocity and acceleration for later call to this function
- goals.col(i_foot) << (c_feet[i_foot])((k+1) * dt_tsid - T_min[i_foot]);
- vgoals.col(i_foot) << (c_feet[i_foot]).derivate((k+1) * dt_tsid - T_min[i_foot], 1);
- agoals.col(i_foot) << (c_feet[i_foot]).derivate((k+1) * dt_tsid - T_min[i_foot], 2);
- mgoals.col(i_foot) << goals(0, i_foot), vgoals(0, i_foot), agoals(0, i_foot), goals(1, i_foot), vgoals(1, i_foot),
- agoals(1, i_foot);*/
-
/*std::cout << "---- " << std::endl;
std::cout << "Processing feet " << i_foot << std::endl;
std::cout << "t0 / tswing " << t0s[i] << " / " << t_swing[i_foot] << std::endl;
diff --git a/src/main.cpp b/src/main.cpp
index 9ea8ae0fce48f330e22d4d766c68068abcb6b9bc..ce4c303381ed90414eea32bf07133358b03dda6f 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -7,37 +7,7 @@
#include "example-adder/MPC.hpp"
#include "other/st_to_cc.hpp"
#include "example-adder/Planner.hpp"
-
#include "pinocchio/math/rpy.hpp"
-#include "curves/fwd.h"
-#include "curves/bezier_curve.h"
-// #include "curves/splines.h"
-#include "curves/helpers/effector_spline.h"
-
-/*namespace curves {
-typedef exact_cubic<double, double, true, Eigen::Matrix<double, 1, 1> > exact_cubic_one;
-typedef exact_cubic_t::spline_constraints spline_constraints_t;
-
-typedef std::pair<double, pointX_t> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-typedef Eigen::Matrix<double, 1, 1> point_one;
-typedef std::pair<double, point_one> WaypointOne;
-typedef std::vector<WaypointOne> T_WaypointOne;
-typedef std::pair<pointX_t, pointX_t> pair_point_tangent_t;
-typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
-
-const double margin = 1e-3;
-bool QuasiEqual(const double a, const double b) { return std::fabs(a - b) < margin; }
-bool QuasiEqual(const point3_t a, const point3_t b) {
- bool equal = true;
- for (size_t i = 0; i < 3; ++i) {
- equal = equal && QuasiEqual(a[i], b[i]);
- }
- return equal;
-}
-} // End namespace curves*/
-
-using namespace curves;
int main(int argc, char** argv) {
if (argc == 3) {
@@ -141,131 +111,6 @@ int main(int argc, char** argv) {
}
}
- /*typedef std::pair<double, Eigen::Vector3d> Waypoint;
- typedef std::vector<Waypoint> T_Waypoint;
-
- // loading helper class namespace
- using namespace curves::helpers;
-
- // Create waypoints
- T_Waypoint waypoints;
- waypoints.push_back(std::make_pair(0., Eigen::Vector3d(0,0,0)));
- waypoints.push_back(std::make_pair(1., Eigen::Vector3d(0.5,0.5,0.5)));
- waypoints.push_back(std::make_pair(2., Eigen::Vector3d(1,1,0)));
-
- exact_cubic_t* eff_traj = effector_spline(waypoints.begin(),waypoints.end());
-
- // evaluate spline
- std::cout << (*eff_traj)(0.) << std::endl; // (0,0,0)
- std::cout << (*eff_traj)(2.) << std::endl; // (1,1,0)*/
-
- point3_t a(1, 2, 0);
- point3_t b(2, 3, 0.5); // * (1000/312.5));
- point3_t c(3, 4, 1);
- std::cout << a(0) << " " << a(1) << " " << a(2) << std::endl;
- bezier_t::curve_constraints_t constraints(3);
- constraints.init_vel = point3_t(-1, -1, 0);
- constraints.init_acc = point3_t(-1, -2, 0);
- constraints.end_vel = point3_t(-10, -10, 0);
- constraints.end_acc = point3_t(-20, -20, 0);
- std::vector<point3_t> params;
- params.push_back(a);
- params.push_back(b);
- params.push_back(c);
- bezier_t::num_t T_min = 0.0;
- bezier_t::num_t T_max = 30.0;
- bezier_t cf(params.begin(), params.end(), constraints, T_min, T_max);
-
- std::cout << "Point a" << std::endl;
- std::cout << a << std::endl;
- std::cout << cf(T_min) << std::endl;
- std::cout << "Point c" << std::endl;
- std::cout << c << std::endl;
- std::cout << cf(T_max) << std::endl;
- std::cout << "Init vel" << std::endl;
- std::cout << constraints.init_vel << std::endl;
- std::cout << cf.derivate(T_min, 1) << std::endl;
- std::cout << "End vel" << std::endl;
- std::cout << constraints.end_vel << std::endl;
- std::cout << cf.derivate(T_max, 1) << std::endl;
- std::cout << "Init acc" << std::endl;
- std::cout << constraints.init_acc << std::endl;
- std::cout << cf.derivate(T_min, 2) << std::endl;
- std::cout << "End acc" << std::endl;
- std::cout << constraints.end_acc << std::endl;
- std::cout << cf.derivate(T_max, 2) << std::endl;
-
- /*// Create waypoints
- // curves::helpers::T_Waypoint waypoints;
- //typedef std::pair<double, Eigen::Vector3d> Waypoint;
- typedef std::vector<point3_t> myWaypoint;
- myWaypoint waypoints;
- waypoints.push_back(std::make_pair(0., Eigen::Vector3d(0,0,0)));
- waypoints.push_back(std::make_pair(1., Eigen::Vector3d(0.5,0.5,0.5)));
- waypoints.push_back(std::make_pair(2., Eigen::Vector3d(0,1,0)));
-
- // Create constraints
- curves::exact_cubic3_t::spline_constraints constraints(3);
- constraints.init_vel = point3_t(0, 0, 0);
- constraints.init_acc = point3_t(-1, -2, 0);
- constraints.end_vel = point3_t(-10, -10, 0);
- constraints.end_acc = point3_t(-20, -20, 0);
-
- // Create spline with constraints
- curves::exact_cubic3_t end_eff(waypoints.begin(), waypoints.end(), constraints);*/
-
- std::cout << cf(15.0)[2] << std::endl;
-
- const float N_test = 100.0;
- for (float i = 0; i < (N_test + 1); i++) {
- if (std::abs(i - 60) < 0.1) {
- params[2] = point3_t(3, 4, 1);
- cf = bezier_t(params.begin(), params.end(), constraints, T_min, T_max);
- }
-
- std::cout << (cf(i * 30.0 / N_test))[2] << ", ";
- }
- std::cout << std::endl;
-
- /*// Constraints
- bezier_t::curve_constraints_t pc_constraints0(3);
- pc_constraints0.init_vel = point3_t(-1, -1, 0);
- pc_constraints0.init_acc = point3_t(-1, -2, 0);
- pc_constraints0.end_vel = point3_t(-10, -10, 0);
- // pc_constraints0.end_acc = point3_t(-20, -20, 0);
- bezier_t::curve_constraints_t pc_constraints1(3);
- pc_constraints1.init_vel = point3_t(-1, -1, 0);
- //pc_constraints0.init_acc = point3_t(-1, -2, 0);
- pc_constraints1.end_vel = point3_t(-10, -10, 0);
- pc_constraints1.end_acc = point3_t(-20, -20, 0);
-
- // Piecewises
- point3_t a0(0.0, 0.0, 0.0);
- point3_t b0(0.5, 0.5, 1.0);
- point3_t c0(1.0, 1.0, 0.0);
- std::vector<point3_t> params0;
- std::vector<point3_t> params1;
- params0.push_back(a0); // bezier between [0,1]
- params0.push_back(b0);
- params1.push_back(b0); // bezier between [1,2]
- params1.push_back(c0);
- boost::shared_ptr<bezier_t> bc0_ptr(new bezier_t(params0.begin(), params0.end(), pc_constraints0, 0., 1.));
- boost::shared_ptr<bezier_t> bc1_ptr(new bezier_t(params1.begin(), params1.end(), pc_constraints1, 1., 2.));
- piecewise_t pc_C0(bc0_ptr);
- pc_C0.add_curve_ptr(bc1_ptr);
-
- for (float i=0; i<(N_test+1); i++) {
- std::cout << (pc_C0(i * 2.0 / N_test))[2] << ", ";
- }*/
-
- /*ComparePoints(a, cf(T_min), errMsg0, error);
- ComparePoints(c, cf(T_max), errMsg0, error);
- ComparePoints(constraints.init_vel, cf.derivate(T_min, 1), errMsg0, error);
- ComparePoints(constraints.end_vel, cf.derivate(T_max, 1), errMsg0, error);
- ComparePoints(constraints.init_acc, cf.derivate(T_min, 2), errMsg0, error);
- ComparePoints(constraints.end_vel, cf.derivate(T_max, 1), errMsg0, error);
- ComparePoints(constraints.end_acc, cf.derivate(T_max, 2), errMsg0, error);*/
-
return EXIT_SUCCESS;
} else {
std::cerr << "This program needs 2 integers" << std::endl;