Modelo y Anlisis Cinemtica de un

Robot
de 6 Grados de Libertad
Profesor: Dr. Jesus Patricio Ordz Oliver
Alumnos
Miguel Angel Camargo Blancas
Marco Jasiel Percastre Mndez
Universidad Politcnica de Pachuca
Departamento de Ingeniera Mecatrnica
Asignatura : Cinemtica de Robots













Introduccin

La robtica es un campo relativamente joven de la tecnologa moderna y
experimento entre las dcadas de los setenta y ochenta un notable auge, llegando
a los noventas a los que muchos han considerado su mayora de edad
caracterizada por la estabilizacin de la demanda , aceptacin y reconocimiento
pleno en la industria.
El trmino robot fue introducido por primera vez en nuestro vocabulario por el
dramaturgo checo KarelCapek, la palabra robota es la palabra checa para trabajo.
Desde entonces, el trmino se ha aplicado a una gran variedad de dispositivos
mecnicos, tales como los teleoperadores , vehculos submarinos, etc.
Prcticamente cualquier cosa que funciona con algunos grado de autonoma , por
lo general bajo control del ordenador , ha sido en algn momento llamado un robot
.

En el siguiente documento se emplearon los fundamentos de la robtica obtenidos
en curso de Cinemtica de Robots impartido por el Dr. Jess patricio Ordaz Oliver,
incluyendo la cinemtica de posicin, velocidades y aceleraciones directas he
inversas de un robot de seis grados de libertad (6 Dof), tipo PRRRRR, con una
mueca esfrica como efector final. El objetivo de este trabajo es proporcionar el
anlisis cinemtico del robot, as como el desarrollo de diferentes mtodos para la
obtencin de parmetros tales como posicin, velocidad, aceleracin del robot.






Descripcin del brazo del robot
A continuacin se describir el siguiente Robot en el cual se llevaran a cabo los
anlisis de las distintas cinemticas as como el modelado. A lo largo del
documento se explicaron conceptos clave para comprender los distintos anlisis y
estudios a los cuales que ser sometido el Robot.

Definicin de Robot Manipulador
Un robot manipulador es un dispositivo electromecnico reconfigurable y
reprogramable diseado para realizar una tarea como mover piezas, pintar o
soldar mediante la programacin de una tarea asignada.

Eslabones y Articulaciones
Mecnicamente los robots esta formados por conjuntos eslabones, los cuales son
elementos mecnicos los cuales pueden ser de dos tipos los cuales son rotacional
denotado por una letra R o prismtico representado por una letra P.
Las articulaciones permiten el movimiento relativo entre dos eslabones
consecutivos, en el figura 1.1 se muestras los tipos de movimientos de las
articulaciones.

Figura 1.1 Tipos de articulacin simbologa en 2D y 3D
Efector Final
Es la parte ultima del extremo del brazo robot, por lo general es una herramienta
ya sea una pistola de pintura, taladro, pinza, o cabeza de soldadura. Esta es la
encargada de realizar la tarea asignada.

Coordenadas Generalizadas
Las coordenadas generalizadas representan un punto nico dentro de un espacio
(cartesiano, polar, esfrico) y se define como la n-upla de puntos nicos
3
1 2
:{q , q ,...q }X
n
q e






Figura 1.2 A) Representacin de un nico punto en el espacio cartesiano 2D
B) La representacin de un punto en un espacio 3D

Grados de Libertad o DoF (DeF del ingles Degree of Freedom)
El moviente relativo permitido entre la articulacin y el eslabn se denomina
grados de libertad. Un grado de libertad es definido como el numero de
coordenadas generalizadas menos el numero de restricciones. Se muestran en la
Figura 1.3 los grados de libertad de las articulaciones ms comunes de un robot.

Figura 1.3 Grados de libertad de las articulaciones ms usadas en los robots



