Refactor the IK mapping into a separate function

mjbots · Aug 3, 2023 · e91c605 · e91c605
1 parent 20e0de0
commit e91c605
Showing 1 changed file with 11 additions and 3 deletions.
diff --git a/mech/quadruped_control.cc b/mech/quadruped_control.cc
@@ -1911,8 +1911,6 @@ class QuadrupedControl::Impl {
           std::min(config_.bounds.max_z_B, leg_B.position.z()));
     }
 
-    std::vector<QC::Joint> out_joints;
-
     const std::vector<IkSolver::Joint> current_joints = [&]() {
       std::vector<IkSolver::Joint> result;
       for (const auto& joint : status_.state.joints) {
@@ -1938,6 +1936,16 @@ class QuadrupedControl::Impl {
       return std::max(1.0, result);
     }();
 
+    std::vector<QC::Joint> out_joints = MapIk(total_stance, current_joints);
+
+    ControlJoints(std::move(out_joints));
+  }
+
+  std::vector<QC::Joint> MapIk(
+      double total_stance,
+      const std::vector<IkSolver::Joint>& current_joints) {
+    std::vector<QC::Joint> out_joints;
+
     const base::Point3D g_M = base::Point3D(0., 0., 1.);
     const base::Point3D g_B = status_.state.robot.frame_MB.pose.inverse() * g_M;
 
@@ -2041,7 +2049,7 @@ class QuadrupedControl::Impl {
       }
     }
 
-    ControlJoints(std::move(out_joints));
+    return out_joints;
   }
 
   void ControlJoints(std::vector<QC::Joint> joints) {