Stack updates from devel branch (#234)

* update orientation transforms in ground_truth_publisher * Correct angular velocity in ground_truth_publisher, orientation still wrong * fix mocap_optitrack rotation, so far untested * Add checkMessages to ID * Fix last_state_time bug in ID * Improve quad_mpc weights * Fix LFP bug computing midstance at end of horizon * Correct angular velocity typo in ground_truth_publisher * add nonlinear MPC for both global and local footstep planner, add IPOPT installation script * keep empty ipopt folder * Add centrifugal terms to footstep planner * update ground truth publisher to subs to /mcu/state/vel * fix vel_sub_ header * Fix wrapping in GBP, change mpc horizon to 3 periods * Update walking speed * Add mesh to grid map converter * Add other basic step environments, have spirit_gazebo.launch load them * Fix GBP replanning and use actual path length * Update footstep planner to add velocity tracking term, use old solutions for linearization and initial foot positions * better warm start * Adding subsampled local plan poses to rviz to visualize planned orientation and not just CG path. * Fix contact plugin by adding publisher * Change traj publisher to only set joint info for full trajectories * Fix GRF bug (zero in x direction for leg 0) * fix angular velocity frame in NMPC * add rbdl, use rbdl for jacobian * Update mpc controller horizon * Update GBP to output ang vel in body frame * move jacobian into kinematics, fix all getJacobian calls, fix convex MPC angular velocity * Adding orientation and foot force tests to test_kinematics. * Get contact forces in the correct frame * Updating test_kinematics to work with RBDL and debugging URDF parameter loading issue. * fix quadruped_mpc_test fail * WIP swing leg inverse dynamics * Unify urdf/sdf parameter loading, fix quad_mpc test bug * merge angvel fix and delete the hotfix in nmpc * Add very basic input saturation warning to ID * add inverse dynamics for swing leg * fix inverse dynamics for swing leg * Another force and torque test * add parameter to switch between convex mpc and nonlinear mpc * fix matrix inverse in inverse dynamics * add nan and inf check for inverse dynamics, fix load param issues when testing * little improvement on nmpc * better warmstart for nmpc * Add grid map filters, use grid maps in GBP and LP * Start adding action and stater classes * Update global plan visualization to include different colors for different primitives * Add visualization features to GBP to animate tree growth * Switch to world frame ang vel in quadruped_mpc * Modify rviz view * Use current foot positions for initialization in LP * Switch to nominal velocity for target states rather than random, add #define support for plotting trajectories * Start switching local planner to variable timesteps * Switch ID to continuous time interp * Re-add additional footstep heuristic terms * delete unused rbdl files * Fix test_quadruped_mpc.cpp * Moved twist input into local_planner directly rather than feeding twist inputs through twist_body_planner * rename variable * Remove .obj file for room test * fix nan when liftoff is the terminal state * Update LP convex gains and remove time limit * Continue SLIP updates, add test for action commutation * Further reverse stance debugging * Initial silly walk commit * Update for better directions * Update CMakeLists and launchfiles to make template clearer * Minor changes * Update body state msg calls * Add legIKLegBaseFrame to kinematics class, fix bug in open_loop_controller test CMakeLists.txt * Initialized to zero position * Update init_remote.sh * Update init_remote.sh * Update remote and robot initialization scripts * Update launch_robot_env.sh * Update launch_robot_env.sh * Add convenient plan testing launch file * Update stance phase propagation * fix nmpc initialization, fix orientation pose plot in rviz * Update Status Badge * Initial commit * Further leg-controller updates * Simple bugfixes * Finish inverse dynamics class * Switch SpiritKinematics to QuadKD, switch inverse dynamics function to QuadKD * Fix swing torque bug * Unify computation of swing and stance torques * Bugfix * Fix torque mapping to index correctly past body coordinates * Put identified leg inertial parameters into spirit.sdf and spirit.urdf files. * Update GRFs in RViz Interface * Update RViz settings * Add GRF Array to inverse dynamics output * Remove branch restrictions from CircleCI config * Update tests for leg controller and inverse dynamics * Make leg parameter definitions uniform in QuadKD by loading leg lengths from RBDL-loaded urdf in QuadKD constructor * update NMPC, adaptive body height and constraints using terrain information * add new topic: /state/ground_truth_body_frame that independent on mocap and use imu twist * Fix most bugs in SLIP GRF parameterization, returns plans * Add rough terrain, adaptive body height and orientation, fix foot velocity pseudo inverse * Remove z filtering so SLIP profiles are unaltered * Fix step_20cm terrain shift * Begin gbp tweaks for efficiency, fix rotate GRF * Update closed form solution for linear dynamics in NMPC * Attempt to handle slopes by predicting z_max * Switch build system to catkin_tools, switch spirit* to quad* * Fix timeout in catkin_tools install * Reduce jobs in circle ci to avoid compiling issues * Initial commit * Fix spirit namespace in parsing scripts * fix sdls matrix inverse by comparing with the max eigen value, change nmpc controller test to a trotting at the same point * Add restart flag to mblink converter * Increase update rate of mblink converter to 500Hz * Update model.config * Update README.md * Update processLog.m * Update LICENSE * Update LICENSE.txt * Update LICENSE * Update global_body_planner.yaml * Update planning_utils.h * Update CMakeLists.txt Removed from line 31: include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../external/QuadSDK/SDK/thirdparty) * Change a few more namespacing instances, remove .travis.yml * Delete parseSpiritBag.m * Finish initial LegControllerTemplate integration * Add joint controller (needs cleaning) * Switch back to grf controller * Switch leg controller to leg controller interface, implement leg controller template as abstract class * Fix leg controller test bug * add panic variable, better warm start * fix inverse dynamics, fix gait round up problem * fix inverse dynamics, add hip clearance for footstep planner, revert plan index and current position for twist planner * set the control and estimation rate to 500Hz so as to match the real robot * Switch FastTerrainMap queries from O(n^2) to O(1) * Add getNearestValidFoothold function * Add traversability mask layer to terrain map * Switch to flying trot * Fix bugs in merge (missed some namespaces) * Switch foot markers to spheres * Fix timestamp bug from merge * fix swing foot ID * fix ID again * Begin cleanup of global body planner spin * Add GlobalBodyPlan class and clean up global_body_planner.cpp interface * Remove matlab gen msg * About to begin switch to Eigen for state * cleaned up the mpc stuff and removed dependencies * removed additional extra files going to write unitest for computeFootPos next * Added spirit_rotors.sdf with rotor inertias and launch flag to switch spirit.sdf/spirit_rotors.sdf. Also spirit.sdf upper link masses now match spirit.urdf (from inertial identification). * Correct bugs in gbp actions, add tests, update sim rate, add features to fast terrain map * removed global footstep planner as well as qpoases and osqp * First commit after gbp overhaul with valid leap plans, update some sim update rates, increase slope penalty on footholds * Fix some bugs in the merge * Reduce traversability threshold * clean up pull request * fix ipopt building error * delete qpOASES * specify coinbrew version * Fix out of range bug in FTM, connect timing bug in GBP, update sim timesteps for worlds * Reasonable GBP performance * Fix bug in NMPC test, rename time_ahead to first_element_duration * online update swing foot trajectory * fix merge issues * init nominal prev foot postion, fix interp when z duration = 0, update pass foothold more frequently * limit acceleration of swing foot * Clean up GBP formatting * Remove NMPC test requirement to solve problem given CIs limitations * better swing foot acceleration limit, fix acceleration interplation at the end, smaller feedback on velocity, consider toe radius * fix init of grf position * PR updates (mostly refactoring for cleanliness and const protection) * Rename GlobalBodyPlan functions * Unify stance and swing foot planning (#199) * Initial commit moving to computeFootPlan, not compiled yet * computeFootPlan works, beginning refactoring * Fix impact acceleration bug, switch to MultiFootState for past footholds, clean LFP * Fix timestamp for foot message, clean Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> * intermediate commit * Finihsed adding tests * intermediate commit before rebase * added pipeline fix * Addressed pr comments * Improve simulation and nmpc (#202) * Improve simulation friction, contact, and physics * Sync new nmpc updates * fix typo in warmstart initialization * Update ipopt options, update discretization * Fix another typo, and hopefully fix the coinbrew building error * update the rotor sdf Co-authored-by: Joe Norby <[email protected]> * Reformat to Google Style for C++ and Add CI Linting (#205) * Reformat all cpp, h, hpp files to google style, implement linting script and add to tests (one error inserted for CI testing) * Make scripts folder for cleaner root, fix test formatting error * Fix CircleCI script path * Format updates to pass linting checks * Update logging and planning launch files, remove global footstep planner (#209) * Update live plot to show ground truth grf, uniform colors and labels (#210) * Add teleop_twist_joy and configure to robot driver control modes (#211) * Add teleop_twist_joy and configure to robot driver control modes * Remove teleop_twist_joy tests of enable button, add cmd_vel to visualization * Remove EKF_tests_2021_12_3 * add grf plan initialization * update initialization * Add stand mode for local planner, remove twist body planner (#217) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove stray lines Co-authored-by: Justin Yim <[email protected]> * Improve swing foot plan (#214) * fix bugs, fix foothold heuristics, better swing foot acc limitation * remove online swing foot update, revert pd gains * Foothold nominal position minimize the maximum distance from hip plan MPC will not modify body and grf plan if fail to solve * fix centrifugal tracking term * Clean * Clean * Remove unused files and functions * Keep the original style * Fix some comments and warning info * Pass test * Resolve review requests * code format * Revert stand pose * Add getNextLiftoffIndex Use contact schedule for end_of_stance Move computeSwingApex to cpp * Fix test All the parameters should match the server... Co-authored-by: Joe Norby <[email protected]> * Clean unused or duplicate params in local planner and nmpc controller (#220) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update local planner test with new param Co-authored-by: Justin Yim <[email protected]> * Add dash profile for rqt and roslaunch support * Fix end of stance bug (#224) * Fix end of stance bug * Add comment clarifying logic * Add tcpnodelay to logging, sim control, and sim contact state publishing (#225) * Add tcpnodelay to controller plugin, contact state publisher, and logging scripts, change local plan timestamp to publish time not state time * Use auto for ros::Time declaration * Add CMU text to spirit.urdf and spirit_meshes.urdf * Realign xml text * Add tcpnodelay hint to sim command topic, may actually affect performance (should improve) * Add state traces to RVizInterface (#230) * Adjust local planner to support arbitrary horizons and match length of body and foot plans (#221) * Add stand mode for local planner, remove twist body planner * Fix weird formatting changes * Change stand threshold variables to ROS params, add cleaner statement of conditions * Update threshold for end of global plan to ROS param * Added teleop-twist-joy to setup_deps since joystick doesn't respond without this install. * Remove local_body_planner params (they are in nmpc yaml) and horizon length/timestep from nmpc (they are in local planner) * Remove stray lines * Update params in CMPC test and remove adaptive complexity test (will come in later PR) * Update local planner test with new param * Modify horizon lengths in local planner, nmpc, and tests * Update nmpc controller to support arbitrary horizons, change RVizInterface to show one frame per gait period * Add comment clarifying indexing difference * Fix body height bug in LFP * Update NMPC temporal weights to saturate at a given parameter (was unbounded before), reduce surface normal filter radius Co-authored-by: Justin Yim <[email protected]> Co-authored-by: Robomechanics Lab <[email protected]> Co-authored-by: RML group <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Justin Yim <[email protected]> Co-authored-by: James <[email protected]> Co-authored-by: justinyim <[email protected]> Co-authored-by: ardalantj <[email protected]> Co-authored-by: Yanhao Yang <[email protected]> Co-authored-by: Bill Yu <[email protected]> Co-authored-by: jrenaf <[email protected]> Co-authored-by: Jiming Ren <[email protected]>
robomechanics · Mar 28, 2022 · 0b6c0b5 · 0b6c0b5
1 parent e4f92e1
commit 0b6c0b5
Show file tree

Hide file tree

Showing 396 changed files with 24,970 additions and 41,708 deletions.
diff --git a/.circleci/config.yml b/.circleci/config.yml
@@ -30,6 +30,9 @@ jobs:
             source /opt/ros/melodic/setup.bash
             source /catkin_ws/devel/setup.bash
             catkin run_tests -j4 -l4 && catkin_test_results
+      - run:
+          name: "Run soft linting checks"
+          command: ./scripts/lint_soft.sh
 workflows:
   version: 2
   melodic-build-and-test:

diff --git a/.gitmodules b/.gitmodules
diff --git a/body_force_estimator/include/body_force_estimator/body_force_estimator.h b/body_force_estimator/include/body_force_estimator/body_force_estimator.h
@@ -1,83 +1,85 @@
 #ifndef BODY_FORCE_ESTIMATOR_H
 #define BODY_FORCE_ESTIMATOR_H
 
-#include <ros/ros.h>
-#include <quad_msgs/RobotState.h>
 #include <quad_msgs/BodyForceEstimate.h>
 #include <quad_msgs/GRFArray.h>
+#include <quad_msgs/RobotState.h>
+#include <ros/ros.h>
 
 // Temporary
-#define USE_SIM 1 // 0 = intended, 1 = Gazebo hack, 2 = old bagfile hack
+#define USE_SIM 1  // 0 = intended, 1 = Gazebo hack, 2 = old bagfile hack
 
 //! Estimates body contact forces
 /*!
-   BodyForceEstimator is a container for all logic used in estimating force from contacts distrbuted across all links of the robot.
-   It requires robot state estimates and motor commands and exposes an update method.
+   BodyForceEstimator is a container for all logic used in estimating force from
+   contacts distrbuted across all links of the robot. It requires robot state
+   estimates and motor commands and exposes an update method.
 */
 class BodyForceEstimator {
-  public:
-    /**
-     * @brief Constructor for BodyForceEstimator Class
-     * @param[in] nh ROS NodeHandle to publish and subscribe from
-     * @return Constructed object of type BodyForceEstimator
-     */
-    BodyForceEstimator(ros::NodeHandle nh);
-
-    /**
-     * @brief Calls ros spinOnce and pubs data at set frequency
-     */
-    void spin();
-
-    /**
-     * @brief Callback function to handle new state estimates
-     * @param[in] Robot state message contining position and velocity for each joint and robot body
-     */
-    #if USE_SIM > 0
-    void robotStateCallback(const sensor_msgs::JointState::ConstPtr& msg);
-    #else
-    void robotStateCallback(const quad_msgs::RobotState::ConstPtr& msg);
-    #endif
-
-    /**
-     * @brief Compute the momentum observer external force estimation update.
-     */
-    void update();
-
-    /**
-     * @brief Publish body force force estimates
-     */
-    void publishBodyForce();
-
-    /// ROS subscriber for the robot state
-    ros::Subscriber robot_state_sub_;
-
-    /// ROS publisher for body force force estimates
-    ros::Publisher body_force_pub_;
-
-    /// ROS publisher for toe force estimates
-    ros::Publisher toe_force_pub_;
-
-    /// Nodehandle to pub to and sub from
-    ros::NodeHandle nh_;
-
-    /// Update rate for sending and receiving data;
-    double update_rate_;
-
-    /// Momentum observer gain
-    double K_O_;
-
-private:
-    // External torque estimate
-    double r_mom[12];
-    // Momentum estimate
-    double p_hat[12];
-
-    // Robot state estimate
-    #if USE_SIM > 0
-    sensor_msgs::JointState::ConstPtr last_state_msg_;
-    #else
-    quad_msgs::RobotState::ConstPtr last_state_msg_;
-    #endif
+ public:
+  /**
+   * @brief Constructor for BodyForceEstimator Class
+   * @param[in] nh ROS NodeHandle to publish and subscribe from
+   * @return Constructed object of type BodyForceEstimator
+   */
+  BodyForceEstimator(ros::NodeHandle nh);
+
+  /**
+   * @brief Calls ros spinOnce and pubs data at set frequency
+   */
+  void spin();
+
+/**
+ * @brief Callback function to handle new state estimates
+ * @param[in] Robot state message contining position and velocity for each joint
+ * and robot body
+ */
+#if USE_SIM > 0
+  void robotStateCallback(const sensor_msgs::JointState::ConstPtr& msg);
+#else
+  void robotStateCallback(const quad_msgs::RobotState::ConstPtr& msg);
+#endif
+
+  /**
+   * @brief Compute the momentum observer external force estimation update.
+   */
+  void update();
+
+  /**
+   * @brief Publish body force force estimates
+   */
+  void publishBodyForce();
+
+  /// ROS subscriber for the robot state
+  ros::Subscriber robot_state_sub_;
+
+  /// ROS publisher for body force force estimates
+  ros::Publisher body_force_pub_;
+
+  /// ROS publisher for toe force estimates
+  ros::Publisher toe_force_pub_;
+
+  /// Nodehandle to pub to and sub from
+  ros::NodeHandle nh_;
+
+  /// Update rate for sending and receiving data;
+  double update_rate_;
+
+  /// Momentum observer gain
+  double K_O_;
+
+ private:
+  // External torque estimate
+  double r_mom[12];
+  // Momentum estimate
+  double p_hat[12];
+
+// Robot state estimate
+#if USE_SIM > 0
+  sensor_msgs::JointState::ConstPtr last_state_msg_;
+#else
+  quad_msgs::RobotState::ConstPtr last_state_msg_;
+#endif
 };
 
-#endif // BODY_FORCE_ESTIMATOR_H
+#endif  // BODY_FORCE_ESTIMATOR_H
diff --git a/body_force_estimator/include/body_force_estimator/body_force_estimator_dynamics.h b/body_force_estimator/include/body_force_estimator/body_force_estimator_dynamics.h
@@ -1,10 +1,11 @@
 #ifndef FORCE_ESTIMATOR_DYNAMICS_H
 #define FORCE_ESTIMATOR_DYNAMICS_H
 
-#include <eigen3/Eigen/Dense>
 #include <math.h>
 #include <ros/ros.h>
 
+#include <eigen3/Eigen/Dense>
+
 namespace force_estimation_dynamics {
 void f_M(Eigen::Vector3d q, int RL, Eigen::Matrix3d &F);
 void f_beta(Eigen::Vector3d q, Eigen::Vector3d qd, int RL, Eigen::Vector3d &F);
@@ -13,6 +14,6 @@ void f_J_MO(Eigen::Vector3d q, int RL, Eigen::Matrix3d &F);
 extern double MO_fric[3];
 extern double MO_damp[3];
 extern double MO_ktau[3];
-}
+}  // namespace force_estimation_dynamics
 
