addContactSurfaceActuators.m

% This function is part of the NMSM Pipeline, see file for full license.
%
% If contact surfaces are present, point and torque actuators are added to
% the model. The actuators are only added to the bodies with contact
% surfaces.
%
% (struct, Model) -> (Model)
% Adds point and torque actuators to model


function model = addContactSurfaceActuators(inputs, model)
import org.opensim.modeling.Vec3
for i = 1:length(inputs.contactSurfaces)
addPointActuator(model, string(inputs.contactSurfaces{i}.parentBodyName), Vec3(1, 0, 0));
addPointActuator(model, string(inputs.contactSurfaces{i}.parentBodyName), Vec3(0, 1, 0));
addPointActuator(model, string(inputs.contactSurfaces{i}.parentBodyName), Vec3(0, 0, 1));
addPointActuator(model, string(inputs.contactSurfaces{i}.childBodyName), Vec3(1, 0, 0));
addPointActuator(model, string(inputs.contactSurfaces{i}.childBodyName), Vec3(0, 1, 0));
addPointActuator(model, string(inputs.contactSurfaces{i}.childBodyName), Vec3(0, 0, 1));
addTorqueActuator(model, string(inputs.contactSurfaces{i}.parentBodyName), Vec3(1, 0, 0));
addTorqueActuator(model, string(inputs.contactSurfaces{i}.parentBodyName), Vec3(0, 1, 0));
addTorqueActuator(model, string(inputs.contactSurfaces{i}.parentBodyName), Vec3(0, 0, 1));
addTorqueActuator(model, string(inputs.contactSurfaces{i}.childBodyName), Vec3(1, 0, 0));
addTorqueActuator(model, string(inputs.contactSurfaces{i}.childBodyName), Vec3(0, 1, 0));
addTorqueActuator(model, string(inputs.contactSurfaces{i}.childBodyName), Vec3(0, 0, 1));
end
end
function addPointActuator(model, bodyName, direction)
import org.opensim.modeling.*
pointActuator = PointActuator();
pointActuator.setMaxControl(Inf);
pointActuator.setMinControl(-Inf);
pointActuator.set_body(bodyName);
pointActuator.set_point(Vec3(0, 0, 0));
pointActuator.set_point_is_global(0);
pointActuator.set_direction(direction);
pointActuator.set_force_is_global(1);
pointActuator.set_optimal_force(1);
model.addForce(pointActuator);
end
function addTorqueActuator(model, bodyName, direction)
import org.opensim.modeling.*
torqueActuator = TorqueActuator();
torqueActuator.setMaxControl(Inf);
torqueActuator.setMinControl(-Inf);
torqueActuator.set_bodyA(bodyName);
torqueActuator.set_bodyB("ground");
torqueActuator.set_torque_is_global(1);
torqueActuator.set_optimal_force(1);
torqueActuator.set_axis(direction);
model.addForce(torqueActuator);
end