bool testClosedKinematicLoop2()
{
SharedPtr<Group> group = new Group("Group");
MobileRootJoint* mechanicRootJoint = new MobileRootJoint("RootJoint");
group->addChild(mechanicRootJoint);
RigidBody* rigidBody = new RigidBody("RigidBody");
group->addChild(rigidBody);
group->connect(mechanicRootJoint->getPort("link"),
rigidBody->addLink("rootLink"));
PrismaticJoint* prismaticJoint = new PrismaticJoint("PrismaticJoint");
group->addChild(prismaticJoint);
group->connect(rigidBody->addLink("jointLink0"),
prismaticJoint->getPort("link0"));
group->connect(rigidBody->addLink("jointLink1"),
prismaticJoint->getPort("link1"));
SharedPtr<System> system = new System("Closed kinematic Loop 1");
system->setNode(group);
if (system->init()) {
std::cerr << "Detection of klosed kinematic loop failed!" << std::endl;
return false;
}
return true;
}
bool testBodyTreeModel1(unsigned numLevels, unsigned numBodies)
{
SharedPtr<Group> group = new Group("Group");
MobileRootJoint* mechanicRootJoint = new MobileRootJoint("RootJoint");
group->addChild(mechanicRootJoint);
RigidBody* rigidBody = new RigidBody("RigidBody");
group->addChild(rigidBody);
group->connect(mechanicRootJoint->getPort("link"),
rigidBody->addLink("rootLink"));
Mass* mass = new Mass("Mass", 1, InertiaMatrix(1, 0, 0, 1, 0, 1));
group->addChild(mass);
group->connect(mass->getPort("link"), rigidBody->addLink("massLink"));
addBodiesRecursive(numLevels, numBodies, rigidBody, group);
SharedPtr<System> system = new System("Closed kinematic Loop 1");
system->setNode(group);
if (!system->init()) {
std::cerr << "Initialization of single body model failed!" << std::endl;
return false;
}
return true;
}
Node* buildDrop()
{
// A simple free falling mass.
SharedPtr<Group> group = new Group("Group");
MobileRootJoint* mobileRootJoint = new MobileRootJoint("Root Joint");
mobileRootJoint->setInitialPosition(Vector3(0, 0, -10000));
mobileRootJoint->setInitialAngularVelocity(Vector3(1, 1, 1));
group->addChild(mobileRootJoint);
RigidBody* rigidBody = new RigidBody("Rigid Body");
rigidBody->addLink("externalInteractLink");
group->addChild(rigidBody);
Mass* mass = new Mass("Mass", 1, InertiaMatrix(1, 0, 0, 1, 0, 1));
group->addChild(mass);
ExternalInteract* externalInteract = new ExternalInteract("ExternalInteract");
externalInteract->setPosition(mass->getPosition());
externalInteract->setEnableAllOutputs(true);
group->addChild(externalInteract);
group->connect(mobileRootJoint->getPort("link"),
rigidBody->getPort("link0"));
group->connect(rigidBody->getPort("link1"),
mass->getPort("link"));
group->connect(rigidBody->getPort("externalInteractLink"),
externalInteract->getPort("link"));
return group.release();
}
Node* buildSimpleMechanicExample()
{
SharedPtr<Group> group = new Group("G");
MobileRootJoint* mobileRootJoint = new MobileRootJoint("Root Joint");
group->addChild(mobileRootJoint);
RigidBody *rigidBody = new RigidBody("Rigid Body");
rigidBody->addLink("link2");
rigidBody->addLink("externalInteractLink");
rigidBody->addLink("internalInteractLink");
rigidBody->addLink("internalInteractLink2");
group->addChild(rigidBody);
InertiaMatrix inertia(1, 0, 0, 1, 0, 1);
Mass* mass = new Mass("Mass", 1, inertia);
group->addChild(mass);
RevoluteJoint* revoluteJoint = new RevoluteJoint("Revolute Joint");
revoluteJoint->setEnableExternalForce(true);
group->addChild(revoluteJoint);
RigidBody *rigidBody2 = new RigidBody("Rigid Body 2");
rigidBody2->addLink("externalInteractLink");
rigidBody2->addLink("internalInteractLink");
rigidBody2->addLink("internalInteractLink2");
group->addChild(rigidBody2);
Mass* mass2 = new Mass("Mass 2", 1, inertia);
group->addChild(mass2);
ExternalInteract* externalInteract = new ExternalInteract("ExternalInteract");
externalInteract->setPosition(mass->getPosition());
externalInteract->setEnableAllOutputs(true);
group->addChild(externalInteract);
ExternalInteract* externalInteract2 = new ExternalInteract("ExternalInteract 2");
externalInteract2->setPosition(mass2->getPosition());
externalInteract2->setEnableAllOutputs(true);
group->addChild(externalInteract2);
group->connect(mobileRootJoint->getPort("link"), rigidBody->getPort("link0"));
group->connect(rigidBody->getPort("link1"), mass->getPort("link"));
group->connect(rigidBody->getPort("link2"), revoluteJoint->getPort("link0"));
group->connect(revoluteJoint->getPort("link1"), rigidBody2->getPort("link0"));
group->connect(rigidBody2->getPort("link1"), mass2->getPort("link"));
group->connect(rigidBody->getPort("externalInteractLink"), externalInteract->getPort("link"));
group->connect(rigidBody2->getPort("externalInteractLink"), externalInteract2->getPort("link"));
ConstModel* jointForce = new ConstModel("Joint Force", 1);
group->addChild(jointForce);
group->connect(jointForce->getPort("output"),
revoluteJoint->getPort("force"));
InternalInteract* internalInteract = new InternalInteract("InternalInteract");
internalInteract->setPosition0(Vector3(0, 0, 1));
internalInteract->setPosition1(Vector3(0, 0, 0.8));
internalInteract->setEnableAllOutputs(true);
internalInteract->setEnableForce(true);
group->addChild(internalInteract);
group->connect(internalInteract->getPort("link0"),
rigidBody->getPort("internalInteractLink"));
group->connect(internalInteract->getPort("link1"),
rigidBody2->getPort("internalInteractLink"));
InternalInteract* internalInteract2 = new InternalInteract("InternalInteract2");
internalInteract2->setPosition0(Vector3(0, 0, 0.8));
internalInteract2->setPosition1(Vector3(0, 0, 1));
internalInteract2->setEnableAllOutputs(true);
group->addChild(internalInteract2);
group->connect(internalInteract2->getPort("link1"),
rigidBody->getPort("internalInteractLink2"));
group->connect(internalInteract2->getPort("link0"),
rigidBody2->getPort("internalInteractLink2"));
LinearSpringDamper* damper = new LinearSpringDamper("LinearSpringDamper");
damper->setSpringConstant(0.5);
damper->setDamperConstant(1);
group->addChild(damper);
group->connect(damper->getPort("velocity"),
internalInteract->getPort("velocity"));
group->connect(damper->getPort("position"),
internalInteract->getPort("distance"));
group->connect(damper->getPort("force"),
internalInteract->getPort("force"));
return group.release();
}
