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

Using manifold points instead of posA, posB for adding constraints.

master
Martin Felis 2020-11-21 20:09:26 +01:00
parent 4b19fd9c0b
commit cb387beb33
2 changed files with 75 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
src/rbdlsim.cc
View File

@ -38,7 +38,7 @@ void SimBody::calcNextPositions(
Vector3d omega(in_qdot.block(joint.q_index, 0, 3, 1));
Quaternion qd = q0.omegaToQDot(omega);
Quaternion q1 = (q0 + qd * dt).normalize();
assert (!isnan(q1.squaredNorm()));
assert(!isnan(q1.squaredNorm()));
mModel.SetQuaternion(i, q1, io_q);
}
}
@ -81,7 +81,7 @@ void SimShapeSupport(
len = ccdVec3Len2(&dir);
if (len - CCD_EPS > CCD_ZERO) {
ccdVec3Copy(v, &dir);
ccdVec3Scale(v, shape->scale[0] / CCD_SQRT(len));
ccdVec3Scale(v, shape->scale[0] * CCD_REAL(0.5) / CCD_SQRT(len));
} else {
ccdVec3Set(v, CCD_ZERO, CCD_ZERO, CCD_ZERO);
}
@ -95,7 +95,7 @@ void SimShapeSupport(
ccdVec3Add(v, &pos);
}
static void sSwapCollisionInfoShapeOrder(CollisionInfo &cinfo) {
static void sSwapCollisionInfoShapeOrder(CollisionInfo& cinfo) {
cinfo.dir *= -1.;
Vector3d temp_pos = cinfo.posA;
cinfo.posA = cinfo.posB;
@ -186,11 +186,6 @@ bool CheckPenetrationSphereVsPlane(
assert(shape_a.mType == SimShape::Sphere);
assert(shape_b.mType == SimShape::Plane);
// For now only support aligned spheres
assert(
(shape_a.orientation - Quaternion(0., 0., 0., 1.)).squaredNorm()
< cCollisionEps);
Vector3d plane_normal =
shape_b.orientation.conjugate().rotate(Vector3d(0., 1., 0.));
Vector3d plane_point = shape_b.pos;
@ -203,6 +198,8 @@ bool CheckPenetrationSphereVsPlane(
cinfo.dir = -plane_normal;
cinfo.depth = sphere_center_height;
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] =
sphere_point_to_plane - sphere_center_height * plane_normal;
cinfo.posA = sphere_point_to_plane;
cinfo.posB = sphere_point_to_plane - sphere_center_height * plane_normal;
@ -249,17 +246,23 @@ bool CheckPenetrationBoxVsPlane(
bool result = CheckPenetrationAABBVsPlane(aabb, plane, cinfo);
if (isnan(cinfo.posA.squaredNorm())) {
gLog ("NaN error!");
gLog("NaN error!");
}
assert (!isnan(cinfo.dir.squaredNorm()));
assert (!isnan(cinfo.posA.squaredNorm()));
assert (!isnan(cinfo.posB.squaredNorm()));
assert(!isnan(cinfo.dir.squaredNorm()));
assert(!isnan(cinfo.posA.squaredNorm()));
assert(!isnan(cinfo.posB.squaredNorm()));
cinfo.posA = shape_a_rot.transpose() * (cinfo.posA) + shape_a.pos;
cinfo.posB = shape_a_rot.transpose() * (cinfo.posB) + shape_a.pos;
cinfo.dir = shape_a_rot.transpose() * (cinfo.dir);
// also transform all contact manifold points
for (int i = 0; i < cinfo.mNumManifoldPoints; i++) {
cinfo.mManifoldPoints[i] =
shape_a_rot.transpose() * cinfo.mManifoldPoints[i] + shape_a.pos;
}
return result;
}
@ -322,7 +325,8 @@ bool CheckPenetrationAABBVsPlane(
for (int i = 0; i < 8; i++) {
distances[i] = (vertices[i] - plane_pos).dot(plane_normal);
if (distances[i] >= 0. && distances[i] < cCollisionEps) {
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = vertices[i];
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints] = vertices[i];
cinfo.mNumManifoldPoints++;
}
max_depth = distances[i] < max_depth ? distances[i] : max_depth;
}
@ -358,9 +362,26 @@ bool CheckPenetrationAABBVsPlane(
assert(s >= 0);
assert(s <= 1.);
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] = v0 + s * (v1 - v0);
s = s < cCollisionEps ? 0. : s;
s = s > 1.0 - cCollisionEps ? 1.0 : s;
Vector3d vc = v0 + s * (v1 - v0);
bool found_duplicate_point = false;
for (int j = 0; j < cinfo.mNumManifoldPoints; j++) {
if ((cinfo.mManifoldPoints[j] - vc).squaredNorm() < cCollisionEps) {
gLog("Removing duplicate point");
found_duplicate_point = true;
break;
}
}
if (!found_duplicate_point) {
cinfo.mManifoldPoints[cinfo.mNumManifoldPoints++] =
v0 + s * (v1 - v0);
}
if (cinfo.mNumManifoldPoints > 4) {
gLog ("Have %d manifold points?!", cinfo.mNumManifoldPoints);
gLog("Have %d manifold points?!", cinfo.mNumManifoldPoints);
}
}
}
@ -487,7 +508,7 @@ void CalcImpulseVariables(
const VectorNd& q = body->q;
const VectorNd& qdot = body->qdot;
assert (!isnan(q.squaredNorm()));
assert(!isnan(q.squaredNorm()));
// Calculate local coordinates of the contact point
UpdateKinematicsCustom(*model, &q, nullptr, nullptr);
@ -504,7 +525,7 @@ void CalcImpulseVariables(
(*jac) = dir.transpose() * G_constr;
*G_MInv_GT = (*jac) * (*MInv) * (*jac).transpose();
assert (!isnan(*G_MInv_GT));
assert(!isnan(*G_MInv_GT));
*bias_vel = (*jac) * qdot * restitution;
}
@ -564,7 +585,7 @@ void CalcConstraintImpulse(
ref = ref_a + ref_b;
double denom = cinfo.GMInvGTA + cinfo.GMInvGTB;
assert (denom > cCollisionEps);
assert(denom > cCollisionEps);
double old_impulse = cinfo.accumImpulse;
// TODO: is this really needed here??
@ -580,18 +601,18 @@ void ApplyConstraintImpulse(
if (body_a && !body_a->mIsStatic) {
body_a->qdot +=
cinfo.MInvA * cinfo.jacA.transpose() * (-cinfo.deltaImpulse);
assert (!isnan(body_a->qdot.squaredNorm()));
assert(!isnan(body_a->qdot.squaredNorm()));
}
if (body_b && !body_b->mIsStatic) {
body_b->qdot += cinfo.MInvB * cinfo.jacB.transpose() * (cinfo.deltaImpulse);
assert (!isnan(body_b->qdot.squaredNorm()));
assert(!isnan(body_b->qdot.squaredNorm()));
}
}
void World::calcUnconstrainedVelUpdate(double dt) {
for (SimBody& body : mBodies) {
assert (!isnan(body.q.squaredNorm()));
assert(!isnan(body.q.squaredNorm()));
ForwardDynamics(body.mModel, body.q, body.qdot, body.tau, body.qddot);
@ -633,15 +654,22 @@ void World::detectCollisions() {
if (has_penetration) {
if (isnan(cinfo.posA.squaredNorm())) {
gLog ("NaN error!");
gLog("NaN error!");
}
cinfo.mBodyA = nullptr;
cinfo.mBodyAIndex = -1;
cinfo.mBodyB = &ref_body;
cinfo.mBodyBIndex = body_col_info.first;
assert (!isnan(cinfo.posA.squaredNorm()));
assert (!isnan(cinfo.posB.squaredNorm()));
mContactPoints.push_back(cinfo);
assert(!isnan(cinfo.posA.squaredNorm()));
assert(!isnan(cinfo.posB.squaredNorm()));
for (int i = 0; i < cinfo.mNumManifoldPoints; i++) {
CollisionInfo cpinfo(cinfo);
cinfo.posA = cinfo.mManifoldPoints[i];
cinfo.posB = cinfo.mManifoldPoints[i];
mContactPoints.push_back(cinfo);
}
}
}
}
@ -767,4 +795,4 @@ SimBody CreateBoxBody(
return result;
}
} // namespace RBDLSim
} // namespace RBDLSim

36
src/simulator.cc
View File

@ -43,23 +43,29 @@ void simulator_init() {
sWorld.mStaticShapes.push_back(sGroundShape);
double restitution = 0.01;
double restitution = 0.3;
int num_bodies = 5;
for (int i = 0; i < num_bodies; i++) {
SimBody body = CreateBoxBody(
1.,
Vector3d(2., 1., 1.),
restitution,
Vector3d::Random() * 5.,
Vector3d::Zero());
SimBody body;
// SimBody body = CreateSphereBody(
// 1.,
// 1.,
// restitution,
// Vector3d::Random() * 5.,
// Vector3d::Zero());
bool create_sphere = false;
if (!create_sphere) {
body = CreateBoxBody(
1.,
Vector3d(2., 1., 1.),
restitution,
Vector3d::Random() * 5.,
Vector3d::Zero());
} else {
body = CreateSphereBody(
1.,
1.,
restitution,
Vector3d::Random() * 5.,
Vector3d::Zero());
}
sWorld.mBodies.push_back(body);
}
@ -232,14 +238,14 @@ void simulator_draw(srcmdbuf* cmdbuf) {
gLog("Error: cannot render shape of type %d", cinfo.second.mType);
}
simd4x4f trans = simd4x4f_create (
simd4x4f trans = simd4x4f_create(
// clang-format off
simd4f_create(1.f, 0.f, 0.f, 0.f),
simd4f_create(0.f, 1.f, 0.f, 0.f),
simd4f_create(0.f, 0.f, 1.f, 0.f),
simd4f_create(cinfo.second.pos[0], cinfo.second.pos[1], cinfo.second.pos[2], 1.f)
// clang-format on
);
);
simd4x4f scale = simd4x4f_create(
// clang-format off