Solve bug in angle displacement in JointConstraint

SofaDefrost · Jan 29, 2024 · f414ef7 · f414ef7
1 parent 1b3ef9d
commit f414ef7
Showing 1 changed file with 1 addition and 1 deletion.
diff --git a/src/SoftRobots/component/constraint/JointConstraint.inl b/src/SoftRobots/component/constraint/JointConstraint.inl
@@ -230,7 +230,7 @@ void JointConstraint<DataTypes>::storeLambda(const ConstraintParams* cParams,
     // Update joint angle
     ReadAccessor<sofa::Data<VecCoord> > positions = m_state->readPositions();
     m_currentAngle = positions[d_index.getValue()][0];
-    d_displacement.setValue(m_initAngle- m_currentAngle);
+    d_displacement.setValue(-m_initAngle+m_currentAngle);
 }