SkeletonPtr createFloor()
{
SkeletonPtr floor = Skeleton::create("floor");
// Give the floor a body
BodyNodePtr body =
floor->createJointAndBodyNodePair<WeldJoint>(nullptr).second;
// Give the body a shape
double floor_width = 10.0;
double floor_height = 0.01;
std::shared_ptr<BoxShape> box(
new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width)));
box->setColor(dart::Color::Black());
body->addVisualizationShape(box);
body->addCollisionShape(box);
// Put the body into position
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.translation() = Eigen::Vector3d(0.0, -1.0, 0.0);
body->getParentJoint()->setTransformFromParentBodyNode(tf);
return floor;
}
void setGeometry(const BodyNodePtr& bn)
{
// Create a BoxShape to be used for both visualization and collision checking
std::shared_ptr<BoxShape> box(new BoxShape(
Eigen::Vector3d(default_width, default_depth, default_height)));
box->setColor(dart::Color::Blue());
// Set the location of the Box
Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity());
Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height / 2.0);
box_tf.translation() = center;
box->setLocalTransform(box_tf);
// Add it as a visualization and collision shape
bn->addVisualizationShape(box);
bn->addCollisionShape(box);
// Move the center of mass to the center of the object
bn->setLocalCOM(center);
}
BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name)
{
BallJoint::Properties properties;
properties.mName = name + "_joint";
properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position);
properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness);
properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping);
BodyNodePtr bn = pendulum->createJointAndBodyNodePair<BallJoint>(
nullptr, properties, BodyNode::Properties(name)).second;
// Make a shape for the Joint
const double& R = default_width;
std::shared_ptr<EllipsoidShape> ball(
new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R)));
ball->setColor(dart::Color::Blue());
bn->addVisualizationShape(ball);
// Set the geometry of the Body
setGeometry(bn);
return bn;
}
BodyNode* addBody(const SkeletonPtr& pendulum, BodyNode* parent,
const std::string& name)
{
// Set up the properties for the Joint
RevoluteJoint::Properties properties;
properties.mName = name + "_joint";
properties.mAxis = Eigen::Vector3d::UnitY();
properties.mT_ParentBodyToJoint.translation() =
Eigen::Vector3d(0, 0, default_height);
properties.mRestPosition = default_rest_position;
properties.mSpringStiffness = default_stiffness;
properties.mDampingCoefficient = default_damping;
// Create a new BodyNode, attached to its parent by a RevoluteJoint
BodyNodePtr bn = pendulum->createJointAndBodyNodePair<RevoluteJoint>(
parent, properties, BodyNode::Properties(name)).second;
// Make a shape for the Joint
const double R = default_width / 2.0;
const double h = default_depth;
std::shared_ptr<CylinderShape> cyl(
new CylinderShape(R, h));
cyl->setColor(dart::Color::Blue());
// Line up the cylinder with the Joint axis
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.linear() = dart::math::eulerXYZToMatrix(
Eigen::Vector3d(90.0 * M_PI / 180.0, 0, 0));
cyl->setLocalTransform(tf);
bn->addVisualizationShape(cyl);
// Set the geometry of the Body
setGeometry(bn);
return bn;
}
