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(); }