simtk.org

Code: Select all

class MocoBalanceGoal : public MocoGoal {
	OpenSim_DECLARE_CONCRETE_OBJECT(MocoBalanceGoal, MocoGoal);

public:
	MocoBalanceGoal() {}
	MocoBalanceGoal(string name) : MocoGoal(move(name)) {}
	MocoBalanceGoal(string name, double weight) : MocoGoal(move(name), weight) {}

protected:
	Mode getDefaultModeImpl() const override { return Mode::Cost; }
	bool getSupportsEndpointConstraintImpl() const override { return true; }
	void initializeOnModelImpl(const Model&) const override {
		setRequirements(1, 1);
	}
	void calcIntegrandImpl(
		const IntegrandInput& input, double& integrand) const override {
		getModel().realizeAcceleration(input.state);

		const auto& COM = getModel().calcMassCenterPosition(input.state);
		const auto& torso_mc = getModel().getBodySet().get("torso").getMassCenter();
		const auto& torso_pos = getModel().getBodySet().get("torso").findStationLocationInGround(input.state, torso_mc);
		const auto& calcn_r_mc = getModel().getBodySet().get("calcn_r").getMassCenter();
		const auto& calcn_r_pos = getModel().getBodySet().get("calcn_r").findStationLocationInGround(input.state, calcn_r_mc);

		integrand = (COM[0] - calcn_r_pos[0]) * (COM[0] - calcn_r_pos[0]);
	}

	void calcGoalImpl(
		const GoalInput& input, SimTK::Vector& cost) const override {
		cost[0] = input.integral;
	}
};

Code: Select all

class MocoBalanceGoal(MocoControlGoal):
    def __init__(self, name="", weight=1.0):
        super().__init__(name)
        self.setWeight(weight)

    def initializeOnModelImpl(self, model):
        self.setRequirements(1, 1)

    def calcIntegrandImpl(self, input, integrand):
        model = self.getModel()
        model.realizeAcceleration(input.state)

        COM = model.calcMassCenterPosition(input.state)
        torso_mc = model.getBodySet().get("torso").getMassCenter()
        torso_pos = model.getBodySet().get("torso").findStationLocationInGround(input.state, torso_mc)
        calcn_r_mc = model.getBodySet().get("calcn_r").getMassCenter()
        calcn_r_pos = model.getBodySet().get("calcn_r").findStationLocationInGround(input.state, calcn_r_mc)

        integrand[0] = (COM[0] - calcn_r_pos[0]) ** 2

    def calcGoalImpl(self, input, cost):
        cost[0] = input.integral

Code: Select all

Costs: (total: 1)
  balance. MocoControlGoal, enabled: true, mode: cost, weight: 10.0
        control: /forceset/hip_actu_r, weight: 1.0
        control: /forceset/knee_actu_r, weight: 1.0
        control: /forceset/ankle_actu_r, weight: 1.0
        control: /forceset/lumbar_actu, weight: 1.0