From a9d1aebf44ff201fd5a2f78892a1baf01a9f185c Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Fri, 13 Nov 2020 01:23:13 +0100 Subject: [PATCH] Properly compute impulse for simple cases (sphere on plane, sphere vs sphere). --- include/rbdlsim.h | 6 +- src/rbdlsim.cc | 41 ++++----- tests/CollisionTests.cc | 180 +++++++++++++++++++++++++++++++++++----- 3 files changed, 174 insertions(+), 53 deletions(-) diff --git a/include/rbdlsim.h b/include/rbdlsim.h index 3e32bd9..f195f11 100644 --- a/include/rbdlsim.h +++ b/include/rbdlsim.h @@ -49,10 +49,8 @@ struct CollisionInfo { int mBodyBIndex; Vector3d posA = Vector3d::Zero(); Vector3d posB = Vector3d::Zero(); - double accumImpulseA = 0.; - double accumImpulseB = 0.; - double deltaImpulseA = 0.; - double deltaImpulseB = 0.; + double accumImpulse = 0.; + double deltaImpulse = 0.; Vector3d dir = Vector3d::Zero(); VectorNd jacA = VectorNd::Zero(1); VectorNd jacB = VectorNd::Zero(1); diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index 558fdf1..83abe64 100644 --- a/src/rbdlsim.cc +++ b/src/rbdlsim.cc @@ -105,10 +105,13 @@ bool CheckPenetration( if (shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) { bool result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo); cinfo.dir *= -1.; + Vector3d temp_pos = cinfo.posA; + cinfo.posA = cinfo.posB; + cinfo.posB = temp_pos; return result; } if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) { - return CheckPenetrationSphereVsSphere(shape_b, shape_a, cinfo); + return CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo); } ccd_t ccd; @@ -327,7 +330,7 @@ void PrepareConstraintImpulse( body_a, cinfo.mBodyAIndex, cinfo.posA, - -cinfo.dir, + cinfo.dir, &cinfo.MInvA, &cinfo.jacA, &cinfo.GMInvGTA); @@ -339,11 +342,9 @@ void PrepareConstraintImpulse( &cinfo.MInvB, &cinfo.jacB, &cinfo.GMInvGTB); - cout << "jacA = " << cinfo.jacA << endl; - cout << "jacB = " << cinfo.jacB << endl; - } +/// Calculates the impulse that we apply on body_b to resolve the contact. void CalcConstraintImpulse( SimBody* body_a, SimBody* body_b, @@ -353,29 +354,18 @@ void CalcConstraintImpulse( double rhs = 0.; if (body_a && !body_a->mIsStatic) { - rhs += (1.0 + cinfo.effectiveRestitution) * cinfo.jacA * body_a->qdot; + rhs += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution); } if (body_b && !body_b->mIsStatic) { - rhs -= (1.0 + cinfo.effectiveRestitution) * cinfo.jacB * (body_b->qdot); + rhs += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution); } double denom = cinfo.GMInvGTA + cinfo.GMInvGTB; - if (body_a && !body_a->mIsStatic) { - double old_impulse = cinfo.accumImpulseA; - cinfo.deltaImpulseA = rhs / denom; - cinfo.accumImpulseA += cinfo.deltaImpulseA; - cinfo.accumImpulseA = std::max(0., cinfo.accumImpulseA); - cinfo.deltaImpulseA = cinfo.accumImpulseA - old_impulse; - } - - if (body_b && !body_b->mIsStatic) { - double old_impulse = cinfo.accumImpulseB; - cinfo.deltaImpulseB = rhs / denom; - cinfo.accumImpulseB += cinfo.deltaImpulseB; - cinfo.accumImpulseB = std::max(0., cinfo.accumImpulseB); - cinfo.deltaImpulseB = cinfo.accumImpulseB - old_impulse; - } + double old_impulse = cinfo.accumImpulse; + cinfo.deltaImpulse = rhs / denom; + cinfo.accumImpulse = std::max(0., cinfo.accumImpulse + cinfo.deltaImpulse); + cinfo.deltaImpulse = cinfo.accumImpulse - old_impulse; } void ApplyConstraintImpulse( @@ -384,12 +374,12 @@ void ApplyConstraintImpulse( CollisionInfo& cinfo) { if (body_a && !body_a->mIsStatic) { body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose() - * (cinfo.deltaImpulseA); + * (-cinfo.deltaImpulse); } if (body_b && !body_b->mIsStatic) { - body_b->qdot -= cinfo.MInvB * cinfo.jacB.transpose() - * (cinfo.deltaImpulseB); + body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() + * (cinfo.deltaImpulse); } } @@ -464,7 +454,6 @@ bool World::integrateWorld(double dt) { body.step(dt); } - mSimTime += dt; return true; } diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc index d9ed537..2a60f0f 100644 --- a/tests/CollisionTests.cc +++ b/tests/CollisionTests.cc @@ -85,6 +85,16 @@ TEST_CASE("CheckCollisionSphereVsPlane", "[Collision]") { REQUIRE(cresult == true); } + + SECTION("Sphere touching shifted") { + sphere.pos = Vector3d(3., 0.75, 0.); + cresult = CheckPenetrationSphereVsPlane(sphere, plane, cinfo); + REQUIRE((cinfo.posA - Vector3d(3., 0.0, 0.)).norm() < 1.0e-12); + REQUIRE((cinfo.posB - Vector3d(3., 0.0, 0.)).norm() < 1.0e-12); + + REQUIRE(cresult == true); + } + } TEST_CASE("CheckCollisionSphereVsSphere", "[Collision]") { @@ -153,6 +163,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { ground_shape.mType = SimShape::Plane; ground_shape.pos = Vector3d::Zero(); ground_shape.orientation = Quaternion(0., 0., 0., 1.); + ground_shape.restitution = 1.0; ground_body.mCollisionShapes.push_back( SimBody::BodyCollisionInfo(-1, ground_shape)); ground_body.mIsStatic = true; @@ -193,13 +204,42 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12); PrepareConstraintImpulse(&ground_body, &sphere_a_body, cinfo); - CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); - double reference_impulseB = -sphere_a_mass * sphere_a_body.qdot[1]; - REQUIRE(cinfo.accumImpulseA < 1.0e-12); - REQUIRE(cinfo.accumImpulseB - reference_impulseB < 1.0e-12); - ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo); - REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12); + SECTION("EnsureImpulseDirection") { + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3); + REQUIRE(cinfo.deltaImpulse > -1.0e-12); + } + + SECTION("CheckForceAndJacobianDirections") { + REQUIRE(cinfo.jacB * sphere_a_body.qdot == -1.23); + cinfo.deltaImpulse = 1.0; + VectorNd qdot_old = sphere_a_body.qdot; + ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo); + REQUIRE(sphere_a_body.qdot[1] > qdot_old[1]); + } + + SECTION("CalculateImpulse") { + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1]; + REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12); + ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo); + REQUIRE(sphere_a_body.qdot[1] > -1.0e-12); + } + + SECTION("ImpulseMustNotPull") { + sphere_a_body.qdot[1] = 1.23; + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + REQUIRE(fabs(cinfo.accumImpulse) < 1.0e-12); + } + + SECTION("CheckBounce") { + cinfo.effectiveRestitution = 1.0; + VectorNd old_vel = sphere_a_body.qdot; + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo); + REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel[1]) < 1.0e-12); + } } SECTION("SphereOnGroundButCollidingReverseBodyOrder") { @@ -220,17 +260,31 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12); PrepareConstraintImpulse(&sphere_a_body, &ground_body, cinfo); - CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0); - double reference_impulseA = -sphere_a_mass * sphere_a_body.qdot[1]; - REQUIRE(cinfo.accumImpulseA - reference_impulseA < 1.0e-12); - REQUIRE(cinfo.accumImpulseB < 1.0e-12); - ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo); - REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12); + SECTION("EnsureImpulseDirection") { + CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0); + REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3); + REQUIRE(cinfo.deltaImpulse > -1.0e-12); + } + + SECTION("CheckForceAndJacobianDirections") { + REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23); + cinfo.deltaImpulse = 1.0; + VectorNd qdot_old = sphere_a_body.qdot; + ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo); + REQUIRE(sphere_a_body.qdot[1] > qdot_old[1]); + } + + SECTION("CalculateImpulse") { + CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0); + double reference_impulse = -sphere_a_mass * sphere_a_body.qdot[1]; + REQUIRE(fabs(cinfo.accumImpulse - reference_impulse) < 1.0e-12); + ApplyConstraintImpulse(&sphere_a_body, &ground_body, cinfo); + REQUIRE(sphere_a_body.qdot[1] > -1.0e-12); + } } SECTION("SphereVsSphereCollision") { - double sphere_b_mass = 1.5; SimBody sphere_b_body = CreateSphereBody( sphere_b_mass, 1.0, @@ -251,20 +305,100 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { REQUIRE(collisions.size() == 1); cinfo = collisions[0]; + REQUIRE((cinfo.dir - Vector3d(0., -1., 0.)).norm() < 1.0e-12); + + PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); + + SECTION("CheckForceAndJacobianDirections") { + REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23); + cinfo.deltaImpulse = -1.0; + VectorNd qdot_a_old = sphere_a_body.qdot; + VectorNd qdot_b_old = sphere_b_body.qdot; + ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); + REQUIRE(sphere_a_body.qdot[1] < qdot_a_old[1]); + REQUIRE(sphere_b_body.qdot[1] > qdot_b_old[1]); + } + + SECTION("CalculateImpulse") { + for (int i = 0; i < 1; i++) { + cout << "Iter " << i << endl; + CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); + // REQUIRE((cinfo.accumImpulse - Vector3d(0., 0., 0.)).norm() < 1.0e-12); + // REQUIRE((cinfo.accumImpulse - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12); + + cout << "pre impulse: " << sphere_a_body.qdot.transpose() << endl; + cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl; + cout << "impulse: " << cinfo.deltaImpulse << endl; + ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); + cout << "pst impulse: " << sphere_a_body.qdot.transpose() << endl; + cout << "pst impulse2: " << sphere_b_body.qdot.transpose() << endl; + } + REQUIRE(sphere_a_body.qdot[1] > -0.1); + REQUIRE(sphere_b_body.qdot[1] < 0.1); + } + + SECTION("CheckBounce") { + cinfo.effectiveRestitution = 1.0; + VectorNd old_vel_a = sphere_a_body.qdot; + VectorNd old_vel_b = sphere_b_body.qdot; + CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); + ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); + REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel_a[1]) < 1.0e-12); + REQUIRE(fabs(sphere_b_body.qdot[1] + old_vel_b[1]) < 1.0e-12); + } + } + + SECTION("SphereVsSphereCollisionReversed") { + SimBody sphere_b_body = CreateSphereBody( + sphere_b_mass, + 1.0, + 0., + Vector3d(0., -0.5, 0.), + Vector3d(0., 1., 0.)); + + sphere_a_body.q[1] = -0.5; + sphere_a_body.qdot[1] = 1.23; + sphere_a_body.updateCollisionShapes(); + + sphere_b_body.q[1] = 0.5; + sphere_b_body.qdot[1] = -1.23; + sphere_b_body.updateCollisionShapes(); + + std::vector collisions; + CalcCollisions(sphere_a_body, sphere_b_body, collisions); + REQUIRE(collisions.size() == 1); + cinfo = collisions[0]; + REQUIRE((cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12); PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); - CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); - // REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12); - // REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12); - cout << "pre impulse: " << sphere_a_body.qdot.transpose() << endl; - cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl; - ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); - cout << "pst impulse: " << sphere_a_body.qdot.transpose() << endl; - cout << "pst impulse2: " << sphere_b_body.qdot.transpose() << endl; + SECTION("CheckForceAndJacobianDirections") { + REQUIRE(cinfo.jacA * sphere_a_body.qdot == 1.23); + cinfo.deltaImpulse = -1.0; + VectorNd qdot_a_old = sphere_a_body.qdot; + VectorNd qdot_b_old = sphere_b_body.qdot; + ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); + REQUIRE(sphere_a_body.qdot[1] > qdot_a_old[1]); + REQUIRE(sphere_b_body.qdot[1] < qdot_b_old[1]); + } - REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12); - REQUIRE(sphere_b_body.qdot.norm() < 1.0e-12); + SECTION("CalculateImpulse") { + for (int i = 0; i < 1; i++) { + cout << "Iter " << i << endl; + CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); + // REQUIRE((cinfo.accumImpulse - Vector3d(0., 0., 0.)).norm() < 1.0e-12); + // REQUIRE((cinfo.accumImpulse - Vector3d(0., -sphere_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12); + + cout << "pre impulse: " << sphere_a_body.qdot.transpose() << endl; + cout << "pre impulse2: " << sphere_b_body.qdot.transpose() << endl; + cout << "impulse: " << cinfo.deltaImpulse << endl; + ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); + cout << "pst impulse: " << sphere_a_body.qdot.transpose() << endl; + cout << "pst impulse2: " << sphere_b_body.qdot.transpose() << endl; + } + REQUIRE(sphere_a_body.qdot[1] < 0.1); + REQUIRE(sphere_b_body.qdot[1] > -0.1); + } } }