Estructura de un Robot
Un robot en general cuenta con cinco subsistemas por los cuales est constituido.
Figura 1.4
Subsistema Mecnico: eslabones, articulaciones, actuadores, etc.
Subsistema Elctrico: fuente de alimentacin, tarjeta de adquisicin de datos,
etapa de potencia, etc.
Subsistema Percepcin: sensores (pticos, numricos, resistivos, mecnicos,
etc.), cmaras, etc.
Subsistema de control: cinemtica, dinmica, acciones de control (matemticas
del robot).
Subsistema comunicacin: Lenguaje de programacin.

Figura 1.5 Morfologa de un Robot

Espacio de trabajo o Work Space (WS)
El espacio de trabajo de un robot manipulador es volumen total por donde podra
posicionarse y moverse el efector final del robot. Como ejemplo se muestra el WS
de un robot esfrico RRP Figura 1.6

Figura 1.6 Espacio de Trabajo de un Robot esfrico
Una vez consientes de los conceptos clave de la morfologa de un robot se podr
continuar con el estudio del robot que a continuacin se describe en la figura X .
Brazo Robot PRRRRR (Prismtico, Rotacional, Rotacional, Rotacional, Rotacional,
Rotacional) el Brazo Robot cuenta con una mueca esfrica encargada de orientar
al efector final del robot que por lo general es una herramienta.

Figura 1.7 Diagrama del Robot completo con la mueca esfrica
Es de tipo Esfrico ya que sus movimiento y su Espacio de Trabajo (WS) es
representado por reas esfricas. Con las diferentes vistas podremos ver el
espacio del trabajo del robot (Figura 1.7)

Figura 1.7 Vistas del WS del Robot PRR con mueca esfrica
El movimiento del robot est limitado por el numero de grados de libertad como se define
anteriormente los grados de libertad representan el movimiento relativo entre dos
eslabones y se define como DoF= Nmero de coordenas generalizadas - Nmero de
restricciones. Se muestra el numero de coordenada Generalizadas en la Figura 1.8

Figura 1.8 19 Coordenadas generalizadas y 12 restricciones, encontramos un Robot de 6 Grados de libertad






Singularidad en el Robot
Una singularidad es cuando el jacobiano de la posicin es nulo es decir que se
indeterminada. El punto mximo del WS es cuando q3 alcanza los lmites del WS es este
caso cada mltiplo de / 2 pi indeterminada al robot ya que el jacobiano se hace nulo
con estos valores.
2 2
det(Jq) 2 * ( 3) * ( 2) l cos q sin q =


Anlisis Cinemtica del Robot PRR con mueca esfrica

Cinemtica de un Robot Manipulador
La cinemtica del robot estudia el movimiento del mismo respecto a un sistema de
referencia. As la cinemtica se interesa por la descripcin analtica del movimiento
espacial del robot as las relaciones que existen entre la posicin y la orientacin del
extremo final del robot con los valores que toman sus coordenadas articulares.
Cinemtica directa
Consiste en determinar cul es la posicin y orientacin del extremo del robot final, con
respecto a un sistema de coordenadas que se toma como referencia, conocidos como
valores articulares.

Cinemtica inversa
Resuelve la configuracin que debe adoptar el robot para una posicin y orientacin del
extremo conocidas.







Desacople cinemtico
Desacoplo cinemtico: El mtodo geomtrico es de gran utilidad para la obtencin de las
tres primeras variables articulares del robot, pero al tener un nmero mayor de variables
articulares se vuelve ms complejo, es por ello que se utiliza el desacoplo cinemtica que
consta de seccionar el robot que a partir de relaciones en las transformaciones se puedan
obtener todas aquellas variables articulares que son desconocidas
El Desacople cinemtica permite separar al Robot de 6 DoF (PRRRRR) en dos robots,
esto permite que determinar la cinemtica de cada uno sea ms sencillo ya que se tiene
dos robots de 3 DoF uno de tipo PRR y la mueca esfrica del tipo RRR. Figura 2.1










Mtodo Geomtrico para cinemtica directa
Este mtodo se basa en encontrar suficiente
nmero de relaciones geomtricas para
hallar la posicin o efector final del robot con
ayuda de coordenadas articulares y
dimensiones fsicas de cada articulacin.
Este mtodo es muy til para robots de
menos de tres grados de libertad ya que con
cada grado de libertad ms que contenga el
robot las relaciones geomtricas se hacen
ms complejas. Figura 2.2 y por la Ecuacin
de posicin 1.1
Figura 2.1 Desacople las 3 primeras articulaciones y la mueca esfrica



