From 0451dc1e6b481d3494c0eeb149697908b9589797 Mon Sep 17 00:00:00 2001 From: EtienneAr Date: Wed, 14 Feb 2024 18:13:02 +0100 Subject: [PATCH] Take pose_init param into account when spawing the robot in pybullet --- .../main_solo12_control.py | 1 + .../quadruped_reactive_walking/tools/pybullet_sim.py | 12 ++++++------ src/Joystick.cpp | 2 +- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py index a14b75cb..fb9b5861 100644 --- a/python/quadruped_reactive_walking/main_solo12_control.py +++ b/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, diff --git a/python/quadruped_reactive_walking/tools/pybullet_sim.py b/python/quadruped_reactive_walking/tools/pybullet_sim.py index e22b9540..9892617f 100644 --- a/python/quadruped_reactive_walking/tools/pybullet_sim.py +++ b/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 diff --git a/src/Joystick.cpp b/src/Joystick.cpp index 5d5011e6..4d692ddb 100644 --- a/src/Joystick.cpp +++ b/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;