simtk.org

Code: Select all

%ToyReflexController_b.m

function controls = ToyReflexController_b(model, state)
% Load Library 
import org.opensim.modeling.*;

% ToyReflexController_b
computeControls(state, controls);

% get time
state.getTime();

% get the list of actuators assigned to the reflex controller
actuators = controls.getActuatorSet();

% muscle lengthening speed
speed = 0;
% max muscle lengthening (stretch) speed
max_speed = 0;
% reflex control
control = 0;

n = actuators.getSize();
    for i = 0 : n-1
        Muscle = model.actuators(i).getMuscles(); 
        speed = Muscle.getLengtheningSpeed(state);
        % un-normalize muscle's maximum contraction velocity (fib_lengths/sec) 
        a = Muscle.getOptimalFiberLength();
        b = Muscle.getMaxContractionVelocity();
        max_speed = a*b;
        control = 0.5*get_gain()*(fabs(speed)+speed)/max_speed;
        actControls(1,control);
        % add reflex controls to whatever controls are already in place.
        Muscle.addInControls(actControls, controls);
    end

end

Code: Select all

%ToyReflexController_b.cpp

/* -------------------------------------------------------------------------- *
 *                      OpenSim:  ToyReflexController.cpp                     *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Ajay Seth                                                       *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied    *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */



//=============================================================================
// INCLUDES
//=============================================================================
#include "ToyReflexController.h"
#include <OpenSim/Simulation/Model/Muscle.h> 

// This allows us to use OpenSim functions, classes, etc., without having to
// prefix the names of those things with "OpenSim::".
using namespace OpenSim;
using namespace std;
using namespace SimTK;


//=============================================================================
// CONSTRUCTOR(S) AND DESTRUCTOR
//=============================================================================
//_____________________________________________________________________________
/* Default constructor. */
ToyReflexController::ToyReflexController()
{
    constructProperties();
}

/* Convenience constructor. */
ToyReflexController::ToyReflexController(double gain)
{
    constructProperties();
    set_gain(gain);
}


/*
 * Construct Properties
 */
void ToyReflexController::constructProperties()
{
    constructProperty_gain(1.0);
}

void ToyReflexController::extendConnectToModel(Model &model)
{
    Super::extendConnectToModel(model);

    // get the list of actuators assigned to the reflex controller
    Set<const Actuator>& actuators = updActuators();

    int cnt=0;
 
    while(cnt < actuators.getSize()){
        const Muscle *musc = dynamic_cast<const Muscle*>(&actuators[cnt]);
        // control muscles only
        if(!musc){
            cout << "ToyReflexController:: WARNING- controller assigned a "
                    "non-muscle actuator ";
            cout << actuators[cnt].getName() << " which will be ignored."
                 << endl;
            actuators.remove(cnt);
        }else
            cnt++;
    }
}

//=============================================================================
// COMPUTATIONS
//=============================================================================
//_____________________________________________________________________________
/**
 * Compute the controls for muscles under influence of this reflex controller
 *
 * @param s         current state of the system
 * @param controls  system wide controls to which this controller can add
 */
void ToyReflexController::computeControls(const State& s,
                                          Vector &controls) const {   
    // get time
    s.getTime();

    // get the list of actuators assigned to the reflex controller
    const Set<const Actuator>& actuators = getActuatorSet();

    // muscle lengthening speed
    double speed = 0;
    // max muscle lengthening (stretch) speed
    double max_speed = 0;
    //reflex control
    double control = 0;

    for(int i=0; i<actuators.getSize(); ++i){
        const Muscle *musc = dynamic_cast<const Muscle*>(&actuators[i]);
        speed = musc->getLengtheningSpeed(s);
        // un-normalize muscle's maximum contraction velocity (fib_lengths/sec) 
        max_speed =
            musc->getOptimalFiberLength()*musc->getMaxContractionVelocity();
        control = 0.5*get_gain()*(fabs(speed)+speed)/max_speed;

        SimTK::Vector actControls(1,control);
        // add reflex controls to whatever controls are already in place.
        musc->addInControls(actControls, controls);
    }
}

Code: Select all

 
% KneeReflex.m

% This script builds a 2-DOF leg model to simulate a knee jerk reflex.

%% Part 1A: Create a Model object.
import org.opensim.modeling.*;
model = Model();
model.setName('leg');
model.setUseVisualizer(true);

%% Part 2: Create the thigh body.
hipHeight = 1.0;
linkLength = 0.5;
linkMass = 5;

