Example #1
0
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()));
    
}
Example #2
0
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()));
}
Example #3
0
//==============================================================================
/// 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);
}
Example #4
0
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;
}