-#endif // FORCE_ESTIMATOR_DYNAMICS_H
+#endif  // FORCE_ESTIMATOR_DYNAMICS_H
diff --git a/body_force_estimator/src/body_force_estimator.cpp b/body_force_estimator/src/body_force_estimator.cpp
@@ -1,13 +1,14 @@
 #include "body_force_estimator/body_force_estimator.h"
+
 #include "body_force_estimator/body_force_estimator_dynamics.h"
 
 using namespace force_estimation_dynamics;
 
 // Temporary
 #if USE_SIM == 1
-int joint_inds [12] = {10, 0, 1, 11, 4, 5, 2, 6, 7, 3, 8, 9};
+int joint_inds[12] = {10, 0, 1, 11, 4, 5, 2, 6, 7, 3, 8, 9};
 #elif USE_SIM == 2
-int joint_inds [12] = {8, 0, 1, 9, 2, 3, 10, 4, 5, 11, 6, 7};
+int joint_inds[12] = {8, 0, 1, 9, 2, 3, 10, 4, 5, 11, 6, 7};
 #endif
 
 // Effective toe force estimate
@@ -18,32 +19,44 @@ BodyForceEstimator::BodyForceEstimator(ros::NodeHandle nh) {
 
   // Load rosparams from parameter server
   std::string robot_state_topic, body_force_topic, toe_force_topic;
-  #if USE_SIM == 2
-  nh.param<std::string>("topics/joint_encoder", robot_state_topic, "/joint_encoder");
-  #else
-  nh.param<std::string>("topics/state/ground_truth", robot_state_topic, "/state/ground_truth");
-  #endif
-  nh.param<std::string>("topics/body_force/joint_torques", body_force_topic, "/body_force/joint_torques");
-  nh.param<std::string>("topics/body_force/toe_forces", toe_force_topic, "/body_force/toe_forces");
-  nh.param<double>("body_force_estimator/update_rate", update_rate_, 100); // add a param for your package instead of using the estimator one
+#if USE_SIM == 2
+  nh.param<std::string>("topics/joint_encoder", robot_state_topic,
+                        "/joint_encoder");
+#else
+  nh.param<std::string>("topics/state/ground_truth", robot_state_topic,
+                        "/state/ground_truth");
+#endif
+  nh.param<std::string>("topics/body_force/joint_torques", body_force_topic,
+                        "/body_force/joint_torques");
+  nh.param<std::string>("topics/body_force/toe_forces", toe_force_topic,
+                        "/body_force/toe_forces");
+  nh.param<double>(
+      "body_force_estimator/update_rate", update_rate_,
+      100);  // add a param for your package instead of using the estimator one
   nh.param<double>("body_force_estimator/K_O", K_O_, 20);
 
-  // Setup pubs and subs
-  #if USE_SIM == 1
-  robot_state_sub_ = nh_.subscribe("/quad/joint_states",1,&BodyForceEstimator::robotStateCallback, this);
-  #elif USE_SIM == 2
-  robot_state_sub_ = nh_.subscribe(robot_state_topic,1,&BodyForceEstimator::robotStateCallback, this);
-  #else
-  robot_state_sub_ = nh_.subscribe(robot_state_topic,1,&BodyForceEstimator::robotStateCallback, this);
-  #endif
-  body_force_pub_ = nh_.advertise<quad_msgs::BodyForceEstimate>(body_force_topic,1);
-  toe_force_pub_ = nh_.advertise<quad_msgs::GRFArray>(toe_force_topic,1);
+// Setup pubs and subs
+#if USE_SIM == 1
+  robot_state_sub_ = nh_.subscribe(
+      "/quad/joint_states", 1, &BodyForceEstimator::robotStateCallback, this);
+#elif USE_SIM == 2
+  robot_state_sub_ = nh_.subscribe(
+      robot_state_topic, 1, &BodyForceEstimator::robotStateCallback, this);
+#else
+  robot_state_sub_ = nh_.subscribe(
+      robot_state_topic, 1, &BodyForceEstimator::robotStateCallback, this);
+#endif
+  body_force_pub_ =
+      nh_.advertise<quad_msgs::BodyForceEstimate>(body_force_topic, 1);
+  toe_force_pub_ = nh_.advertise<quad_msgs::GRFArray>(toe_force_topic, 1);
 }
 
 #if USE_SIM > 0