thigh = Body('thigh', linkMass, Vec3(0), Inertia(1));
thigh.attachGeometry(Ellipsoid(linkLength/10, linkLength/2, linkLength/10));
model.addBody(thigh);

%% Part 3: Create the hip joint.
hip = PinJoint('hip', ...
    model.getGround(), ...        % parent body
    Vec3(0, hipHeight, 0), ...    % location in parent
    Vec3(0), ...                  % orientation in child
    thigh, ...                    % child body
    Vec3(0, linkLength/2, 0), ... % location in child
    Vec3(0));                     % orientation in child 
model.addJoint(hip);

%% Part 4: Set the default value of the hip coordinate to +90 degrees.
hip.getCoordinate().setDefaultValue(0.5*pi);

%% Part 8A: Lock the hip coordinate.
hip.getCoordinate().setDefaultLocked(true);

%% Part 5: Create the shank body and knee joint.
% - Create a body named 'shank', with the same mass properties as the thigh.
% - Attach the same ellipsoid geometry as for the thigh.
% - Add the shank body to the model.
shank = Body('shank', linkMass, Vec3(0), Inertia(1));
shank.attachGeometry(Ellipsoid(linkLength/10, linkLength/2, linkLength/10));
model.addBody(shank);

% - Create a PinJoint named 'knee' Parent: thigh; child: shank.
% - The location of the joint in the parent body is (0, -linkLength/2, 0).
% - The location of the joint in the child body is (0, +linkLength/2, 0).
knee = PinJoint('knee', ...
    thigh, Vec3(0, -linkLength/2, 0), Vec3(0), ...
    shank, Vec3(0, linkLength/2, 0), Vec3(0));
model.addJoint(knee);

%% Part 8B: Set the default value of the knee coordinate.
knee.getCoordinate().setDefaultValue(-0.5*pi);

%% Part 6A: Add a vastus muscle (actuator).
vastus = Millard2012EquilibriumMuscle();
vastus.setName('vastus');
vastus.setMaxIsometricForce(500);
vastus.setOptimalFiberLength(0.19);
vastus.setTendonSlackLength(0.19);

vastus.addNewPathPoint('origin', thigh, Vec3(linkLength/10, 0, 0));
insertion = Vec3(0.75 * linkLength/10, 0.7 * linkLength/2, 0);
vastus.addNewPathPoint('insertion', shank, insertion);

model.addForce(vastus);

%% Part 7: The vastus muscle wraps over the knee cap.
patella = WrapCylinder();
patella.setName('patella');
patella.set_translation(Vec3(0, -linkLength/2, 0));
patella.set_radius(0.04);
patella.set_length(0.1);
patella.set_quadrant('x');
thigh.addWrapObject(patella);
vastus.updGeometryPath().addPathWrap(patella);

%% Part 9: Add an open-loop controller for the muscle.
brain = PrescribedController();
brain.addActuator(vastus);
% Between 0.3 and 0.35 seconds, excitation transitions from 0.05 to 0.5.
brain.prescribeControlForActuator('vastus', ...
    StepFunction(0.3, 0.35, 0.05, 0.5));
model.addController(brain);

%% Part 10A: Add reflex control (and disable the open-loop control).
brain.setEnabled(false);

reflex = ToyReflexController_b();
reflex.addActuator(vastus);
reflex.set_gain(40.0);
model.addController(reflex);

%% Part 11A: Add a reporter to obtain muscle behavior.
reporter = TableReporter();
reporter.set_report_time_interval(0.01);
reporter.addToReport(vastus.getOutput('normalized_fiber_length'), 'lm');
reporter.addToReport(...
    vastus.getOutput('active_force_length_multiplier'), 'fl');
model.addComponent(reporter);

%% Part 1B: Build the model and obtain its default state.
state = model.initSystem();

%% Part 10B: Set the initial speed of the knee coordinate.
% Units: radians per second.
knee.getCoordinate().setSpeedValue(state, -3.0);

%% Part 6B: Initialize the muscle fiber length state.
model.equilibrateMuscles(state);

%% Part 1C: Simulate the model.
osimSimulate(model, state, 3.0);

%% Part 11B: Plot muscle behavior.
table = reporter.getTable();
results = osimTableToStruct(table);
fieldnames(results);
hold on;
plot(results.lm, results.fl);
xlabel('normalized fiber length');
ylabel('active force-length multiplier');
axis([0, 1.5, 0, 1])