C++ (Cpp) MultiBodyGraph::linkBodies Examples

Example #1

File: TrajTest.cpp Project: haudren/TrajPlanning

/// @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);
}

Example #2

File: PGJacobianTest.cpp Project: francois-keith/Tasks

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

Example #3

File: CoMTest.cpp Project: ahundt/RBDyn

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