diff --git a/include/rbdlsim.h b/include/rbdlsim.h index f38eae8..ee02870 100644 --- a/include/rbdlsim.h +++ b/include/rbdlsim.h @@ -11,6 +11,10 @@ namespace RBDLSim { using namespace RigidBodyDynamics; using namespace RigidBodyDynamics::Math; +struct SimShape; +struct SimBody; +struct CollisionInfo; + struct SimShape { enum ShapeType { Box = 0, @@ -30,6 +34,8 @@ struct SimBody { std::vector mCollisionShapes; void step(double ts); + void resolveCollisions (double dt, std::vector& collisions); + void calcNextPositions (double dt, const VectorNd& in_qdot, VectorNd& out_q); void updateCollisionShapes(); }; @@ -38,8 +44,11 @@ struct CollisionInfo { const SimBody* mBodyB; const SimShape* mShapeA; const SimShape* mShapeB; + int mBodyAIndex; + int mBodyBIndex; Vector3d pos; Vector3d dir; + double depth; }; struct World { @@ -50,7 +59,8 @@ struct World { void updateCollisionShapes(); void detectCollisions(); - bool step(double dt); + void resolveCollisions(double dt); + bool integrateWorld(double dt); }; bool CheckPenetration( diff --git a/src/main.cc b/src/main.cc index 59dfdb1..8daa957 100644 --- a/src/main.cc +++ b/src/main.cc @@ -9,7 +9,6 @@ void simplesim() { World world; SimBody sphere_body = CreateSphereBody(10., 1., Vector3d(0., 5.405, 0.), Vector3d::Zero()); - sphere_body.qdot[3] = 1 * M_PI; world.mBodies.push_back(sphere_body); SimShape ground_shape; @@ -23,14 +22,13 @@ void simplesim() { world.mSimTime = 0.; cout << world.mBodies[0].q.transpose() << endl; + double dt = 1.0e-3; do { world.updateCollisionShapes(); world.detectCollisions(); - world.step(1.0e-3); - } while (world.mContactPoints.size() == 0); - - cout << "Collision at t = " << world.mSimTime << endl - << "q = " << world.mBodies[0].q.transpose() << endl; + world.resolveCollisions(dt); + world.integrateWorld(dt); + } while (world.mSimTime < 1.01); } int main(int argc, char* argv[]) { diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc index 693220b..6ceef21 100644 --- a/src/rbdlsim.cc +++ b/src/rbdlsim.cc @@ -15,25 +15,28 @@ using namespace std; namespace RBDLSim { void SimBody::step(double dt) { - ForwardDynamics(mModel, q, qdot, tau, qddot); - - // semi-implicit eulers - qdot += dt * qddot; + // Prerequisite: qdot has already been updated by resolveCollisions(); + calcNextPositions(dt, qdot, q); +} +void SimBody::calcNextPositions( + double dt, + const VectorNd& in_qdot, + VectorNd& io_q) { for (int i = 1; i < mModel.mJoints.size(); ++i) { const Joint& joint = mModel.mJoints[i]; if (joint.mJointType != JointTypeSpherical) { - q.block(joint.q_index, 0, joint.mDoFCount, 1) += - dt * qdot.block(joint.q_index, 0, joint.mDoFCount, 1); + io_q.block(joint.q_index, 0, joint.mDoFCount, 1) += + dt * in_qdot.block(joint.q_index, 0, joint.mDoFCount, 1); continue; } // Integrate the Quaternion - Quaternion q0 = mModel.GetQuaternion(i, q); - Vector3d omega(qdot.block(joint.q_index, 0, 3, 1)); + Quaternion q0 = mModel.GetQuaternion(i, io_q); + Vector3d omega(in_qdot.block(joint.q_index, 0, 3, 1)); Quaternion qd = q0.omegaToQDot(omega); Quaternion q1 = (q0 + qd * dt).normalize(); - mModel.SetQuaternion(i, q1, q); + mModel.SetQuaternion(i, q1, io_q); } } @@ -105,6 +108,7 @@ bool CheckPenetration( if (intersect == 0) { cinfo.pos.set(pos.v[0], pos.v[1], pos.v[2]); cinfo.dir.set(dir.v[0], dir.v[1], dir.v[2]); + cinfo.depth = depth; } return !intersect; @@ -124,6 +128,139 @@ void SimBody::updateCollisionShapes() { } } +bool SolveGaussSeidelProj( + MatrixNd& A, + VectorNd& b, + VectorNd& x, + VectorNd& xlo, + VectorNd& xhi, + double tol = 1.0e-6, + int maxiter = 100) { + int n = A.cols(); + for (int iter = 0; iter < maxiter; iter++) { + double residual = 0.; + for (int i = 0; i < n; i++) { + double sigma = 0.; + for (int j = 0; j < i; j++) { + sigma += A(i, j) * x[j]; + } + for (int j = i + 1; j < n; j++) { + sigma += A(i, j) * x[j]; + } + double x_old = x[i]; + x[i] = (b[i] - sigma) / A(i, i); + + // Projection onto admissible values + if (x[i] < xlo[i]) { + x[i] = xlo[i]; + } + if (x[i] > xhi[i]) { + x[i] = xhi[i]; + } + + double diff = x[i] - x_old; + residual += diff * diff; + } + + if (residual < tol) { + cout << "Numiter: " << iter << endl; + return true; + } + + if (iter > maxiter) { + break; + } + } + + return false; +} + +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].pos, + 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 + VectorNd::Constant(nconstraints, 0.0001) ) * 1. / dt + G * (qdot + Minv * dt * (tau - N)) * -1.0; + + 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::updateCollisionShapes() { for (SimBody& body : mBodies) { body.updateCollisionShapes(); @@ -146,7 +283,9 @@ void World::detectCollisions() { if (has_penetration) { cinfo.mBodyA = nullptr; + cinfo.mBodyAIndex = -1; cinfo.mBodyB = &body; + cinfo.mBodyBIndex = body_col_info.first; mContactPoints.push_back(cinfo); } } @@ -154,7 +293,50 @@ void World::detectCollisions() { } } -bool World::step(double dt) { +void World::resolveCollisions(double dt) { + // so far only solve Body vs World collisions + for (SimBody& body : mBodies) { + // collect all collisions for current body + std::vector body_collisions; + + for (const CollisionInfo cinfo : mContactPoints) { + if (cinfo.mBodyA == &body) { + body_collisions.push_back(cinfo); + } else if (cinfo.mBodyB == &body) { + // Make sure the collision info is expressed in terms + // of mBodyA. + CollisionInfo rev_cinfo; + + rev_cinfo.mBodyA = &body; + rev_cinfo.mShapeA = cinfo.mShapeB; + rev_cinfo.mBodyAIndex = cinfo.mBodyBIndex; + + rev_cinfo.mBodyB = cinfo.mBodyA; + rev_cinfo.mShapeB = cinfo.mShapeA; + rev_cinfo.mBodyBIndex = cinfo.mBodyAIndex; + + rev_cinfo.pos = cinfo.pos; + rev_cinfo.dir = -cinfo.dir; + rev_cinfo.depth = cinfo.depth; + + body_collisions.push_back(rev_cinfo); + } + } + + if (body_collisions.size() > 0) { + cout << "Collision at t = " << mSimTime + << ", pos = " << body_collisions[0].pos.transpose() + << ", depth = " << body_collisions[0].depth << endl + << " qdotpre = " << mBodies[0].qdot.transpose() << endl; + } + body.resolveCollisions(dt, body_collisions); + if (body_collisions.size() > 0) { + cout << " qdotpost = " << mBodies[0].qdot.transpose() << endl; + } + } +} + +bool World::integrateWorld(double dt) { for (SimBody& body : mBodies) { body.step(dt); }