This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
I.- Obtención del Modelo Dinámico del Robot RP mediante la formulación
Lagrange –Euler
La complejidad computacional de este algoritmo depende de la cantidad de
grados de libertad que posea el robot a trabajar O(n4), por lo cual, ya que nuestro
robot solo posee dos grados de libertad es factible realizar este procedimiento
para el modelado dinámico cuyas ecuaciones finales quedarán bien estructuradas
para obtener los pares y fuerzas respectivos para su movimiento.
A continuación detallaremos paso a paso el proceso con su respectivo código en el
programa Matlab. Para trabajar primero definimos nuestros parámetros a tratar,
tanto las fijas como las variables mediante el comando “syms” e ingresamos
nuestra tabla de parámetros D-H. Nuestros puntos serán posición de
coordenadas (q1 y q2), velocidades (dq1 y dq2) y aceleraciones (ddq1 y ddq2).
Luego nuestros fijos son longitud del primer eslabón y desplazamiento (d2) para
el movimiento prismático del segundo eslabón (cabe resaltar que el segundo
eslabón se desplaza desde una distancia inicial 0.5 m y seguido de un
movimiento longitudinal definido por q2).
disp('DINAMICA DE UN ROBOT 2DOF USANDO EL METODO DE LAGRANGE - EULER') disp('--------------------------------------------------------------') syms q1 q2 dq1 dq2 ddq1 ddq2; syms l1 d2; %longitud de los eslabones syms g; %parametro de gravedad [m/seg^2] syms m1 m2 x1 x2 y1 y2 z1 z2 %------------------------------------------------------------ disp('1.- Tabla de Parametros DH') disp('-a---alpha---d---theta---') DH=[ 0 -pi/2 0 q1 0 0 q2 0 ]
1.1.- Paso 1
Se asigna a cada eslabón un sistema de referencia de acuerdo a las normas D-H.
Figura N°1.- Prototipo a trabajar robot RP
2012-B Robótica Control Dinámico Robot 2DOF - RP
3
1.2.- Paso 2
Obtenemos las matrices de transformación para cada elemento i. Para esto
estaremos usando la función “denavit” para obtener nuestras matrices deseadas,
también vamos a usar el comando “simplify” para obtener resultados de manera
simbólica, es decir, en función de nuestras variables.
disp('2.- Calculo de las matrices de transformacion: Aij') disp('-------------------------------------------------------------') %------------------------------------------------------------ %T=rotz(theta)*transl(0,0,d)*transl(a,0,0)*rotx(alpha) disp('Matriz de TH del primer eslabon movil desde la base del robot') disp('-------------------------------------------------------------') A01=simple(simplify(denavit(DH(1,1),DH(1,2),DH(1,3),DH(1,4)))) A12=simple(simplify(denavit(DH(2,1),DH(2,2),DH(2,3),DH(2,4))));
%Matriz de transformacion de la base al efector final disp('--------------------------------------------------------------') disp('Matriz de TH del efector final desde la base del robot') disp('-------------------------------------------------------------') A02=simple(simplify(A01*A12))
A01 = [ cos(q1), 0, -sin(q1), 0] [ sin(q1), 0, cos(q1), 0] [ 0, -1, 0, 0] [ 0, 0, 0, 1] -------------------------------------------------------------- Matriz de TH del efector final desde la base del robot ------------------------------------------------------------- A02 = [ cos(q1), 0, -sin(q1), -q2*sin(q1)] [ sin(q1), 0, cos(q1), q2*cos(q1)] [ 0, -1, 0, 0] [ 0, 0, 0, 1]
1.3.- Paso 3
Obtenemos las matrices definidas por:
2012-B Robótica Control Dinámico Robot 2DOF - RP
4
Es decir obtenemos las derivadas parciales de nuestras matrices de
transformación homogénea respecto a las base solidaria
con referencia a
nuestras variables de posición q1 y q2, ahora usaremos el comando “diff” el cual
nos permite derivar elementos respecto a un punto de referencia; seguidamente
de comando simplify para trabajar en modo simbólico.
disp('3.- Calculo de las matrices : Uij') disp('-------------------------------------------------------------') %------------------------------------------------------------ U11=simple(simplify(diff(A01,q1))) U12=simple(simplify(diff(A01,q2))) U21=simple(simplify(diff(A02,q1))) U22=simple(simplify(diff(A02,q2))) %------------------------------------------------------------
Obtenemos las matrices de pseudoinercia para cada elemento, que viene
definidas por:
{
∫
∫
∫ ∫
∫
∫
∫
∫
∫ ∫
∫ ∫
∫
∫
∫
∫ }
Donde las integrales están extendidas al elemento i considerado, y ( ) son las
coordenadas del diferencial de masa dm respecto al sistema de coordenadas del
elemento.
Figura 2
2012-B Robótica Control Dinámico Robot 2DOF - RP
7
Para este paso vamos a utilizar los valores que nos da nuestro diagrama respecto
a los frames, por lo cual para el primer eslabón y respetando su centro de masa y
gravedad hacia el primer frame tendremos:
x1=0 y1=0 z1=L1=0.5 m1=1.4 Kg Análogamente para el segundo eslabón: x2=0 y2=0;
z2=0; m2=1 Kg Ahora estos valores los remplazamos en la matriz de pseudoinercias definido de manera correcta en el matlab. disp('5.- Matrices de Pseudoinercia : Ji') disp('-------------------------------------------------------------') %------------------------------------------------------------ %Para el primer eslabon: J1[Kg.m^2] %m1=1.4Kg %Coordenadas del centro de gravedad 1 respecto al frame 1 %x1=0;y1=0;z1=l1=0.5m; disp('Para el Primer Eslabon') disp('-------------------------------------------------------------') J1=[ 0 0 0 0 0 0 0 0 0 0 (m1*l1^2) (m1*l1) 0 0 (m1*l1) m1 ] %------------------------------------------------------------ %Para el primer eslabon: J1[Kg.m^2] %m2=1Kg %Coordenadas del centro de gravedad 2 respecto al frame 2 %x2=0;y2=0;z2=0; disp('-------------------------------------------------------------') disp('Para el segundo eslabon') disp('-------------------------------------------------------------') J2=[ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 m2] %------------------------------------------------------------------------
Para el segundo eslabon ------------------------------------------------------------- J2 = [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, 0] [ 0, 0, 0, m2]
1.6.- Paso 6
Obtenemos la matriz de inercias D=[ ] cuyos elementos vienen definidos por:
∑
Con: i,j= 1,2 n=2 (dos grados de libertad)
Para esta etapa haremos uso del comando “trace” el cual realizara la traza de la
matriz correspondiente, lo manipularemos en modo simbolico con “simplify” y
haremos la respectiva suma de las matrices que cumplen con cada posición de D.
disp('6.- Calculo de las matrices de inercia : D = [dij]') disp('-------------------------------------------------------------') %------------------------------------------------------------------------
-- %Matriz de Coriolis H=[h1;h2] %--------------------------------------------------------------------
H = 2*dq1*dq2*m2*q2 -dq1^2*m2*q2
1.9.- Paso 9
Obtener la matriz columna de fuerzas de gravedad C=[ ] cuyos elementos están
definidos por:
∑
Con: i=1,2 g = gravedad
La estructura horizontal de nuestro robot permiten que las fuerzas de gravedad
se proyecten sobre los apoyos, por lo cual la matriz de gravedad C se ve afectada
debido a que el vector gravedad g expresado en el sistema de referencia de la base
{S0} es g = {g 0 0 0}
2012-B Robótica Control Dinámico Robot 2DOF - RP
11
Figura 3
disp('9.-Calculo de la matriz columna de fuerzas de gravedad: C=[ci]') disp('--------------------------------------------------------------') %------------------------------------------------------------------------
-- g1=[g 0 0 0]; % Segun el frame 0 , gravedad actuando en el eje X %------------------------------------------------------------------------
-- % Vector de compuesto por coordenadas de posicion del centro de masas % para cada eslabon r11=[0;0;l1;1]; %CG para el eslabon 1 r22=[0;0;0;1]; %CG para el eslabon 2
-- % Matriz de gravedad C=[c1;c2] %---------------------------------------------------------------------
C = g*l1*m1*cos(q1) + g*m2*q2*cos(q1) g*m2*sin(q1)
1.10.- Paso 10
Finalmente tenemos la ecuación dinámica del sistema
Donde es el vector de fuerzas y pares motores efectivos aplicados sobre cada
coordenada qi.
2012-B Robótica Control Dinámico Robot 2DOF - RP
12
Donde el vector gravedad sería:
[
]
disp('10.- Ecuacion dinamica del robot') disp('-------------------------------------------------------------') %------------------------------------------------------------------------
-- tau=simplify(D*[ddq1;ddq2]+H+C); disp('El torque o par para la primera articulacion revoluta es: ') disp('-------------------------------------------------------------') T1 = tau(1) % Juntura Revoluta disp('-------------------------------------------------------------') disp('La fuerza para al articulacion prismatica es:') disp('-------------------------------------------------------------') F2 = tau(2) % Juntura Prismatica disp('-------------------------------------------------------------') disp('La notacion general de la ecuacion dinamica es:') disp('-------------------------------------------------------------') tau=[tau(1); tau(2)]
T1 = ddq1*(m1*l1^2 + m2*q2^2) + 2*dq1*dq2*m2*q2 + g*l1*m1*cos(q1) + g*m2*q2*cos(q1) ------------------------------------------------------------- La fuerza para al articulación prismática es: ------------------------------------------------------------- F2 = m2*(- q2*dq1^2 + ddq2 + g*sin(q1)) -------------------------------------------------------------
La notación general de la ecuación dinámica es: ------------------------------------------------------------- tau = [ddq1*(m1*l1^2 + m2*q2^2) + 2*dq1*dq2*m2*q2 + g*l1*m1*cos(q1) + g*m2*q2*cos(q1) m2*(- q2*dq1^2 + ddq2 + g*sin(q1))]
2012-B Robótica Control Dinámico Robot 2DOF - RP
13
II.- Prueba de generación de trayectoria
Para elaborar la trayectoria del robot RP necesitamos de las variables de posición
tanto inicial como final (q0 y qf), además de ello vamos a trabajar con dos
funciones adjuntas al código principal que son “cinversa” y “plot RP”, además
utilizaremos el comando jtraj para que nos revuelva la velocidad y aceleración de
la trayectoria con dq y ddq respectivamente.
2.1.- Función cinversa
Toma tus coordenadas de posición q y tu espacio de trayectoria ok, se inicializan
estos parámetros para nuestro robot y se defines las constantes de medida L1 y
d2.
Como nuestro robot es un RP puede crear diversas dimensiones de radio en su
espacio de desplazamiento y rotación, es así que le involucramos parámetros de
radio R para los ejes x e y. Ahora como tenemos al eslabón 2 que toma un inicio
de 0.5m creamos una condicional que si este eslabón es menor a esa medida
retorne y corrija la falla. Nuestros valores de posición q van a actualizarse a cada
paso de nuestra trayectoria usando el comando “atan2” entre x e y para la
rotación y radio total con la dimensión del eslabón 1, cuando la trayectoria es
correcta ok=1 queda definido como proceso correcto y te arrojan los puntos q
siguientes hasta fin del trayecto.
function [q ok]=cinversa(x,y) ok=0; q=[0 0]'; L1=0.5;d2=0.5; R=sqrt(x^2+y^2); if R<0.5 return end q(1)=atan2(y,x); q(2)=R - L1; ok=1; q=[q(1) q(2)]; end
2.2.- Función plotRP
Trabaja con los valores de posición q y los valores fijos de dimensión de los
eslabones, en esta función también se realizan actualización de q, fijando puntos
iniciales a cero, ahora en el ploteo cada eslabón quedará definido con una
función específica en función a la rotación q1, L1 y el enlongamiento q2.
function plotRP(q) L1=0.5; d2=0.5; q1=q(1); q2=q(2); x(1)=0;
2012-B Robótica Control Dinámico Robot 2DOF - RP
14
y(1)=0; % Eslabon 1 x(2)=L1*cos(q1); y(2)=L1*sin(q1); % Eslabon 2 x(3)=(L1+q2)*cos(q1); y(3)=(L1+q2)*sin(q1); hold on; plot(x,y,'m','linewidth',1); plot(x,y,'k.','MarkerSize',20); grid on; axis([-1 4 -1 4]); hold off; end
2.3.- Algoritmo Principal
Aquí ingresando puntos iniciales y finales nos arroja una trayectoria guiada por
un número de puntos N, utilizando por defecto un polinomio de grado 7 con
condiciones limite cero para velocidad y aceleración.
Tomamos puntos iniciales y finales, aplicamos función cinversa a tales puntos y
creamos una condicional de espacio de trayectoria para no salir del rango fijado,
si las condiciones son favorables el trayecto se da sin mayor problema y comienza
a plotear cada uno de los N pares de puntos establecidos.
clear all; close all; clc % Coordenadas cartesiana inicial x0=1; y0=0; % Coordenadas cartesiana final xf=0; yf=3; [q0 ok0]=cinversa(x0,y0); % Calculo de Coordenadas articulares iniciales [qf okf]=cinversa(xf,yf); % Calculo de Coordenada articulares finales if ok0*okf==1 clf; hold on; plot([x0 xf],[y0 yf],'b','LineWidth',2); %Plotea la recta que debe
seguir la trayectoria a interpolar con el jtraj plot([x0 xf],[y0 yf],'r.','MarkerSize',20); title('\bf Prueba de Trayectoria 1 para Manipulador RP') xlabel('\bf x') ylabel('\bf y') grid on; axis([-1 4 -1 4]) hold off;
Con N=50 número de puntos a seguir en la trayectoria invocamos el comando
“jtraj” cuyo papel es devolvernos las evoluciones de velocidad y aceleración para
este proceso en el tiempo así como también la trayectoria en q para cada
2012-B Robótica Control Dinámico Robot 2DOF - RP
15
articulación del robot, creamos un bucle de iteraciones de 1 a N en el cual se va
plotear nuestra actualización de puntos.
N=50; % Numero de puntos [q dq ddq]=jtraj(q0,qf,N);% nos devuelve los valores de la posición,
velocidad y aceleración respectivamente. for k=1:N plotRP(q(k,:)); pause(0.1); end figure, subplot(311), plot(q),title('\bf Posicion Angular (rad)') legend('q_1(R)','q_2(P)'),grid subplot(312), plot(dq),title('\bf Velocidad Angular (rad/seg)') legend('dq_1(R)','dq_2(P)'),grid subplot(313), plot(ddq),title('\bf Aceleracion Angular (rad/seg^2)') legend('ddq_1(R)','ddq_2(P)'),grid else display('Fuera del campo de acción'); end
A continuación se mostrará los diagramas obtenidos mediante el uso de Matlab
para nuestro robot RP:
Figura N°2.- Patrón de trayectoria estimado por el jtraj de HEMERO
Del diagrama de trayectoria, la revoluta es constante para q1 que barre un
ángulo de 90 grados y el desplazamiento de d2 es variable en el espacio de
trabajo hasta llegar al punto final.
2012-B Robótica Control Dinámico Robot 2DOF - RP
16
Figura N°3.- Diagrama donde podemos ver las actualizaciones de posición (q),
velocidad (dq) y aceleración (ddq) para la trayectoria definida en el algoritmo
principal.
Tenemos también otro algoritmo para obtener la trayectoria del robot, pero en
este caso usamos el linspace para el ploteo de inicio a final.
El comando jtraj arrojara trayectoria para los "q, q' y q''" cada dos puntos
consecutivos, ya que se realizara interpolación con N1 puntos entre dos puntos
contenidos consecutivamente en la recta general. Ya no con el inicio y fin de ésta
como en la prueba anterior
close all; clear all; clc; % Coordenadas cartesiana inicial x0=1; y0=-0.5; % Coordenadas cartesiana final xf=0; yf=3; [q0 ok0]=cinversa(x0,y0); % Calculo de Coordenadas articulares iniciales [qf okf]=cinversa(xf,yf); % Calculo de Coordenada articulares finales if ok0*okf==1 clf; hold on; plot([x0 xf],[y0 yf],'b','LineWidth',2); plot([x0 xf],[y0 yf],'r.','MarkerSize',20); title('\bf Prueba de Trayectoria 2 para Manipulador RP') xlabel('\bf x') ylabel('\bf y') axis square axis([-1 4 -1 4]); hold off; N=10; % Cantidad de parejas de puntos a intepolar x=linspace(x0,xf,N); y=linspace(y0,yf,N); % Para interpolacion con N1 puntos entre dos puntos N1=3;
2012-B Robótica Control Dinámico Robot 2DOF - RP
17
%Creando almacenadores de datos Q=zeros(N1,N); dQ=zeros(N1,N); ddQ=zeros(N1,N); for k=2:N [q dq ddq]=jtraj(cinversa(x(k-1),y(k-
1)),cinversa(x(k),y(k)),N1); Q(:,k-1:k)=[q(:,1),q(:,2)]; dQ(:,k-1:k)=[dq(:,1),dq(:,2)]; ddQ(:,k-1:k)=[ddq(:,1),ddq(:,2)]; for j=1:length(q) plotRP(q(j,:)); % Dibuja la evolucion de los eslabones en
XY pause(0.1); end
end end
Figura N°4.- Diagrama de trayectoria para dos puntos consecutivos definidos en la
línea recta de inicio a final.
2012-B Robótica Control Dinámico Robot 2DOF - RP
18
III.- Modelo Dinámica Directa ROBOT RP
El modelo dinámico de un robot tiene por objetivo conocer la relación entre el
movimiento del robot y las fuerzas implicadas en el mismo. El modelo dinámico
directo o dinámica directa expresa la evolución temporal de las coordenadas
articulares del robot en función de las fuerzas y pares que intervienen.
La parte inicial del informe muestra el modelamiento de nuestro Robot RP de 2
grados de libertad en función de Lagrange – Euler. Una vez obtenido este
modelamiento vamos a realizar una simulación de la dinámica. Para ello
emplearemos el software Matlab junto a las herramientas de Simulink que nos
ofrece.
Es de nuestro conocimiento que el robot al ser del tipo Revoluta – Prisma
expresará la evolución de sus coordenadas articulares en función de Torque y
Fuerza. Por tal motivo simularemos el comportamiento de ambos mediantes
señales sinusoidales. Estas señales ingresarán a la dinámica directa del robot y
permitirán observar como ya se dijo la evolución temporal de las coordenadas
articulares del Robot. La idea fundamental de esta simulación es observar esta
evolución.
El esquema general a implementar es el siguiente:
Figura N° 5.- Esquema General de Dinámica Directa del Robot RP
El torque y la fuerza tendrán comportamiento de señales sinusoidales, las
configuraciones respectivas son las siguientes:
2012-B Robótica Control Dinámico Robot 2DOF - RP
19
Figura N° 6.- Configuración de señales sinusoidales para torque y fuerza
El torque se comportará como una señal sinusoidal de amplitud 8 y de frecuencia
2: . La fuerza se comportará como una señal sinusoidal de amplitud 5 y
de frecuencia 1:
Figura N° 7.- Comportamiento del torque y fuerza del robot RP
2012-B Robótica Control Dinámico Robot 2DOF - RP
20
3.1.- La dinámica directa de nuestro robot será el reflejo de las ecuaciones de
fuerza y torque obtenidas en la etapa de modelamiento:
………………………………………………..(1)
Donde la matriz de Masas es:
[
] ………………………………….(2)
La matriz de Coriolis es:
[
]……………………………………………..(3)
Y la matriz de Gravedad es:
[
]…………………………………..(4)
Resultando finalmente:
[(
)
]
(
) …………(5)
…………………………………………………..(6)
3.2.- Recordando el concepto de la dinámica directa donde obtendremos la
evolución de las coordenadas articulares a partir de la fuerza o torque,
despejamos la ecuación general y tenemos:
……………………………………………………………….(7)
Obtenemos entonces la aceleración del motor respectivo. Si empleamos
integradores podemos obtener el comportamiento de la velocidad y posición de
cada articulación.
2012-B Robótica Control Dinámico Robot 2DOF - RP
21
De acuerdo a esto elaboraremos el esquema general de la dinámica directa:
Figura N° 8.- Dinámica directa del Robot RP
Según lo anterior:
“u” es la señal de torque o fuerza.
“invD” es una función matemática que describe la matriz inversa de la
matriz de masas del robot.
“Coriolis” y “gravedad” son las matrices de respectivas del robot.
“invD*R” es función de multiplicación para obtener
Se emplean integradores para obtener no solo la aceleración del sistema,
sino también de la velocidad y posición.
3.3.- Describiremos cada función matemática empleada:
a) Coriolis: la matriz de coriolis se debe al movimiento relativo existente entre los
diversos elementos, así como las fuerzas centrípetas que dependen de la
configuración instantánea del manipulador.
De acuerdo al análisis realizado mediante Lagrange-Euler:
[
]
2012-B Robótica Control Dinámico Robot 2DOF - RP
22
A partir de ello creamos una función matemática en Matlab para emplearla en
d) invD*R: este bloque representa la multiplicación de matrices para obtener la
aceleración de las articulaciones:
2012-B Robótica Control Dinámico Robot 2DOF - RP
23
Una vez implementadas las funciones y el esquema en Simulink obtenemos los
siguientes resultados:
Figura N° 9.- Evolución temporal de las coordenadas articulares de cada articulación
empleando simulink
La imagen anterior representa la evolución temporal de las posiciones,
velocidades y aceleraciones de cada articulación. Las gráficas de color rojo
representan las coordenadas articulares de la primera articulación, es decir
provenientes del torque de la revoluta. Las gráficas de color azul representan las
provenientes de la fuerza del prisma.
Si observamos el Simulink en la parte inferior existe un bloque llamado salidas.
Este bloque permite almacenar los valores obtenidos y poder emplearlos en todo
Matlab. Para realizar una contrastación de los resultados obtenidos realizamos
una pequeña programación en el Editor.
clear all; close all; clc
tf=10; ang1=30; q1=pi*ang1/180; q2=0.5; t=sim('Din_robot_2dof',tf); % Tiempo de simulación al código Simulink Q=zeros(length(t),6); % Arreglo de 10x6, almacena los datos q, dq y ddq
for i=1:6 Q(:,i)=salidas(:,i);%almacenamos q1 y d2 en fila 1 y 2, dq1 y dd2
Los parámetros iniciales son q1 = 30°, es decir la revoluta con un ángulo inicial, y
q2= 0.5, que es la posición inicial del prisma.
El arreglo “Q” de 6 columnas y tantas filas como muestras se tomen en los 10
segundos de simulación. Las filas representan: q1, q2, dq1, dq2, ddq1, ddq2.
Los resultados obtenidos son los siguientes:
Figura N° 10.- Evolución temporal de las coordenadas articulares de cada
articulación empleando el Editor de Matlab
0 1 2 3 4 5 6 7 8 9 10-1000
0
1000
tiempo
q1 &
d2
q1
d2
0 1 2 3 4 5 6 7 8 9 10-200
0
200
tiempo
dq
1 &
dd d
2
dq1
d d2
0 1 2 3 4 5 6 7 8 9 10-50
0
50
tiempo
ddq
1 &
dd d
2
ddq1
dd d2
2012-B Robótica Control Dinámico Robot 2DOF - RP
25
3.4.- Alternativa en código puro Matlab para Dinámica Directa
Así como empleamos Simulink para realizar la dinámica directa, podemos realizar
este proceso implementando un código en el editor de Matlab.
3.4.a.- En la primera parte del código mostramos las condiciones iniciales del
sistema:
clear all; close all; clc % Condiciones iniciales T=0.01; % Tiempo de muestreo q=[pi/6 0]'; % ángulo inicial y posición inicial dq=[0 0]'; % velocidades iniciales ddq=[0 0]'; % aceleraciones iniciales plotRP(q); % dibujo del robot en su posición inicial
3.4.b.- Podemos evaluar varias situaciones de nuestro robot. Las cuales se
muestran a continuación:
% Casos del torque Torq=[0 0]'; %Sin torque de entrada y solo interactuando con la gravedad %Torq=[8 0]'; %Torque < gravedad y sin fuerza al motor 2 %Torq=[0 5]'; %Sin torque al primer motor , solo fuerza al prisma %Torq=[10 5]';%Insertamos torque mayor que al fuerza de gravedad
N=100; % número de muestras Q=zeros(2,N); % arreglo para almacenar posiciones dQ=zeros(2,N); % arreglo para almacenar velocidades ddQ=zeros(2,N); % arreglo para almacenar aceleraciones
3.4.c.- Realizamos la implementación de la dinámica directa recordando el mismo
criterio que las fórmulas empleadas en el simulink:
Debemos indicar antes que para realizar las integraciones respectivas
recordaremos lo siguiente:
2012-B Robótica Control Dinámico Robot 2DOF - RP
26
for k=1:N ddq=Minv(q)*(Torq - G(q(1),q(2))- C(dq(1),dq(2),q(2))); dq=dq+ddq*T; q=q+dq*T; Q(:,k)=[q(1);q(2)]; dQ(:,k)=[dq(1);dq(2)]; ddQ(:,k)=[ddq(1);ddq(2)]; plotRP(q); axis([-5 2 -1 2]) pause(0.01); end
3.4.d.- Finalmente realizaremos los ploteos de la evolución de las coordenadas