| 372 | } |
| 373 | |
| 374 | BodyNode* addBody( |
| 375 | const SkeletonPtr& pendulum, BodyNode* parent, const std::string& name) |
| 376 | { |
| 377 | // Set up the properties for the Joint |
| 378 | RevoluteJoint::Properties properties; |
| 379 | properties.mName = name + "_joint"; |
| 380 | properties.mAxis = Eigen::Vector3d::UnitY(); |
| 381 | properties.mT_ParentBodyToJoint.translation() |
| 382 | = Eigen::Vector3d(0, 0, default_height); |
| 383 | properties.mRestPositions[0] = default_rest_position; |
| 384 | properties.mSpringStiffnesses[0] = default_stiffness; |
| 385 | properties.mDampingCoefficients[0] = default_damping; |
| 386 | |
| 387 | // Create a new BodyNode, attached to its parent by a RevoluteJoint |
| 388 | BodyNodePtr bn = pendulum |
| 389 | ->createJointAndBodyNodePair<RevoluteJoint>( |
| 390 | parent, properties, BodyNode::AspectProperties(name)) |
| 391 | .second; |
| 392 | |
| 393 | // Make a shape for the Joint |
| 394 | const double R = default_width / 2.0; |
| 395 | const double h = default_depth; |
| 396 | std::shared_ptr<CylinderShape> cyl(new CylinderShape(R, h)); |
| 397 | |
| 398 | // Line up the cylinder with the Joint axis |
| 399 | Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); |
| 400 | tf.linear() = dart::math::eulerXYZToMatrix( |
| 401 | Eigen::Vector3d(dart::math::toRadian(90.0), 0, 0)); |
| 402 | |
| 403 | auto shapeNode = bn->createShapeNodeWith<VisualAspect>(cyl); |
| 404 | shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); |
| 405 | shapeNode->setRelativeTransform(tf); |
| 406 | |
| 407 | // Set the geometry of the Body |
| 408 | setGeometry(bn); |
| 409 | |
| 410 | return bn; |
| 411 | } |
| 412 | |
| 413 | int main(int argc, char* argv[]) |
| 414 | { |
no test coverage detected