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 天）
Samah Salha
Samah Salha 2021-2-15
评论： Samah Salha 2021-2-16
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 Center 和 File Exchange 中查找有关 MATLAB 的更多信息

标签

Community Treasure Hunt

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

Start Hunting!

Translated by