| 345 | } |
| 346 | |
| 347 | BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) |
| 348 | { |
| 349 | BallJoint::Properties properties; |
| 350 | properties.mName = name + "_joint"; |
| 351 | properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position); |
| 352 | properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness); |
| 353 | properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); |
| 354 | |
| 355 | auto bodyProp = BodyNode::Properties(BodyNode::AspectProperties(name)); |
| 356 | BodyNodePtr bn = pendulum |
| 357 | ->createJointAndBodyNodePair<BallJoint>( |
| 358 | nullptr, properties, bodyProp) |
| 359 | .second; |
| 360 | |
| 361 | // Make a shape for the Joint |
| 362 | const double& R = default_width; |
| 363 | std::shared_ptr<EllipsoidShape> ball( |
| 364 | new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R))); |
| 365 | auto shapeNode = bn->createShapeNodeWith<VisualAspect>(ball); |
| 366 | shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); |
| 367 | |
| 368 | // Set the geometry of the Body |
| 369 | setGeometry(bn); |
| 370 | |
| 371 | return bn; |
| 372 | } |
| 373 | |
| 374 | BodyNode* addBody( |
| 375 | const SkeletonPtr& pendulum, BodyNode* parent, const std::string& name) |
no test coverage detected