/** * Method for building the Luxo Jr articulating model. It sets up the system of * rigid bodies and joint articulations to define Luxo Jr lamp geometry. */ void createLuxoJr(OpenSim::Model &model){ // Create base //-------------- OpenSim::Body* base = new OpenSim::Body("base", baseMass, Vec3(0.0), Inertia::cylinderAlongY(0.1, baseHeight)); // Add visible geometry base->attachMeshGeometry("Base_meters.obj"); // Define base to float relative to ground via free joint FreeJoint* base_ground = new FreeJoint("base_ground", // parent body, location in parent body, orientation in parent model.getGround(), Vec3(0.0), Vec3(0.0), // child body, location in child body, orientation in child *base, Vec3(0.0,-baseHeight/2.0,0.0),Vec3(0.0)); // add base to model model.addBody(base); model.addJoint(base_ground); /*for (int i = 0; i<base_ground->get_CoordinateSet().getSize(); ++i) { base_ground->upd_CoordinateSet()[i].set_locked(true); }*/ // Fix a frame to the base axis for attaching the bottom bracket SimTK::Transform* shift_and_rotate = new SimTK::Transform(); //shift_and_rotate->setToZero(); shift_and_rotate->set(Rotation(-1*SimTK::Pi/2, SimTK::CoordinateAxis::XCoordinateAxis()), Vec3(0.0, bracket_location, 0.0)); PhysicalOffsetFrame pivot_frame_on_base("pivot_frame_on_base", *base, *shift_and_rotate); // Create bottom bracket //----------------------- OpenSim::Body* bottom_bracket = new OpenSim::Body("bottom_bracket", bracket_mass, Vec3(0.0), Inertia::brick(0.03, 0.03, 0.015)); // add bottom bracket to model model.addBody(bottom_bracket); // Fix a frame to the bracket for attaching joint shift_and_rotate->setP(Vec3(0.0)); PhysicalOffsetFrame pivot_frame_on_bottom_bracket( "pivot_frame_on_bottom_bracket", *bottom_bracket, *shift_and_rotate); // Add visible geometry bottom_bracket->attachMeshGeometry("bottom_bracket_meters.obj"); // Make bottom bracket to twist on base with vertical pin joint. // You can create a joint from any existing physical frames attached to // rigid bodies. One way to reference them is by name, like this... PinJoint* base_pivot = new PinJoint("base_pivot", pivot_frame_on_base, pivot_frame_on_bottom_bracket); base_pivot->append_frames(pivot_frame_on_base); base_pivot->append_frames(pivot_frame_on_bottom_bracket); // add base pivot joint to the model model.addJoint(base_pivot); // add some damping to the pivot // initialized to zero stiffness and damping BushingForce* pivotDamper = new BushingForce("pivot_bushing", "pivot_frame_on_base", "pivot_frame_on_bottom_bracket"); pivotDamper->set_rotational_damping(pivot_damping); model.addForce(pivotDamper); // Create posterior leg //----------------------- OpenSim::Body* posteriorLegBar = new OpenSim::Body("posterior_leg_bar", bar_mass, Vec3(0.0), Inertia::brick(leg_bar_dimensions/2.0)); posteriorLegBar->attachMeshGeometry("Leg_meters.obj"); PhysicalOffsetFrame posterior_knee_on_bottom_bracket( "posterior_knee_on_bottom_bracket", *bottom_bracket, Transform(posterior_bracket_hinge_location) ); PhysicalOffsetFrame posterior_knee_on_posterior_bar( "posterior_knee_on_posterior_bar", *posteriorLegBar, Transform(inferior_bar_hinge_location) ); // Attach posterior leg to bottom bracket using another pin joint. // Another way to reference physical frames in a joint is by creating them // in place, like this... OpenSim::PinJoint* posteriorKnee = new OpenSim::PinJoint("posterior_knee", posterior_knee_on_bottom_bracket, posterior_knee_on_posterior_bar); // posteriorKnee will own and serialize the attachment offset frames posteriorKnee->append_frames(posterior_knee_on_bottom_bracket); posteriorKnee->append_frames(posterior_knee_on_posterior_bar); // add posterior leg to model model.addBody(posteriorLegBar); model.addJoint(posteriorKnee); // allow this joint's coordinate to float freely when assembling constraints // the joint we create next will drive the pose of the 4-bar linkage posteriorKnee->upd_CoordinateSet()[0] .set_is_free_to_satisfy_constraints(true); // Create anterior leg Hlink //---------------------------- OpenSim::Body* leg_Hlink = new OpenSim::Body("leg_Hlink", bar_mass, Vec3(0.0), Inertia::brick(leg_Hlink_dimensions/2.0)); leg_Hlink->attachMeshGeometry("H_Piece_meters.obj"); PhysicalOffsetFrame anterior_knee_on_bottom_bracket( "anterior_knee_on_bottom_bracket", *bottom_bracket, Transform(anterior_bracket_hinge_location)); PhysicalOffsetFrame anterior_knee_on_anterior_bar( "anterior_knee_on_anterior_bar", *leg_Hlink, Transform(inferior_Hlink_hinge_location)); // Connect anterior leg to bottom bracket via pin joint OpenSim::PinJoint* anterior_knee = new OpenSim::PinJoint("anterior_knee", anterior_knee_on_bottom_bracket, anterior_knee_on_anterior_bar); anterior_knee->append_frames(anterior_knee_on_bottom_bracket); anterior_knee->append_frames(anterior_knee_on_anterior_bar); // add anterior leg to model model.addBody(leg_Hlink); model.addJoint(anterior_knee); // this anterior knee joint defines the motion of the lower 4-bar linkage // set it's default coordinate value to a slightly flexed position. anterior_knee->upd_CoordinateSet()[0].set_default_value(SimTK::Pi/6); // Create pelvis bracket //----------------------- OpenSim::Body* pelvisBracket = new OpenSim::Body("pelvis_bracket", bracket_mass, Vec3(0.0), Inertia::brick(pelvis_dimensions/2.0)); pelvisBracket->attachMeshGeometry("Pelvis_bracket_meters.obj"); // Connect pelvis to Hlink via pin joint SimTK::Transform pelvis_anterior_shift( anterior_superior_pelvis_pin_location); PhysicalOffsetFrame anterior_hip_on_Hlink( "anterior_hip_on_Hlink", *leg_Hlink, Transform(superior_Hlink_hinge_location)); PhysicalOffsetFrame anterior_hip_on_pelvis( "anterior_hip_on_pelvis", *pelvisBracket, pelvis_anterior_shift); OpenSim::PinJoint* anteriorHip = new OpenSim::PinJoint("anterior_hip", anterior_hip_on_Hlink, anterior_hip_on_pelvis); anteriorHip->append_frames(anterior_hip_on_Hlink); anteriorHip->append_frames(anterior_hip_on_pelvis); // add anterior leg to model model.addBody(pelvisBracket); model.addJoint(anteriorHip); // since the previous, anterior knee joint drives the pose of the lower // 4-bar linkage, set the anterior hip angle such that it's free to satisfy // constraints that couple it to the 4-bar linkage. anteriorHip->upd_CoordinateSet()[0] .set_is_free_to_satisfy_constraints(true); // Close the loop for the lower, four-bar linkage with a constraint //------------------------------------------------------------------ // Create and configure point on line constraint OpenSim::PointOnLineConstraint* posteriorHip = new OpenSim::PointOnLineConstraint(); posteriorHip->setLineBodyByName(pelvisBracket->getName()); posteriorHip->setLineDirection(Vec3(0.0,0.0,1.0)); posteriorHip->setPointOnLine(inferior_pelvis_pin_location); posteriorHip->setFollowerBodyByName(posteriorLegBar->getName()); posteriorHip->setPointOnFollower(superior_bar_hinge_location); // add constraint to model model.addConstraint(posteriorHip); // Create chest piece //----------------------- OpenSim::Body* chest = new OpenSim::Body("chest_bar", bar_mass, Vec3(0.0), Inertia::brick(torso_bar_dimensions/2.0)); chest->attachMeshGeometry("Anterior_torso_bar.obj"); PhysicalOffsetFrame anterior_torso_hinge_on_pelvis( "anterior_torso_hinge_on_pelvis", *pelvisBracket, Transform(anterior_superior_pelvis_pin_location) ); PhysicalOffsetFrame anterior_torso_hinge_on_chest( "anterior_torso_hinge_on_chest", *chest, Transform(inferior_torso_hinge_location) ); // Attach chest piece to pelvice with pin joint OpenSim::PinJoint* anteriorTorsoHinge = new OpenSim::PinJoint( "anterior_torso_hinge", anterior_torso_hinge_on_pelvis, anterior_torso_hinge_on_chest); anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_pelvis); anteriorTorsoHinge->append_frames(anterior_torso_hinge_on_chest); // add posterior leg to model model.addBody(chest); model.addJoint(anteriorTorsoHinge); // set torso rotation slightly anterior anteriorTorsoHinge->upd_CoordinateSet()[0].setDefaultValue(-1*SimTK::Pi/4); // Create chest piece //----------------------- OpenSim::Body* back = new OpenSim::Body("back_bar", bar_mass, Vec3(0.0), Inertia::brick(torso_bar_dimensions/2.0)); back->attachMeshGeometry("Posterior_torso_bar.obj"); PhysicalOffsetFrame posterior_torso_hinge_on_pelvis( "posterior_torso_hinge_on_pelvis", *pelvisBracket, Transform(posterior_superior_pelvis_pin_location) ); PhysicalOffsetFrame posterior_torso_hinge_on_back( "posterior_torso_hinge_on_back", *back, Transform(back_peg_center) ); // Attach chest piece to pelvis with pin joint OpenSim::PinJoint* posteriorTorsoHinge = new OpenSim::PinJoint( "posterior_torso_hinge", posterior_torso_hinge_on_pelvis, posterior_torso_hinge_on_back); posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_pelvis); posteriorTorsoHinge->append_frames(posterior_torso_hinge_on_back); // add posterior leg to model model.addBody(back); model.addJoint(posteriorTorsoHinge); // set posterior back joint to freely follow anterior joint through 4-bar // linkage coupling. posteriorTorsoHinge->upd_CoordinateSet()[0] .set_is_free_to_satisfy_constraints(true); // Create shoulder bracket //----------------------- OpenSim::Body* shoulderBracket = new OpenSim::Body("shoulder_bracket", bracket_mass, Vec3(0.0), Inertia::brick(shoulder_dimensions/2.0)); shoulderBracket->attachMeshGeometry("Shoulder_meters.obj"); // add anterior leg to model model.addBody(shoulderBracket); PhysicalOffsetFrame anterior_thoracic_joint_on_chest( "anterior_thoracic_joint_on_chest", *chest, Transform(superior_torso_hinge_location) ); PhysicalOffsetFrame anterior_thoracic_joint_on_shoulder( "anterior_thoracic_joint_on_shoulder", *shoulderBracket, Transform(anterior_thoracic_joint_center)); // Connect pelvis to Hlink via pin joint OpenSim::PinJoint* anteriorThoracicJoint = new OpenSim::PinJoint("anterior_thoracic_joint", anterior_thoracic_joint_on_chest, anterior_thoracic_joint_on_shoulder); anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_chest); anteriorThoracicJoint->append_frames(anterior_thoracic_joint_on_shoulder); // add back joint model.addJoint(anteriorThoracicJoint); // since the previous, anterior thoracic joint drives the pose of the lower // 4-bar linkage, set the anterior shoulder angle such that it's free to // satisfy constraints that couple it to the 4-bar linkage. anteriorThoracicJoint->upd_CoordinateSet()[0] .set_is_free_to_satisfy_constraints(true); // Close the loop for the lower, four-bar linkage with a constraint //------------------------------------------------------------------ // Create and configure point on line constraint OpenSim::PointOnLineConstraint* posteriorShoulder = new OpenSim::PointOnLineConstraint(); posteriorShoulder->setLineBodyByName(shoulderBracket->getName()); posteriorShoulder->setLineDirection(Vec3(0.0,0.0,1.0)); posteriorShoulder->setPointOnLine(posterior_thoracic_joint_center); posteriorShoulder->setFollowerBodyByName(back->getName()); posteriorShoulder->setPointOnFollower(superior_torso_hinge_location); // add constraint to model model.addConstraint(posteriorShoulder); // Create and add luxo head OpenSim::Body* head = new OpenSim::Body("head", head_mass, Vec3(0), Inertia::cylinderAlongX(0.5*head_dimension[1], head_dimension[1])); head->attachMeshGeometry("luxo_head_meters.obj"); head->attachMeshGeometry("Bulb_meters.obj"); model.addBody(head); PhysicalOffsetFrame cervical_joint_on_shoulder("cervical_joint_on_shoulder", *shoulderBracket, Transform(superior_shoulder_hinge_location) ); PhysicalOffsetFrame cervical_joint_on_head("cervical_joint_on_head", *head, Transform(cervicle_joint_center)); // attach to shoulder via pin joint OpenSim::PinJoint* cervicalJoint = new OpenSim::PinJoint("cervical_joint", cervical_joint_on_shoulder, cervical_joint_on_head); cervicalJoint->append_frames(cervical_joint_on_shoulder); cervicalJoint->append_frames(cervical_joint_on_head); // add a neck joint model.addJoint(cervicalJoint); // lock the kneck coordinate so the head doens't spin without actuators or // passive forces cervicalJoint->upd_CoordinateSet()[0].set_locked(true); // Coordinate Limit forces for restricting leg range of motion. //----------------------------------------------------------------------- CoordinateLimitForce* kneeLimitForce = new CoordinateLimitForce( anterior_knee->get_CoordinateSet()[0].getName(), knee_flexion_max, joint_softstop_stiffness, knee_flexion_min, joint_softstop_stiffness, joint_softstop_damping, transition_region); model.addForce(kneeLimitForce); // Coordinate Limit forces for restricting back range motion. //----------------------------------------------------------------------- CoordinateLimitForce* backLimitForce = new CoordinateLimitForce( anteriorTorsoHinge->get_CoordinateSet()[0].getName(), back_extension_max, joint_softstop_stiffness, back_extension_min, joint_softstop_stiffness, joint_softstop_damping, transition_region); model.addForce(backLimitForce); // Contact //----------------------------------------------------------------------- ContactHalfSpace* floor_surface = new ContactHalfSpace(SimTK::Vec3(0), SimTK::Vec3(0, 0, -0.5*SimTK::Pi), model.updGround(), "floor_surface"); OpenSim::ContactMesh* foot_surface = new ContactMesh( "thin_disc_0.11_by_0.01_meters.obj", SimTK::Vec3(0), SimTK::Vec3(0), *base, "foot_surface"); // add contact geometry to model model.addContactGeometry(floor_surface); model.addContactGeometry(foot_surface); // define contact as an elastic foundation force OpenSim::ElasticFoundationForce::ContactParameters* contactParameters = new OpenSim::ElasticFoundationForce::ContactParameters( stiffness, dissipation, friction, friction, viscosity); contactParameters->addGeometry("foot_surface"); contactParameters->addGeometry("floor_surface"); OpenSim::ElasticFoundationForce* contactForce = new OpenSim::ElasticFoundationForce(contactParameters); contactForce->setName("contact_force"); model.addForce(contactForce); // MUSCLES //----------------------------------------------------------------------- // add a knee extensor to control the lower 4-bar linkage Millard2012EquilibriumMuscle* kneeExtensorRight = new Millard2012EquilibriumMuscle( "knee_extensor_right", knee_extensor_F0, knee_extensor_lm0, knee_extensor_lts, pennationAngle); kneeExtensorRight->addNewPathPoint("knee_extensor_right_origin", *leg_Hlink, knee_extensor_origin); kneeExtensorRight->addNewPathPoint("knee_extensor_right_insertion", *bottom_bracket, knee_extensor_insertion); kneeExtensorRight->set_ignore_tendon_compliance(true); model.addForce(kneeExtensorRight); // add a second copy of this knee extensor for the left side Millard2012EquilibriumMuscle* kneeExtensorLeft = new Millard2012EquilibriumMuscle(*kneeExtensorRight); kneeExtensorLeft->setName("kneeExtensorLeft"); // flip the z coordinates of all path points PathPointSet& points = kneeExtensorLeft->updGeometryPath().updPathPointSet(); for (int i=0; i<points.getSize(); ++i) { points[i].setLocationCoord(2, -1*points[i].getLocationCoord(2)); } kneeExtensorLeft->set_ignore_tendon_compliance(true); model.addForce(kneeExtensorLeft); // add a back extensor to controll the upper 4-bar linkage Millard2012EquilibriumMuscle* backExtensorRight = new Millard2012EquilibriumMuscle( "back_extensor_right", back_extensor_F0, back_extensor_lm0, back_extensor_lts, pennationAngle); backExtensorRight->addNewPathPoint("back_extensor_right_origin", *chest, back_extensor_origin); backExtensorRight->addNewPathPoint("back_extensor_right_insertion", *back, back_extensor_insertion); backExtensorRight->set_ignore_tendon_compliance(true); model.addForce(backExtensorRight); // copy right back extensor and use to make left extensor Millard2012EquilibriumMuscle* backExtensorLeft = new Millard2012EquilibriumMuscle(*backExtensorRight); backExtensorLeft->setName("back_extensor_left"); PathPointSet& pointsLeft = backExtensorLeft->updGeometryPath() .updPathPointSet(); for (int i=0; i<points.getSize(); ++i) { pointsLeft[i].setLocationCoord(2, -1*pointsLeft[i].getLocationCoord(2)); } backExtensorLeft->set_ignore_tendon_compliance(true); model.addForce(backExtensorLeft); // MUSCLE CONTROLLERS //________________________________________________________________________ // specify a piecwise linear function for the muscle excitations PiecewiseConstantFunction* x_of_t = new PiecewiseConstantFunction(3, times, excitations); PrescribedController* kneeController = new PrescribedController(); kneeController->addActuator(*kneeExtensorLeft); kneeController->addActuator(*kneeExtensorRight); kneeController->prescribeControlForActuator(0, x_of_t); kneeController->prescribeControlForActuator(1, x_of_t->clone()); model.addController(kneeController); PrescribedController* backController = new PrescribedController(); backController->addActuator(*backExtensorLeft); backController->addActuator(*backExtensorRight); backController->prescribeControlForActuator(0, x_of_t->clone()); backController->prescribeControlForActuator(1, x_of_t->clone()); model.addController(backController); /* You'll find that these muscles can make Luxo Myo stand, but not jump. * Jumping will require an assistive device. We'll add two frames for * attaching a point to point assistive actuator. */ // add frames for connecting a back assitance device between the chest // and pelvis PhysicalOffsetFrame* back_assist_origin_frame = new PhysicalOffsetFrame("back_assist_origin", *chest, back_assist_origin_transform); PhysicalOffsetFrame* back_assist_insertion_frame = new PhysicalOffsetFrame("back_assist_insertion", *pelvisBracket, back_assist_insertion_transform); model.addFrame(back_assist_origin_frame); model.addFrame(back_assist_insertion_frame); // add frames for connecting a knee assistance device between the posterior // leg and bottom bracket. PhysicalOffsetFrame* knee_assist_origin_frame = new PhysicalOffsetFrame("knee_assist_origin", *posteriorLegBar, knee_assist_origin_transform); PhysicalOffsetFrame* knee_assist_insertion_frame = new PhysicalOffsetFrame("knee_assist_insertion", *bottom_bracket, knee_assist_insertion_transform); model.addFrame(knee_assist_origin_frame); model.addFrame(knee_assist_insertion_frame); // Temporary: make the frame geometry disappear. for (auto& c : model.getComponentList<OpenSim::FrameGeometry>()) { const_cast<OpenSim::FrameGeometry*>(&c)->set_scale_factors( SimTK::Vec3(0.001, 0.001, 0.001)); } }
void createPointCostraint(OpenSim::Model &aModel, std::string parentBodyName, SimTK::Vec3 locationInParent, std::string childBodyName, SimTK::Vec3 locationInChild ){ OpenSim::PointConstraint *costr = new OpenSim::PointConstraint(aModel.updBodySet().get(parentBodyName), locationInParent, aModel.updBodySet().get(childBodyName), locationInChild); costr->setName(childBodyName+"To"+parentBodyName); aModel.addConstraint(costr); }