Merge pull request #7 from inria-paris-robotics-lab/Remove_foot_tracking

Remove foot tracking

config/walk_parameters.yaml

@@ -69,7 +69,6 @@ task:
     ground_collision_w: 1000.
     vertical_velocity_reg_w: 1.
     base_velocity_tracking_w: 800000.
-    foot_tracking_w: 0.
     impact_altitude_w: 1000000.
     impact_velocity_w: 100000.
     friction_cone_w: 0.

examples/run_jump_ocp.py

@@ -2,22 +2,21 @@
 import crocoddyl  # noqa
 import aligator
 import numpy as np
+import pinocchio as pin
 import matplotlib.pyplot as plt
 
 from quadruped_reactive_walking import Params
 from quadruped_reactive_walking.ocp_defs import jump
-from quadruped_reactive_walking.wb_mpc.target import Target, make_footsteps_and_refs
 from crocoddyl import ShootingProblem
 from quadruped_reactive_walking.tools.kinematics_utils import get_translation_array
 from quadruped_reactive_walking.tools.meshcat_viewer import make_meshcat_viz
 from aligator.croc import convertCrocoddylProblem
 
 
 params = Params.create_from_file()
-target = Target(params)
-footsteps, base_vel_refs = make_footsteps_and_refs(params, target)
+base_vel_refs = [pin.Motion(np.zeros(6)) for _ in range(params.N_gait)]
 
-ocp_spec = jump.JumpOCPBuilder(params, footsteps, base_vel_refs)
+ocp_spec = jump.JumpOCPBuilder(params, base_vel_refs)
 robot = ocp_spec.task.robot
 rmodel = robot.model
 

examples/simple_ocp.py

@@ -1,4 +1,5 @@
 import numpy as np
+import pinocchio as pin
 import quadruped_reactive_walking as qrw
 from quadruped_reactive_walking.controller import Controller
 import time
@@ -9,7 +10,6 @@
 
 from quadruped_reactive_walking.wb_mpc import AlgtrOCPProx, CrocOCP, OCP_TYPE_MAP
 from quadruped_reactive_walking.wb_mpc.task_spec import TaskSpec
-from quadruped_reactive_walking.wb_mpc.target import Target, make_footsteps_and_refs
 
 print("OCP registered types:", pprint.pformat(OCP_TYPE_MAP), sep="\n")
 params = qrw.Params.create_from_file()
@@ -18,14 +18,12 @@
 
 print(task)
 
-target = Target(params)
-
-footsteps, base_refs = make_footsteps_and_refs(params, target)
+base_vel_refs = [pin.Motion(np.zeros(6)) for _ in range(params.N_gait)]
 
 x0 = task.x0
 print("x0:", x0)
 
-ocp = CrocOCP(params, footsteps, base_refs)
+ocp = CrocOCP(params, base_vel_refs)
 
 nsteps = ocp.ddp.problem.T
 xs_i = [x0] * (nsteps + 1)
@@ -37,7 +35,7 @@
 
 print("============== PARALLEL ===================")
 
-ocp2 = AlgtrOCPProx(params, footsteps, base_refs)
+ocp2 = AlgtrOCPProx(params, base_vel_refs)
 
 ts = time.time()
 ocp2.solve(0)
@@ -51,7 +49,7 @@
 
 print("============== SERIAL ===================")
 
-ocp3 = AlgtrOCPProx(params, footsteps, base_refs, aligator)
+ocp3 = AlgtrOCPProx(params, base_vel_refs, aligator)
 
 ts = time.time()
 ocp3.solve(0)

include/qrw/Estimator.hpp

