[constraint] fixes joint actuator violation (#27)

SofaDefrost · Apr 15, 2024 · d7b438d · d7b438d
1 parent 8c078e5
commit d7b438d
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 7 deletions.
diff --git a/src/SoftRobots.Inverse/component/constraint/CableActuator.h b/src/SoftRobots.Inverse/component/constraint/CableActuator.h
@@ -115,8 +115,8 @@ class CableActuator : public Actuator<DataTypes> , public softrobots::constraint
     sofa::Data<bool>                  d_displayCableLimit;
 
     sofa::type::RGBAColor    m_color;
-
-    void initDatas();
+    
+    void initData();
     void initLimit();
     void updateLimit();
     void updateVisualization();

diff --git a/src/SoftRobots.Inverse/component/constraint/CableActuator.inl b/src/SoftRobots.Inverse/component/constraint/CableActuator.inl
@@ -80,7 +80,7 @@ template<class DataTypes>
 void CableActuator<DataTypes>::init()
 {
     softrobots::constraint::CableModel<DataTypes>::init();
-    initDatas();
+    initData();
     initLimit();
 }
 
@@ -89,19 +89,19 @@ template<class DataTypes>
 void CableActuator<DataTypes>::reinit()
 {
     softrobots::constraint::CableModel<DataTypes>::reinit();
-    initDatas();
+    initData();
     initLimit();
 }
 
 template<class DataTypes>
 void CableActuator<DataTypes>::reset()
 {
-    initDatas();
+    initData();
     initLimit();
 }
 
 template<class DataTypes>
-void CableActuator<DataTypes>::initDatas()
+void CableActuator<DataTypes>::initData()
 {
     d_displacement.setValue(0.0);
     d_force.setValue(d_initForce.getValue());

diff --git a/src/SoftRobots.Inverse/component/constraint/JointActuator.inl b/src/SoftRobots.Inverse/component/constraint/JointActuator.inl
@@ -247,7 +247,7 @@ void JointActuator<DataTypes>::getConstraintViolation(const ConstraintParams* cP
     if(!this->isComponentStateValid())
         return ;
 
-    Real dFree = Jdx->element(0) - d_initAngle.getValue() + d_angle.getValue();
+    Real dFree = Jdx->element(0) - d_initAngle.getValue() + m_state->readPositions()[d_index.getValue()][0];
     resV->set(m_constraintId, dFree);
 }