-void BodyForceEstimator::robotStateCallback(const sensor_msgs::JointState::ConstPtr& msg) {
+void BodyForceEstimator::robotStateCallback(
+    const sensor_msgs::JointState::ConstPtr& msg) {
 #else
-void BodyForceEstimator::robotStateCallback(const quad_msgs::RobotState::ConstPtr& msg) {
+void BodyForceEstimator::robotStateCallback(
+    const quad_msgs::RobotState::ConstPtr& msg) {
 #endif
   // ROS_INFO("In robotStateCallback");
   last_state_msg_ = msg;
@@ -52,23 +65,21 @@ void BodyForceEstimator::robotStateCallback(const quad_msgs::RobotState::ConstPt
 void BodyForceEstimator::update() {
   Eigen::Matrix3d M;
   Eigen::Vector3d beta;
-  Eigen::Matrix3d J_MO; // Jacobian
+  Eigen::Matrix3d J_MO;  // Jacobian
 
-  Eigen::Vector3d q; // joint positions
-  Eigen::Vector3d qd; // joint velocities
-  Eigen::Vector3d tau; // joint actuator torques
-  Eigen::Vector3d re; // momentum observer external torque estimates
-  Eigen::Vector3d pe; // momentum observer momentum estimate
-  Eigen::Vector3d fe_toe; // effective toe forces
+  Eigen::Vector3d q;       // joint positions
+  Eigen::Vector3d qd;      // joint velocities
+  Eigen::Vector3d tau;     // joint actuator torques
+  Eigen::Vector3d re;      // momentum observer external torque estimates
+  Eigen::Vector3d pe;      // momentum observer momentum estimate
+  Eigen::Vector3d fe_toe;  // effective toe forces
 
   // Momentum observer gain (todo: this should be precomputed once)
   Eigen::Matrix3d K_O;
-  K_O << K_O_, 0, 0,
-         0, K_O_, 0,
-         0, 0, K_O_;
+  K_O << K_O_, 0, 0, 0, K_O_, 0, 0, 0, K_O_;
 
   // Joint directions (todo: make this a parameter or read the URDF)
-  int joint_dirs [3] = {1, -1, 1};
+  int joint_dirs[3] = {1, -1, 1};
 
   if (last_state_msg_ == NULL) {
     return;
@@ -77,24 +88,24 @@ void BodyForceEstimator::update() {
   for (int i = 0; i < 4; i++) {
     // Loop over four legs: FL, BL, FR, BR
     for (int j = 0; j < 3; j++) {
-      // read joint data from message
-      #if USE_SIM > 0
-      int ind = joint_inds[3*i+j];
-      q[j] = joint_dirs[j]*last_state_msg_->position[ind];
-      qd[j] = joint_dirs[j]*last_state_msg_->velocity[ind];
-      tau[j] = MO_ktau[j]*joint_dirs[j]*last_state_msg_->effort[ind];
-      #else
-      int ind = 3*i+j;
-      q[j] = joint_dirs[j]*last_state_msg_->joints.position[ind];
-      qd[j] = joint_dirs[j]*last_state_msg_->joints.velocity[ind];
-      tau[j] = MO_ktau[j]*joint_dirs[j]*last_state_msg_->joints.effort[ind];
-      #endif
+// read joint data from message
+#if USE_SIM > 0
+      int ind = joint_inds[3 * i + j];
+      q[j] = joint_dirs[j] * last_state_msg_->position[ind];
+      qd[j] = joint_dirs[j] * last_state_msg_->velocity[ind];
+      tau[j] = MO_ktau[j] * joint_dirs[j] * last_state_msg_->effort[ind];
+#else
+      int ind = 3 * i + j;
+      q[j] = joint_dirs[j] * last_state_msg_->joints.position[ind];
+      qd[j] = joint_dirs[j] * last_state_msg_->joints.velocity[ind];
+      tau[j] = MO_ktau[j] * joint_dirs[j] * last_state_msg_->joints.effort[ind];
+#endif
 
       tau[j] += (qd[j] > 0 ? 1 : -1) * MO_fric[j] + qd[j] * MO_damp[j];
 
       // read this leg's estimates
-      re[j] = r_mom[3*i+j];
-      pe[j] = p_hat[3*i+j];
+      re[j] = r_mom[3 * i + j];
+      pe[j] = p_hat[3 * i + j];
     }
     // Compute dynamics matrices and vectors
     int RL = i < 2 ? -1 : 1;
@@ -103,19 +114,19 @@ void BodyForceEstimator::update() {
     f_J_MO(q, RL, J_MO);
 
     // Momentum observer update
-    Eigen::Vector3d p = M*qd;
+    Eigen::Vector3d p = M * qd;
     Eigen::Vector3d pd_hat = tau - beta + re;
     p = p - pe;
-    re = K_O*p;
-    pe = pe + pd_hat/update_rate_;
+    re = K_O * p;
+    pe = pe + pd_hat / update_rate_;
 
     // Effective toe forces
     fe_toe = J_MO.transpose().colPivHouseholderQr().solve(re);
 
     for (int j = 0; j < 3; j++) {
-      r_mom[3*i+j] = re[j];
-      p_hat[3*i+j] = pe[j];
-      f_toe_MO[3*i+j] = fe_toe[j];
+      r_mom[3 * i + j] = re[j];
+      p_hat[3 * i + j] = pe[j];
+      f_toe_MO[3 * i + j] = fe_toe[j];
     }
   }
 }
@@ -129,13 +140,13 @@ void BodyForceEstimator::publishBodyForce() {
     geometry_msgs::Wrench w;
     geometry_msgs::Vector3 ft;
 
-    w.torque.x = r_mom[3*i+0];
-    w.torque.y = r_mom[3*i+1];
-    w.torque.z = r_mom[3*i+2];
+    w.torque.x = r_mom[3 * i + 0];
+    w.torque.y = r_mom[3 * i + 1];
+    w.torque.z = r_mom[3 * i + 2];
 
-    ft.x = f_toe_MO[3*i+0];
-    ft.y = f_toe_MO[3*i+1];
-    ft.z = f_toe_MO[3*i+2];
+    ft.x = f_toe_MO[3 * i + 0];
+    ft.y = f_toe_MO[3 * i + 1];
+    ft.z = f_toe_MO[3 * i + 2];
     /*
     #if USE_SIM > 0
     if (last_state_msg_ != NULL) {