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