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; }
TVector3 testfunction(TScalar l1, TScalar l2, TScalar q1, TScalar q2) { using namespace mbslib; MbsCompoundWithBuilder mbs; mbs.addFixedBase(); Joint1DOF & j1 = *mbs.addRevoluteJoint(TVector3::UnitZ()); mbs.addRigidLink(TVector3(l1, 0, 0), TVector3::Zero(), TScalar(1), TMatrix3x3::Zero()); Joint1DOF & j2 = *mbs.addRevoluteJoint(TVector3::UnitZ()); mbs.addRigidLink(TVector3(l2, 0, 0), TVector3::Zero(), TScalar(1), TMatrix3x3::Zero()); Endpoint * tcp = mbs.addEndpoint(); j1.setJointPosition(q1); j2.setJointPosition(q2); mbs.doDirkin(); TMatrix6xX J = mbs.calculateJacobian(*tcp); std::cout << J << std::endl; return tcp->getCoordinateFrame().r; }