diff --git a/include/qrw/Params.hpp b/include/qrw/Params.hpp index 4a5209d..a659678 100644 --- a/include/qrw/Params.hpp +++ b/include/qrw/Params.hpp @@ -109,10 +109,10 @@ struct Params { bool mpc_in_rosnode; // Run the MPC on a separate rosnode // General control parameters - 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 + 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 diff --git a/python/quadruped_reactive_walking/controller.py b/python/quadruped_reactive_walking/controller.py index db3838e..68458ea 100644 --- a/python/quadruped_reactive_walking/controller.py +++ b/python/quadruped_reactive_walking/controller.py @@ -187,6 +187,7 @@ def compute(self, device, qc=None): self.mpc.solve(self.k, x, self.target_footstep.copy(), self.target_base.copy()) except ValueError: import traceback + self.error = True traceback.print_exc() diff --git a/python/quadruped_reactive_walking/ocp_defs/walking.py b/python/quadruped_reactive_walking/ocp_defs/walking.py index 234ce36..fd62ddf 100644 --- a/python/quadruped_reactive_walking/ocp_defs/walking.py +++ b/python/quadruped_reactive_walking/ocp_defs/walking.py @@ -30,8 +30,8 @@ def __init__(self, params: Params, footsteps, base_vel_refs): self.rdata = self.rmodel.createData() self.life_gait = params.gait - self.starting_gait = np.array([[1,1,0,0]] * params.starting_nodes, dtype=np.int32) - self.ending_gait = np.array([[1,1,0,0]] * params.starting_nodes, dtype=np.int32) + self.starting_gait = np.array([[1, 1, 0, 0]] * params.starting_nodes, dtype=np.int32) + self.ending_gait = np.array([[1, 1, 0, 0]] * params.starting_nodes, dtype=np.int32) self.current_gait = np.append( self.starting_gait, self.ending_gait[0].reshape(1, -1), diff --git a/python/quadruped_reactive_walking/tools/pybullet_sim.py b/python/quadruped_reactive_walking/tools/pybullet_sim.py index e99a8bd..f9e790e 100644 --- a/python/quadruped_reactive_walking/tools/pybullet_sim.py +++ b/python/quadruped_reactive_walking/tools/pybullet_sim.py @@ -281,8 +281,8 @@ def __init__(self, pose_init, q_init, env_id, use_flat_plane, enable_pyb_GUI, dt # Set base pose pyb.resetBasePositionAndOrientation( self.robotId, - [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(), + [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 diff --git a/python/quadruped_reactive_walking/wb_mpc/task_spec.py b/python/quadruped_reactive_walking/wb_mpc/task_spec.py index dc19cf1..8908453 100644 --- a/python/quadruped_reactive_walking/wb_mpc/task_spec.py +++ b/python/quadruped_reactive_walking/wb_mpc/task_spec.py @@ -120,5 +120,3 @@ def __init__(self, params: Params): self.xref = self.x0_reduced # self.uref = self.u0 self.u0 = self.uref - - diff --git a/src/Joystick.cpp b/src/Joystick.cpp index 8f28aef..d9c6434 100644 --- a/src/Joystick.cpp +++ b/src/Joystick.cpp @@ -12,12 +12,9 @@ Joystick::Joystick(Params const& params) gp_alpha_pos = 0.0; p_ref_.head(3) = params_->pose_init.head(3); p_ref_.tail(3) = pinocchio::rpy::matrixToRpy( - Eigen::Quaternion( - params_->pose_init(3), - params_->pose_init(4), - params_->pose_init(5), - params_->pose_init(6) - ).toRotationMatrix()); + Eigen::Quaternion( + 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(); @@ -160,12 +157,9 @@ void Joystick::update_v_ref(int k, bool gait_is_static) { gp_alpha_vel = params_->gp_alpha_vel; p_ref_.head(3) = params_->pose_init.head(3); p_ref_.tail(3) = pinocchio::rpy::matrixToRpy( - Eigen::Quaternion( - params_->pose_init(3), - params_->pose_init(4), - params_->pose_init(5), - params_->pose_init(6) - ).toRotationMatrix()); + Eigen::Quaternion( + params_->pose_init(3), params_->pose_init(4), params_->pose_init(5), params_->pose_init(6)) + .toRotationMatrix()); gp_alpha_pos = 0.0; } } diff --git a/src/Params.cpp b/src/Params.cpp index cbdaa54..e76d0ac 100644 --- a/src/Params.cpp +++ b/src/Params.cpp @@ -58,7 +58,7 @@ Params::Params() v_switch(), fc_v_esti(0.0), - T_gait(0.0), // Period of the gait + T_gait(0.0), // Period of the gait footsteps_under_shoulders(12) // Fill with zeros, will be filled with values later { Kp_main.setZero();