int w_RevoluteJoint_getMotorTorque(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); float inv_dt = (float)luaL_checknumber(L, 2); lua_pushnumber(L, t->getMotorTorque(inv_dt)); return 1; }
int w_RevoluteJoint_setMotorSpeed(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); float arg1 = (float)luaL_checknumber(L, 2); t->setMotorSpeed(arg1); return 0; }
int w_RevoluteJoint_setLowerLimit(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); float arg1 = (float)luaL_checknumber(L, 2); luax_catchexcept(L, [&](){ t->setLowerLimit(arg1); }); return 0; }
RevoluteJoint* RevoluteJoint::createObject(PxU8*& address, PxDeserializationContext& context) { RevoluteJoint* obj = new (address) RevoluteJoint(PxBaseFlag::eIS_RELEASABLE); address += sizeof(RevoluteJoint); obj->importExtraData(context); obj->resolveReferences(context); return obj; }
int w_RevoluteJoint_setLimits(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); float arg1 = (float)luaL_checknumber(L, 2); float arg2 = (float)luaL_checknumber(L, 3); t->setLimits(arg1, arg2); return 0; }
PxRevoluteJoint* physx::PxRevoluteJointCreate(PxPhysics& physics, PxRigidActor* actor0, const PxTransform& localFrame0, PxRigidActor* actor1, const PxTransform& localFrame1) { PX_CHECK_AND_RETURN_NULL(localFrame0.isValid(), "PxRevoluteJointCreate: local frame 0 is not a valid transform"); PX_CHECK_AND_RETURN_NULL(localFrame1.isValid(), "PxRevoluteJointCreate: local frame 1 is not a valid transform"); RevoluteJoint* j = PX_NEW(RevoluteJoint)(physics.getTolerancesScale(), actor0, localFrame0, actor1, localFrame1); if(j->attach(physics, actor0, actor1)) return j; PX_DELETE(j); return NULL; }
//============================================================================== void RevoluteJoint::copy(const RevoluteJoint& _otherJoint) { if(this == &_otherJoint) return; setProperties(_otherJoint.getRevoluteJointProperties()); }
PxRevoluteJoint* physx::PxRevoluteJointCreate(PxPhysics& physics, PxRigidActor* actor0, const PxTransform& localFrame0, PxRigidActor* actor1, const PxTransform& localFrame1) { PX_CHECK_AND_RETURN_NULL(localFrame0.isSane(), "PxRevoluteJointCreate: local frame 0 is not a valid transform"); PX_CHECK_AND_RETURN_NULL(localFrame1.isSane(), "PxRevoluteJointCreate: local frame 1 is not a valid transform"); PX_CHECK_AND_RETURN_NULL(actor0 != actor1, "PxRevoluteJointCreate: actors must be different"); PX_CHECK_AND_RETURN_NULL((actor0 && actor0->is<PxRigidBody>()) || (actor1 && actor1->is<PxRigidBody>()), "PxRevoluteJointCreate: at least one actor must be dynamic"); RevoluteJoint* j; PX_NEW_SERIALIZED(j,RevoluteJoint)(physics.getTolerancesScale(), actor0, localFrame0, actor1, localFrame1); if(j->attach(physics, actor0, actor1)) return j; PX_DELETE(j); return NULL; }
QList< QPair<QVector3D, QVector3D> > KinTreeNode::computeJacobian( QVector3D F, QVector3D T ) { QList< QPair<QVector3D, QVector3D> > J; QVector3D p = getPos(); KinTreeNode* thisNode = this; while (true) { //printf("entered computeJacobian()\n"); if ( thisNode->getNodeType() == RJOINT ) { //printf("found a joint\n"); RevoluteJoint* joint = dynamic_cast<KinematicModel::RevoluteJoint*>(thisNode); if (joint) { QVector3D q = joint->getPos(); QVector3D qp = p-q; QVector3D jointAxis = joint->getCurrentNodeAxis(); QPair<QVector3D, QVector3D> jColumn( QVector3D::crossProduct(jointAxis, qp), jointAxis ); // project F and T onto this joint qreal jointTorque = QVector3D::dotProduct(jColumn.first, F) + QVector3D::dotProduct(jColumn.second, T); joint->parentMotor()->addTorque(jointTorque); //printf("jColumn: %f, %f, %f, %f, %f, %f\n", jColumn.first.x(), jColumn.first.y(), jColumn.first.z(), // jColumn.second.x(), jColumn.second.y(), jColumn.second.z()); J.prepend(jColumn); if ( joint->isBodyPartRoot() ) break; } //else printf("cast failed unexpectedly\n"); } //else printf("found a non-joint\n"); if (!thisNode->parent()) break; else thisNode = thisNode->parent(); } return J; }
int main(void) { using namespace mbslib; MbsCompoundWithBuilder mbs; mbs.addFixedBase(); mbs.addFork(); PrismaticJoint * pj = mbs.addPrismaticJoint(TVector3(1, 0, 0)); Endpoint * s1End1 = mbs.addEndpoint(); mbs.addFixedTranslation(TVector3(1, 0, 0)); mbs.addFork(); Endpoint * s1Midpoint = mbs.addEndpoint(); mbs.addFixedTranslation(TVector3(1, 0, 0)); RevoluteJoint * rj = mbs.addRevoluteJoint(TVector3(0, 0, 1)); mbs.addFork(); mbs.addFixedTranslation(TVector3(0, 1, 0)); Endpoint * s1End2 = mbs.addEndpoint(); mbs.addRigidLink(TVector3(1, 0, 0), TVector3::Zero(), 1, TMatrix3x3::Zero()); Endpoint * tcp = mbs.addEndpoint(); LinearSpringWithRopeModel springModel(1, 0, 1); Spring3D * spring = mbs.addSpring(springModel); (*spring) << s1End1 << s1Midpoint << s1End2; //mbs.doDirkin(); //spring.resetForce(); //spring.applyForce(); mbs.doRne(); std::cout << s1End1->getCoordinateFrame().r.transpose() << std::endl; std::cout << s1End2->getCoordinateFrame().r.transpose() << std::endl; std::cout << tcp->getCoordinateFrame().r.transpose() << std::endl; std::cout << spring->getSpringForce() << std::endl; std::cout << pj->getJointForceTorque() << std::endl; std::cout << rj->getJointForceTorque() << std::endl; }
void addBodiesRecursive(unsigned numLevels, unsigned numBodies, RigidBody* rigidBody, Group* group) { if (!numLevels) return; for (unsigned i = 0; i < numBodies; ++i) { RevoluteJoint* revoluteJoint = new RevoluteJoint("RevoluteJoint"); group->addChild(revoluteJoint); group->connect(revoluteJoint->getPort("link0"), rigidBody->addLink("childLink")); RigidBody* rigidBody2 = new RigidBody("RigidBody"); group->addChild(rigidBody2); group->connect(revoluteJoint->getPort("link1"), rigidBody2->addLink("parentLink")); Mass* mass = new Mass("Mass", 1, InertiaMatrix(1, 0, 0, 1, 0, 1)); group->addChild(mass); group->connect(mass->getPort("link"), rigidBody2->addLink("massLink")); addBodiesRecursive(numLevels - 1, numBodies, rigidBody2, group); } }
int w_RevoluteJoint_hasLimitsEnabled(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); luax_pushboolean(L, t->hasLimitsEnabled()); return 1; }
int main(int argc, char *argv[]) { real_type omega = 2; SharedPtr<System> system = new System("Tire Testrig"); system->addSampleTime(real_type(1)/real_type(100)); system->setTimestepper(new DoPri5); // set the moving ground system->getEnvironment()->setGround(new MovingGround(Vector3(10, 0, 0))); system->getEnvironment()->setGravity(new ZGravity()); // First build up the mechanical system FixedRootJoint* fixedRootJoint = new FixedRootJoint("Fixed Root Joint"); system->addModel(fixedRootJoint); fixedRootJoint->setRefPosition(Vector3(0, 0, 1)); PrismaticJoint* prismaticJoint = new PrismaticJoint("Normal Force joint"); prismaticJoint->setJointAxis(Vector3::unit(2)); system->addModel(prismaticJoint); Summer* normalForceSum = new Summer("Normal Force Sum"); normalForceSum->setNumSummands(2); normalForceSum->setInputSign(0, Summer::Minus); system->addModel(normalForceSum); Connection::connect(prismaticJoint->getInputPort(0), normalForceSum->getOutputPort(0)); ConstModel* normalForce = new ConstModel("Normal force"); normalForce->setScalarValue(2000); system->addModel(normalForce); Connection::connect(normalForceSum->getInputPort(0), normalForce->getOutputPort(0)); LinearSpringDamper* strutDamper = new LinearSpringDamper("Strut Damper"); strutDamper->setSpringConstant(0); strutDamper->setDamperConstant(-30); system->addModel(strutDamper); Connection::connect(normalForceSum->getInputPort(1), strutDamper->getOutputPort(0)); Connection::connect(strutDamper->getInputPort(0), prismaticJoint->getOutputPort(0)); Connection::connect(strutDamper->getInputPort(1), prismaticJoint->getOutputPort(1)); RigidBody* rootMount = new RigidBody("Root Mount"); system->addModel(rootMount); rootMount->setInboardJoint(fixedRootJoint); rootMount->addInteract(prismaticJoint); RevoluteActuator* camberActuator = new RevoluteActuator("Camber Actuator"); system->addModel(camberActuator); ConstModel* camberAngle = new ConstModel("Camber Angle"); camberAngle->setScalarValue(0); system->addModel(camberAngle); Connection::connect(camberActuator->getInputPort(0), camberAngle->getOutputPort(0)); RigidBody* normalForceStrut = new RigidBody("Normal Force Strut"); system->addModel(normalForceStrut); normalForceStrut->setInboardJoint(prismaticJoint); normalForceStrut->addInteract(camberActuator); RevoluteActuator* sideActuator = new RevoluteActuator("Sideslip Actuator"); system->addModel(sideActuator); ConstModel* sideslipAngle = new ConstModel("Sideslip Angle"); sideslipAngle->setScalarValue(0); system->addModel(sideslipAngle); Connection::connect(sideActuator->getInputPort(0), sideslipAngle->getOutputPort(0)); RigidBody* camberStrut = new RigidBody("Camber Strut"); system->addModel(camberStrut); camberStrut->setInboardJoint(camberActuator); camberStrut->addInteract(sideActuator); RevoluteJoint* hubJoint = new RevoluteJoint("Hub Joint"); hubJoint->setJointAxis(Vector3(0, 1, 0)); system->addModel(hubJoint); RigidBody* hubStrut = new RigidBody("Hub Strut"); system->addModel(hubStrut); hubStrut->setInboardJoint(sideActuator); hubStrut->addInteract(hubJoint); RigidBody* rimAndTire = new RigidBody("Rim And Tire"); system->addModel(rimAndTire); rimAndTire->setInboardJoint(hubJoint); Mass* tireAndRimMass = new Mass("Rim And Tire Mass"); tireAndRimMass->setInertia(1, InertiaMatrix(1, 0, 0, 1, 0, 1)); system->addModel(tireAndRimMass); rimAndTire->addInteract(tireAndRimMass); WheelContact* wheelContact = new WheelContact("Wheel Contact"); wheelContact->setWheelRadius(0.3); wheelContact->setSpringConstant(1000); wheelContact->setSpringDamping(sqrt(wheelContact->getSpringConstant())/10); system->addModel(wheelContact); rimAndTire->addInteract(wheelContact); if (!system->init()) { std::cout << "Could not initialize the system" << std::endl; return EXIT_FAILURE; } HDF5Writer hwriter("system.h5"); system->accept(hwriter); while (system->getTime() < 10) { system->simulate(system->getTime() + 0.01); system->accept(hwriter); } return EXIT_SUCCESS; }
int w_RevoluteJoint_getMaxMotorTorque(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); lua_pushnumber(L, t->getMaxMotorTorque()); return 1; }
bool SelectJointOP::OnMouseDrag(int x, int y) { if (SelectBodyOP::OnMouseDrag(x, y)) return true; if (m_selected) { sm::vec2 pos = m_stage->TransPosScrToProj(x, y); switch (m_selected->m_type) { case Joint::e_revoluteJoint: { RevoluteJoint* joint = static_cast<RevoluteJoint*>(m_selected); const float disA = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorA()), disB = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorB()); if (disA < disB) joint->SetLocalAnchorA(pos); else joint->SetLocalAnchorB(pos); } break; case Joint::e_prismaticJoint: { PrismaticJoint* joint = static_cast<PrismaticJoint*>(m_selected); const float disA = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorA()), disB = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorB()); if (disA < disB) joint->SetLocalAnchorA(pos); else joint->SetLocalAnchorB(pos); } break; case Joint::e_distanceJoint: { DistanceJoint* joint = static_cast<DistanceJoint*>(m_selected); const float disA = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorA()), disB = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorB()); if (disA < disB) joint->SetLocalAnchorA(pos); else joint->SetLocalAnchorB(pos); } break; case Joint::e_pulleyJoint: { PulleyJoint* joint = static_cast<PulleyJoint*>(m_selected); const float disA = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorA()), disB = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorB()); const float disGA = sm::dis_pos_to_pos(pos, joint->m_ground_anchor_a), disGB = sm::dis_pos_to_pos(pos, joint->m_ground_anchor_b); float dis = std::min(std::min(disA, disB), std::min(disGA, disGB)); if (dis == disA) joint->SetLocalAnchorA(pos); else if (dis == disB) joint->SetLocalAnchorB(pos); else if (dis == disGA) joint->m_ground_anchor_a = pos; else joint->m_ground_anchor_b = pos; } break; case Joint::e_gearJoint: { } break; case Joint::e_wheelJoint: { WheelJoint* joint = static_cast<WheelJoint*>(m_selected); const float disA = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorA()), disB = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorB()); if (disA < disB) joint->SetLocalAnchorA(pos); else joint->SetLocalAnchorB(pos); } break; case Joint::e_weldJoint: { WeldJoint* joint = static_cast<WeldJoint*>(m_selected); const float disA = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorA()), disB = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorB()); if (disA < disB) joint->SetLocalAnchorA(pos); else joint->SetLocalAnchorB(pos); } break; case Joint::e_frictionJoint: { FrictionJoint* joint = static_cast<FrictionJoint*>(m_selected); const float disA = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorA()), disB = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorB()); if (disA < disB) joint->SetLocalAnchorA(pos); else joint->SetLocalAnchorB(pos); } break; case Joint::e_ropeJoint: { RopeJoint* joint = static_cast<RopeJoint*>(m_selected); const float disA = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorA()), disB = sm::dis_pos_to_pos(pos, joint->GetWorldAnchorB()); if (disA < disB) joint->SetLocalAnchorA(pos); else joint->SetLocalAnchorB(pos); } break; case Joint::e_motorJoint: { } break; } ee::SetCanvasDirtySJ::Instance()->SetDirty(); } return false; }
int w_RevoluteJoint_getUpperLimit(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); lua_pushnumber(L, t->getUpperLimit()); return 1; }
int w_RevoluteJoint_isMotorEnabled(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); luax_pushboolean(L, t->isMotorEnabled()); return 1; }
int w_RevoluteJoint_getJointSpeed(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); lua_pushnumber(L, t->getJointSpeed()); return 1; }
int w_RevoluteJoint_getLimits(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); lua_remove(L, 1); return t->getLimits(L); }
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(); }
int _wrap_RevoluteJoint_getAngle(lua_State * L) { RevoluteJoint * t = luax_checkrevolutejoint(L, 1); lua_pushnumber(L, t->getAngle()); return 1; }
int w_RevoluteJoint_setMotorEnabled(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); bool arg1 = luax_toboolean(L, 2); t->setMotorEnabled(arg1); return 0; }
RevoluteJoint *luax_checkrevolutejoint(lua_State *L, int idx) { RevoluteJoint *j = luax_checktype<RevoluteJoint>(L, idx, PHYSICS_REVOLUTE_JOINT_ID); if (!j->isValid()) luaL_error(L, "Attempt to use destroyed joint."); return j; }
int _wrap_RevoluteJoint_setLowerLimit(lua_State * L) { RevoluteJoint * t = luax_checkrevolutejoint(L, 1); float arg1 = (float)luaL_checknumber(L, 2); t->setLowerLimit(arg1); return 0; }
int w_RevoluteJoint_enableLimit(lua_State *L) { RevoluteJoint *t = luax_checkrevolutejoint(L, 1); bool arg1 = luax_toboolean(L, 2); t->enableLimit(arg1); return 0; }
int _wrap_RevoluteJoint_setMaxMotorTorque(lua_State * L) { RevoluteJoint * t = luax_checkrevolutejoint(L, 1); float arg1 = (float)luaL_checknumber(L, 2); t->setMaxMotorTorque(arg1); return 0; }