| 21 | |
| 22 | namespace { |
| 23 | pinocchio::Model SimpleModel() { |
| 24 | pinocchio::Model model; |
| 25 | |
| 26 | const auto joint_id = model.addJoint(0, pinocchio::JointModelPZ(), |
| 27 | pinocchio::SE3::Identity(), "joint"); |
| 28 | const auto frame_id = model.addJointFrame(joint_id, -1); |
| 29 | model.addBodyFrame("link", joint_id, pinocchio::SE3::Identity(), |
| 30 | static_cast<int>(frame_id)); |
| 31 | |
| 32 | Eigen::Vector3d const CoM = Eigen::Vector3d::Zero(); |
| 33 | double constexpr mass = 0.001; |
| 34 | Eigen::Matrix3d const I = 0.001 * Eigen::Matrix3d::Identity(); |
| 35 | model.appendBodyToJoint(joint_id, pinocchio::Inertia(mass, CoM, I), |
| 36 | pinocchio::SE3::Identity()); |
| 37 | |
| 38 | return model; |
| 39 | } |
| 40 | |
| 41 | using StageSpace = aligator::MultibodyPhaseSpace<double>; |
| 42 | |