Compute jerk for feet trajectories + python bindings (jerk and timing of flying period)

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

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);
 

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);
 }