2 2 3
1 2 2 3
1 1 2 3
cos(q ) l cos(q )
(x, y, z) sin(q ) l cos(q )
sin(q )
q
d l l

(
(
= +
(
(
+ +


Ecuacin 1.1





Figura 2.2 Primeras tres articulaciones del Robot
obtenidas por mtodo geomtrico
Mtodo de Denavit y Hartenberg:
Es un mtodo sistemtico para describir
y representar la geometra espacial de
un robot con respecto a un sistema de
referencia fijo. Se basa en una matriz
de trasformacin homognea que
describe la relacin espacial entre dos
elemento rgidos adyacentes, con esto reduce el problema de cinemtica directa a solo
encontrar una matriz de transformacin homognea que relacione la localizacin espacial
del efector final en base al sistema de coordenadas de su base. En general el mtodo D-H
es muy empleado para obtener la cinemtica directa de robots manipuladores, adems
una caracterstica muy importante, es que se pueden anexar los parmetros de otro robot
y as obtener la cinemtica directa de la unin de dos robots. En este caso es de muy
importante ya que como se menciono anteriormente el robot se desacoplo en 2 robots
para una fcil obtencin de sus cinemticas.






Para obtener los cuatro parmetros de D-H , , ,
i i i i
a d o u solo dependen de las
caractersticas geomtricas de cada eslabn y de las articulacin inmediatas la anterior y
la siguiente.

1
a Es a la distancia a lo largo del eje Xi que va desde la interseccin del eje Zi-1 con el
eje Xi hasta el origen del sistema i-esimo, en el caso de articulaciones giratorias. En el
caso de articulaciones prismticas, se calcula como la distancia ms corta entre los ejes
Zi-1 y Zi.
i
o Es el ngulo de separacin del eje Zi-1 y el eje Zi, medido en un plano perpendicular al
eje Xi, utilizando la regla de la mano derecha.
1
d Es la distancia a lo largo del eje Zi-1 desde el origen del sistema de coordenadas (i-1)-
esimo hasta la interseccin del eje Zi-1 con el eje Xi. Se trata de un parmetro variable en
articulaciones prismticas.
i
u Es el ngulo que se forma entre Xi-1 y Xi medido en un plano perpendicular al eje Zi-1,
utilizando la regla de la mano derecha. Se trata de un parmetro variable en articulaciones
giratorias.
Matriz general de trasformacin homognea de los parmetros de D-H
1
0
0 0 0 1
i i i i i i i
i i i i i i
i
i
c s c s s a c
s c c c s a s
A
s c i d
u u o u o u
u u o u o u
o o
(
(

(
=
(
(


Matriz de transformacin homognea (T) se define como una matriz de 4x4 que
representa la transformacin de un vector de coordenadas homogneas de un sistema de
coordenadas a otro.

3 3 3 1
1 3 1 1
x x
x x
R p Rotacin Posicin
T
f w Prespectiva Escala
| | | |
| |
= =
| |
| |
\ . \ .






Nota: en robtica se emplea la siguiente notacin Sin(q1+q2) se representa S12 al igual
que Cos(q1+q2) C12 si se estuviera multiplicando Sinq1*Sinq2 S1S2.

Parmetros D-H del Robot
1
*
1
*
1 2
*
2 3
1 0 0
2
2 0 0
2
3 0
2
4 0 0
i i i i i
l a d
pi
d
pi
q
pi
l
l
o u
u
u

Matrices de trasformacin de los parmetros de D-H
1 2
1 1
1 0 0 0 1 0 0 0
0 0 1 0 0 0 1 0
0 1 0 0 1 0
0 0 0 1 0 0 0 1
T T
d q
( (
( (

( (
= =
( (
( (



2 2 3 3 2 3
2 2 3 3 2 3
3 4
1
cos( ) 0 sin( ) 0 cos( ) sin( ) 0 cos(q )
sin( ) 0 cos( ) 0 sin( ) cos( ) 0 sin(q )
0 1 0 0 0 1 0
0 0 0 1 0 0 0 1
q q q q l
q q q q l
T T
l
( (
( (

( (
= =
( (
( (


Figura 2.3 Robot PRR sus nuevos sistema de referencias
Matriz de trasformacin final, la ltima columna representa la posicin la cual es la misma
que se obtuvo por mtodo geomtrico (Ecuacin 1.1) por lo que se corrobora que es
correcta.
2
cos(q2)*cos(q3) -cos(q2)*sin(q3) sin(q2) l2*cos(q2)*cos(q3)
cos(q3)*sin(q2) -cos(q2)*sin(q3) cos( ) q1 + l2*cos(q3)*sin(q2)
sin(q3) cos(q3) 0 d1 + l1 + l2*sin(q3)
0 0 0 1
t
q
T
(
(

(
=
(
(


Mueca esfrica
La mueca esfrica es la encargada de determinar la posicin y del efector final o del la
herramienta, en este caso en especifico por cuestiones prcticas se determino que la
mueca esfrica tendra distancias cero por lo que nicamente orientara a la herramienta.
Parmetros DH de la mueca

*
4
*
5
*
6
1 0 0
2
2 0 0
2
3 0 0 0
i i i i i
l a d
pi
pi
o u
u
u
u

4* 5* 6 6 7
cos(q4) 0 -sin(q4) 0 cos(q5) 0 sin(q5) 0 cos(q6) -sin(q6) 0 0
sin(q4) 0 cos(q4) 0 sin(q5) 0 -cos(q5) 0 sin(q6) cos(q6) 0 0
0 1 0 0 0 1 0 0 0 0 1 0
0 0 0 1 0 0 0 1 0 0 0 1
t t t
T T T
( ( (
( ( (
( ( (
= = =
( ( (
( ( (


4,5,6
s4s6 + c4c5c6 c6s4 - c4c5s6 c4s5 0
c5c6s4 - c4s6 - c4c6- c5s4s6 s4s5 0
-c6*s5 s5*s6 c5 0
0 0 0 1
t
T
(
(
(
=
(
(





Cinemtica de todo el robot
Como la esfera se le dio valores de ceros en sus distancias estas no afectan a la posicin
del robot pero sin en su orientacin. La cinemtica directa de todo el robot se da
anexando las 2 tablas de parmetros de D-H colocando a la mueca esfrica como el
efector final del primer robot PRR.
La cinemtica final de todo el Robot queda de la siguiente manera agregando la mueca
esfrica con distancias ceros. Ecuacin 1.2
2 2 3
1 2 2 3
1 1 2 3
cos(q ) l cos(q )
(x, y, z) sin(q ) l cos(q )
sin(q )
q
d l l

(
(
= +
(
(
+ +


Ecuacin 1.2


Cinemtica inversa
Para obtener los valores que necesitan tomar los eslabones y articulaciones es necesario
obtener la cinemtica inversa de posicin, existen mtodos para obtener la cinemtica
inversa de posicin.
- Mtodo geomtrico
- Matrices homogneas
- Desacople cinemtica
- Cuaterniones
En este caso como se menciona al principio se emplea el desacople mecnico para sacar
los tres primeros valores de cada eslabn, seccionando al robot de 6 grados de libertad
en uno de 3 DoF y su efector final que ser la mueca esfrica de 3 Dof.





Mediante el mtodo geomtrico
Se pueden obtener los 3 primeros valores de
1 2 3
, ,
n
q q q q = mediante relaciones
geomtricas ya que la complejidad del los primeros tres eslabones del robot es
relativamente sencilla y es posible obtener estos valores por medio de este mtodo. Con
ayuda de las siguientes relaciones trigonomtricas se pudieron obtener las inversas de
posicin .



2
3
2
x=(Pz-d1-l1)
( 2 ) (( 1 1
tan 2(x,r)
) r
q i
l Pz
a
d
nv
l =
=


2 2
2 tan 2(s
( ( )
, P )
)
x
s
q i
r Px
nv a
=
=


1 q inv Py s =








Figura 2.4 Relaciones trigonomtricas utilizadas para calcular las inversas de posicin

Cinemtica inversa de mueca esfrica
Dado el desacoplo cinemtica que se realiz con el robot obtendremos la cinemtica
inversa de la mueca esfrica a travs de sus rotaciones ngulos de Euler, y se tiene la
rotacin general como:


| u u | u | | u
| u u | u | u
u u u
| | + + | |
|
|
= + +
|
|
|
|

\ .
\ .
11 12 13
21 22 23
31 32 33
C C S C C S S S S C S C R R R
R S C C C S S S C S S S C R R R
S C S C C R R R

De la matriz anterior se pueden obtener las siguientes relaciones.
| u u
u
| u u
u
u
u u u u
u u u
= = =
= = =
=
+ =
= =
32
11 32
33
21 33
31
2 2 2
2 2
31
R
R
1
1 C 1
R
R C C C S S
C
R
R S C C C C
C
R S
S C S C
C S R

Despejando las relaciones para obtener las variables desconocidas | u , , se muestra
que:
( )
( )
u
u
u
u
u

=
=
| |
=
|
\ .
31
2
31
2
31 31
32 32
33 33
32
33
sin
tan =
cos
1
arctan / 1
/ C sin
tan = =
cos / C
arctan
R
R
R R
R R
R R
R
R

u
u
|
|
|
|
=
=
=
| |
|
\ .
11
21
21
11
21
11
/
/
tan

=arctan
C R C
S R C
R
R
R
R

Cinemtica directa de velocidad
La relacin entre las velocidades de las coordenadas articulares y las de la posicin y
orientacin del robot se les denomina matriz jacobina . La matriz jacobina directa permite
conocer las velocidades del extremo del robot a partir de los valores de las velocidades de
cada articulacin, y la matriz jacobina inversa permite conocer las velocidades articulares
necesarias para obtener unas velocidades determinadas en el extremo del robot .

Posicin
2 2 3
1 2 2 3
1 1 2 3
cos(q ) l cos(q )
(x, y, z) sin(q ) l cos(q )
sin(q )
q
d l l

(
(
= +
(
(
+ +



Jacobino
2 2 3 2 2 3
2 2 3 2 2 3
2 3
0 sin(q )*l *cos(q ) cos(q )*l *sin(q )
(x, y, z) 1 cos(q )*l *cos(q ) -sin(q )*l *sin(q )
0 0 l *cos(q )
Jq
(
(
=
(
(



La cinemtica de velocidad est dada por:
1 2 2 3 2 2 3
2 2 3 2 2 3 2
2 3 3
0 sin(q )*l *cos(q ) cos(q )*l *sin(q )
(x, y, z) 1 cos(q )*l *cos(q ) -sin(q )*l *sin(q )
0 0 l *cos(q )
q
Jq q
q

( (
( (
=
( (
( (

Cinemtica inversa de velocidad
Se define como:
( )
{ }
1
*V
n q
q J xyz

=


2 2 3 2 2 3 3
2
2
2 2
cos(q2)
1
sin(q2)
0 cos(q ) sin(q ) cos(q ) sin(q )
1
q 0 *
l2*cos(q3
( 3) * ( 2) ( 3) * ( 2)
( 3) * ( 2)
( 2) * ( 3)
2* ( 3) )*sin(q2)
1
0 0
l2*cos(q )
* ( 2)
3
n
sin q cos q sin q sin q
cos q sin q
cos q sin q
l cos q s
l q l q
q
in q
(
(
(

(
=
(
(
(

+
(

1 2 3 2 3 2 2 2 3
2 3 3
sin(q ) sin(q ) l q cos(q )sin(q )
0 0 l q cos(q )
l q
(
(

(
(

2 2
1 ( 2( 2 2 3 2 2 3 2) 3)) / 2 2 3 2 3 ( 2 3 ( 3 2 3 2 )) / 2 2
(l2q2c3s2 + l2q3c2s3)/(l2c3)s2) - (q3c2s3)/(c3s2)
q3
2 2 3
n
q c l q c s l q c s
q
s l q s s l q p s c s s s l q c c
=
(
(
(
(

+ +

+ +

Cinemtica de Aceleracin
Se define como:
Se pude ver el cdigo de la comprobacin numrica en la seccin de Anexos, Anexo 3.
( )
{ }
( ) = J J *
q q n
d d
a V xyz q xyz q
dt dt

-
= +
1
0 l2*sin(q2)*sin(q3) - l2*cos(q2)*cos(q3) l2*sin(q2)*sin(q3) - l2*cos(q2)*cos(q3)
0 - l2*cos(q2)*sin(q3) - l2*cos(q3)*sin(q2) - l2*cos(q2)*sin(q3) - l2*cos(q3)*sin(q2) *
0 0 -l2*sin(q3)
q
a
(

(
=

(

2
3
1 2 2 3 2 2 3
2 2 3 2 2 3 2
2 3 3
0 sin(q ) *l *cos(q ) cos(q ) *l *sin(q )
1 cos(q )*l *cos(q ) -sin(q )*l *sin(q )
0 0 l *cos(q )
q
q
q
q
q
(

(
`
(

(
)
( (
( (
+
( (
( (

- q2*(l2*c2*c3 - l2*s2s3) - q3*(l2*c2c3 - l2*s2s3) - l2*q2*c3s2 - l2*q3*c2s3)
q1 - q2*(l2*c2s3 + l2*c3q2) - q3*(l2*c2s3 + l2*c3s2) - l2*q3*s2s3 + l2*q2*c2c3)
l2*(q3*c3 - q3*s3)
a

(
(
(
(

Cinemtica de aceleracin inversa
( )
1

J q n
d
a J q
n
dt
q

` `
) )
=

2 2
3
1 ( 2*( 2* 2* 3 2 2* 3* 2* 3)) / 2 2* 3* 2 3 ( 2* 3*( 3 2 3)* 2 )) / 2 2
(l2*q2*c3s2) + l2*q3*c2s3)/(l2*c3*s2) - (q3*c2*s3)/(c3*
* 2* 3
2)
2
s
n
q c l q c s l q c s s l q s s l q s c s s s l q c
q
q
c + (
(
=
(
(

+ + +

Simulacin en Matlab del Robot
Matlab ofrece la Toolbox de robtica con la cual se pude obtener una simulacin de los
movimiento que puede hacer el robot, as como asignar valores y verificar si es correcto el
anlisis cinemtico.
Simplemente se debern de introducir los parmetros de D-H que anteriormente se
obtuvieron. en la Figura 2.5 se pude observar el modelo de nuestro robot. El cdigo de la
simulacin se pude ver como el anexo 5 al final del documento.

Figura 2.5 Simulacin del Robot en Matlab con ayuda de la Toolbox de robtica


Anexos y resultados
1 Trasformacin total por medio de Parmetros D-H para la obtencin de la cinemtica
directa
clear all; close all; clc;

syms d1 q1 q2 q3 l1 l2 q4 q5 q6 d2;
t1=[1,0,0,0;0,0,1,0;0,-1,0,d1;0,0,0,1]
t2=[1,0,0,0; 0,0,-1,0;0,1,0,q1;0,0,0,1]
t3=[cos(q2), 0, sin(q2), 0; sin(q2), 0, -cos(q2), 0; 0, 1, 0, l1;
0,0,0,1]
t4=[cos(q3), -sin(q3), 0, l2*cos(q3); sin(q3), cos(q3), 0, l2*sin(q3);
0,0,1,0;0,0,0,1]
t5=[cos(q4), 0, -sin(q4), 0; sin(q4), 0, cos(q4), 0; 0,-1, 0, 0; 0,0,0,1]
t6=[cos(q5),0,sin(q5),0; sin(q5), 0,-cos(q5), 0;0,-1,0,0;0,0,0,1]
t7=[cos(q6), -sin(q6), 0, 0; sin(q6),cos(q6),0,0; 0,0,1,0;0,0,0,1]

tf=t1*t2*t3*t4t5*t6*t7
tfs=simplify(tf)


2 Comprobacin por medio de MatLab las cinemticas inversas
clear all; close all; clc;
q1=.5;
q2=pi/4;
q3=-pi/3; %q3 deve de ser menor a 90 sino se indetermina q3 y q2 ya que
su determnate del jacobiano del q2 depende del q3

d1=0;
l1=.5;
l2=.5;

Px=cos(q2)*l2*cos(q3)
Py=q1+sin(q2)*l2*cos(q3)
Pz=d1+l1+l2*sin(q3)

r=(((l2^2)-((Pz-d1-l1)^2)))^.5;
q3i=atan2((Pz-d1-l1),r);

s=(((r^2)-((Px)^2)))^.5;
q2i=atan2(s,Px);

q1original=q1;
q1i=Py-s;

Pxyz=[Px;Py;Pz]
qoriginales=[q1; q2; q3]
qinversos=[q1i;q2i;q3i]


3 Comprobacin de velocidad directa as como su inversa y aceleracin directa e inversa
syms l2 q2 q3 q1p q2p q3p q1pp q2pp q3pp
% l2=.5;
% q2=pi/4;
% q3=pi/4;
%
% q1p=0.1;
% q2p=0.5;
% q3p=0.3;
%
% q1pp=.2;
% q2pp=.3
% q3pp=.6

qp=[q1p;q2p;q3p]
qpp=[q1pp;q2pp;q3pp]
Jq=[0, -sin(q2)*l2*cos(q3), -cos(q2)*l2*sin(q3); 1, cos(q2)*l2*cos(q3), -
sin(q2)*l2*sin(q3); 0, 0, l2*cos(q3)]
% z=det(Jq)
% simplify(z)

%velocidad
velq=Jq*qp
clear q1p q2p q3p
%velocidad inversa
inv(Jq)
qpi=inv(Jq)*(velq)
simplify (qpi)

%aceleracion

clear q1pp q2pp q3pp
Jqt=[0, -(cos(q2)*l2*cos(q3))+(sin(q2)*l2*sin(q3)), sin(q2)*l2*sin(q3)-
(cos(q2)*l2*cos(q3)); 0, -(sin(q2)*l2*cos(q3))-cos(q2)*l2*sin(q3), -
(cos(q2)*l2*sin(q3))-(sin(q2)*l2*cos(q3)); 0, 0, -l2*sin(q3)]
A=(Jqt*qp)+(Jq*qpp)
simplify(A)

qppinv=(inv(Jq))*(A-Jqt*qp)

simplify(qppinv)

4 Simulacin del Robot con la Toolbox de Robtica de Matlab
clear all; close all; clc;

q1=.5;
q2=pi/4;
q3=pi/4;


d1=.5;
l1=.5;
l2=.5;

Px=cos(q2)*l2*cos(q3);
Py=q1+sin(q2)*l2*cos(q3);
Pz=d1+l1+l2*sin(q3);

Pxyz=[Px;Py;Pz]



L1=link([ -pi/2, 0, 0, 0, 1], 'standard');
L2=link([ pi/2, 0, 0, 0, 1], 'standard');
L3=link([ pi/2, 0, 0, .5, 0], 'standard');
L4=link([ 0, .5, 0, 0, 0], 'standard');
r=robot({L1,L2,L3,L4})

q1=.5;
q2=.5;
q3=pi/4;
q4=pi/4;
% % q2=1*.001:2;
% % q3=[0:0.1:2*(pi)];
% % q4=[0:0.1:2*(pi)];

q=[q1;q2;q3;q4]';
q1=0;
q2=.5;
q3=0;
q4=0;

plot (r, q)
drivebot(r)
























Bibliografa
Barrientos Antonio, Aracil Rafael., Fundamentos de Robtica, Segunda Edicin, Editorial
McGrow Hill.
Spong W. Mark, Vidyasagar M., Robot Modeling and Control, First Edition, JONH WILEY
& SONS INC.

Alex S. Poznyak., Modelado matemtico de los sistemas mecnicos elctricos y
electromecnicos.