Human Neuromechanics, ERROR IN CODE

Running this code give me an error :

(q)[-Length_S*sin(q(1,:,:))-Length_E*sin(q(1,:,:)+q(2,:,:)),-Length_E*sin(q(1,:,:)+q(2,:,:));Length_S*cos(q(1,:,:))+Length_E*cos(q(1,:,:)+q(2,:,:)),Length_E*cos(q(1,:,:)+q(2,:,:))]

Too many input arguments

Code is below

clear all; clc; close all; set(0,'DefaultFigureWindowStyle','docked'); tic

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Modelless learning of feedforward command as a function of time to

% compensate for a VF.

%

% DESIRED TRAJECTORY IN CARTESIAN COORDINATES (column is time)

% NFTraj = [x_position; x_velocity; y_position; y_velocity];

%

% DESIRED TRAJECTORY IN JOINT COORDINATES (column is time)

% qDesired = [angle_shoulder; angle_velocity_shoulder;

% angle_elbow; angle_velocity_elbow];

%

%%%%%%%%%%%%%%%%%%%%%%%%%

% Load reference trajectory for simulation

load Ref_traj.mat;

% Name of experimental conditions in data

FieldName = char('I_NF','II_VF','III_Channel','IV_VF','V_NF');

SizeStruct = zeros(1,size(FieldName,1));

% Simulation start and end trial numbers for different conditions

TrialStructure = [1 7 32 62 65 70];

% Parameters for the arm

Mass_S = 3; Mass_E = 3; % in kg

Length_S = 0.4; Length_E = 0.3; % in m

CoM_S = Length_S/2; CoM_E = Length_E/2; % in m

MoI_S = 0.125; MoI_E = 0.125; % in kg.m^2

% PD parameters

K = 150; D = 60;

% Channel parameters

KChan = 10000; % in N/m and Ns/m

% Simulation step size

dt = 0.002; % in s

% Velocity-dependent force field in CARTESIAN COORDINATES

CurlField = [0 25; -25 0]; % in Ns/m

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% FF parameters (alpha = proportion of PD learnt, gamma = forgetting)

Alpha = 0.08; Gamma = 0.001;

% Curl velocity force

