From 3fac49ed9efd53fbeb1d597e981c7b96218a3338 Mon Sep 17 00:00:00 2001 From: Martin Felis Date: Sun, 8 Nov 2020 12:50:12 +0100 Subject: [PATCH] Added restitution to CreateSphereBody, debugging impulse computation. --- src/main.cc | 2 +- src/rbdlsim.cc | 105 +++------------------------------------- src/simulator.cc | 4 +- tests/CollisionTests.cc | 65 +++---------------------- 4 files changed, 18 insertions(+), 158 deletions(-) diff --git a/src/main.cc b/src/main.cc index 7db693f..745e895 100644 --- a/src/main.cc +++ b/src/main.cc @@ -8,7 +8,7 @@ using namespace RBDLSim; void simplesim() { World world; SimBody sphere_body = - CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero()); + CreateSphereBody(10., 1., 0., Vector3d(0., 5.405, 0.), Vector3d::Zero()); world.mBodies.push_back(sphere_body); SimShape ground_shape; diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index b7f7c50..e6bb48b 100644 --- a/src/rbdlsim.cc +++ b/src/rbdlsim.cc @@ -338,6 +338,9 @@ void PrepareConstraintImpulse( &cinfo.MInvB, &cinfo.jacB, &cinfo.GMInvGTB); + cout << "jacA = " << cinfo.jacA << endl; + cout << "jacB = " << cinfo.jacB << endl; + } void CalcConstraintImpulse( @@ -367,121 +370,28 @@ void CalcConstraintImpulse( if (body_b && !body_b->mIsStatic) { double old_impulse = cinfo.accumImpulseB; - cinfo.deltaImpulseB = std::max(0., rhs / denom); + cinfo.deltaImpulseB = -rhs / denom; cinfo.accumImpulseB += cinfo.deltaImpulseB; cinfo.accumImpulseB = std::max(0., cinfo.accumImpulseB); cinfo.deltaImpulseB = cinfo.accumImpulseB - old_impulse; - gLog ("deltaImpulse: %f", cinfo.deltaImpulseB); } } -void ApplyConstraintImpulse( - SimBody* body, - const MatrixNd& MInv, - const VectorNd& jac, - double impulse) {} - void ApplyConstraintImpulse( SimBody* body_a, SimBody* body_b, CollisionInfo& cinfo) { if (body_a && !body_a->mIsStatic) { body_a->qdot += cinfo.MInvA * cinfo.jacA.transpose() - * (-cinfo.deltaImpulseA); + * (cinfo.deltaImpulseA); } if (body_b && !body_b->mIsStatic) { body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() - * (-cinfo.deltaImpulseB); + * (cinfo.deltaImpulseB); } } -void SimBody::resolveCollisions( - double dt, - std::vector& collisions) { - if (collisions.size() == 0) { - // No contacts, calculate new qdot using simple forward - // dynamics - ForwardDynamics(mModel, q, qdot, tau, qddot); - - // semi-implicit eulers - qdot += dt * qddot; - - return; - } - - int ndof = mModel.qdot_size; - int nconstraints = collisions.size(); - - // Allocate space for the constraint system - MatrixNd M(MatrixNd::Zero(ndof, ndof)); - MatrixNd Minv(MatrixNd::Zero(ndof, ndof)); - - VectorNd N(VectorNd::Zero(ndof)); - MatrixNd G(MatrixNd::Zero(nconstraints, ndof)); - VectorNd gamma(VectorNd::Zero(nconstraints)); - - // Calculate local coordinates of the contact point - std::vector pos_local; - VectorNd constr_value(VectorNd::Zero(nconstraints)); - UpdateKinematicsCustom(mModel, &q, nullptr, nullptr); - for (int i = 0; i < nconstraints; i++) { - constr_value[i] = -collisions[i].depth; - pos_local.push_back(CalcBaseToBodyCoordinates( - mModel, - q, - collisions[i].mBodyAIndex, - collisions[i].posA, - false)); - } - - // Calculate predicted position state - VectorNd q_next_pred(q); - calcNextPositions(dt, qdot, q_next_pred); - - // Compute vectors and matrices of the contact system - NonlinearEffects(mModel, q_next_pred, qdot, N); - CompositeRigidBodyAlgorithm(mModel, q_next_pred, M, false); - Minv = M.inverse(); - - // Calculate contact Jacobians - for (int i = 0; i < nconstraints; i++) { - MatrixNd G_constr(MatrixNd::Zero(3, ndof)); - CalcPointJacobian( - mModel, - q_next_pred, - collisions[i].mBodyAIndex, - pos_local[i], - G_constr, - false); - G.block(i, 0, 1, ndof) = collisions[i].dir.transpose() * G_constr; - } - - MatrixNd A(MatrixNd::Zero(nconstraints, nconstraints)); - VectorNd b(VectorNd::Zero(nconstraints)); - - // Solve for the impules hlambda - A = G * Minv * G.transpose(); - b = (constr_value)*1. / dt + G * (qdot + Minv * dt * (tau - N)); - - VectorNd hlambda(VectorNd::Zero(nconstraints)); - VectorNd hlambda_lo(VectorNd::Constant(nconstraints, 0.)); - VectorNd hlambda_hi(VectorNd::Constant(nconstraints, 1.0e9)); - - bool solve_result = false; - solve_result = SolveGaussSeidelProj(A, b, hlambda, hlambda_lo, hlambda_hi); - - // VectorNd hlambda = A.colPivHouseholderQr().solve(b) * -1.; - - if (!solve_result) { - cout << "Impulse Solve Failed!! " << endl; - } - cout << "Contact impulse: " << hlambda.transpose() << endl; - - // solve for the new velocity - qdot = qdot + Minv * (dt * tau + dt * N + G.transpose() * hlambda); -} - void World::calcUnconstrainedVelUpdate(double dt) { for (SimBody& body : mBodies) { ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot); @@ -560,6 +470,7 @@ bool World::integrateWorld(double dt) { SimBody CreateSphereBody( double mass, double radius, + double restitution, const Vector3d& pos, const Vector3d& vel) { SimBody result; @@ -596,7 +507,7 @@ SimBody CreateSphereBody( shape.pos = pos; shape.orientation.set(0., 0., 0., 1.); shape.scale.set(radius, radius, radius); - shape.restitution = 0.3; + shape.restitution = restitution; result.mCollisionShapes.push_back( SimBody::BodyCollisionInfo(sphere_body, shape)); diff --git a/src/simulator.cc b/src/simulator.cc index 21b8abf..196e518 100644 --- a/src/simulator.cc +++ b/src/simulator.cc @@ -36,11 +36,11 @@ void simulator_init() { sWorld.mStaticShapes.push_back(sGroundShape); sSphereBody = - CreateSphereBody(1., 1., Vector3d(0., 1.405, 0.), Vector3d::Zero()); + CreateSphereBody(1., 1., 0., Vector3d(0., 1.405, 0.), Vector3d::Zero()); sWorld.mBodies.push_back(sSphereBody); sSphereBody2 = - CreateSphereBody(1., 1., Vector3d(0.3, 2.405, 0.), Vector3d::Zero()); + CreateSphereBody(1., 1., 0., Vector3d(0.3, 2.405, 0.), Vector3d::Zero()); // sWorld.mBodies.push_back(sSphereBody2); sWorld.mSimTime = 0.; diff --git a/tests/CollisionTests.cc b/tests/CollisionTests.cc index 1e9e859..2e83b67 100644 --- a/tests/CollisionTests.cc +++ b/tests/CollisionTests.cc @@ -160,9 +160,9 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { double sphere_b_mass = 1.5; SimBody sphere_a_body = CreateSphereBody( - sphere_a_mass, 1.0, Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.)); + sphere_a_mass, 1.0, 0., Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.)); SimBody sphere_b_body = CreateSphereBody( - sphere_b_mass, 1.0, Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.)); + sphere_b_mass, 1.0, 0., Vector3d (0., 0.5, 0.), Vector3d (0., -1., 0.)); CollisionInfo cinfo; @@ -182,9 +182,10 @@ 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); - REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12); - REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_a_mass * sphere_a_body.qdot[1], 0.)).norm() < 1.0e-12); + 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); @@ -192,7 +193,7 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { SECTION ("SphereVsSphereCollision") { double sphere_b_mass = 1.5; - SimBody sphere_b_body = CreateSphereBody(sphere_b_mass, 1.0, Vector3d (0., -0.5, 0.), Vector3d (0., 1., 0.)); + 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; @@ -223,56 +224,4 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") { REQUIRE(sphere_a_body.qdot.norm() < 1.0e-12); REQUIRE(sphere_b_body.qdot.norm() < 1.0e-12); } - - SECTION ("DoubleSpheresOnGround") { - sphere_b_body = CreateSphereBody( - sphere_b_mass, 5.0, Vector3d (0., 1.5, 0.), Vector3d (0., -1., 0.)); - - sphere_a_body.q[1] = 0.5; - sphere_a_body.qdot[1] = -1.8; - sphere_a_body.updateCollisionShapes(); - - sphere_b_body.q[1] = 1.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); - CollisionInfo sph_v_sph_cinfo = collisions[0]; -// REQUIRE((sph_v_sph_cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12); - - CalcCollisions(ground_body, sphere_a_body, collisions); - REQUIRE(collisions.size() == 1); - CollisionInfo sph_v_ground_cinfo = collisions[0]; - REQUIRE((sph_v_ground_cinfo.dir - Vector3d(0., 1., 0.)).norm() < 1.0e-12); - - PrepareConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo); - PrepareConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo); - - int num_iter = 20; - for (int i = 0; i < num_iter; i++) { - cout << "-- Iter " << i << endl; - - // REQUIRE((cinfo.accumImpulseA - Vector3d(0., 0., 0.)).norm() < 1.0e-12); - // REQUIRE((cinfo.accumImpulseB - Vector3d(0., -sphere_a_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; - CalcConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo, 0); - ApplyConstraintImpulse(&sphere_a_body, &sphere_b_body, sph_v_sph_cinfo); - cout << "Sph v Sph Impulse: " << sph_v_sph_cinfo.accumImpulseB.transpose() * sph_v_sph_cinfo.dir << endl; - cout << "pst sph_v_sph impulse: " << sphere_a_body.qdot.transpose() - << endl; - cout << "pst sph_v_sph impulse2: " << sphere_b_body.qdot.transpose() - << endl; - - CalcConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo, 0); - ApplyConstraintImpulse(&ground_body, &sphere_a_body, sph_v_ground_cinfo); - cout << "Gnd v Sph Impulse: " << sph_v_ground_cinfo.accumImpulseB.transpose() * sph_v_ground_cinfo.dir << endl; - - cout << "pst gnd_v_sph impulse: " << sphere_a_body.qdot.transpose() - << endl; - } - } }