void addCustomMarkers(OpenSim::Model& model, SimTK::Markers& markers, SimTK::OrientationSensors& imus) { // add orientation osensor m_humerus_mx = imus.addOSensor( "humerus", model.updBodySet().get(m_humerus_body_name).getMobilizedBodyIndex(), SimTK::Rotation(SimTK::BodyOrSpaceType::BodyRotationSequence, -SimTK::Pi / 2, SimTK::ZAxis, SimTK::Pi / 2, SimTK::XAxis), 1); m_radius_mx = imus.addOSensor( "radius", model.updBodySet().get(m_radius_body_name).getMobilizedBodyIndex(), SimTK::Rotation(SimTK::BodyOrSpaceType::BodyRotationSequence, -SimTK::Pi / 2, SimTK::ZAxis, SimTK::Pi , SimTK::XAxis), 1); // finalize observation order (to allocate ObservationIx) static const char* osensor_observation_order[OSENSORS] = {"humerus", "radius"}; imus.defineObservationOrder(OSENSORS, osensor_observation_order); // get all ObservationIx m_humerus_ox = imus.getObservationIxForOSensor(m_humerus_mx); m_radius_ox = imus.getObservationIxForOSensor(m_radius_mx); }
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); }