// advance the solution by one time step
PetscErrorCode RigidKinematicsSolver::advance()
{
PetscErrorCode ierr;
PetscFunctionBeginUser;
// note: use of `t + dt` because `t` is update in the Navier-Stokes method
ierr = moveBodies(t + dt); CHKERRQ(ierr);
ierr = DecoupledIBPMSolver::advance(); CHKERRQ(ierr);
PetscFunctionReturn(0);
} // advance
/*! One of the two main functions of the dynamics time step. This function is
called to move the bodies according to the velocities and accelerations found
in the previous step. The move is attempted for the duration of the time step
given in \a timeStep.
After all the bodies are moved, then collisions are checked. If any collisions
are found, the move is traced back and the value of the times step is
interpolated until the exact moment of contact is found. The actual value
of the time step until contact is made is returned. If interpolation fails,
a negative actual time step is returned. All the resulting contacts are added
to the bodies that are in contact to be used for subsequent computations.
The same procedure is carried out if, by executing a full time step, a joint
ends up outside of its legal range.
*/
double GraspitDynamics::moveDynamicBodies(double timeStep) {
int i, numDynBodies, numCols, moveErrCode;
std::vector<DynamicBody *> dynBodies;
static CollisionReport colReport;
bool jointLimitHit;
double contactTime, delta, tmpDist, minDist, dofLimitDist;
int numBodies = mWorld->getNumBodies();
int numRobots = mWorld->getNumRobots();
// save the initial position
for (i = 0; i < numBodies; i++) {
if (mWorld->getBody(i)->isDynamic()) {
dynBodies.push_back((DynamicBody *)mWorld->getBody(i));
((DynamicBody *)mWorld->getBody(i))->markState();
}
}
numDynBodies = dynBodies.size();
// call to the dynamics engine to perform the move by the full time step
DBGP("moving bodies with timestep: " << timeStep);
moveErrCode = moveBodies(numDynBodies, dynBodies, timeStep);
if (moveErrCode == 1) { // object out of bounds
mWorld->popDynamicState();
turnOffDynamics();
return -1.0;
}
// this sets the joints internal values according to how bodies have moved
for (i = 0; i < numRobots; i++) {
mWorld->getRobot(i)->updateJointValuesFromDynamics();
}
// check if we have collisions
if (numDynBodies > 0) { numCols = mWorld->getCollisionReport(&colReport); }
else { numCols = 0; }
// check if we have joint limits exceeded
jointLimitHit = false;
for (i = 0; i < numRobots; i++) {
if (mWorld->getRobot(i)->jointLimitDist() < 0.0) { jointLimitHit = true; }
}
// if so, we must interpolate until the exact moment of contact or limit hit
if (numCols || jointLimitHit) {
// return to initial position
for (i = 0; i < numDynBodies; i++) {
dynBodies[i]->returnToMarkedState();
}
minDist = 1.0e+255;
dofLimitDist = 1.0e+255;
#ifdef GRASPITDBG
if (numCols) {
std::cout << "COLLIDE!" << std::endl;
for (i = 0; i < numCols; i++) {
std::cout << colReport[i].first->getName().toStdString().c_str() << " collided with " <<
colReport[i].second->getName().toStdString().c_str() << std::endl;
}
for (i = 0; i < numCols; i++) {
tmpDist = mWorld->getDist(colReport[i].first, colReport[i].second);
if (tmpDist < minDist) { minDist = tmpDist; }
std::cout << "minDist: " << tmpDist << " between " << std::endl;
std::cout << colReport[i].first->getName().toStdString().c_str() << " and " <<
colReport[i].second->getName().toStdString().c_str() << std::endl;
}
}
#endif
// this section refines the timestep until the objects are separated
// by a distance less than CONTACT_THRESHOLD
bool done = false;
contactTime = timeStep;
delta = contactTime / 2;
contactTime -= delta;
while (!done) {
delta /= 2.0;
for (i = 0; i < numDynBodies; i++) {
dynBodies[i]->returnToMarkedState();
}
DBGP("moving bodies with timestep: " << contactTime);
moveErrCode = moveBodies(numDynBodies, dynBodies, contactTime);
if (moveErrCode == 1) { // object out of bounds
mWorld->popDynamicState();
turnOffDynamics();
return -1.0;
}
const char *min_body_1, *min_body_2;
// this computes joints values according to how dynamic bodies have moved
for (i = 0; i < numRobots; i++) {
mWorld->getRobot(i)->updateJointValuesFromDynamics();
}
if (numCols) {
minDist = 1.0e+255;
for (i = 0; i < numCols; i++) {
tmpDist = mWorld->getDist(colReport[i].first, colReport[i].second);
if (tmpDist < minDist) {
minDist = tmpDist;
min_body_1 = colReport[i].first->getName().toStdString().c_str();
min_body_2 = colReport[i].second->getName().toStdString().c_str();
DBGP("minDist: " << minDist << " between " << colReport[i].first->getName().toStdString().c_str() <<
" and " << colReport[i].second->getName().toStdString().c_str());
}
}
}
if (jointLimitHit) {
dofLimitDist = 1.0e10;
for (i = 0; i < numRobots; i++) {
dofLimitDist = MIN(dofLimitDist, mWorld->getRobot(i)->jointLimitDist());
}
}
if (minDist <= 0.0 || dofLimitDist < -resabs) {
contactTime -= delta;
}
else if (minDist > Contact::THRESHOLD * 0.5 && dofLimitDist > 0.01) { // why is this not resabs
contactTime += delta;
}
else { break; }
if (fabs(delta) < 1.0E-15 || contactTime < 1.0e-7) {
if (minDist <= 0) {
fprintf(stderr, "Delta failsafe due to collision: %s and %s\n", min_body_1, min_body_2);
} else {
fprintf(stderr, "Delta failsafe due to joint\n");
}
done = true; // failsafe
}
}
// COULD NOT FIND COLLISION TIME
if (done && contactTime < 1.0E-7) {
DBGP("!! could not find contact time !!");
for (i = 0; i < numDynBodies; i++) {
dynBodies[i]->returnToMarkedState();
}
}
mWorld->getWorldTimeRef() += contactTime;
} else { // if no collision
mWorld->getWorldTimeRef() += timeStep;
contactTime = timeStep;
}
#ifdef GRASPITDBG
std::cout << "CHECKING COLLISIONS AT MIDDLE OF STEP: ";
numCols = mWorld->getCollisionReport(&colReport);
if (!numCols) {
std::cout << "None." << std::endl;
} else {
std::cout << numCols << " found!!!" << std::endl;
for (i = 0; i < numCols; i++) {
std::cout << colReport[i].first->getName().toStdString().c_str() << " collided with " <<
colReport[i].second->getName().toStdString().c_str() << std::endl;
}
}
#endif
if (numDynBodies > 0) {
mWorld->findAllContacts();
}
for (i = 0; i < numRobots; i++) {
if (mWorld->getRobot(i)->inherits("HumanHand")) { ((HumanHand *)mWorld->getRobot(i))->updateTendonGeometry(); }
mWorld->getRobot(i)->emitConfigChange();
}
mWorld->tendonChange();
if (contactTime < 1.0E-7) { return -1.0; }
return contactTime;
}
