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);
}