Merge pull request #240 from Ipuch/master

SoftContact torque applied in world frame.
pyomeca · Oct 22, 2021 · b8d14d3 · b8d14d3
2 parents 5b59f72 + 9cd690a
commit b8d14d3
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 2 deletions.
diff --git a/src/RigidBody/SoftContactNode.cpp b/src/RigidBody/SoftContactNode.cpp
@@ -94,7 +94,7 @@ RigidBodyDynamics::Math::SpatialVector rigidbody::SoftContactNode::computeForceA
 
     // Find the application point of the force
     RigidBodyDynamics::Math::SpatialVector out(0, 0, 0, 0, 0, 0);
-    out.block(0, 0, 3, 1) = force.cross(CoMinGlobal - applicationPoint(x));
+    out.block(0, 0, 3, 1) = force.cross(- applicationPoint(x));
     out.block(3, 0, 3, 1) = force;
     return out;
 }

diff --git a/test/test_rigidbody.cpp b/test/test_rigidbody.cpp
@@ -409,7 +409,7 @@ TEST(SoftContacts, ForceAtCoM){
 
         RigidBodyDynamics::Math::SpatialVector force = sphere.computeForceAtCom(model, Q, QDot);
 
-        std::vector<double> forceExpected = {847.90514495586694, -529.16654948198982, 694.87983562160457,
+        std::vector<double> forceExpected = {-169.49529328017607, 835.69754838406357, -2.2059677321320312,
                                              -275.74596651650978, -55.149193303301956, 294.47295042042418};
         for (unsigned int i = 0; i < 6; ++i) {
             SCALAR_TO_DOUBLE(f, force(i));