Take pose_init param into account when spawing the robot in pybullet

python/quadruped_reactive_walking/main_solo12_control.py

@@ -196,6 +196,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(
@@ -283,14 +283,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
@@ -702,7 +702,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
 
@@ -714,7 +714,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

src/Joystick.cpp

@@ -110,7 +110,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;