int w_PrismaticJoint_getMotorForce(lua_State *L)
{
PrismaticJoint *t = luax_checkprismaticjoint(L, 1);
float inv_dt = (float)luaL_checknumber(L, 2);
lua_pushnumber(L, t->getMotorForce(inv_dt));
return 1;
}
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;
}
int w_PrismaticJoint_setLowerLimit(lua_State *L)
{
PrismaticJoint *t = luax_checkprismaticjoint(L, 1);
float arg1 = (float)luaL_checknumber(L, 2);
luax_catchexcept(L, [&](){ t->setLowerLimit(arg1); });
return 0;
}
int w_PrismaticJoint_setMotorSpeed(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
float arg1 = (float)luaL_checknumber(L, 2);
t->setMotorSpeed(arg1);
return 0;
}
int _wrap_PrismaticJoint_setMaxMotorForce(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
float arg1 = (float)luaL_checknumber(L, 2);
t->setMaxMotorForce(arg1);
return 0;
}
int _wrap_PrismaticJoint_setLowerLimit(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
float arg1 = (float)luaL_checknumber(L, 2);
t->setLowerLimit(arg1);
return 0;
}
int w_PrismaticJoint_setLimitsEnabled(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
bool arg1 = luax_toboolean(L, 2);
t->setLimitsEnabled(arg1);
return 0;
}
PrismaticJoint *luax_checkprismaticjoint(lua_State *L, int idx)
{
PrismaticJoint *j = luax_checktype<PrismaticJoint>(L, idx, "PrismaticJoint", PHYSICS_PRISMATIC_JOINT_T);
if (!j->isValid())
luaL_error(L, "Attempt to use destroyed joint.");
return j;
}
int w_PrismaticJoint_setLimits(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
float arg1 = (float)luaL_checknumber(L, 2);
float arg2 = (float)luaL_checknumber(L, 3);
t->setLimits(arg1, arg2);
return 0;
}
PxPrismaticJoint* physx::PxPrismaticJointCreate(PxPhysics& physics,
PxRigidActor* actor0, const PxTransform& localFrame0,
PxRigidActor* actor1, const PxTransform& localFrame1)
{
PX_CHECK_AND_RETURN_NULL(localFrame0.isValid(), "PxPrismaticJointCreate: local frame 0 is not a valid transform");
PX_CHECK_AND_RETURN_NULL(localFrame1.isValid(), "PxPrismaticJointCreate: local frame 1 is not a valid transform");
PX_CHECK_AND_RETURN_NULL(actor0 && actor0->is<PxRigidBody>() || actor1 && actor1->is<PxRigidBody>(), "PxPrismaticJointCreate: at least one actor must be dynamic");
PrismaticJoint* j = PX_NEW(PrismaticJoint)(physics.getTolerancesScale(), actor0, localFrame0, actor1, localFrame1);
if(j->attach(physics, actor0, actor1))
return j;
PX_DELETE(j);
return NULL;
}
//==============================================================================
void PrismaticJoint::copy(const PrismaticJoint& _otherJoint)
{
if(this == &_otherJoint)
return;
setProperties(_otherJoint.getPrismaticJointProperties());
}
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;
}
int w_PrismaticJoint_getMotorForce(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
lua_pushnumber(L, t->getMotorForce());
return 1;
}
int _wrap_PrismaticJoint_isLimitsEnabled(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
luax_pushboolean(L, t->isLimitsEnabled());
return 1;
}
int w_PrismaticJoint_isMotorEnabled(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
luax_pushboolean(L, t->isMotorEnabled());
return 1;
}
int _wrap_PrismaticJoint_getTranslation(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
lua_pushnumber(L, t->getTranslation());
return 1;
}
int w_PrismaticJoint_getJointSpeed(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
lua_pushnumber(L, t->getJointSpeed());
return 1;
}
int w_PrismaticJoint_getLimits(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
lua_remove(L, 1);
return t->getLimits(L);
}
int w_PrismaticJoint_getUpperLimit(lua_State * L)
{
PrismaticJoint * t = luax_checkprismaticjoint(L, 1);
lua_pushnumber(L, t->getUpperLimit());
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;
}
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;
}
