/// @return An simple Z*6 arm with Y as up axis.
std::tuple<rbd::MultiBody, rbd::MultiBodyConfig> makeZ6Arm(bool isFixed=true)
{
using namespace Eigen;
using namespace sva;
using namespace rbd;
MultiBodyGraph mbg;
double mass = 1.;
Matrix3d I = Matrix3d::Identity();
Vector3d h = Vector3d::Zero();
RBInertiad rbi(mass, h, I);
for(int i = 0; i < 7; ++i)
{
std::stringstream ss;
ss << "b" << i;
mbg.addBody({rbi, i, ss.str()});
}
for(int i = 0; i < 6; ++i)
{
std::stringstream ss;
ss << "j" << i;
mbg.addJoint({Joint::RevZ, true, i, ss.str()});
}
PTransformd to(Vector3d(0., 1., 0.));
PTransformd from(Vector3d(0., 0., 0.));
mbg.linkBodies(0, from, 1, from, 0);
for(int i = 1; i < 6; ++i)
{
mbg.linkBodies(i, to, i + 1, from, i);
}
MultiBody mb = mbg.makeMultiBody(0, isFixed);
MultiBodyConfig mbc(mb);
mbc.zero(mb);
return std::make_tuple(mb, mbc);
}
std::tuple<rbd::MultiBody, rbd::MultiBodyConfig> makeFreeXArm()
{
using namespace Eigen;
using namespace sva;
using namespace rbd;
MultiBodyGraph mbg;
double mass = 1.;
Matrix3d I = Matrix3d::Identity();
Vector3d h = Vector3d::Zero();
RBInertiad rbi(mass, h, I);
Body b0(rbi, 0, "b0");
Body b1(rbi, 1, "b1");
mbg.addBody(b0);
mbg.addBody(b1);
Joint j0(Joint::RevX, true, 0, "j0");
mbg.addJoint(j0);
// Root j0
// ---- b0 ---- b1
// Free X
PTransformd to(Vector3d(0., 0.5, 0.));
mbg.linkBodies(0, to, 1, PTransformd::Identity(), 0);
MultiBody mb = mbg.makeMultiBody(0, false);
MultiBodyConfig mbc(mb);
mbc.q = {{1., 0., 0., 0., 0., 0., 0.}, {0.}};
mbc.alpha = {{0., 0., 0., 0., 0., 0.}, {0.}};
mbc.alphaD = {{0., 0., 0., 0., 0., 0.}, {0.}};
mbc.jointTorque = {{0., 0., 0., 0., 0., 0.}, {0.}, {0.}, {0.}};
ForceVecd f0(Vector6d::Zero());
mbc.force = {f0, f0};
return std::make_tuple(mb, mbc);
}
std::tuple<rbd::MultiBody, rbd::MultiBodyConfig, rbd::MultiBodyGraph>
makeXYZSarmRandomCoM(bool isFixed=true)
{
using namespace Eigen;
using namespace sva;
using namespace rbd;
typedef Eigen::Matrix<double, 1, 1> EScalar;
MultiBodyGraph mbg;
double mass = 1.;
Matrix3d I = Matrix3d::Identity();
Vector3d h = Vector3d::Random();
RBInertiad rbi(mass, h, I);
Body b0(RBInertiad((fabs(EScalar::Random()(0))+1e-8)*10., h, I), "b0");
Body b1(RBInertiad((fabs(EScalar::Random()(0))+1e-8)*10., h, I), "b1");
Body b2(RBInertiad((fabs(EScalar::Random()(0))+1e-8)*10., h, I), "b2");
Body b3(RBInertiad((fabs(EScalar::Random()(0))+1e-8)*10., h, I), "b3");
Body b4(RBInertiad((fabs(EScalar::Random()(0))+1e-8)*10., h, I), "b4");
mbg.addBody(b0);
mbg.addBody(b1);
mbg.addBody(b2);
mbg.addBody(b3);
mbg.addBody(b4);
Joint j0(Joint::RevX, true, "j0");
Joint j1(Joint::RevY, true, "j1");
Joint j2(Joint::RevZ, true, "j2");
Joint j3(Joint::Spherical, true, "j3");
mbg.addJoint(j0);
mbg.addJoint(j1);
mbg.addJoint(j2);
mbg.addJoint(j3);
// b4
// j3 | Spherical
// Root j0 | j1 j2
// ---- b0 ---- b1 ---- b2 ----b3
// Fixed RevX RevY RevZ
PTransformd to(Vector3d(0., 0.5, 0.));
PTransformd from(Vector3d(0., -0.5, 0.));
mbg.linkBodies("b0", to, "b1", from, "j0");
mbg.linkBodies("b1", to, "b2", from, "j1");
mbg.linkBodies("b2", to, "b3", from, "j2");
mbg.linkBodies("b1", PTransformd(Vector3d(0.5, 0., 0.)),
"b4", PTransformd(Vector3d(-0.5, 0., 0.)), "j3");
MultiBody mb = mbg.makeMultiBody("b0", isFixed);
MultiBodyConfig mbc(mb);
mbc.zero(mb);
return std::make_tuple(mb, mbc, mbg);
}