Trying to get the angles at each time step. The angles should be in radians 0 to pi. From my loop, the angles are qs, qe, qw. Each time we get a new angle by adding the q_dot. I am getting angle 10^4 , which does not make sense.

for i=1:length(t)

if i~=1

dxdqs=la*cos(qs)+lf*cos(qs+qe)+lh*cos(qs+qe+qw);

dxdqe=lf*cos(qs+qe)+lh*cos(qs+qe+qw);

dxdqw=lh*cos(qs+qe+qw);

dydqs=la*sin(qs)+lf*sin(qs+qe)+lh*sin(qe+qs+qw);

dydqe=lf*sin(qs+qe)+lh*sin(qe+qs+qw);

dydqw=lh*sin(qe+qs+qw);

dtdqs=1;

dtdqe=1;

dtdqw=1;

J=[dxdqs dxdqe dxdqw; dydqs dydqe dydqw; dtdqs dtdqe dtdqw];

end

w=[1 0 0;0 1 0;0 0 1];

Jcross=inv(w)*J'*inv(J*inv(w)*J');

Q_dot(:,i)=Jcross*dxdt(:,i);

Q_dot(1,i)=qs+Q_dot(1,i);

Q_dot(2,i)=qe+Q_dot(2,i);

Q_dot(3,i)=qw+Q_dot(3,i);

qs=Q_dot(1,i);

qe=Q_dot(2,i);

qw=Q_dot(3,i);

% qw=0;

end

[Jan]

We cannot run the code due to the missing variables t, l1, qs, qe, qw, lf, lh and so on.

All we know is that you obtain "angle 10^4" and that you assume to get something else.

So what is your question? What do you expect? What are the values of the variables? How can we help you?

[Samah Salha]

this is the full code

I am trying to study a human arm movement and get a plot of the angle vs time. I am unable to get the graph, the angles obtained in the loop seems wrong too. I am not sure where am making the mistake

clc;

close all;

load('Sub3_new.mat');

%Length of each limb

h=1.75;

la=0.189*h; %In m

lh=0.032*h; %In m

lf=0.145*h; %In m

lss=0.259*h; %In m

% Inputs

%Initial position (angle between limbs)

X1=Sub3.Left.Pos(1,1);

Y1=-Sub3.Left.Pos(1,2)-abs(lss/2);

qe=(acos((X1^2+Y1^2-la^2-(lf+lh)^2)/(2*la*(lf + lh))));

qs=(atan2(abs(X1),abs(Y1)) - acos((X1.^2 + Y1.^2 + la^2 - (lf+lh)^2)/(2*la*sqrt(X1.^2 + Y1.^2))));

qw=0;

%%Velocities

x_dot = Sub3.Left.Vel(:,1:3)';

t=(Sub3.Time).';

t=t(1:length(t)-1);

Q_dot=zeros(3,1);

%This loop is used to calculate the qs, qe and qw at each instance

%Algorithm: 1)Calculate the Jcross

% 2)Calculate the Qdot

% 3)Calculate the new qs, qe and qw

% 4)Calculate the new J

% 5)Repeat all the steps to all the moments of t.

for i=1:length(t)

dxdqs=la*cos(qs)+lf*cos(qs+qe)+lh*cos(qs+qe+qw);

dxdqe=lf*cos(qs+qe)+lh*cos(qs+qe+qw);

dxdqw=lh*cos(qs+qe+qw);

dydqs=la*sin(qs)+lf*sin(qs+qe)+lh*sin(qe+qs+qw);

dydqe=lf*sin(qs+qe)+lh*sin(qe+qs+qw);

dydqw=lh*sin(qe+qs+qw);

dtdqs=1;

dtdqe=1;

dtdqw=1;

J=[dxdqs dxdqe dxdqw; dydqs dydqe dydqw; dtdqs dtdqe dtdqw];

w=[1 0 0;0 1 0;0 0 1];

Jcross=J'*(J*J')^-1;

% Q_dot = Jcross*x(:,i);

Q(:,i)=Jcross*dxdt(:,i);

Q(1,i)=qs+Q(1,i);

Q(2,i)=qe+Q(2,i);

Q(3,i)=qw+Q(3,i);

qs=Q(1,i);

qe=Q(2,i);

qw=Q(3,i);

end

plot(qs,t)

title('qs vs time');

xlabel('Time (s)');

ylabel('qs');

Answer 1

[Image Analyst]

If you want equal spaced angles, and a certain number of them, do this:

allAngles = linspace(startingAngle, endingAngle, numberOfAngles);

assign the 3 inputs to some actual numbers and it should work.

[Samah Salha]

Can you explain it more? if I am to do it in my code, how can I?