Merge pull request #6 from inria-paris-robotics-lab/change_formatting

Change auto formatting

.pre-commit-config.yaml

@@ -3,7 +3,7 @@ repos:
     rev: v17.0.2
     hooks:
     -   id: clang-format
-        args: ['--style={BasedOnStyle: Google, SortIncludes: false}']
+        args: ['--style={BasedOnStyle: Google, SortIncludes: false, ColumnLimit: 120, BinPackParameters: false, BinPackArguments: false}']
 -   repo: meta
     hooks:
     -   id: check-useless-excludes
@@ -27,6 +27,7 @@ repos:
     rev: 23.9.1
     hooks:
     -   id: black
+        args: ["--line-length", "120"]
 -   repo: https://github.com/charliermarsh/ruff-pre-commit
     # Ruff version.
     rev: v0.0.292

compare_logs.py

@@ -93,11 +93,7 @@ def infNorm(x, **kwargs):
 plt.yscale("log")
 plt.legend()
 plt.grid(which="both")
-plt.title(
-    "Comparison between {:s} and {:s}".format(
-        get_solver_name(data1), get_solver_name(data2)
-    )
-)
+plt.title("Comparison between {:s} and {:s}".format(get_solver_name(data1), get_solver_name(data2)))
 
 # Error over time & mpc iteration
 Xerr_over_time = infNorm(Xerr[::WBC_RATIO], axis=2)

examples/bench_croc_ocp.py

@@ -75,23 +75,9 @@ def runShootingProblemCalcDiffBenchmark(xs, us, problem):
 print("Python bindings:")
 xs, us, problem = createProblem()
 avrg_duration, min_duration, max_duration = runDDPSolveBenchmark(xs, us, problem)
-print(
-    "  DDP.solve [ms]: {0} ({1}, {2})".format(avrg_duration, min_duration, max_duration)
-)
-avrg_duration, min_duration, max_duration = runShootingProblemCalcBenchmark(
-    xs, us, problem
-)
-print(
-    "  ShootingProblem.calc [ms]: {0} ({1}, {2})".format(
-        avrg_duration, min_duration, max_duration
-    )
-)
-avrg_duration, min_duration, max_duration = runShootingProblemCalcDiffBenchmark(
-    xs, us, problem
-)
-print(
-    "  ShootingProblem.calcDiff [ms]: {0} ({1}, {2})".format(
-        avrg_duration, min_duration, max_duration
-    )
-)
+print("  DDP.solve [ms]: {0} ({1}, {2})".format(avrg_duration, min_duration, max_duration))
+avrg_duration, min_duration, max_duration = runShootingProblemCalcBenchmark(xs, us, problem)
+print("  ShootingProblem.calc [ms]: {0} ({1}, {2})".format(avrg_duration, min_duration, max_duration))
+avrg_duration, min_duration, max_duration = runShootingProblemCalcDiffBenchmark(xs, us, problem)
+print("  ShootingProblem.calcDiff [ms]: {0} ({1}, {2})".format(avrg_duration, min_duration, max_duration))
 print("\033[0m")

include/qrw/Estimator.hpp

@@ -31,22 +31,24 @@ class Estimator {
   /// \brief Run one iteration of the estimator to get the position and velocity
   /// states of the robot
   ///
-  /// \param[in] gait Gait matrix that stores current and future contact status
-  /// of the feet
+  /// \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] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated)
   /// \param[in] baseAngularVelocity Angular velocity of the IMU
   /// \param[in] baseOrientation Quaternion orientation of the IMU
   /// \param[in] q_mes Position of the 12 actuators
   /// \param[in] v_mes Velocity of the 12 actuators
   /// \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,
+  void run(MatrixN const& gait,
+           MatrixN const& goals,
            VectorN const& baseLinearAcceleration,
-           VectorN const& baseAngularVelocity, VectorN const& baseOrientation,
-           VectorN const& q_mes, VectorN const& v_mes,
-           VectorN const& perfectPosition, Vector3 const& b_perfectVelocity);
+           VectorN const& baseAngularVelocity,
+           VectorN const& baseOrientation,
+           VectorN const& q_mes,
+           VectorN const& v_mes,
+           VectorN const& perfectPosition,
+           Vector3 const& b_perfectVelocity);
 
   /// \brief Update state vectors of the robot (q and v)
   ///        Update transformation matrices between world and horizontal frames
