I'm creating walking simulation model which is calculated by neural control and feedback in MATLAB. I want to implement adjustable controller to the model. I tried to refer to the controller of KeeReflex.m and ToyReflexController.cpp (which are provided for example), rewrited from ToyReflexController.cpp to ToyReflexController.m and called it from KeeReflex.m. however it didn 't work. How can I make the controller work?
Best regards.
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])