Initial pose

config/walk_parameters.yaml

@@ -27,7 +27,7 @@ robot:
   # q_init: [0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583, 0.0, 0.865, -1.583]  # h_com = 0.2
   # q_init: [0.0, 0.764, -1.407, 0.0, 0.76407, -1.4, 0.0, 0.76407, -1.407, 0.0, 0.764, -1.407]  # h_com = 0.218
   q_init: [0.0, 0.7, -1.4, 0.0, 0.7, -1.4, 0.0, -0.7, 1.4, 0.0, -0.7, 1.4]  # Initial articular positions
-  h_ref: 0.260748  # Reference height for the base
+  pose_init: [0.0, 0.0, 0.260748, 0.0, 0.0, 0.0, 1.0]  # Initiel base pose
   window_size: 2  # MPC window size
   dt_wbc: 0.001  # Time step of the whole body control
   dt_mpc: 0.012  # Time step of the model predictive control

include/qrw/Joystick.hpp

@@ -7,8 +7,7 @@
 ///
 //////////////////////////////////////////////////////////////////////////////////////////////////
 
-#ifndef JOYSTICK_H_INCLUDED
-#define JOYSTICK_H_INCLUDED
+#pragma once
 
 #include <chrono>
 #include <linux/joystick.h>
@@ -102,5 +101,3 @@ class Joystick : public AnimatorBase {
   int js;                  // Identifier of the gamepad object
   struct js_event event;   // Gamepad event object
 };
-
-#endif  // JOYSTICK_H_INCLUDED

include/qrw/Params.hpp