@@ -90,8 +92,8 @@ class Estimator {
  private:
   /// \brief Retrieve and update IMU data
   ///
-  /// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity
-  /// compensated) \param[in] baseAngularVelocity Angular velocity of the IMU
+  /// \param[in] baseLinearAcceleration Linear acceleration of the IMU (gravity compensated)
+  /// \param[in] baseAngularVelocity Angular velocity of the IMU
   /// \param[in] baseOrientation Euler orientation of the IMU
   void updateIMUData(Vector3 const& baseLinearAcceleration,
                      Vector3 const& baseAngularVelocity,
@@ -157,67 +159,50 @@ class Estimator {
   /// \brief Filter the estimated velocity over a moving window
   void filterVelocity();
 
-  bool perfectEstimator_;  //< Enable perfect estimator (directly from the
-                           // PyBullet simulation)
+  bool perfectEstimator_;  //< Enable perfect estimator (directly from the PyBullet simulation)
   bool solo3D_;            //< Perfect estimator including yaw angle
   double dt_;              //< Time step of the estimator
   bool initialized_;       //< Is intiialized after the first update of the IMU
   Vector4i feetFrames_;    //< Frame indexes of the four feet
   double footRadius_;      //< radius of a foot
-  Vector3
-      alphaPos_;  // w Alpha coeefficient for the position complementary filter
-  double alphaVelMax_;  // w Maximum alpha value for the velocity complementary
-                        // filter
-  double alphaVelMin_;  // w Minimum alpha value for the velocity complementary
-                        // filter
-  double alphaSecurity_;  // w Low pass coefficient for the outputted filtered
-                          // velocity for security check
-
-  pinocchio::SE3
-      b_M_IMU_;          //< Transform between the base frame and the IMU frame
-  double IMUYawOffset_;  //< Yaw orientation of the IMU at startup
-  Vector3 IMULinearAcceleration_;  //< Linear acceleration of the IMU (gravity
-                                   // compensated)
-  Vector3 IMUAngularVelocity_;     //< Angular velocity of the IMU
-  Vector3 IMURpy_;                 //< Roll Pitch Yaw orientation of the IMU
+  Vector3 alphaPos_;       // w Alpha coeefficient for the position complementary filter
+  double alphaVelMax_;     // w Maximum alpha value for the velocity complementary filter
+  double alphaVelMin_;     // w Minimum alpha value for the velocity complementary filter
+  double alphaSecurity_;   // w Low pass coefficient for the outputted filtered velocity for security check
+
+  pinocchio::SE3 b_M_IMU_;              //< Transform between the base frame and the IMU frame
+  double IMUYawOffset_;                 //< Yaw orientation of the IMU at startup
+  Vector3 IMULinearAcceleration_;       //< Linear acceleration of the IMU (gravity compensated)
+  Vector3 IMUAngularVelocity_;          //< Angular velocity of the IMU
+  Vector3 IMURpy_;                      //< Roll Pitch Yaw orientation of the IMU
   pinocchio::SE3::Quaternion IMUQuat_;  // Quaternion orientation of the IMU
 
   Vector12 qActuators_;  //< Measured positions of actuators
   Vector12 vActuators_;  //< Measured velocities of actuators
 
-  int phaseRemainingDuration_;  //< Number of iterations left for the current
-                                // gait phase
-  Vector4 feetStancePhaseDuration_;  //< Number of loops during which each foot
-                                     // has been in contact
+  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)
+  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
 
-  ComplementaryFilter
-      positionFilter_;  //< Complementary filter for base position
-  ComplementaryFilter
-      velocityFilter_;  //< Complementary filter for base velocity
-  Vector19 qEstimate_;  //< Filtered output configuration
-  Vector18 vEstimate_;  //< Filtered output velocity
-  Vector12 vSecurity_;  //< Filtered output velocity for security check
+  ComplementaryFilter positionFilter_;  //< Complementary filter for base position
+  ComplementaryFilter velocityFilter_;  //< Complementary filter for base velocity
+  Vector19 qEstimate_;                  //< Filtered output configuration
+  Vector18 vEstimate_;                  //< Filtered output velocity
+  Vector12 vSecurity_;                  //< Filtered output velocity for security check
 
-  uint windowSize_;    //< Number of samples in the averaging window
-  Vector6 vFiltered_;  //< Base velocity (in base frame) filtered by averaging
-                       // window
-  std::deque<double> vx_queue_, vy_queue_,
-      vz_queue_;  //< Queues that hold samples
+  uint windowSize_;                                    //< Number of samples in the averaging window
+  Vector6 vFiltered_;                                  //< Base velocity (in base frame) filtered by averaging window
+  std::deque<double> vx_queue_, vy_queue_, vz_queue_;  //< Queues that hold samples
 
   Vector18 qRef_;        //< Configuration vector in ideal world frame
   Vector18 vRef_;        //< Velocity vector in ideal world frame
@@ -227,7 +212,6 @@ class Estimator {
   Matrix3 hRb_;          //< Rotation between base and horizontal frame
   Vector3 oTh_;          //< Translation between horizontal and world frame
   Vector6 h_v_;          //< Velocity vector in horizontal frame
-  Vector6 h_vFiltered_;  //< Base velocity (in horizontal frame) filtered by
-                         //< averaging window
+  Vector6 h_vFiltered_;  //< Base velocity (in horizontal frame) filtered by averaging window
 };
 #endif  // ESTIMATOR_H_INCLUDED

include/qrw/IMPCWrapper.hpp

@@ -16,8 +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, Vector4 footstep, Motion base_vel_ref) = 0;
   virtual MPCResult get_latest_result() const = 0;
 };
 

