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

Added restitution to CreateSphereBody, debugging impulse computation.

master
Martin Felis 2020-11-08 12:50:12 +01:00
parent 60ec46d090
commit 3fac49ed9e
4 changed files with 18 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main.cc
View File

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

105
src/rbdlsim.cc
View File

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

4
src/simulator.cc
View File

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

63
tests/CollisionTests.cc
View File

@ -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;
@ -183,8 +183,9 @@ TEST_CASE("CalcConstraintImpulse", "[Collision]") {
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);
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<CollisionInfo> 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;
}
}
}