CurlForce = @(x)((CurlField*x')');

% Euler method

UpdateSystem = @(x,a)([x(1,1)+dt*x(2,1)+0.5*dt^2*a(1,1); x(2,1)+a(1,1)*dt;

x(3,1)+dt*x(4,1)+0.5*dt^2*a(2,1); x(4,1)+a(2,1)*dt]);

% Forward kinematics

ForwardKin = @(q)[Length_S*cos(q(1,:,:))+Length_E*cos(q(1,:,:)+q(2,:,:));

Length_S*sin(q(1,:,:))+Length_E*sin(q(1,:,:)+q(2,:,:))];

% Inverse kinematics

InvKin = @(x,y)[atan2(y,x)-acos((x.^2+y.^2+Length_S^2-Length_E^2)./(2*Length_S*sqrt(x.^2+y.^2)));

acos((x.^2+y.^2-Length_S^2-Length_E^2)/(2*Length_S*Length_E))];

% Jacobian

Jacobian = @(q)[-Length_S*sin(q(1,:,:))-Length_E*sin(q(1,:,:)+q(2,:,:)), -Length_E*sin(q(1,:,:)+q(2,:,:));

Length_S*cos(q(1,:,:))+Length_E*cos(q(1,:,:)+q(2,:,:)), Length_E*cos(q(1,:,:)+q(2,:,:))];

% Mass matrix

MassMatrix = @(q)[MoI_S+MoI_E+Mass_S*CoM_S^2+Mass_E*(Length_S^2+CoM_E^2+2*Length_S*CoM_E*cos(q(2,1))), MoI_E+Mass_E*(CoM_E^2+Length_S*CoM_E*cos(q(2,1)));

MoI_E+Mass_E*(CoM_E^2+Length_S*CoM_E*cos(q(2,1))), MoI_E+Mass_E*CoM_E^2];

% Dynamics

JointAccel = @(Torque,JointAngle,JointVel,H)(H\(Torque-[-Mass_E*Length_S*CoM_E*JointVel(2,1)*sin(JointAngle(2,1))*(2*JointVel(2,1)+JointVel(1,1));

Mass_E*Length_S*CoM_E*JointVel(1,1)^2*sin(JointAngle(2,1))]));

% Allocate memory

uPD = zeros(2,length(NFTraj),TrialStructure(end));

uFF = uPD;

uExt = uPD;

Force = uPD;

% Desired joint trajectories

qDesired = InvKin(NFTraj(1,:),NFTraj(3,:));

qDesired = [qDesired(1,:);

0,diff(qDesired(1,:))/dt;

qDesired(2,:);

0,diff(qDesired(2,:))/dt];

q = zeros(4,size(qDesired,2),TrialStructure(end));

q(:,1,:) = repmat(qDesired(:,1),[1,1,TrialStructure(end)]);

%% Experiment simulation loop

for Trial=1:TrialStructure(end)

if Trial>1

uFF(:,:,Trial) = Alpha*uPD(:,:,Trial-1) + (1-Gamma)*uFF(:,:,Trial-1);

end

% Trial simulation loop

for i=1:size(NFTraj,2)-1

% Compute Jacobian

J = Jacobian([Length_S Length_E],[q(1,i,Trial) q(3,i,Trial)]);

% Compute cartesian velocity

CartVel = J*[q(2,i,Trial);q(4,i,Trial)];

% External forces

if Trial>=TrialStructure(2) && Trial<TrialStructure(3) || Trial>=TrialStructure(4) && Trial<TrialStructure(5)

% CURL VELOCITY DEPENDENT FIELD

uExt(:,i,Trial) = J'*CurlField*CartVel;

elseif Trial>=TrialStructure(3) && Trial<TrialStructure(4)

% Forward kinematics

CartPos = kin([Length_S Length_E], [q(1,i,Trial) q(3,i,Trial)]);

% CHANNEL

uExt(:,i,Trial) = J'*[-KChan*CartPos(1,1)-D*CartVel(1,1);0];

end

% Compute PD control

uPD(:,i,Trial) = [K*(qDesired(1,i)-q(1,i,Trial))+D*(qDesired(2,i)-q(2,i,Trial));

K*(qDesired(3,i)-q(3,i,Trial))+D*(qDesired(4,i)-q(4,i,Trial))];

% Calculate mass matrix of arm

H = mass([Mass_S Mass_E],[Length_S Length_E],[CoM_S CoM_E],[MoI_S MoI_E],[q(1,i,Trial) q(3,i,Trial)]);

% Exit loop if H is singular

if sum(isnan(H))~=0

warning('SIMULATION STOPPED AS MASS MATRIX IS SINGULAR');

break;

end

% Update system

q(:,i+1,Trial) = UpdateSystem(q(:,i,Trial),JointAccel(uPD(:,i,Trial)+uFF(:,i,Trial)+uExt(:,i,Trial),[q(1,i,Trial);q(3,i,Trial)],[q(2,i,Trial);q(4,i,Trial)],H));

end

if sum(isnan(H))~=0

% Exit loop if H is singular

break;

end

end

% Convert to cartesian space

for Trial=1:TrialStructure(end)

for i=1:size(NFTraj,2)-1

x(:,i,Trial)= kin([Length_S Length_E],[(q(1,i,Trial)) (q(3,i,Trial))]);

end

end

toc

[Jan]

I tried it for 2 minutes to edit the question to format the code as code. My Firefox browser has exhausted the resources of my computer and reloading the page was the only way to get back the control over the browser.

Please, @Samah Salha, use the possibility to format code as code in the forum, because this improves the readability.

Answer 1

[Jan]

The failing part of the code:

% Jacobian
Jacobian = @(q) [-Length_S * sin(q(1,:,:)) - Length_E * sin(q(1,:,:) + q(2,:,:)), ...
                 -Length_E * sin(q(1,:,:)  + q(2,:,:)); ...
                  Length_S * cos(q(1,:,:)) + Length_E * cos(q(1,:,:) + q(2,:,:)), ...
                  Length_E * cos(q(1,:,:)  + q(2,:,:))];

Now Jacobian() is a function, which accepts 1 input argument q. You call it by:

J = Jacobian([Length_S Length_E], [q(1,i,Trial) q(3,i,Trial)]);

with 2 input arguments. Either remove the additional input at the call, or insert a 2nd input in the definition of the function.

The expression "mass" seems to be undefined.

Your code is extremely hard to read. I would not be able to debug it.