include/qrw/IOCPAbstract.hpp

@@ -14,8 +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, Matrix34 footsteps, Motion base_vel_ref) = 0;
   virtual void solve(std::size_t k) = 0;
 
   Params params_;

include/qrw/Joystick.hpp

@@ -63,22 +63,19 @@ class Joystick : public AnimatorBase {
   bool getR1() { return gamepad.R1 == 1; }
 
  private:
-  Vector6 p_gp_;  // Raw position reference of the gamepad
-  Vector6 v_gp_;  // Raw velocity reference of the gamepad
-  Vector6
-      v_ref_heavy_filter_;  // Reference velocity after heavy low pass filter
+  Vector6 p_gp_;                // Raw position reference of the gamepad
+  Vector6 v_gp_;                // Raw velocity reference of the gamepad
+  Vector6 v_ref_heavy_filter_;  // Reference velocity after heavy low pass filter
 
   int joystick_code_ = 0;  // Code to trigger gait changes
   bool stop_ = false;      // Flag to stop the controller
   bool start_ = false;     // Flag to start the controller
 
   // How much the gamepad velocity and position is filtered to avoid sharp
   // changes
-  double gp_alpha_vel =
-      0.0;  // Low pass filter coefficient for v_ref_ (if gamepad-controlled)
-  double gp_alpha_pos = 0.0;  // Low pass filter coefficient for p_ref_
-  double gp_alpha_vel_heavy_filter =
-      0.002;  // Low pass filter coefficient for v_ref_heavy_filter_
+  double gp_alpha_vel = 0.0;                 // Low pass filter coefficient for v_ref_ (if gamepad-controlled)
+  double gp_alpha_pos = 0.0;                 // Low pass filter coefficient for p_ref_
+  double gp_alpha_vel_heavy_filter = 0.002;  // Low pass filter coefficient for v_ref_heavy_filter_
 
   // Maximum velocity values
   double vXScale = 0.3;    // Lateral
@@ -93,13 +90,11 @@ class Joystick : public AnimatorBase {
 
   // Variable to handle the automatic static/trot switching
   bool switch_static = false;   // Flag to switch to a static gait
-  bool lock_gp = true;          // Flag to lock the output velocity when we are
-                                // switching back to trot gait
+  bool lock_gp = true;          // Flag to lock the output velocity when we are switching back to trot gait
   double lock_duration_ = 1.0;  // Duration of the lock in seconds
-  std::chrono::time_point<std::chrono::system_clock>
-      lock_time_static_;  // Timestamp of the start of the lock
-  std::chrono::time_point<std::chrono::system_clock>
-      lock_time_L1_;  // Timestamp of the latest L1 pressing
+
+  std::chrono::time_point<std::chrono::system_clock> lock_time_static_;  // Timestamp of the start of the lock
+  std::chrono::time_point<std::chrono::system_clock> lock_time_L1_;      // Timestamp of the latest L1 pressing
 
   // Gamepad client variables
   gamepad_status gamepad;  // Structure that stores gamepad status

include/qrw/LowPassFilter.hpp

@@ -43,10 +43,8 @@ class LowPassFilter {
   VectorN y_;      // Latest result
   Vector6 accum_;  // Used to compute the result (accumulation)
 
-  std::deque<Vector6> x_queue_;  // Store the last measurements for product with
-                                 // denominator coefficients
-  std::deque<Vector6> y_queue_;  // Store the last results for product with
-                                 // numerator coefficients
+  std::deque<Vector6> x_queue_;  // Store the last measurements for product with denominator coefficients
+  std::deque<Vector6> y_queue_;  // Store the last results for product with numerator coefficients
 
   bool init_;  // Initialisation flag
 };

include/qrw/MPCResult.hpp

@@ -22,8 +22,7 @@ struct MPCResult {
     gait.setZero();
   }
 
-  MPCResult(uint Ngait, uint nx, uint nu, uint ndx)
-      : MPCResult(Ngait, nx, nu, ndx, Ngait) {}
+  MPCResult(uint Ngait, uint nx, uint nu, uint ndx) : MPCResult(Ngait, nx, nu, ndx, Ngait) {}
 
   uint get_window_size() { return static_cast<uint>(us.size()); }
 };