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

Basic but not yet working LCP time-stepping based collision response

remotes/origingl/master
Martin Felis 2020-10-04 17:28:02 +02:00
parent 5b6d7ec170
commit 7e7b08b919
3 changed files with 207 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
include/rbdlsim.h
View File

@ -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<BodyCollisionInfo> mCollisionShapes;
void step(double ts);
void resolveCollisions (double dt, std::vector<CollisionInfo>& 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(

10
src/main.cc
View File

@ -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[]) {

202
src/rbdlsim.cc
View File

@ -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<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].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<CollisionInfo> 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);
}