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