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.

조회 수: 3 (최근 30일)
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
  댓글 수: 2
Jan
Jan 2021년 2월 15일
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
Samah Salha 2021년 2월 16일
편집: Samah Salha 2021년 2월 16일
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');

댓글을 달려면 로그인하십시오.

답변 (1개)

Image Analyst
Image Analyst 2021년 2월 16일
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.

카테고리

Help CenterFile Exchange에서 MATLAB에 대해 자세히 알아보기

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by