simtk.org

Code: Select all

% ==== Add parameters to optimise to the problem ==== %
        
% optimise stiffness across all contact forces
stiffnessParam = MocoParameter('contactStiffness',StdVectorString(HalfSpaceForces),'stiffness',MocoBounds(1e-5,1e6));
problem.addParameter(stiffnessParam); % append contact stiffness to problem
        
% optimise contact geometry radii
contactRadii = MocoParameter('contactRadii',StdVectorString(contactGeometryPathNames),'radius',MocoBounds(0.0025,0.05));
problem.addParameter(contactRadii); % append contact radii to problem

Code: Select all

% ==== Add a MocoOutputGoal to the problem ==== %

radiiGoal = MocoOutputGoal('radii');
problem.addGoal(radiiGoal);
radiiGoal.setOutputPath(append(contactGeometryPathNames(1),'|radius'));
radiiGoal.setWeight(radiiWeight);

Code: Select all

% Construct the MocoInverse
inverse = MocoInverse();
modelProcessor = ModelProcessor(model); 
inverse.setModel(modelProcessor);

% initialise Moco Study
study = inverse.initialize();
problem = study.updProblem();

% optimise contact geometry radii
contactRadii = MocoParameter('contactRadii',StdVectorString(contactGeometryPathNames),'radius',MocoBounds(0.0025,0.05));
problem.addParameter(contactRadii); % append contact radii to problem

% configure the solver and solve the problem
solver = MocoCasADiSolver.safeDownCast(study.updSolver()); % get solver
solver.resetProblem(problem); % reset problem

% get MocoTrajectory
trajectory = solver.createGuess();

Code: Select all

% Cost function for contact radii parameter regularisation
function value = calcMyCustomGoalValue(problem, trajectory)
	Ropt = trajectory.getParameter('contactRadii'); % get optimised parameter
	model = problem.getPhase(0).getModelProcessor().process(); % get model
	Rinit = org.opensim.modeling.ContactSphere.safeDownCast(model.getContactGeometrySet.get(0)).getRadius; % get initial value
	value = (Ropt/Rinit - Rinit/Ropt)^2; % calculate cost value
end

Code: Select all

% Cost function for general parameter regularisation
function value = calcMyCustomGoalValue(trajectory, target, targetName)
	opt = trajectory.getParameter(targetName); % get optimised parameter
	value = (opt/target - target/opt)^2; % calculate cost value
end

Code: Select all

#ifndef OPENSIM_MOCOPARAMETERREGULARISATIONGOAL_H
#define OPENSIM_MOCOPARAMETERREGULARISATIONGOAL_H

#include "MocoGoal.h"

namespace OpenSim {

/** The squared difference between a reference value and an optimised 
MocoParameter.
@ingroup mocogoal */

class OSIMMOCO_API MocoParameterRegularisationGoal : public MocoGoal {
    OpenSim_DECLARE_CONCRETE_OBJECT(MocoParameterRegularisationGoal, MocoGoal);
public:
    MocoParameterRegularisationGoal() { constructProperties(); }
    MocoParameterRegularisationGoal(std::string name) : MocoGoal(std::move(name)) { 
        constructProperties(); 
    }
    MocoParameterRegularisationGoal(std::string name, double weight)
            : MocoGoal(std::move(name), weight) { 
        constructProperties(); 
    }
    
    /// The name of the MocoParameter in the MocoTrajectory which should be 
    /// regularised
    void setParameterName(std::string parameterName) {
        set_parameter_name(std::move(parameterName))
    }

    /// Set desired value of the parameter
    void setReferenceValue(double referenceValue) {
        set_reference_value(std::move(referenceValue))
    }

protected:
    void initializeTrajectoryImpl(const MocoTrajectory&) const override;
    void calcGoalImpl(
            const GoalInput& input, SimTK::Vector& cost) const override; 

private:

    OpenSim_DECLARE_PROPERTY(parameter_name, std::string,
        "The name of the MocoParameter in the MocoTrajectory which should be "
        "regularised.");
    OpenSim_DECLARE_PROPERTY(reference_value, SimTK::Vec3,
        "The desired reference value of the MocoParameter (i.e., initial or "
        "optimal value)");

    void constructProperties();

    mutable SimTK::ReferencePtr<const MocoParameter> m_parameter;
};

} // namespace OpenSim

#endif // OPENSIM_MOCOPARAMETERREGULARISATIONGOAL_H

Code: Select all

#include "MocoParameterRegularisationGoal.h"

#include <OpenSim/Moco/MocoTrajectory.h>

using namespace OpenSim;

void MocoParameterRegularisationGoal::initializeTrajectoryImpl(const MocoTrajectory& trajectory) const {
    m_parameter.reset(&trajectory.getParameter<MocoParameter>(get_parameter_name()));
}

void MocoParameterRegularisationGoal::calcGoalImpl(
        const GoalInput& input, SimTK::Vector& cost) const override {
    cost[0] = ((m_parameter/get_reference_value())-(get_reference_value()/m_parameter))^2.;
}

void MocoParameterRegularisationGoal::constructProperties() {
    constructProperty_parameter_name("");
    constructProperty_reference_value(double(0));
}