ConstantAngle constraint: a Simbody wrapper for OpenSim v4.0

ConstantAngleConstraint.h

/**
* This constraint consists of a single constraint equation that enforces that
* a unit vector v1 fixed to one body (the "base body") must maintain a fixed
* angle theta with respect to a unit vector v2 fixed on the other body (the
* "follower body"). This can be done with a single constraint equation as long
* as theta is sufficiently far away from 0 and +/-Pi (180 degrees), with the
* numerically best performance at theta=Pi/2 (90 degrees).
*/
class ConstantAngleConstraint : public OpenSim::Constraint {
    OpenSim_DECLARE_CONCRETE_OBJECT(ConstantAngleConstraint, Constraint);
public:
    OpenSim_DECLARE_SOCKET(baseBody, PhysicalFrame,
        "The base body.");
    OpenSim_DECLARE_SOCKET(followerBody, PhysicalFrame,
        "The follower body.");
    OpenSim_DECLARE_PROPERTY(axisInBaseBody, SimTK::Vec3,
        "The axis fixed in the base body local frame.");
    OpenSim_DECLARE_PROPERTY(axisInFollowerBody, SimTK::Vec3,
        "The axis fixed in the follower body local frame.");
    OpenSim_DECLARE_PROPERTY(angle, double,
        "The angle between the two axis.");

    /**
    * Constructor.
    *
    * @param baseBody               the base body frame
    * @param axisInBaseBody         the axis fixed in the base body local frame
    * @param followerBody           the follower body frame
    * @param axisInFollowerBody:    the axis fixed in the follower body local frame
    * @param angle:                 the angle between the two axis
    */
    ConstantAngleConstraint(
        const PhysicalFrame& baseBody, const SimTK::Vec3& axisInBaseBody,
        const PhysicalFrame& followerBody, const SimTK::Vec3& axisInFollowerBody,
        double angle) : Constraint() {
        constructProperties();
        setBaseBodyByName(baseBody.getName());
        setFollowerBodyByName(followerBody.getName());
        set_axisInBaseBody(axisInBaseBody);
        set_axisInFollowerBody(axisInFollowerBody);
        set_angle(angle);
    }

    virtual ~ConstantAngleConstraint() {};

    void extendAddToSystem(SimTK::MultibodySystem& system) const {
        Super::extendAddToSystem(system);

        const PhysicalFrame& baseBody =
            getSocket<PhysicalFrame>("baseBody").getConnectee();
        const PhysicalFrame& followerBody =
            getSocket<PhysicalFrame>("followerBody").getConnectee();

        SimTK::MobilizedBody baseMobB = baseBody.getMobilizedBody();
        SimTK::MobilizedBody followerMobB = followerBody.getMobilizedBody();

        SimTK::Constraint::ConstantAngle constAngle(
            baseMobB, UnitVec3(get_axisInBaseBody()),
            followerMobB, UnitVec3(get_axisInFollowerBody()),
            get_angle());

        assignConstraintIndex(constAngle.getConstraintIndex());
    }

private:
    void constructProperties() {
        constructProperty_axisInBaseBody(UnitVec3());
        constructProperty_axisInFollowerBody(UnitVec3());
        constructProperty_angle(Pi / 2);
    }

    void setBaseBodyByName(const std::string& name) {
        updSocket<PhysicalFrame>("baseBody").setConnecteeName(name);
    }

    void setFollowerBodyByName(const std::string& name) {
        updSocket<PhysicalFrame>("followerBody").setConnecteeName(name);
    }
};