diff --git a/include/rbdlsim.h b/include/rbdlsim.h index eaa8619..d9425eb 100644 --- a/include/rbdlsim.h +++ b/include/rbdlsim.h @@ -55,26 +55,24 @@ struct CollisionInfo { double biasVelocityB = 0.; double accumImpulse = 0.; double deltaImpulse = 0.; - Vector3d dir = Vector3d::Zero(); + Vector3d dir = Vector3d(0., 1., 0.); VectorNd jacA = VectorNd::Zero(1); VectorNd jacB = VectorNd::Zero(1); VectorNd MInvJacTA = VectorNd::Zero(1); VectorNd MInvJacTB = VectorNd::Zero(1); double GMInvGTA = 0.; double GMInvGTB = 0.; - double tangentAccumImpulse0 = 0.; - double tangentAccumImpulse1 = 0.; - double tangentDeltaImpulse0 = 0.; - double tangentDeltaImpulse1 = 0.; - Vector3d tangent0 = Vector3d::Zero(); - Vector3d tangent1 = Vector3d::Zero(); - VectorNd tangentJacA = MatrixNd::Zero(2,2); - VectorNd tangentJacB = MatrixNd::Zero(2,2); - MatrixNd tangentMInvA = MatrixNd::Zero(2, 2); - MatrixNd tangentMInvB = MatrixNd::Zero(2, 2); - MatrixNd tangentGMInvGTA = MatrixNd::Zero(2,2); - MatrixNd tangentGMInvGTB = MatrixNd::Zero(2,2); + double accumFrictionImpulse[2] = {0.,0.}; + double deltaFrictionImpulse[2] = {0.,0.}; + Vector3d tangents[2] = {Vector3d::Zero(), Vector3d::Zero()}; + VectorNd tangentJacA[2] = {VectorNd::Zero(1), VectorNd::Zero(1)}; + VectorNd tangentJacB[2] = {VectorNd::Zero(1), VectorNd::Zero(1)}; + VectorNd tangentMInvJacTA[2] = {VectorNd::Zero(1), VectorNd::Zero(1)}; + VectorNd tangentMInvJacTB[2] = {VectorNd::Zero(1), VectorNd::Zero(1)}; + double tangentGMInvGTA[2] = {0., 0.}; + double tangentGMInvGTB[2] = {0., 0.}; double effectiveRestitution = 1.0; + double effectiveFriction = 0.2; double depth = 0.; }; @@ -141,11 +139,21 @@ void CalcCollisions( SimBody& body_a, SimBody& body_b, std::vector& collisions); + +void CalcFrictionImpulse( + SimBody* body_a, + SimBody* body_b, + CollisionInfo& cinfo); + +void ApplyFrictionImpulse( + SimBody* body_a, + SimBody* body_b, + CollisionInfo& cinfo); + void CalcConstraintImpulse( SimBody* body_a, SimBody* body_b, - CollisionInfo& cinfo, - const double dt); + CollisionInfo& cinfo); void ApplyConstraintImpulse( SimBody* body_a, SimBody* body_b, diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index 2ff0371..d635aab 100644 --- a/src/rbdlsim.cc +++ b/src/rbdlsim.cc @@ -110,6 +110,9 @@ static void sCalcTangentVectors(const Vector3d &normal, Vector3d* tangent0, Vect *tangent0 = normal.cross(Vector3d(1., 0., 0.)); *tangent1 = tangent0->cross(normal); } + + assert (tangent0->squaredNorm() > cCollisionEps); + assert (tangent1->squaredNorm() > cCollisionEps); } bool CheckPenetration( @@ -121,29 +124,29 @@ bool CheckPenetration( bool result = false; if (shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Plane) { result = CheckPenetrationSphereVsPlane(shape_a, shape_b, cinfo); - sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1); + sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]); return result; } else if ( shape_b.mType == SimShape::Sphere && shape_a.mType == SimShape::Plane) { result = CheckPenetrationSphereVsPlane(shape_b, shape_a, cinfo); sSwapCollisionInfoShapeOrder(cinfo); - sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1); + sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]); return result; } else if ( shape_a.mType == SimShape::Sphere && shape_b.mType == SimShape::Sphere) { result = CheckPenetrationSphereVsSphere(shape_a, shape_b, cinfo); - sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1); + sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]); return result; } else if ( shape_a.mType == SimShape::Box && shape_b.mType == SimShape::Plane) { result = CheckPenetrationBoxVsPlane(shape_a, shape_b, cinfo); - sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1); + sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]); return result; } else if ( shape_a.mType == SimShape::Plane && shape_b.mType == SimShape::Box) { bool result = CheckPenetrationBoxVsPlane(shape_b, shape_a, cinfo); sSwapCollisionInfoShapeOrder(cinfo); - sCalcTangentVectors(cinfo.dir, &cinfo.tangent0, &cinfo.tangent1); + sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]); return result; } @@ -166,6 +169,8 @@ bool CheckPenetration( cinfo.depth = depth; } + sCalcTangentVectors(cinfo.dir, &cinfo.tangents[0], &cinfo.tangents[1]); + return !intersect; } @@ -511,12 +516,14 @@ void CalcImpulseVariables( unsigned int body_index, const Vector3d& pos, const Vector3d& dir, + const Vector3d* tangents, const double depth, VectorNd* MInvJacT, VectorNd* jac, double* G_MInv_GT, - MatrixNd* tangentJac, - MatrixNd* tangentGMInvGT, + VectorNd* tangentJac, + VectorNd* tangentMInvJacT, + double* tangentGMInvGT, double* bias_vel, double restitution) { if (body == nullptr || body->mIsStatic) { @@ -546,18 +553,24 @@ void CalcImpulseVariables( CalcPointJacobian(*model, q, body_index, point_local_b, G_constr, false); (*jac) = dir.transpose() * G_constr; - (*MInvJacT) = M.llt().solve(jac->transpose()); + SimpleMath::LLT M_llt = M.llt(); + (*MInvJacT) = M_llt.solve(jac->transpose()); *G_MInv_GT = (*jac) * (*MInvJacT); assert(!isnan(*G_MInv_GT)); double beta = 0.01; - double delta_slop = cCollisionEps; - *bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - 0.05); + double delta_slop = 0.05; + *bias_vel = (*jac) * qdot * restitution - beta / dt * std::max (0., -depth - delta_slop); - (*tangentJac).resize(2,ndof); - (*tangentJac).block(0,0,1,ndof) = tangent0.transpose() * G_constr; - (*tangentJac).block(1,0,1,ndof) = tangent0.transpose() * G_constr; - (*tangentGMInvGT) = (*tangentJac) * (*MInv) * (*tangentJac).transpose(); + tangentJac[0] = tangents[0].transpose() * G_constr; + tangentJac[1] = tangents[1].transpose() * G_constr; + tangentMInvJacT[0] = M_llt.solve(tangentJac[0].transpose()); + tangentMInvJacT[1] = M_llt.solve(tangentJac[1].transpose()); + tangentGMInvGT[0] = tangentJac[0] * tangentMInvJacT[0]; + tangentGMInvGT[1] = tangentJac[1] * tangentMInvJacT[1]; + + assert (tangentGMInvGT[0] > 0.); + assert (tangentGMInvGT[1] > 0.); } void PrepareConstraintImpulse( @@ -571,12 +584,14 @@ void PrepareConstraintImpulse( cinfo.mBodyAIndex, cinfo.posA, cinfo.dir, + cinfo.tangents, cinfo.depth, &cinfo.MInvJacTA, &cinfo.jacA, &cinfo.GMInvGTA, - &cinfo.tangentJacA, - &cinfo.tangentGMInvGTA, + cinfo.tangentJacA, + cinfo.tangentMInvJacTA, + cinfo.tangentGMInvGTA, &cinfo.biasVelocityA, cinfo.effectiveRestitution); @@ -586,43 +601,93 @@ void PrepareConstraintImpulse( cinfo.mBodyBIndex, cinfo.posB, cinfo.dir, + cinfo.tangents, -cinfo.depth, &cinfo.MInvJacTB, &cinfo.jacB, &cinfo.GMInvGTB, - &cinfo.tangentJacB, - &cinfo.tangentGMInvGTB, + cinfo.tangentJacB, + cinfo.tangentMInvJacTB, + cinfo.tangentGMInvGTB, &cinfo.biasVelocityB, cinfo.effectiveRestitution); } +/// Calculates the impulse that we apply on body_b to resolve the contact. +void CalcFrictionImpulse( + SimBody* body_a, + SimBody* body_b, + CollisionInfo& cinfo) { + // Todo: add nonlinear effects * dt + + double rhs_tangent[2] = {0., 0.}; + if (body_a && !body_a->mIsStatic) { + rhs_tangent[0] += cinfo.tangentJacA[0] * body_a->qdot; + rhs_tangent[1] += cinfo.tangentJacA[1] * body_a->qdot; + } + if (body_b && !body_b->mIsStatic) { + rhs_tangent[0] -= cinfo.tangentJacB[0] * body_b->qdot; + rhs_tangent[1] -= cinfo.tangentJacB[1] * body_b->qdot; + } + + + for (int i = 0; i < 2; i++) { + double denom = cinfo.tangentGMInvGTA[i] + cinfo.tangentGMInvGTB[i]; + assert (denom > cCollisionEps); + double old_impulse = cinfo.accumFrictionImpulse[i]; + + cinfo.deltaFrictionImpulse[i] = rhs_tangent[i] / denom; + cinfo.accumFrictionImpulse[i] = cinfo.accumFrictionImpulse[i] + cinfo.deltaFrictionImpulse[i]; + if (cinfo.accumFrictionImpulse[i] >= cinfo.effectiveFriction * cinfo.accumImpulse) { + cinfo.accumFrictionImpulse[i] = cinfo.effectiveFriction * cinfo.accumImpulse; + } + if (cinfo.accumFrictionImpulse[i] < -cinfo.effectiveFriction * cinfo.accumImpulse) { + cinfo.accumFrictionImpulse[i] = -cinfo.effectiveFriction * cinfo.accumImpulse; + } + cinfo.deltaFrictionImpulse[i] = cinfo.accumFrictionImpulse[i] - old_impulse; + + assert (!isnan(cinfo.deltaFrictionImpulse[i])); + } +} + +void ApplyFrictionImpulse( + SimBody* body_a, + SimBody* body_b, + CollisionInfo& cinfo) { + if (body_a && !body_a->mIsStatic) { + body_a->qdot += + cinfo.tangentMInvJacTA[0] * (-cinfo.deltaFrictionImpulse[0]); + body_a->qdot += + cinfo.tangentMInvJacTA[1] * (-cinfo.deltaFrictionImpulse[1]); + + assert(!isnan(body_a->qdot.squaredNorm())); + } + + if (body_b && !body_b->mIsStatic) { + body_b->qdot += + -cinfo.tangentMInvJacTB[0] * (-cinfo.deltaFrictionImpulse[0]); + body_b->qdot += + -cinfo.tangentMInvJacTB[1] * (-cinfo.deltaFrictionImpulse[1]); + + assert(!isnan(body_b->qdot.squaredNorm())); + } +} + /// Calculates the impulse that we apply on body_b to resolve the contact. void CalcConstraintImpulse( SimBody* body_a, SimBody* body_b, - CollisionInfo& cinfo, - const double dt) { + CollisionInfo& cinfo) { // Todo: add nonlinear effects * dt - double ref_a = 0.; - double ref_b = 0.; - double vel_a = 0.; - double vel_b = 0.; - double ref = 0.; double rhs = 0.; if (body_a && !body_a->mIsStatic) { - vel_a = cinfo.jacA * body_a->qdot; - ref_a += cinfo.jacA * body_a->qdot * (1.0 + cinfo.effectiveRestitution); rhs += cinfo.jacA * body_a->qdot + cinfo.biasVelocityA; } if (body_b && !body_b->mIsStatic) { - vel_b = cinfo.jacB * body_b->qdot; - ref_b += -cinfo.jacB * (body_b->qdot) * (1.0 + cinfo.effectiveRestitution); rhs += -cinfo.jacB * body_b->qdot - cinfo.biasVelocityB; } - ref = ref_a + ref_b; - double denom = cinfo.GMInvGTA + cinfo.GMInvGTB; assert(denom > cCollisionEps); @@ -723,7 +788,10 @@ void World::resolveCollisions(double dt) { int num_iter = 20; for (int i = 0; i < num_iter; i++) { for (CollisionInfo& cinfo : mContactPoints) { - CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo, dt); + CalcFrictionImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo); + ApplyFrictionImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo); + + CalcConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo); ApplyConstraintImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo); } } diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc index 14d0b7f..9b81f8b 100644 --- a/tests/CollisionTests.cc +++ b/tests/CollisionTests.cc @@ -275,7 +275,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo); SECTION("EnsureImpulseDirection") { - CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo); REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3); REQUIRE(cinfo.deltaImpulse > -1.0e-12); } @@ -289,7 +289,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { } SECTION("CalculateImpulse") { - CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo); 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); @@ -298,7 +298,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { SECTION("ImpulseMustNotPull") { sphere_a_body.qdot[1] = 1.23; - CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo); REQUIRE(fabs(cinfo.accumImpulse) < 1.0e-12); } @@ -306,7 +306,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { cinfo.effectiveRestitution = 1.0; PrepareConstraintImpulse(0.001, &ground_body, &sphere_a_body, cinfo); VectorNd old_vel = sphere_a_body.qdot; - CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo, 0); + CalcConstraintImpulse(&ground_body, &sphere_a_body, cinfo); ApplyConstraintImpulse(&ground_body, &sphere_a_body, cinfo); REQUIRE(fabs(sphere_a_body.qdot[1] + old_vel[1]) < 1.0e-12); } @@ -332,7 +332,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { PrepareConstraintImpulse(0.001, &sphere_a_body, &ground_body, cinfo); SECTION("EnsureImpulseDirection") { - CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0); + CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo); REQUIRE(fabs(cinfo.deltaImpulse) > 1.0e-3); REQUIRE(cinfo.deltaImpulse > -1.0e-12); } @@ -346,7 +346,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { } SECTION("CalculateImpulse") { - CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo, 0); + CalcConstraintImpulse(&sphere_a_body, &ground_body, cinfo); 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); @@ -390,7 +390,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { } SECTION("CalculateImpulse") { - CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); + CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); REQUIRE(sphere_a_body.qdot[1] > -0.1); REQUIRE(sphere_b_body.qdot[1] < 0.1); @@ -401,7 +401,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { PrepareConstraintImpulse(0.001, &sphere_a_body, &sphere_b_body, cinfo); 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); + CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); 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); @@ -444,7 +444,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { } SECTION("CalculateImpulse") { - CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo, 0); + CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, cinfo); REQUIRE(sphere_a_body.qdot[1] < 0.1); REQUIRE(sphere_b_body.qdot[1] > -0.1);