bool PhysicsJointFixed::init(PhysicsBody* a, PhysicsBody* b, const Vec2& anchr)
{
do
{
CC_BREAK_IF(!PhysicsJoint::init(a, b));
getBodyNode(a)->setPosition(anchr);
getBodyNode(b)->setPosition(anchr);
// add a pivot joint to fixed two body together
auto constraint = cpPivotJointNew(a->getCPBody(), b->getCPBody(),
PhysicsHelper::point2cpv(anchr));
CC_BREAK_IF(constraint == nullptr);
_cpConstraints.push_back(constraint);
// add a gear joint to make two body have the same rotation.
constraint = cpGearJointNew(a->getCPBody(), b->getCPBody(), 0, 1);
CC_BREAK_IF(constraint == nullptr);
_cpConstraints.push_back(constraint);
setCollisionEnable(false);
return true;
} while (false);
return false;
}
bool PhysicsJointFixed::init(PhysicsBody* a, PhysicsBody* b, const Point& anchr)
{
do
{
CC_BREAK_IF(!PhysicsJoint::init(a, b));
getBodyNode(a)->setPosition(anchr);
getBodyNode(b)->setPosition(anchr);
// add a pivot joint to fixed two body together
cpConstraint* joint = cpPivotJointNew(getBodyInfo(a)->getBody(), getBodyInfo(b)->getBody(),
PhysicsHelper::point2cpv(anchr));
CC_BREAK_IF(joint == nullptr);
m_pInfo->add(joint);
// add a gear joint to make two body have the same rotation.
joint = cpGearJointNew(getBodyInfo(a)->getBody(), getBodyInfo(b)->getBody(), 0, 1);
CC_BREAK_IF(joint == nullptr);
m_pInfo->add(joint);
setCollisionEnable(false);
return true;
} while (false);
return false;
}
int Skeleton::getBodyNodeIndex(const std::string& _name) const
{
const int nNodes = getNumBodyNodes();
for(int i = 0; i < nNodes; i++)
{
BodyNode* node = getBodyNode(i);
if (_name == node->getName())
return i;
}
return -1;
}
Eigen::Vector3d Skeleton::getWorldCOM()
{
Eigen::Vector3d com(0, 0, 0);
assert(mTotalMass != 0);
const int nNodes = getNumBodyNodes();
for(int i = 0; i < nNodes; i++)
{
BodyNode* bodyNode = getBodyNode(i);
com += (bodyNode->getMass() * bodyNode->getWorldCOM());
}
return com / mTotalMass;
}
//==============================================================================
void ReferentialSkeleton::clearCollidingBodies()
{
for (auto i = 0u; i < getNumBodyNodes(); ++i)
{
auto bodyNode = getBodyNode(i);
DART_SUPPRESS_DEPRECATED_BEGIN
bodyNode->setColliding(false);
DART_SUPPRESS_DEPRECATED_END
auto softBodyNode = bodyNode->asSoftBodyNode();
if (softBodyNode)
{
auto& pointMasses = softBodyNode->getPointMasses();
for (auto pointMass : pointMasses)
pointMass->setColliding(false);
}
}
}
void Skeleton::computeEquationsOfMotionID(
const Eigen::Vector3d& _gravity)
{
int n = getDOF();
// skip immobile objects in forward simulation
if (getImmobileState() == true || n == 0)
{
return;
}
// Save current tau
Eigen::VectorXd tau_old = get_tau();
// Set ddq as zero
set_ddq(Eigen::VectorXd::Zero(n));
// M(q) * ddq + b(q,dq) = tau
computeInverseDynamicsLinear(_gravity, true);
mCg = get_tau();
// Calcualtion mass matrix, M
mM = Eigen::MatrixXd::Zero(n,n);
for (int i = 0; i < getNumBodyNodes(); i++)
{
BodyNode *nodei = getBodyNode(i);
nodei->updateMassMatrix();
nodei->aggregateMass(mM);
}
// Inverse of mass matrix
mMInv = mM.ldlt().solve(Eigen::MatrixXd::Identity(n,n));
// Restore the torque
set_tau(tau_old);
// Evaluate external forces in generalized coordinate.
updateExternalForces();
// Update damping forces
updateDampingForces();
}
void Skeleton::initDynamics()
{
initKinematics();
int DOF = getDOF();
mM = Eigen::MatrixXd::Zero(DOF, DOF);
mMInv = Eigen::MatrixXd::Zero(DOF, DOF);
mC = Eigen::MatrixXd::Zero(DOF, DOF);
mCvec = Eigen::VectorXd::Zero(DOF);
mG = Eigen::VectorXd::Zero(DOF);
mCg = Eigen::VectorXd::Zero(DOF);
set_tau(Eigen::VectorXd::Zero(DOF));
mFext = Eigen::VectorXd::Zero(DOF);
mFc = Eigen::VectorXd::Zero(DOF);
mDampingForce = Eigen::VectorXd::Zero(DOF);
// calculate mass
// init the dependsOnDof stucture for each bodylink
mTotalMass = 0.0;
for(int i = 0; i < getNumBodyNodes(); i++)
mTotalMass += getBodyNode(i)->getMass();
}
//==============================================================================
TEST(Issue1184, Accuracy)
{
struct ShapeInfo
{
dart::dynamics::ShapePtr shape;
double offset;
};
std::function<ShapeInfo()> makePlaneGround =
[]()
{
return ShapeInfo{
std::make_shared<dart::dynamics::PlaneShape>(
Eigen::Vector3d::UnitZ(), 0.0),
0.0};
};
std::function<ShapeInfo()> makeBoxGround =
[]()
{
const double thickness = 0.1;
return ShapeInfo{
std::make_shared<dart::dynamics::BoxShape>(
Eigen::Vector3d(100.0, 100.0, thickness)),
-thickness/2.0};
};
std::function<dart::dynamics::ShapePtr(double)> makeBoxObject =
[](const double s) -> dart::dynamics::ShapePtr
{
return std::make_shared<dart::dynamics::BoxShape>(
Eigen::Vector3d::Constant(2*s));
};
std::function<dart::dynamics::ShapePtr(double)> makeSphereObject =
[](const double s) -> dart::dynamics::ShapePtr
{
return std::make_shared<dart::dynamics::SphereShape>(s);
};
#ifndef NDEBUG
const auto groundInfoFunctions = {makePlaneGround};
const auto objectShapeFunctions = {makeSphereObject};
const auto halfsizes = {10.0};
const auto fallingModes = {true};
const double dropHeight = 0.1;
const double tolerance = 1e-3;
#else
const auto groundInfoFunctions = {makePlaneGround, makeBoxGround};
const auto objectShapeFunctions = {makeBoxObject, makeSphereObject};
const auto halfsizes = {0.25, 1.0, 5.0, 10.0, 20.0};
const auto fallingModes = {true, false};
const double dropHeight = 1.0;
const double tolerance = 1e-3;
#endif
for(const auto& groundInfoFunction : groundInfoFunctions)
{
for(const auto& objectShapeFunction : objectShapeFunctions)
{
for(const double halfsize : halfsizes)
{
for(const bool falling : fallingModes)
{
auto world = dart::simulation::World::create("test");
world->getConstraintSolver()->setCollisionDetector(
dart::collision::BulletCollisionDetector::create());
Eigen::Isometry3d tf_object = Eigen::Isometry3d::Identity();
const double initialHeight = falling? dropHeight+halfsize : halfsize;
tf_object.translate(initialHeight*Eigen::Vector3d::UnitZ());
auto object = dart::dynamics::Skeleton::create("ball");
object->createJointAndBodyNodePair<dart::dynamics::FreeJoint>()
.first->setTransform(tf_object);
const auto objectShape = objectShapeFunction(halfsize);
object->getBodyNode(0)->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect>(objectShape);
world->addSkeleton(object);
const ShapeInfo groundInfo = groundInfoFunction();
auto ground = dart::dynamics::Skeleton::create("ground");
ground->createJointAndBodyNodePair<dart::dynamics::WeldJoint>()
.second->createShapeNodeWith<
dart::dynamics::VisualAspect,
dart::dynamics::CollisionAspect>(groundInfo.shape);
Eigen::Isometry3d tf_ground = Eigen::Isometry3d::Identity();
tf_ground.translate(groundInfo.offset*Eigen::Vector3d::UnitZ());
ground->getJoint(0)->setTransformFromParentBodyNode(tf_ground);
world->addSkeleton(ground);
// time until the object will strike
const double t_strike = falling?
sqrt(-2.0*dropHeight/world->getGravity()[2]) : 0.0;
// give the object some time to settle
const double min_time = 0.5;
const double t_limit = 30.0*t_strike + min_time;
double lowestHeight = std::numeric_limits<double>::infinity();
double time = 0.0;
while(time < t_limit)
{
world->step();
const double currentHeight =
object->getBodyNode(0)->getTransform().translation()[2];
if(currentHeight < lowestHeight)
lowestHeight = currentHeight;
time = world->getTime();
}
// The simulation should have run for at least two seconds
ASSERT_LE(min_time, time);
EXPECT_GE(halfsize+tolerance, lowestHeight)
<< "object type: " << objectShape->getType()
<< "\nground type: " << groundInfo.shape->getType()
<< "\nfalling: " << falling << "\n";
const double finalHeight =
object->getBodyNode(0)->getTransform().translation()[2];
EXPECT_NEAR(halfsize, finalHeight, tolerance)
<< "object type: " << objectShape->getType()
<< "\nground type: " << groundInfo.shape->getType()
<< "\nfalling: " << falling << "\n";
}
}
}
}
}
