Add getter to receive raw solver output

ARC-OPT · Jun 23, 2023 · 4539ead · 4539ead
1 parent e9a65e4
commit 4539ead
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 4 deletions.
diff --git a/src/core/Scene.hpp b/src/core/Scene.hpp
@@ -26,6 +26,7 @@ class Scene{
     base::commands::Joints solver_output_joints;
     JointWeights joint_weights, actuated_joint_weights;
     std::vector<TaskConfig> wbc_config;
+    base::VectorXd solver_output;
 
     /**
      * brief Create a task and add it to the WBC scene
@@ -125,6 +126,11 @@ class Scene{
      */
     const base::commands::Joints& getSolverOutput() const { return solver_output_joints; }
 
+    /**
+     * @brief Get current solver output in raw values
+     */
+    const base::VectorXd& getSolverOutputRaw() const { return solver_output; }
+
     /**
      * @brief set Joint weights by given name
      */

diff --git a/src/scenes/acceleration/AccelerationScene.hpp b/src/scenes/acceleration/AccelerationScene.hpp
@@ -28,7 +28,7 @@ class AccelerationScene : public Scene{
 protected:
     static SceneRegistry<AccelerationScene> reg;
 
-    base::VectorXd solver_output, robot_acc;
+    base::VectorXd robot_acc;
 
     /**
      * brief Create a task and add it to the WBC scene

diff --git a/src/scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.hpp b/src/scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.hpp
@@ -41,7 +41,7 @@ class AccelerationSceneReducedTSID : public Scene{
     static SceneRegistry<AccelerationSceneReducedTSID> reg;
 
     // Helper variables
-    base::VectorXd solver_output, robot_acc, solver_output_acc;
+    base::VectorXd robot_acc, solver_output_acc;
     base::samples::Wrenches contact_wrenches;
     double hessian_regularizer;
 

diff --git a/src/scenes/acceleration_tsid/AccelerationSceneTSID.hpp b/src/scenes/acceleration_tsid/AccelerationSceneTSID.hpp
@@ -41,7 +41,7 @@ class AccelerationSceneTSID : public Scene{
     static SceneRegistry<AccelerationSceneTSID> reg;
 
     // Helper variables
-    base::VectorXd solver_output, robot_acc, solver_output_acc;
+    base::VectorXd robot_acc, solver_output_acc;
     base::samples::Wrenches contact_wrenches;
     double hessian_regularizer;
 

diff --git a/src/scenes/velocity/VelocityScene.hpp b/src/scenes/velocity/VelocityScene.hpp
@@ -26,7 +26,7 @@ class VelocityScene : public Scene{
 protected:
     static SceneRegistry<VelocityScene> reg;
 
-    base::VectorXd solver_output, q, qd, qdd;
+    base::VectorXd q, qd, qdd;
 
     /**
      * @brief Create a task and add it to the WBC scene