martin
/
rbdlsim
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Properly compute impulse for simple cases (sphere on plane, sphere vs sphere).

master
Martin Felis 2020-11-13 01:23:13 +01:00
parent 5037eda185
commit a9d1aebf44
3 changed files with 174 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/rbdlsim.h
View File

@ -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);

41
src/rbdlsim.cc
View File

@ -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;
}

180
tests/CollisionTests.cc
View File

@ -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<CollisionInfo> 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);
}
}
}