From 57c4a27ce42bffa24d1f699191b4fafc3195f15e Mon Sep 17 00:00:00 2001
From: thomascbrs <thomas.corberes@laposte.net>
Date: Thu, 16 Sep 2021 11:49:08 +0100
Subject: [PATCH] Compute jerk for feet trajectories + python bindings (jerk
and timing of flying period)
---
include/qrw/FootTrajectoryGenerator.hpp | 4 ++++
python/gepadd.cpp | 8 +++++++-
src/FootTrajectoryGenerator.cpp | 7 +++++++
3 files changed, 18 insertions(+), 1 deletion(-)
diff --git a/include/qrw/FootTrajectoryGenerator.hpp b/include/qrw/FootTrajectoryGenerator.hpp
index 1b48ea9c..70507a25 100644
--- a/include/qrw/FootTrajectoryGenerator.hpp
+++ b/include/qrw/FootTrajectoryGenerator.hpp
@@ -72,6 +72,9 @@ class FootTrajectoryGenerator {
MatrixN getFootPosition() { return position_; } // Get the next foot position
MatrixN getFootVelocity() { return velocity_; } // Get the next foot velocity
MatrixN getFootAcceleration() { return acceleration_; } // Get the next foot acceleration
+ MatrixN getFootJerk() { return jerk_; } // Get the next foot jerk
+ Vector4 getT0s() { return t0s; } // Get the t0s for each foot
+ Vector4 getTswing() { return t_swing; } // Get the flying period for each foot
private:
Gait *gait_; // Target lock before the touchdown
@@ -93,6 +96,7 @@ class FootTrajectoryGenerator {
Matrix34 position_; // Position computed in updateFootPosition
Matrix34 velocity_; // Velocity computed in updateFootPosition
Matrix34 acceleration_; // Acceleration computed in updateFootPosition
+ Matrix34 jerk_; // Jerk computed in updateFootPosition
Matrix34 position_base_; // Position computed in updateFootPosition in base frame
Matrix34 velocity_base_; // Velocity computed in updateFootPosition in base frame
diff --git a/python/gepadd.cpp b/python/gepadd.cpp
index 56ff1cf6..8ed3627a 100644
--- a/python/gepadd.cpp
+++ b/python/gepadd.cpp
@@ -194,13 +194,18 @@ struct FootTrajectoryGeneratorPythonVisitor : public bp::def_visitor<FootTraject
.def("getFootPosition", &FootTrajectoryGenerator::getFootPosition, "Get position_ matrix.\n")
.def("getFootVelocity", &FootTrajectoryGenerator::getFootVelocity, "Get velocity_ matrix.\n")
.def("getFootAcceleration", &FootTrajectoryGenerator::getFootAcceleration, "Get acceleration_ matrix.\n")
+ .def("getFootJerk", &FootTrajectoryGenerator::getFootJerk, "Get jerk_ matrix.\n")
.def("initialize", &FootTrajectoryGenerator::initialize, bp::args("params", "gaitIn"),
"Initialize FootTrajectoryGenerator from Python.\n")
// Compute target location of footsteps from Python
.def("update", &FootTrajectoryGenerator::update, bp::args("k", "targetFootstep"),
- "Compute target location of footsteps from Python.\n");
+ "Compute target location of footsteps from Python.\n")
+
+ // Get flying period of the feet
+ .def("getT0s", &FootTrajectoryGenerator::getT0s, "Get the current timings of the flying feet.\n")
+ .def("getTswing", &FootTrajectoryGenerator::getTswing, "Get the flying period of the feet.\n");
}
@@ -452,6 +457,7 @@ struct ParamsPythonVisitor : public bp::def_visitor<ParamsPythonVisitor<Params>>
.def_readwrite("h_ref", &Params::h_ref)
.def_readwrite("shoulders", &Params::shoulders)
.def_readwrite("lock_time", &Params::lock_time)
+ .def_readwrite("vert_time", &Params::vert_time)
.def_readwrite("footsteps_init", &Params::footsteps_init)
.def_readwrite("footsteps_under_shoulders", &Params::footsteps_under_shoulders);
diff --git a/src/FootTrajectoryGenerator.cpp b/src/FootTrajectoryGenerator.cpp
index 7eb545de..8987d40e 100644
--- a/src/FootTrajectoryGenerator.cpp
+++ b/src/FootTrajectoryGenerator.cpp
@@ -17,6 +17,7 @@ FootTrajectoryGenerator::FootTrajectoryGenerator()
position_(Matrix34::Zero()),
velocity_(Matrix34::Zero()),
acceleration_(Matrix34::Zero()),
+ jerk_(Matrix34::Zero()),
position_base_(Matrix34::Zero()),
velocity_base_(Matrix34::Zero()),
acceleration_base_(Matrix34::Zero()) {}
@@ -152,6 +153,8 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
velocity_(1, j) = 0.0;
acceleration_(0, j) = 0.0;
acceleration_(1, j) = 0.0;
+ jerk_(0, j) = 0.0;
+ jerk_(1, j) = 0.0;
} else {
position_(0, j) = Ax(5, j) + Ax(4, j) * ev + Ax(3, j) * std::pow(ev, 2) + Ax(2, j) * std::pow(ev, 3) +
Ax(1, j) * std::pow(ev, 4) + Ax(0, j) * std::pow(ev, 5);
@@ -165,11 +168,15 @@ void FootTrajectoryGenerator::updateFootPosition(int const j, Vector3 const &tar
2 * Ax(3, j) + 3 * 2 * Ax(2, j) * ev + 4 * 3 * Ax(1, j) * std::pow(ev, 2) + 5 * 4 * Ax(0, j) * std::pow(ev, 3);
acceleration_(1, j) =
2 * Ay(3, j) + 3 * 2 * Ay(2, j) * ev + 4 * 3 * Ay(1, j) * std::pow(ev, 2) + 5 * 4 * Ay(0, j) * std::pow(ev, 3);
+
+ jerk_(0, j) = 3 * 2 * Ax(2, j) + 4 * 3 * 2 * Ax(1, j) * ev + 5 * 4 * 3 * Ax(0, j) * std::pow(ev, 2);
+ jerk_(1, j) = 3 * 2 * Ay(2, j) + 4 * 3 * 2 * Ay(1, j) * ev + 5 * 4 * 3 * Ay(0, j) * std::pow(ev, 2);
}
velocity_(2, j) = 3 * Az(3, j) * std::pow(evz, 2) + 4 * Az(2, j) * std::pow(evz, 3) +
5 * Az(1, j) * std::pow(evz, 4) + 6 * Az(0, j) * std::pow(evz, 5);
acceleration_(2, j) = 2 * 3 * Az(3, j) * evz + 3 * 4 * Az(2, j) * std::pow(evz, 2) +
4 * 5 * Az(1, j) * std::pow(evz, 3) + 5 * 6 * Az(0, j) * std::pow(evz, 4);
+ jerk_(2, j) = 2 * 3 * Az(3, j) + 3 * 4 * 2 * Az(2, j) * evz + 4 * 5 * 3 * Az(1, j) * std::pow(evz, 2) + 5 * 6 * 4 * Az(0, j) * std::pow(evz, 3);
position_(2, j) = Az(3, j) * std::pow(evz, 3) + Az(2, j) * std::pow(evz, 4) + Az(1, j) * std::pow(evz, 5) +
Az(0, j) * std::pow(evz, 6);
}
--
GitLab