Add posture gains to task space control

Two new variables, Kp_posture and Kd_posture, are added to the GainMatrices structure to represent gains for posture control. These are used in calculating the torque for posture control. This update only affects the Task Space Control samples, where these gains have been set to 100 and 10 respectively.

src/legged_control_cpp/forward_kinematics.cpp

@@ -42,6 +42,7 @@ KinematicsResult compute_end_effector_forward_kinematics(MultibodyModel const &m
 {
   if (!model.get_ee_transform()) { throw std::runtime_error("No end effector placement"); }
 
+  // NOLINTNEXTLINE(bugprone-unchecked-optional-access)
   SE3 const &nTee = model.get_ee_transform().value();
   auto const &[name, n_bodies, joints, parent, X_tree, I, _] = model;
 

src/samples/task_space_control/gains.hpp

@@ -14,8 +14,11 @@ struct GainMatrices
   DiagonalMatrix Kd_pos;
   DiagonalMatrix Kp_orient;
   DiagonalMatrix Kd_orient;
+  double Kp_posture;
+  double Kd_posture;
 
-  GainMatrices(double Kpp, double Kdp, double Kpo, double Kdo) : Kp_pos(3), Kd_pos(3), Kp_orient(3), Kd_orient(3)
+  GainMatrices(double Kpp, double Kdp, double Kpo, double Kdo, double Kp_posture_, double Kd_posture_)
+    : Kp_pos(3), Kd_pos(3), Kp_orient(3), Kd_orient(3), Kp_posture(Kp_posture_), Kd_posture(Kd_posture_)
   {
     Kp_pos.diagonal() = legged_ctrl::Vector3{ Kpp, Kpp, Kpp };// NOLINT
     Kd_pos.diagonal() = legged_ctrl::Vector3{ Kdp, Kdp, Kdp };// NOLINT

src/samples/task_space_control/task_space_control.cpp

@@ -35,7 +35,7 @@ VectorX
   auto v = twist.tail<3>();
   auto omega = twist.head<3>();
 
-  auto [Kxp, Kxd, Kop, Kod] = gains;
+  auto [Kxp, Kxd, Kop, Kod, Kp_posture, Kd_posture] = gains;
 
   SpatialVector F;
   F.tail(3) = Kxp * (wPd - wPee) + Kxd * (-v);
@@ -55,9 +55,9 @@ VectorX
 
 #if ENABLE_POSTURE_CONTROL
   auto JTpinv = J.transpose().completeOrthogonalDecomposition().pseudoInverse();
-  MatrixX N = eye(model.n_bodies) - J.transpose() * JTpinv;
-  VectorX desired_posture = mujoco.state.get_state_from_keyframe("home");
-  tau += N * M * (100 * (desired_posture - q) + 10 * -qd);
+  MatrixX const N = eye(model.n_bodies) - J.transpose() * JTpinv;
+  VectorX const desired_posture = mujoco.state.get_state_from_keyframe("home");
+  tau += N * M * (Kp_posture * (desired_posture - q) + Kd_posture * -qd);
 #endif
 
   return tau;
@@ -80,9 +80,9 @@ void simulation_and_control_loop(Mujoco &mujoco, std::string const &scene_path,
     mujoco.scene.get_actuator_ids({ "joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7" });
 
 #if ENABLE_INVERSE_DYNAMICS
-  GainMatrices const gains{ 300, 30, 100, 10 };
+  GainMatrices const gains{ 300, 30, 100, 10, 100, 10 };
 #else
-  GainMatrices const gains{ 500, 50, 10, .1 };
+  GainMatrices const gains{ 500, 50, 10, .1, 100, 10  };
 #endif
 
   while (!mujoco.gui.is_exit_requested()) {