| 279 | } |
| 280 | |
| 281 | BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name) |
| 282 | { |
| 283 | BallJoint::Properties properties; |
| 284 | properties.mName = name + "_joint"; |
| 285 | properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position); |
| 286 | properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness); |
| 287 | properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping); |
| 288 | |
| 289 | BodyNodePtr bn |
| 290 | = pendulum |
| 291 | ->createJointAndBodyNodePair<BallJoint>( |
| 292 | nullptr, properties, BodyNode::AspectProperties(name)) |
| 293 | .second; |
| 294 | |
| 295 | // Make a shape for the Joint |
| 296 | const double& R = default_width; |
| 297 | std::shared_ptr<EllipsoidShape> ball( |
| 298 | new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R))); |
| 299 | auto shapeNode = bn->createShapeNodeWith<VisualAspect>(ball); |
| 300 | shapeNode->getVisualAspect()->setColor(dart::Color::Blue()); |
| 301 | |
| 302 | // Set the geometry of the Body |
| 303 | setGeometry(bn); |
| 304 | |
| 305 | return bn; |
| 306 | } |
| 307 | |
| 308 | BodyNode* addBody( |
| 309 | const SkeletonPtr& pendulum, BodyNode* parent, const std::string& name) |
no test coverage detected