Merge pull request #31 from ami-iit/debug-tpose

Debug tpose
ami-iit · May 16, 2024 · 8dcee31 · 8dcee31
2 parents 3ce6330 + 575da1f
commit 8dcee31
Show file tree

Hide file tree

Showing 5 changed files with 234 additions and 322 deletions.
diff --git a/.clang-format b/.clang-format
@@ -43,7 +43,7 @@ BreakConstructorInitializersBeforeComma: true
 BreakConstructorInitializers: BeforeComma
 BreakAfterJavaFieldAnnotations: false
 BreakStringLiterals: true
-ColumnLimit:   100
+ColumnLimit:   140
 CommentPragmas:  '^ IWYU pragma:'
 CompactNamespaces: false
 ConstructorInitializerAllOnOneLineOrOnePerLine: false

diff --git a/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h b/src/IK/include/BiomechanicalAnalysis/IK/InverseKinematics.h
@@ -48,37 +48,44 @@ class HumanIK
      * @param handler pointer to the parameters handler
      * @return true if the SO3 task is initialized correctly
      */
-    bool initializeOrientationTask(
-        const std::string& taskName,
-        const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
+    bool initializeOrientationTask(const std::string& taskName,
+                                   const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
 
     /**
      * initialize the gravity task
      * @param taskName name of the task
      * @param handler pointer to the parameters handler
      * @return true if the gravity task is initialized correctly
      */
-    bool initializeGravityTask(
-        const std::string& taskName,
-        const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
+    bool initializeGravityTask(const std::string& taskName,
+                               const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
 
     /**
      * initialize the R3 task
      * @param taskName name of the task
      * @param handler pointer to the parameters handler
      * @return true if the R3 task is initialized correctly
      */
-    bool initializeFloorContactTask(
-        const std::string& taskName,
-        const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
+    bool initializeFloorContactTask(const std::string& taskName,
+                                    const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
 
-    bool initializeJointRegularizationTask(
-        const std::string& taskName,
-        const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
+    /**
+     * Initialize the joint regularization task.
+     * @param taskName Name of the task.
+     * @param taskHandler Pointer to the parameters handler.
+     * @return True if the joint regularization task is initialized correctly, false otherwise.
+     */
+    bool initializeJointRegularizationTask(const std::string& taskName,
+                                           const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
 
-    bool initializeJointConstraintsTask(
-        const std::string& taskName,
-        const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
+    /**
+     * Initialize the joint constraints task.
+     * @param taskName Name of the task.
+     * @param taskHandler Pointer to the parameters handler.
+     * @return True if the joint constraints task is initialized correctly, false otherwise.
+     */
+    bool initializeJointConstraintsTask(const std::string& taskName,
+                                        const std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> taskHandler);
 
     std::chrono::nanoseconds m_dtIntegration; /** Integration time step in nanoseconds */
 
@@ -90,8 +97,7 @@ class HumanIK
         std::shared_ptr<BipedalLocomotion::ContinuousDynamicalSystem::ForwardEuler<
             BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseSystemKinematics>>
             integrator;
-        std::shared_ptr<BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseSystemKinematics>
-            dynamics;
+        std::shared_ptr<BipedalLocomotion::ContinuousDynamicalSystem::FloatingBaseSystemKinematics> dynamics;
     };
 
     System m_system; /** Struct containing the integrator and the dynamics */
@@ -154,10 +160,9 @@ class HumanIK
         double verticalForceThreshold;
     };
 
-    std::shared_ptr<BipedalLocomotion::IK::JointTrackingTask>
-        m_jointRegularizationTask; /** Joint
-                                     regularization
-                                     task */
+    std::shared_ptr<BipedalLocomotion::IK::JointTrackingTask> m_jointRegularizationTask; /** Joint
+                                                                                           regularization
+                                                                                           task */
 
     std::shared_ptr<BipedalLocomotion::IK::JointLimitsTask> m_jointConstraintsTask; /** Joint limits
                                                                                        task */
@@ -180,6 +185,7 @@ class HumanIK
     object */
 
     int m_nrDoFs; /** Number of Joint Degrees of Freedom */
+    bool m_tPose{false}; /** Flag for resetting the integrator state */
 
     BipedalLocomotion::IK::QPInverseKinematics m_qpIK; /** QP Inverse Kinematics solver */
     BipedalLocomotion::System::VariablesHandler m_variableHandler; /** Variables handler */
@@ -318,9 +324,8 @@ class HumanIK
      * weight                          0.000001
     */
     // clang-format on
-    bool
-    initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
-               std::shared_ptr<iDynTree::KinDynComputations> kinDyn);
+    bool initialize(std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler> handler,
+                    std::shared_ptr<iDynTree::KinDynComputations> kinDyn);
 
     /**
      * set the integration time step
@@ -349,9 +354,7 @@ class HumanIK
      * @return true if the orientation setpoint is set correctly
      */
     bool
-    updateOrientationTask(const int node,
-                          const manif::SO3d& I_R_IMU,
-                          const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero());
+    updateOrientationTask(const int node, const manif::SO3d& I_R_IMU, const manif::SO3Tangentd& I_omega_IMU = manif::SO3d::Tangent::Zero());
 
     /**
      * set the orientation setpoint for a given node of a gravity task