@@ -109,9 +109,10 @@ struct Params {
   bool mpc_in_rosnode;    // Run the MPC on a separate rosnode
 
   // General control parameters
-  VectorN q_init;  // Initial articular positions
-  Scalar dt_wbc;   // Time step of the whole body control
-  Scalar dt_mpc;   // Time step of the model predictive control
+  VectorN q_init;     // Initial articular positions
+  VectorN pose_init;  // Initial base pose
+  Scalar dt_wbc;      // Time step of the whole body control
+  Scalar dt_mpc;      // Time step of the model predictive control
   int mpc_wbc_ratio;
   uint N_periods;                        // Number of gait periods in the MPC prediction horizon
   bool save_guess;                       // true to save the initial result of the mpc
@@ -147,7 +148,6 @@ struct Params {
   Scalar T_gait;                      // Period of the gait
   int N_gait;                         // Number of steps in gait
   uint window_size;                   // Window size
-  Scalar h_ref;                       // Reference height for the base
   VectorN footsteps_under_shoulders;  // Positions of footsteps to be "under the shoulder"
 
   YAML::Node task;

python/expose-animator.cpp

@@ -16,6 +16,7 @@ void exposeAnimators() {
 #ifdef QRW_JOYSTICK_SUPPORT
   bp::class_<Joystick, bp::bases<AnimatorBase>>(
       "Joystick", "Animator using an external joystick peripheral.", bp::no_init)
+      .def(bp::init<const Params&>(bp::args("self", "params")))
       .def("get_joystick_code", &Joystick::getJoystickCode, bp::args("self"), "Get Joystick Code")
       .def("get_start", &Joystick::getStart, bp::args("self"), "Get Joystick Start")
       .def("get_stop", &Joystick::getStop, bp::args("self"), "Get Joystick Stop")

python/expose-params.cpp

@@ -56,6 +56,7 @@ void exposeParams() {
       .def_readonly("dt_wbc", &Params::dt_wbc)
       .def_readonly("env_id", &Params::env_id)
       .def_readonly("q_init", &Params::q_init)
+      .def_readonly("pose_init", &Params::pose_init)
       .def_readonly("dt_mpc", &Params::dt_mpc)
       .def_readonly("mpc_wbc_ratio", &Params::mpc_wbc_ratio)
       .def_readonly("N_periods", &Params::N_periods)
@@ -87,7 +88,6 @@ void exposeParams() {
       .def_readonly("T_gait", &Params::T_gait)
       .def_readonly("N_gait", &Params::N_gait)
       .def_readonly("window_size", &Params::window_size)
-      .def_readonly("h_ref", &Params::h_ref)
       .def_readonly("footsteps_under_shoulders", &Params::footsteps_under_shoulders)
       .add_property("task", bp::make_getter(&Params::task, rvp_by_value))
       .add_property("sim", bp::make_getter(&Params::sim, rvp_by_value));

python/quadruped_reactive_walking/controller.py

@@ -32,13 +32,10 @@ def __init__(self, params):
 
 
 class DummyDevice:
-    def __init__(self, h):
+    def __init__(self, base_pose):
         self.imu = self.IMU()
         self.joints = self.Joints()
-        self.base_position = np.zeros(3)
-        self.base_position[2] = 0.1944
-        self.b_base_velocity = np.zeros(3)
-        self.baseState = ((0.0, 0.0, h), (0.0, 0.0, 0.0, 1.0))
+        self.baseState = (base_pose[:3], base_pose[3:])
         self.baseVel = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
 
     class IMU:
@@ -121,7 +118,7 @@ def __init__(self, params: qrw.Params, q_init, solver_cls: Type[wb_mpc.OCPAbstra
         self.filter_q = qrw.LowPassFilter(params)
         self.filter_v = qrw.LowPassFilter(params)
 
-        device = DummyDevice(params.h_ref)
+        device = DummyDevice(params.pose_init)
         device.joints.positions = q_init
         self.compute(device)
 

python/quadruped_reactive_walking/main_solo12_control.py

@@ -194,6 +194,7 @@ def main(args):
 
     if params.SIMULATION:
         device.Init(
+            params.pose_init,
             q_init,
             params.env_id,
             params.use_flat_plane,

python/quadruped_reactive_walking/tools/pybullet_sim.py

@@ -29,7 +29,7 @@ class PybulletWrapper:
         dt (float): time step of the inverse dynamics
     """
 
-    def __init__(self, q_init, env_id, use_flat_plane, enable_pyb_GUI, dt=0.001):
+    def __init__(self, pose_init, q_init, env_id, use_flat_plane, enable_pyb_GUI, dt=0.001):
         self.applied_force = np.zeros(3)
         self.enable_gui = enable_pyb_GUI
         GUI_OPTIONS = "--width={} --height={}".format(VIDEO_CONFIG["width"], VIDEO_CONFIG["height"])
@@ -275,14 +275,14 @@ def __init__(self, q_init, env_id, use_flat_plane, enable_pyb_GUI, dt=0.001):
             if lowest_dist >= 0.001:
                 break  # Robot is above ground already
 
-            z_offset += -lowest_dist  # raise the robot start pos
+            z_offset -= lowest_dist  # raise the robot start pos
             z_offset += 0.01  # give an extra centimeter margin
 
             # Set base pose
             pyb.resetBasePositionAndOrientation(
                 self.robotId,
-                [0.0, 0.0, z_offset],
-                pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
+                [pose_init[0], pose_init[1], z_offset],  # Ignore Z_height to put the robot on the ground by default
+                pose_init[3:],  # pin.Quaternion(pin.rpy.rpyToMatrix(p_roll, p_pitch, 0.0)).coeffs(),
             )
 
         # Fix the base in the world frame
@@ -692,7 +692,7 @@ def __init__(self, record_video=False):
         self.record_video = record_video
         self.video_frames = []
 
-    def Init(self, q, env_id, use_flat_plane, enable_pyb_GUI, dt):
+    def Init(self, pose_init, q, env_id, use_flat_plane, enable_pyb_GUI, dt):
         """
         Initialize the PyBullet simultor with a given environment and a given state of the robot
 
@@ -704,7 +704,7 @@ def Init(self, q, env_id, use_flat_plane, enable_pyb_GUI, dt):
             enable_pyb_GUI (bool): to display PyBullet GUI or not
             dt (float): time step of the simulation
         """
-        self.pyb_sim = PybulletWrapper(q, env_id, use_flat_plane, enable_pyb_GUI, dt)
+        self.pyb_sim = PybulletWrapper(pose_init, q, env_id, use_flat_plane, enable_pyb_GUI, dt)
         self.q_init = q
         self.joints.positions[:] = q
         self.dt = dt

python/quadruped_reactive_walking/wb_mpc/target.py

@@ -16,7 +16,7 @@ def __init__(self, params):
         self.base_vel_ref = pin.Motion()
 
         if params.movement == "base_circle":
-            self.initial_base = np.array([0.0, 0.0, params.h_ref])
+            self.initial_base = params.pose_init[:3]
             self.A = np.array([0.02, 0.015, 0.0])
             self.offset = np.array([0.0, 0.0, 0.0])
             self.freq = np.array([0.5, 0.5, 0.0])
@@ -106,15 +106,14 @@ def update_interpolator(self, initial, target, velocity):
 
 def make_footsteps_and_refs(params, target: Target):
     """
-    Build a list of both footstep position and base pose references.
+    Build a list of both footstep position and base velocity references.
     Footsteps is a list of 3,4-matrices
-    Base_vel_refs is a list of 6-vectors (6D velocity references)
+    Base_vel_refs is a list of pin.Motion (6D values)
     """
     footsteps = []
     base_refs = []
     for k in range(params.N_gait):
-        target_base_vel = np.array([0.0, 0.0, params.h_ref, 0.0, 0.0, 0.0])
-        target_base_vel = pin.Motion(target_base_vel)
+        target_base_vel = pin.Motion(np.zeros(6))
         kk = k * params.mpc_wbc_ratio
         target_footstep = target.compute(kk).copy()
 

python/quadruped_reactive_walking/wb_mpc/task_spec.py

@@ -8,7 +8,7 @@ class TaskSpecBase:
     def __init__(self, params: Params, frozen_names=[]):
         self.robot = erd.load("solo12")
         self.q0 = self.robot.q0
-        self.q0[:7] = np.array([0.0, 0.0, params.h_ref, 0, 0, 0, 1])
+        self.q0[:7] = params.pose_init
         self.q0[7:] = params.q_init
 
         self.model: pin.Model = self.robot.model

src/Estimator.cpp

@@ -72,10 +72,10 @@ void Estimator::initialize(Params &params) {
   alphaSecurity_ = -y + std::sqrt(y * y + 2 * y);
 
   // Initialize Quantities
-  basePositionFK_(2) = params.h_ref;
+  basePositionFK_ = params.pose_init.head(3);
   velocityFilter_.initialize(dt_, Vector3::Zero(), Vector3::Zero());
   positionFilter_.initialize(dt_, Vector3::Zero(), basePositionFK_);
-  qRef_(2, 0) = params.h_ref;
+  qRef_.head(7) = params.pose_init;
   qRef_.tail(12) = Vector12(params.q_init.data());
 
   // Initialize Pinocchio

src/Joystick.cpp

@@ -3,13 +3,18 @@
 #include <fcntl.h>
 #include <stdio.h>
 #include <unistd.h>
+#include <pinocchio/math/rpy.hpp>
+#include <Eigen/Geometry>
 
 Joystick::Joystick(Params const& params)
     : AnimatorBase(params), p_gp_(Vector6::Zero()), v_gp_(Vector6::Zero()), v_ref_heavy_filter_(Vector6::Zero()) {
   gp_alpha_vel = params.gp_alpha_vel;
   gp_alpha_pos = 0.0;
-  p_ref_.setZero();
-  p_ref_(2, 0) = params.h_ref;
+  p_ref_.head(3) = params_->pose_init.head(3);
+  p_ref_.tail(3) = pinocchio::rpy::matrixToRpy(
+      Eigen::Quaternion<double>(
+          params_->pose_init(3), params_->pose_init(4), params_->pose_init(5), params_->pose_init(6))
+          .toRotationMatrix());
 
   lock_time_L1_ = std::chrono::system_clock::now();
 
@@ -99,7 +104,7 @@ void Joystick::update_v_ref(int k, bool gait_is_static) {
   // Retrieve data from gamepad for velocity
   double pRoll = gamepad.v_x * pRollScale;
   double pPitch = gamepad.v_y * pPitchScale;
-  double pHeight = gamepad.v_z * pHeightScale + params_->h_ref;
+  double pHeight = gamepad.v_z * pHeightScale + params_->pose_init(2);
   double pYaw = gamepad.w_yaw * pYawScale;
   p_gp_ << 0.0, 0.0, pHeight, pRoll, pPitch, pYaw;
 
@@ -150,8 +155,11 @@ void Joystick::update_v_ref(int k, bool gait_is_static) {
     } else if (lock_gp) {
       lock_gp = false;
       gp_alpha_vel = params_->gp_alpha_vel;
-      p_ref_.setZero();
-      p_ref_(2, 0) = params_->h_ref;
+      p_ref_.head(3) = params_->pose_init.head(3);
+      p_ref_.tail(3) = pinocchio::rpy::matrixToRpy(
+          Eigen::Quaternion<double>(
+              params_->pose_init(3), params_->pose_init(4), params_->pose_init(5), params_->pose_init(6))
+              .toRotationMatrix());
       gp_alpha_pos = 0.0;
     }
   }

src/Params.cpp

@@ -32,6 +32,7 @@ Params::Params()
       mpc_in_rosnode(false),
 
       q_init(12),  // Fill with zeros, will be filled with values later
+      pose_init(7),
       dt_wbc(0.0),
       dt_mpc(0.0),
       N_periods(0),
@@ -57,13 +58,13 @@ Params::Params()
       v_switch(),
       fc_v_esti(0.0),
 
-      T_gait(0.0),  // Period of the gait
-      h_ref(0.0),
+      T_gait(0.0),                   // Period of the gait
       footsteps_under_shoulders(12)  // Fill with zeros, will be filled with values later
 {
   Kp_main.setZero();
   Kd_main.setZero();
   q_init.setZero();
+  pose_init.setZero();
   footsteps_under_shoulders.setZero();
 }
 
@@ -154,8 +155,8 @@ bool convert<Params>::decode(const Node &robot_node, Params &rhs) {
   assert_yaml_parsing(robot_node, "robot", "q_init");
   YAML::convert<VectorN>::decode(robot_node["q_init"], rhs.q_init);
 
-  assert_yaml_parsing(robot_node, "robot", "h_ref");
-  rhs.h_ref = robot_node["h_ref"].as<Scalar>();
+  assert_yaml_parsing(robot_node, "robot", "pose_init");
+  YAML::convert<VectorN>::decode(robot_node["pose_init"], rhs.pose_init);
 
   assert_yaml_parsing(robot_node, "robot", "window_size");
   rhs.window_size = robot_node["window_size"].as<uint>();