@@ -32,7 +32,6 @@ class Estimator {
   /// states of the robot
   ///
   /// \param[in] gait Gait matrix that stores current and future contact status of the feet
-  /// \param[in] goals Target positions of the four feet
   /// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated)
   /// \param[in] baseAngularVelocity Angular velocity of the IMU
   /// \param[in] baseOrientation Quaternion orientation of the IMU
@@ -41,7 +40,6 @@ class Estimator {
   /// \param[in] perfectPosition Position of the robot in world frame
   /// \param[in] b_perfectVelocity Velocity of the robot in base frame
   void run(MatrixN const& gait,
-           MatrixN const& goals,
            VectorN const& baseLinearAcceleration,
            VectorN const& baseAngularVelocity,
            VectorN const& baseOrientation,
@@ -62,10 +60,8 @@ class Estimator {
   VectorN getVEstimate() { return vEstimate_; }
   VectorN getVSecurity() { return vSecurity_; }
   VectorN getFeetStatus() { return feetStatus_; }
-  MatrixN getFeetTargets() { return feetTargets_; }
   Vector3 getBaseVelocityFK() { return baseVelocityFK_; }
   Vector3 getBasePositionFK() { return basePositionFK_; }
-  Vector3 getFeetPositionBarycenter() { return feetPositionBarycenter_; }
   Vector3 getBBaseVelocity() { return b_baseVelocity_; }
 
   Vector3 getFilterVelX() { return velocityFilter_.getX(); }
@@ -107,22 +103,16 @@ class Estimator {
   void updateJointData(Vector12 const& q, Vector12 const& v);
 
   /// \brief Update the feet relative data
-  /// \details update feetStatus_, feetTargets_, feetStancePhaseDuration_ and
+  /// \details update feetStatus_, feetStancePhaseDuration_ and
   /// phaseRemainingDuration_
   ///
   /// \param[in] gait Gait matrix that stores current and future contact status
-  /// of the feet \param[in] feetTargets Target positions of the four feet
-  void updateFeetStatus(MatrixN const& gait, MatrixN const& feetTargets);
+  /// of the feet
+  void updateFeetStatus(MatrixN const& gait);
 
   /// \brief Estimate base position and velocity using Forward Kinematics
   void updateForwardKinematics();
 
-  /// \brief Compute barycenter of feet in contact
-  ///
-  /// \param[in] feet_status Contact status of the four feet
-  /// \param[in] goals Target positions of the four feet
-  void computeFeetPositionBarycenter();
-
   /// \brief Estimate the velocity of the base with forward kinematics using a
   /// contact point that
   ///        is supposed immobile in world frame
@@ -183,16 +173,14 @@ class Estimator {
   int phaseRemainingDuration_;       //< Number of iterations left for the current gait phase
   Vector4 feetStancePhaseDuration_;  //< Number of loops during which each foot has been in contact
   Vector4 feetStatus_;               //< Contact status of the four feet
-  Matrix34 feetTargets_;             //< Target positions of the four feet
-
-  pinocchio::Model model_;          //< Pinocchio models for frame computations and forward kinematics
-  pinocchio::Data data_;            //< Pinocchio datas for frame computations and forward kinematics
-  Vector19 q_FK_;                   //< Configuration vector for Forward Kinematics
-  Vector18 v_FK_;                   //< Velocity vector for Forward Kinematics
-  Vector3 baseVelocityFK_;          //< Base linear velocity estimated by Forward Kinematics
-  Vector3 basePositionFK_;          //< Base position estimated by Forward Kinematics
-  Vector3 b_baseVelocity_;          //< Filtered estimated velocity at center base (base frame)
-  Vector3 feetPositionBarycenter_;  // Barycenter of feet in contact
+
+  pinocchio::Model model_;  //< Pinocchio models for frame computations and forward kinematics
+  pinocchio::Data data_;    //< Pinocchio datas for frame computations and forward kinematics
+  Vector19 q_FK_;           //< Configuration vector for Forward Kinematics
+  Vector18 v_FK_;           //< Velocity vector for Forward Kinematics
+  Vector3 baseVelocityFK_;  //< Base linear velocity estimated by Forward Kinematics
+  Vector3 basePositionFK_;  //< Base position estimated by Forward Kinematics
+  Vector3 b_baseVelocity_;  //< Filtered estimated velocity at center base (base frame)
 
   ComplementaryFilter positionFilter_;  //< Complementary filter for base position
   ComplementaryFilter velocityFilter_;  //< Complementary filter for base velocity

include/qrw/IMPCWrapper.hpp

@@ -16,7 +16,7 @@ struct IMPCWrapper {
 
   int N_gait() const { return params_.N_gait; }
   uint window_size() const { return params_.window_size; }
-  virtual void solve(uint k, const ConstVecRefN &x0, Vector4 footstep, Motion base_vel_ref) = 0;
+  virtual void solve(uint k, const ConstVecRefN &x0, Motion base_vel_ref) = 0;
   virtual MPCResult get_latest_result() const = 0;
 };
 

include/qrw/IOCPAbstract.hpp

@@ -14,7 +14,7 @@ class IOCPAbstract {
   IOCPAbstract(Params const& params);
   virtual ~IOCPAbstract() = default;
 
-  virtual void push_node(uint k, const ConstVecRefN& x0, Matrix34 footsteps, Motion base_vel_ref) = 0;
+  virtual void push_node(uint k, const ConstVecRefN& x0, Motion base_vel_ref) = 0;
   virtual void solve(std::size_t k) = 0;
 
   Params params_;

python/expose-estimator.cpp

@@ -21,10 +21,8 @@ struct EstimatorVisitor : public bp::def_visitor<EstimatorVisitor<Estimator>> {
         .def(
             "get_v_security", &Estimator::getVSecurity, bp::args("self"), "Get filtered velocity for security check.\n")
         .def("get_feet_status", &Estimator::getFeetStatus, bp::args("self"))
-        .def("get_feet_targets", &Estimator::getFeetTargets, bp::args("self"))
         .def("get_base_velocity_FK", &Estimator::getBaseVelocityFK, bp::args("self"))
         .def("get_base_position_FK", &Estimator::getBasePositionFK, bp::args("self"))
-        .def("get_feet_position_barycenter", &Estimator::getFeetPositionBarycenter, "")
         .def("get_b_base_velocity", &Estimator::getBBaseVelocity, "")
         .def("get_filter_vel_X", &Estimator::getFilterVelX, "")
         .def("get_filter_vel_DX", &Estimator::getFilterVelDX, "")
@@ -49,7 +47,6 @@ struct EstimatorVisitor : public bp::def_visitor<EstimatorVisitor<Estimator>> {
              &Estimator::run,
              bp::args("self",
                       "gait",
-                      "goals",
                       "baseLinearAcceleration",
                       "baseAngularVelocity",
                       "baseOrientation",

python/expose-mpc-interface.cpp

@@ -8,9 +8,9 @@ struct PyMPCWrapper : MPCDerived, bp::wrapper<MPCDerived> {
   using MPCDerived::MPCDerived;
   using StdVecVecN = std::vector<VectorN>;
 
-  void solve(uint k, const ConstVecRefN &x0, Vector4 footstep, Motion base_vel_ref) override {
+  void solve(uint k, const ConstVecRefN &x0, Motion base_vel_ref) override {
     bp::override fn = this->get_override("solve");
-    fn(k, x0, footstep, base_vel_ref);
+    fn(k, x0, base_vel_ref);
   }
 
   MPCResult get_latest_result() const override { return this->get_override("get_latest_result")(); }
@@ -25,7 +25,7 @@ void exposeMPCInterface() {
       .def("get_latest_result", bp::pure_virtual(&PyMPCWrapper<>::get_latest_result), bp::args("self"))
       .def("solve",
            bp::pure_virtual(&PyMPCWrapper<>::solve),
-           (bp::arg("self"), bp::arg("k"), bp::arg("x0"), bp::arg("footstep"), bp::arg("base_vel_ref")));
+           (bp::arg("self"), bp::arg("k"), bp::arg("x0"), bp::arg("base_vel_ref")));
 }
 
 }  // namespace qrw

python/expose-ocp-interface.cpp

@@ -8,9 +8,9 @@ namespace qrw {
 struct OCPWrapper : IOCPAbstract, bp::wrapper<IOCPAbstract> {
   using IOCPAbstract::IOCPAbstract;
 
-  void push_node(uint k, const ConstVecRefN &x0, Matrix34 footsteps, Motion base_vel_ref) override {
+  void push_node(uint k, const ConstVecRefN &x0, Motion base_vel_ref) override {
     bp::override fn = get_override("push_node");
-    fn(k, x0, footsteps, base_vel_ref);
+    fn(k, x0, base_vel_ref);
   }
 
   void solve(std::size_t k) override {
@@ -30,7 +30,7 @@ void exposeSolverInterface() {
       .def("solve", bp::pure_virtual(&OCPWrapper::solve), bp::args("self", "k"))
       .def("push_node",
            bp::pure_virtual(&OCPWrapper::push_node),
-           bp::args("self", "k", "x0", "footsteps", "base_vel_ref"),
+           bp::args("self", "k", "x0", "base_vel_ref"),
            "Push a new node to the OCP.")
       .def("get_results",
            bp::pure_virtual(&OCPWrapper::get_results),

python/quadruped_reactive_walking/controller.py

@@ -8,10 +8,9 @@
 
 from crocoddyl import StateMultibody
 from . import wb_mpc
-from .wb_mpc.target import Target, make_footsteps_and_refs
 from .wb_mpc.task_spec import TaskSpec
 from .wbmpc_wrapper_abstract import MPCResult
-from .tools.utils import quaternionToRPY, make_initial_footstep
+from .tools.utils import quaternionToRPY
 from typing import Type
 
 
@@ -92,12 +91,9 @@ def __init__(self, params: qrw.Params, q_init, solver_cls: Type[wb_mpc.OCPAbstra
         self.result.q_des = self.task.q0[7:].copy()
         self.result.v_des = self.task.v0[6:].copy()
 
-        self.target = Target(params)
-        self.footsteps, self.base_refs = make_footsteps_and_refs(self.params, self.target)
+        self.base_vel_refs = [pin.Motion(np.zeros(6)) for _ in range(self.params.N_gait)]
 
-        self.default_footstep = make_initial_footstep(params.q_init)
         self.target_base = pin.Motion.Zero()
-        self.target_footstep = np.zeros((3, 4))
 
         self.mpc = self._create_mpc(solver_cls=solver_cls)
         assert self.mpc is not None, "Error while instanciating MPCWrapper"
@@ -127,11 +123,11 @@ def _create_mpc(self, solver_cls):
             if self.params.asynchronous_mpc:
                 from .wbmpc_wrapper_ros_mp import ROSMPCAsyncClient
 
-                return ROSMPCAsyncClient(self.params, self.footsteps, self.base_refs, solver_cls)
+                return ROSMPCAsyncClient(self.params, self.base_vel_refs, solver_cls)
             else:
                 from .wbmpc_wrapper_ros import ROSMPCWrapperClient
 
-                return ROSMPCWrapperClient(self.params, self.footsteps, self.base_refs, solver_cls, True)
+                return ROSMPCWrapperClient(self.params, self.base_vel_refs, solver_cls, True)
         else:
             if self.params.asynchronous_mpc:
                 from .wbmpc_wrapper_multiprocess import (
@@ -141,8 +137,7 @@ def _create_mpc(self, solver_cls):
                 from .wbmpc_wrapper_sync import SyncMPCWrapper as MPCWrapper
             return MPCWrapper(
                 self.params,
-                self.footsteps,
-                self.base_refs,
+                self.base_vel_refs,
                 solver_cls=solver_cls,
             )
 
@@ -167,10 +162,8 @@ def compute(self, device, qc=None):
 
         if self.params.movement == "base_circle" or self.params.movement == "walk":
             self.target_base.np[:] = self.v_ref
-            self.target_footstep[:] = 0.0
         else:
             self.target_base.np[:] = 0.0
-            self.target_footstep[:] = self.target.compute(self.k + self.params.N_gait * self.params.mpc_wbc_ratio)
 
         if self.k % self.params.mpc_wbc_ratio == 0:
             if self.mpc_solved:
@@ -184,7 +177,7 @@ def compute(self, device, qc=None):
 
             try:
                 self.t_mpc_start = time.time()
-                self.mpc.solve(self.k, x, self.target_footstep.copy(), self.target_base.copy())
+                self.mpc.solve(self.k, x, self.target_base.copy())
             except ValueError:
                 import traceback
 
@@ -361,7 +354,6 @@ def run_estimator(self, device, q_perfect=np.zeros(6), b_baseVel_perfect=np.zero
 
         self.estimator.run(
             self.gait,
-            self.default_footstep,
             device.imu.linear_acceleration,
             device.imu.gyroscope,
             device.imu.attitude_euler,

python/quadruped_reactive_walking/ocp_defs/jump.py

@@ -8,21 +8,19 @@
 from quadruped_reactive_walking import Params
 from crocoddyl import CostModelSum
 from . import walking
-from ..tools.utils import make_initial_footstep
 
 
 class JumpOCPBuilder:
-    def __init__(self, params: Params, footsteps, base_vel_refs):
+    def __init__(self, params: Params, base_vel_refs):
         self.params = params
-        self._base_builder = walking.WalkingOCPBuilder(params, footsteps, base_vel_refs)
+        self._base_builder = walking.WalkingOCPBuilder(params, base_vel_refs)
         self.task = self._base_builder.task
         self.state = self._base_builder.state
         self.rdata = self._base_builder.rdata
 
         self.x0 = self.task.x0
         self.jump_spec = params.task["jump"]
-        feet_pos = make_initial_footstep(params.q_init)
-        self.ground_models_1 = self.create_ground_models(feet_pos)
+        self.ground_models_1 = self.create_ground_models()
         jump_vel = np.asarray(self.jump_spec["jump_velocity"])
         jump_vel = pin.Motion(jump_vel)
         self.jump_models = self.create_jump_model(jump_vel)
@@ -47,11 +45,11 @@ def build_timeline(self):
         assert len(rms) == N
         return rms, ground_tm
 
-    def create_ground_models(self, feet_pos):
+    def create_ground_models(self):
         rms = []
         support_feet = np.asarray(self.task.feet_ids)
         for k in range(self.params.N_gait):
-            m = self._base_builder.make_running_model(support_feet, [], feet_pos, None)
+            m = self._base_builder.make_running_model(support_feet, [], None)
             rms.append(m)
         return rms, self._base_builder.make_terminal_model(support_feet)