void DH2HG(Eigen::Isometry3d &B, double t, double f, double r, double d)
{
// Convert DH parameters (standard convention) to Homogenuous transformation matrix.
B = Eigen::Matrix4d::Identity();
B.translate(Eigen::Vector3d(0.,0.,d));
B.rotate(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
B.translate(Eigen::Vector3d(r,0,0));
B.rotate(Eigen::AngleAxisd(f, Eigen::Vector3d::UnitX()));
}
void randomize_transform(Eigen::Isometry3d& tf,
double translation_limit=100,
double rotation_limit=100)
{
Eigen::Vector3d r = random_vec<3>(translation_limit);
Eigen::Vector3d theta = random_vec<3>(rotation_limit);
tf.setIdentity();
tf.translate(r);
if(theta.norm()>0)
tf.rotate(Eigen::AngleAxisd(theta.norm(), theta.normalized()));
}
//==============================================================================
/// Add an end-effector to the last link of the given robot
void addEndEffector(SkeletonPtr robot, BodyNode* parent_node, Vector3d dim)
{
// Create the end-effector node with a random dimension
BodyNode::Properties node(BodyNode::AspectProperties("ee"));
std::shared_ptr<Shape> shape(new BoxShape(Vector3d(0.2, 0.2, 0.2)));
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translate(Eigen::Vector3d(0.0, 0.0, dim(2)));
Joint::Properties joint("eeJoint", T);
auto pair = robot->createJointAndBodyNodePair<WeldJoint>(
parent_node, joint, node);
auto bodyNode = pair.second;
bodyNode->createShapeNodeWith<
VisualAspect, CollisionAspect, DynamicsAspect>(shape);
}
Skeleton* createFreeFloatingTwoLinkRobot(Vector3d dim1,
Vector3d dim2, TypeOfDOF type2,
bool finished = true)
{
Skeleton* robot = new Skeleton();
// Create the first link, the joint with the ground and its shape
double mass = 1.0;
BodyNode* node = new BodyNode("link1");
Joint* joint = new FreeJoint();
joint->setName("joint1");
Shape* shape = new BoxShape(dim1);
node->setLocalCOM(Vector3d(0.0, 0.0, dim1(2)/2.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
node->setParentJoint(joint);
robot->addBodyNode(node);
// Create the second link, the joint with link1 and its shape
BodyNode* parent_node = robot->getBodyNode("link1");
node = new BodyNode("link2");
joint = create1DOFJoint(0.0, -DART_PI, DART_PI, type2);
joint->setName("joint2");
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translate(Eigen::Vector3d(0.0, 0.0, dim1(2)));
joint->setTransformFromParentBodyNode(T);
shape = new BoxShape(dim2);
node->setLocalCOM(Vector3d(0.0, 0.0, dim2(2)/2.0));
node->addVisualizationShape(shape);
node->addCollisionShape(shape);
node->setMass(mass);
node->setParentJoint(joint);
parent_node->addChildBodyNode(node);
robot->addBodyNode(node);
// If finished, initialize the skeleton
if(finished)
{
addEndEffector(robot, node, dim2);
robot->init();
}
return robot;
}