INSTITUTO POLITECNICO NACIONAL

ESCUELA SUPERIOR DE INGENIERIA

MECÁNICA Y ELÉCTRICA

SECCIÓN DE ESTUDIOS DE POSGRADO E INVESTIGACION

SÍNTESIS DIMENSIONAL Y ANÁLISIS CINEMÁTICO

DEL MECANISMO RSSR-SS
PARA GENERAR UNA TRAYECTORIA

T E S I S
QUE PARA OBTENER EL GRADO ACADÉMICO DE
MAESTRO EN CIENCIAS
P R E S E N T A

ING.. ROBERTO MUÑOZ CASTELLANOS

DIRECTOR: M.EN C. CANDIDO PALACIOS MONTUFAR

México, D.F. Marzo - 2003

AGRADECIMIENTOS Y DEDICATORIA.

Deseo expresar mi agradecimiento a todos los profesores de la Sección de Estudios de

Posgrado e Investigación de la ESIME-IPN que contribuyeron en mi formación profesonal.

Agradezco al M. en C. Cándido Palacios Montufar, mi director de tesis, por su apoyo

brindado en la realización de este trabajo.

Dedico con profundo respeto y cariño este trabajo de tesis a mis padres, por el
incondicional apoyo moral y económico que siempre me han brindado, así como a mis
hermanos y a todas las personas que han creído en mi.
INDICE DE FIGURAS Y TABLAS I
SIMBOLOGIA IV
RESUMEN VI
ABSTRACT VII
JUSTIFICACIÓN VIII
OBJETIVO IX
INTRODUCCIÓN X

CAPITULO I.- CONCEPTOS FUNDAMENTALES

1.1. Cuerpo rígido 2

1.2. Eslabón 2
1.3. Par cinemático 2
1.3.1. Por el número de grados de libertad 3
1.3.1.1. Pares de un grado de libertad o de clase I 3
1.3.1.2. Pares de dos grados de libertad o de clase II 4
1.3.1.3. Pares de tres grados de libertad o de clase III 4
1.3.1.4. Pares de cinco grados de libertad o de clase IV 5
1.3.1.5. Pares de cinco grados de libertad o de clase V 5
1.3.2. Por el tipo de contacto entre los elementos 6
1.3.3. Por el tipo de cierre de la junta 6
1.3.4. Por el número de eslabones conectados 7
1.4. Cadena cinemática 8
1.5. Grados de libertad 10
1.6. Mecanismo 15
1.7. Sistema de coordenadas 16
1.8. Posición absoluta 17
1.9. Posición aparente 17
1.10. Sumario 18
1.11 Referencias 19
CAPITULO II.- SÍNTESIS DE ESLABONAMIENTOS
2.1. Síntesis de tipo 21
2.2. Síntesis dimensional 21
2.2.1. Generación de función 22
2.2.1.1. Espaciamiento de Chebyshev 24
2.2.1.2. Modelado con diadas 32
2.2.2. Generación de trayectoria 32
2.2.3. Generación de movimiento o conducción de cuerpo rígido 38
2.3. Sumario 42
2.4. Referencias 43

CAPITULO III.- SÍNTESIS PARA GENERACIÓN DE TRAYECTORIA

DEL MECANISMO RSSR-SS

3.1. Matrices de rotación 45

3.2. Matrices de rotación compuesta 50
3.3. Matriz de rotación respecto de un eje arbitrario 51
3.4. Interpretación geométrica de las matrices de rotación 53
3.5. Síntesis dimensional para el mecanismo con pares cinemáticos RSSR-SS 56
3.6. Sumario 74
3.7. Referencias 75

CAPITULO IV.- ANÁLISIS CINEMATICO

4.1. Velocidad 77
4.2. Movimiento lineal 77
4.3. Movimiento relativo 79
4.4. Análisis de velocidad 80
4.5. Análisis de aceleraciones 85
4.6. Desarrollo de las ecuaciones cinemáticas para el cálculo de las velocidades 85
 y aceleraciones del mecanismo con pares RSSR-SS
4.7. Sumario 93
4.8. Referencias 94
CONCLUSIONES Y RECOMENDACIONES
ANEXOS
INDICE DE FIGURAS Y TABLAS.

Figura 1.1 Pares de un grado de libertad 3

Figura 1.2 Pares de dos grados de libertad. 4
Figura 1.3 Pares de tres grados de libertad 4
Figura 1.4 Pares de cuatro grados de libertad 5
Figura 1.5 Pares de cinco grados de libertad 5
Figura 1.6 Junta con cierre de forma 6
Figura 1.7 Junta con cierre de fuerza 7
Figura 1.8 Orden de junta 7
Figura 1.9 Cadena simple abierta. 8
Figura 1.10 Cadena simple cerrada 9
Figura 1.11 Cadena compuesta abierta 9
Figura 1.12 Cadena compuesta cerrada 10
Figura 1.13 Grados de libertad 11
Figura 1.14 Mecanismo con grado de libertad cero 14
Figura 1.15 Mecanismo espacial de 4 eslabones 15
Figura 1.16 Mecanismo plano de 4 eslabones 16
Figura 1.17 Sistema de coordenadas 17
Figura 1.18 Posición aparente 18

Figura 2.1 Analogía entre desplazamiento lineal y ángulo de rotación 23

Figura 2.2 Espaciamiento de Chebyshev 25
Figura 2.3 Cierre de circuito de un mecanismo plano 28
Figura 2.4 Posición inicial de un mecanismo plano de 4 eslabones 31
Figura 2.5 Representación por medio de diadas de un mecanismo plano 32
Figura 2.6 Generación de trayectoria 33
Figura 2.7 Diada derecha de un mecanismo plano 34
Figura 2.8 Mecanismo plano de 4 eslabones para generar trayectoria 35
 Figura 2.9 Mecanismo de 4 eslabones para conducción de cuerpo rígido 39
Figura 2.10 Cierre de circuito diada izquierda conducción de cuerpo rígido 39
Figura 2.11 Cierre de circuito diada derecha para conducción de cuerpo rígido 40
Tabla 2.1 Parámetros de un mecanismo plano de cuatro eslabones para 30
generar función.
Tabla 2.2 Parámetros del mecanismo plano de cuatro eslabones para 37
generar trayectoria

Figura 3.1 Orientación de un sistema ouv respecto a otro oxy en un plano 46

Figura 3.2 Rotación del sistema uvw con respecto al eje ox 48
Figura 3.3 Rotación del sistema uvw con respecto a los ejes OY y OZ 49
Figura 3.4 Rotación respecto al eje z 50
Figura 3.5 Rotación respecto de un eje arbitrario 51
Figura 3.6 Mecanismo espacial con pares RSSR-SS 56
Figura 3.7 Secciones que relacionan los puntos de precisión 57
con los eslabones del mecanismo
Figura 3.8 Circuito que relaciona los puntos de precisión con el 57
eslabón W1.
Figura 3.9 Localización del pivote de tierra del eslabón W1 60
Figura 3.10 Circuito para relacionar los puntos de precisión con el eslabón 61
W2
Figura 3.11 Representación del sistema de coordenadas uvw 61
Figura 3.12 Rotación del sistema uvw respecto al sistema xyz 62
Figura 3.13 Cadena cerrada para relacionar W1 y W2 64
Figura 3.14 Mecanismo RSSR-SS para generar trayectoria 65
Figura 3.15 Circuito para relacionar el eslabón C1 con los puntos de precisión 65
Figura 3.16 Mecanismo RSSR-SS para generar trayectoria 73
Figura 4.1 Movimiento de una partícula 78
Figura 4.2 Movimiento relativo 80
Figura 4.3 Velocidad relativa 80
Figura 4.4 Sistemas de coordenadas 81
Figura 4.5 Circuito para determinar las velocidades del eslabón W2 y de la 83
barra acopladora
Figura 4.6 Circuito para determinar las velocidades del eslabón C1 84
Figura 4.7 Gráfica de velocidades angulares 87
Figura 4.8 Gráfica de aceleraciones angulares 92
Tabla 4.1 Velocidades angulares de los eslabones del mecanismo RSSR-SS 86
Tabla 4.2 Aceleraciones angulares de los eslabones del mecanismo RSSR- 91
SS
SIMBOLOGIA

W Movilidad o número de grados de libertad

n Número total de elementos del mecanismo
p1 Número de pares cinemáticos de un movimiento
p2 Número de pares cinemáticos de dos movimiento
p3 Número de pares cinemáticos de tres movimiento
p4 Número de pares cinemáticos de cuatro movimiento
p5 Número de pares cinemáticos de cinco movimiento

Rp Vector de posición del punto P con relación al sistema XYZ

R0 Vector de posición del sistema xyz con relación al sistema
XYZ.
pj j-ésimo punto de precisión
r Vector unitario
rx,ry,rz Componentes del vector unitario r
ix,iy,iz Vectores unitarios del sistema XYZ
iu,jv,kw Vectores unitarios del sistema uvw
ω Velocidad angular
Vp Velocidad lineal de una partícula P respecto al sistema XYZ
V Velocidad lineal de una partícula P respecto al sistema xyz
VPQ Velocidad relativa de P respecto a Q
VQP Velocidad relativa de Q respecto a P
Wa Magnitud del eslabón conductor
Wb Magnitud del eslabón conducido
a,b,c Magnitudes de eslabones
α Aceleración angular
ωβ velocidad angular del eslabón de entrada
ωφ velocidad angular del eslabón acoplador respecto al eje z
ωθ velocidad angular del eslabón acoplador respecto al eje u
ωϕ velocidad angular del eslabón acoplador respecto al eje
 rotado w
ωα velocidad angular del eslabón W2
ωµ velocidad angular del eslabón C1 respecto al eje z
ωρ velocidad angular del eslabón C1 respecto al eje rotado u
ωη velocidad angular del eslabón C1 respecto al eje rotado w
αβ aceleración angular del eslabón de entrada
αφ aceleración angular del eslabón acoplador respecto al eje z
αθ aceleración angular del eslabón acoplador respecto al eje u
αϕ aceleración angular del eslabón acoplador respecto al eje
rotado w
αα aceleración angular del eslabón W2
αµ aceleración angular del eslabón c respecto al eje z
αρ aceleración angular del eslabón c respecto al eje rotado u
αη aceleración angular del eslabón c respecto al eje rotado w
RESUMEN.
Para que un mecanismo sea útil, las dimensiones así como los movimientos de los
eslabones no pueden ser completamente arbitrarios, estos también deben restringirse para
producir los movimientos relativos adecuados para el trabajo particular que se deba
desarrollar.
En el estudio de los mecanismos, en muchas ocasiones se presenta la necesidad de generar
un movimiento preestablecido y con base en este movimiento determinar las proporciones
del mecanismo que produzcan dicho movimiento.
En este trabajo se desarrolla la síntesis dimensional del mecanismo con topología RSSR-SS
para generar trayectoria.
La metodología empleada se basa en la utilización de vectores para representar los
eslabones que constituyen el mecanismo, así como en el uso de matrices de rotación,
mediante las cuales se obtienen las ecuaciones de diseño, necesarias para generar un
sistema de ecuaciones algebraicas sobredeterminadas.
El sistema de ecuaciones es resuelto por medio de un programa desarrollado utilizando el
paquete MATLAB, empleando el método de mínimos cuadrados.
La cinemática del mecanismo es resuelto por medio de la resolución del sistema de
ecuaciones generado de la derivación de las ecuaciones de diseño.
ABSTRACT.

A mechanism is useful, when the dimensions as well as the movements of the links cannot
be totally arbitrary, these should also be restricted to produce the appropriate relative
movements for the particular work that should be developed.
In the study of the mechanisms, in many occasions the necessity is presented of generating
a preset movement and with base in this movement to determine the proportions of the
mechanism that produce this movement.
In this work the dimensional synthesis of the mechanism is developed with topology
RSSR-SS to generate trajectory.
The methodology is based on the use of vectors to represent the links that constitute the
mechanism, as well as in the use of rotation matrix, by means of which the design equations
are obtained, necessary to generate a system of equations algebraic over determined.
The system of equations is resolved by means of a developed program using the package
MATLAB, using the method of square minima.
The kinematics of the mechanism is resolved by means of the resolution of the generated
system of equations of the derivation of the design equations.
JUSTIFICACION:

En la práctica muchos problemas de diseño requieren la creación de un dispositivo con

características de movimientos específicos. Quizá se necesite mover o desplazar una
herramienta de la posición A a la posición B en un intervalo de tiempo particular.
Durante mucho tiempo, los mecanismos comúnmente utilizados han sido los mecanismos
planos, llamados así porque todas sus partículas describen curvas planas en el espacio.
Sin embargo, no siempre se necesita generar trayectorias coplanares y en muchos casos lo
que se necesita es generar una trayectoria en un espacio tridimensional como puede ser el
hecho de insertar una pieza en un ensamble, las posibilidades son interminables.
De acuerdo a lo anterior, los mecanismos espaciales permiten la automatización de
operaciones que no se pueden resolver con mecanismos planos y que se han dejado a los
humanos.
A pesar de la necesidad de estos su utilización es muy escasa, esto es debido a que no se
contaba con las herramientas necesarias para la resolución de los sistemas de ecuaciones
obtenidas al realizar el análisis de dichos mecanismos. En la actualidad, muchos de los
obstáculos que impedían el diseño y el empleo de los dispositivos espaciales han sido
eliminados gracias a los avances en las computadoras, reconociéndose cada vez más que los
movimientos en espacios tridimensionales pueden ser automatizados.
Uno de los objetivos de la línea de Automatización y Robótica es la creación de
mecanismos y dispositivos nuevos; para generar funciones matemáticas, generar
trayectorias y ensamblar dispositivos mecánicos para el laboratorio de mecanismos.
El mecanismo RSSR-SS (mecanismo con dos pares de revolución y cuatro pares esféricos)
es un dispositivo mecánico que enriquecerá el laboratorio y además servirá como material
pedagógico para el curso de análisis y síntesis de mecanismos.
 OBJETIVOS

Objetivo General:

• Calcular las dimensiones significativas del mecanismo con topología

RSSR-SS que genere una trayectoria de trabajo tridimensional
especificada.

Objetivos Específicos:

• Establecer una metodología de síntesis de mecanismos tridimensionales

dentro de la línea de investigación de automatización y robótica.

• Realizar el cálculo de la síntesis dimensional para generar trayectoria del

mecanismo RSSR-SS.

• Realizar el análisis cinemático del mecanismo RSSR-SS

 INTRODUCCIÓN

Los mecanismos y máquinas que se encuentran actualmente en explotación son muy

diversos, tanto por su construcción como por su destino.
Con relación a esto, el examen y estudio detallado de los distintos mecanismos y máquinas
se efectúa en una serie de disciplinas especiales. No obstante, al examinar cualquier
mecanismo o máquina en concreto, se debe tener en cuenta no solo las cuestiones
específicas de estos, sino una serie de problemas que se relacionan en igual medida con
todos los mecanismos o con cierto grupo de ellos.
El dimensionar los eslabones de un mecanismo no es un proceso arbitrario, ya que un
mecanismo para que sea útil debe satisfacer un conjunto deseado de características de
movimiento, el cual puede ser el de generar una función, una trayectoria o conducir un
cuerpo.
Hasta hace poco tiempo había un escaso interés en el diseño y análisis de mecanismos
espaciales y manipuladores robóticos. Las operaciones demasiado difíciles de automatizar
con mecanismos planos se dejaban frecuentemente a los humanos sin considerar la
naturaleza tediosa y peligrosa del trabajo.
Muchos de los obstáculos que impedían el diseño y el empleo de los dispositivos espaciales
han sido eliminados gracias a los notables avances en las computadoras y en la electrónica.
Cada vez hay un reconocimiento mayor de que los mecanismos espaciales son alternativas
viables que deben ser evaluadas en el proceso de diseño.

En el presente trabajo de tesis se resuelve la síntesis dimensional del mecanismo espacial

con pares cinemáticos RSSR-SS (mecanismo con dos pares de revolución y cuatro pares
esféricos) para generar una trayectoria .

La metodología empleada se basa en la representación de los eslabones que constituyen el

mecanismo por medio de vectores, así como en el uso de matrices de rotación , mediante
las cuales se obtienen las ecuaciones de diseño, dichas ecuaciones son resueltas por medio
de un programa desarrollado utilizando el paquete MATLAB y empleando el método de
Newton-Raphson.
El desarrollo de esta tesis se plantea como un material pedagógico para el curso de análisis
y síntesis de mecanismos.

El contenido de este trabajo se desglosa de la siguiente manera:

En el primer capítulo se describen las partes que conforman un mecanismo, la clasificación

que se hace de ellos de acuerdo con algunas de sus características y se describen algunos de
los conceptos básicos para su estudio.

En el segundo capítulo se describen cada una de las tareas para las cuales se realiza la
síntesis dimensional y se obtienen las ecuaciones de diseño que rigen el movimiento,
utilizando para ello un mecanismo plano, utilizando para ello un mecanismo de cuatro
barras.

En el tercer capítulo se desarrollan las ecuaciones de diseño para realizar la síntesis

dimensional del mecanismo espacial con pares cinemáticos RSSR-SS para generar una
trayectoria prescrita.

En el cuarto capítulo se calculan la velocidad y aceleración de los eslabones que conforman

el mecanismo RSSR-SS a través de la derivación de las ecuaciones de diseño.

Finalmente se presentan las Conclusiones que se derivan de este trabajo de tesis.

 CAPITULO
I

CONCEPTOS
EN EL ESTUDIO DE
MECANISMOS

1
 CONCEPTOS EN EL ESTUDIO DE MECANISMOS.
Aún con los continuos avances realizados en el diseño de instrumentos, controles
automáticos y equipos automatizados, los mecanismos como constituyentes de las
máquinas ocupan un papel primordial en el desarrollo de la tecnología, encontrándose
presente en todas las actividades del ser humano, desde la vida cotidiana hasta en los
sectores productivos.
En este capítulo se describen cada una de las partes que conforman un mecanismo, así
como se describen algunos conceptos que son útiles para el estudio de los mismos.

1.1. CUERPO RIGIDO.

En la mayoría de los casos se pueden representar matemáticamente los fenómenos físicos,
utilizando idealizaciones con el fin de crear modelos básicos simplificados, a los cuales se
les conoce como modelos matemáticamente ideales o simplemente, modelos matemáticos
ideales.
Esta técnica permite tratar con problemas que de otra manera sería extremadamente difícil
resolver [1.1].
Un cuerpo rígido, es un modelo matemático de un cuerpo material o de un sistema de
partículas, en el cual la distancia entre dos puntos cualesquiera permanece constante. En
otras palabras, un cuerpo rígido es un sistema tal que el cambio de distancia entre dos
cualesquiera de sus partículas, entendido como deformación no tiene lugar [1.2].

1.2. ESLABON.
Un eslabón se define como un cuerpo o un conjunto de piezas rígidamente unidas entre sí y
que tienen al menos dos nodos o puntos de unión para formar cadenas cinemáticas cerradas.
Los eslabones de un mecanismo o máquina, sirven para la transmisión de movimientos y de
fuerzas. Esta transmisión se efectúa de un eslabón conductor a otro conducido [1.3].

1.3 PAR CINEMATICO.

Se llama par cinemático a la unión de dos cuerpos, elementos o eslabones y que permiten
un movimiento relativo [1.1].
Los pares cinemáticos o articulaciones, pueden clasificarse por:

2
 - El número de grados de libertad permitidos en el par cinemático.
- El tipo de contacto entre los elementos: de línea, de punto o de superficie.
- El tipo de cierre de la articulación, de fuerza o de forma.
- El número de eslabones conectados (orden de la junta o articulación).
1.3.1 POR EL NÚMERO DE GRADOS DE LIBERTAD.
Al formar un par cinemático, los eslabones pierden la posibilidad de efectuar ciertos
movimientos, y el número de grados de libertad de los mismos disminuye.
Las limitaciones introducidas en el movimiento relativo de estos eslabones al ser unidos, se
denomina condición de enlace, las cuales eliminan del movimiento relativo de cada eslabón
el mismo número de grados de libertad.
De acuerdo con el número de grados de libertad que les quedan al efectuarse los enlaces, se
pueden considerar cinco clases de pares cinemáticos.
Un par es de primera clase, cuando al unir los eslabones quedan eliminados cinco grados de
libertad; un par es de quinta clase, cuando únicamente queda eliminado un grado de
libertad.

1.3.1.1 PARES DE UN GRADO DE LIBERTAD O DE CLASE I:

Estos pares están formados por uniones móviles que permiten un movimiento de traslación,
como se observa en la figura 1.1a o un movimiento rotación presentado en la figura 1.1b ,
quedando eliminados cinco grados de libertad [1.4].

a b

FIG. 1.1 Pares de un grado de libertad

3
1.3.1.2 PARES DE DOS GRADOS DE LIBERTAD O DE CLASE II.
Los pares de dos grados de libertad o de segunda clase están formados por uniones móviles
que permiten dos movimientos, que pueden ser un movimiento de rotación y un
movimiento de traslación que se ilustra en la figura 1. 2.

FIG. 1.2 Pares de dos grados de libertad

1.3.1.3 PARES DE TRES GRADOS DE LIBERTAD O DE CLASE III.

Las uniones móviles de los pares de tercera clase permiten tres movimientos, que pueden
ser: tres movimientos de rotación ver figura 1.3a o dos movimientos de rotación y uno de
traslación que se presenta en la figura 1. 3b o dos movimientos de traslación u uno de
rotación ilustrado en la figura 1. 3c.

a)

FIG. 1.3 Pares de tres grados de libertad

4
1.3.1.4 PARES DE CUATRO GRADOS DE LIBERTAD O DE CLASE IV:
Las uniones de cuarta clase son las que permiten cuatro movimientos que pueden ser: dos
movimientos de rotación y dos de traslación, como se observa en la figura 1. 4 .

FIG 1.4 Pares de cuatro grados de libertad

1.3.1.5 PARES DE CINCO GRADOS DE LIBERTAD O DE CLASE V:

Las uniones móviles de los pares de quinta clase permiten cinco movimientos, es decir, que
se elimina únicamente un grado de libertad, pueden ser tres movimientos de rotación y dos
de traslación, con un punto de contacto entre los eslabones, que se presenta en la figura 1.5.

FIG. 1.5 Pares de cinco grados de libertad

1.3.2 POR EL TIPO DE CONTACTO ENTRE LOS ELEMENTOS.

5
 En función del contacto que se establece a partir de la unión de eslabones, permite
clasificarlos en pares cinemáticos superiores y pares inferiores.
En los pares cinemáticos superiores, el contacto entre los eslabones es por medio de
una línea o un punto.
Los pares cinemáticos inferiores son aquellos en los cuales el contacto que resulta
de la unión de los eslabones es a través de una superficie.

1.3.3 POR EL TIPO DE CIERRE DE LA JUNTA.

El tipo de cierre de la junta permite clasificarlos en [1.1]:
Cierre de forma
Cierre de fuerza

Una junta con cierre de forma es aquella que se mantiene unida o cerrada por la
configuración o forma que presenta, ver figura 1.6.

FIG. 1.6 Junta con cierre de forma

Una junta con cierre de fuerza es aquella en la que se requiere alguna fuerza externa para
mantenerse en contacto o cierre. Esta fuerza podría ser proporcionada por la gravedad o por
otros medios externos, como lo ilustra la figura 1.7.

6
 FIG. 1.7 Junta con cierre de fuerza

1.3.4 POR EL NÚMERO DE ESLABONES CONECTADOS.

Esta clasificación también es conocida como orden de la junta y se define como el número
de eslabones conectados menos uno. Por lo que se necesitan dos eslabones para construir
una junta de primer orden o junta simple, como se puede observar en la figura 1.8

FIG. 1.8 Orden de junta

7
1.4 CADENA CINEMATICA.
Una cadena cinemática es un ensamble o enlaces de eslabones interconectados por medio
de pares cinemáticos o juntas, de modo que proporcionen un movimiento de salida
controlado en respuesta a un movimiento de entrada proporcionado [1.5].
Las cadenas cinemáticas pueden dividirse en simples y compuestas, pero estas cadenas
simples y compuestas a su vez pueden ser abiertas o cerradas.
Una cadena cinemática es simple, cuando sus eslabones están unidos a lo mas a dos
eslabones, es decir, tienen un eslabón unido a cada extremo.
Esta cadena simple como se mencionó anteriormente puede ser abierta o cerrada, esto
dependerá de la configuración que presente.
Una cadena es simple abierta, cuando en los eslabones de sus extremos no se tiene ninguna
unión, ver figura 1.9 , y una cadena es simple cerrada cuando cada eslabón se conecta con
otros dos eslabones (uno en cada extremo) , formando un circuito cerrado como se
presenta en la figura 1.10 .

FIG 1.9 Cadena simple abierta

8
 FIG. 1.10 Cadena simple cerrada

Una cadena es compuesta, cuando en alguna de sus uniones existen tres o más eslabones.
De manera semejante a las cadenas simples, se tiene que una cadena es compuesta abierta
cuando posee eslabones con extremos libres figura 1.11; si no posee eslabones con
extremos libres se dice que es una cadena compuesta cerrada figura 1.12.

FIG. 1.11 Cadena compuesta abierta

9
 FIG. 1.12 Cadena compuesta cerrada
1.5 GRADOS DE LIBERTAD.

Durante el funcionamiento de un mecanismo, todos los elementos que lo conforman, con

excepción del elemento inmóvil (bancada) se desplazan y en cada instante ocupan
posiciones definidas. Para determinar las posiciones de todos los elementos, es preciso
conocer los parámetros independientes que permitan conocer de manera unívoca la
posición de cada uno de los elementos o eslabones del mecanismo con respecto al eslabón
fijo; estos parámetros pueden ser: los ángulos de giro (coordenadas angulares) o sus
coordenadas lineales.
Por lo que se puede decir que las coordenadas generalizadas del mecanismo, son cada uno
de los parámetros independientes que determinan la posición correspondiente de los
elementos del mecanismo. Al número de coordenadas generalizadas se les denomina
también número de grados de libertad [1.6].
Un eslabón sencillo restringido o limitado a moverse con movimiento plano, como el
mostrado en la figura 1.13a posee tres grados de libertad.
Las coordenadas x , y del punto P junto con el ángulo θ forman un conjunto independiente
de tres parámetros que describen la posición del punto. La figura 1.13b muestra dos
eslabones con movimiento plano (desconectados). Debido a que cada eslabón posee tres
grados de libertad, los eslabones muestran un total de seis grados de libertad. Si estos se

10
unen en un punto mediante una unión de giro o revoluta, como se muestra en la figura
1.13c, el sistema formado por los dos eslabones tendrá solamente cuatro grados de libertad .

a) b)

c)
FIG. 1.13 Grados de libertad

Los cuatro parámetros independientes que describen la posición de los dos eslabones
podrían ser, por ejemplo, las coordenadas x , y del punto P1, el ángulo θ 1 y el ángulo θ 2.
Hay muchos otros parámetros que se pueden emplear para especificar la posición de estos
eslabones, pero solo cuatro de ellos pueden ser independientes.
Una vez que se especifican los valores de los parámetros independientes, la posición de
cada punto en ambos eslabones queda determinada.
La conexión de dos eslabones con movimiento plano mediante una unión de giro o revoluta
tuvo el efecto de restarle dos grados de libertad al sistema. Dicho en otras palabras, la unión
de giro o revoluta tuvo el efecto de restarle dos grados de libertad al sistema. Dicho en
otras palabras, la unión giratoria permite un solo grado de libertad (rotación pura) entre los
eslabones que conecta.

11
P.L. Chebyshev en el año 1869 encontró la relación existente entre el número de elementos
y el número de pares cinemáticos de un movimiento, para el cual el número de grados de
libertad de un mecanismo plano será igual a 1. Esta relación es la primera fórmula
empleada para determinar el número de grados de libertad de un mecanismo. A.P.
Mályshev dedujo la fórmula para hallar el número de grados de libertad de un mecanismo
espacial.
Para deducir esta fórmula se designan por:

n - el número de todos los elementos del mecanismo

p1 - número de pares cinemáticos de un movimiento
p2 - número de pares cinemáticos de dos movimientos
p3 - número de pares cinemáticos de tres movimientos
p4 - número de pares cinemáicos de cuatro movimientos
p5 - número de pares cinemáticos de cinco movimientos
w - número de grados de libertad del mecanismo

En cada mecanismo un elemento es inmóvil, por esto el número de elementos móviles del
primero será igual a ( n – 1 ). Cada elemento libre posee, en caso general, seis grados de
libertad. Si se consideran estos elementos independientes entre sí, entonces todos los
elementos móviles, es decir ( n – 1 ), tendrán 6 ( n – 1 ) grados de libertad. Sin embargo,
los elementos forman pares cinemáticos, los cuales disminuyen el número de grados de
libertad de los mismos. Por lo tanto para cada par cinemático de un movimiento disminuye
en cinco el número de grados de libertad, ya que de seis movimientos permite solo uno.
Todos los pares cinemáticos de un movimiento cuyo número es p1, disminuyen el número
de grados de libertad en 5p1.
Cada par cinemático de dos movimientos, disminuye en 4 el número de grados de libertad,
ya que permite dos movimientos de seis. Todos los pares cinemáticos de dos movimientos
p2, disminuyen el número de grados de libertad en 4p2.
Análogamente todos los pares cinemáticos de tres, cuatro y cinco movimientos disminuyen
el número total de grados de libertad respectivamente en 3p3, 2p4, 1p5.
Por lo que se obtiene la siguiente fórmula para determinar el número de grados de
libertad de un mecanismo espacial:

12
 W = 6 ( n – 1 ) – 5p1 – 4p2 – 3p3 – 2p4 – 1p5 ( 1.1 )

Al deducir esta formula se supuso que cada elemento libre puede tener seis movimientos,
sin embargo, en los mecanismos planos solo pueden tres movimientos. Así como existir
pares cinemáticos sólo de uno y de dos movimientos. Además, cada par cinemático de un
movimiento, disminuye el número de grados de libertad en uno. Por ello el número de
grados de libertad de un mecanismo plano, se determina según la fórmula:

W = 3 ( n – 1 ) – 2p1 – 1p2 ( 1.2 )

Es posible encontrar casos en que, los elementos teniendo magnitudes determinadas y

posiciones especiales, uno o varios pares cinemáticos eliminan a algún elemento de un
movimiento, del cual este elemento es además privado por otros pares cinemáticos. Esta
privación repetida o múltiple de un movimiento, corresponde a una superposición de
enlaces también repetida o múltiple. Estos últimos generalmente se denominan enlaces
pasivos.
En la figura (1.14 ) se muestran el esquema de un mecanismo plano. En el cual el número
de elementos es n = 5 y el número de pares cinemáticos de un grado de movilidad es p1 = 6.
Sustituyendo en la fórmula (1.2) se tiene:

W = 3 (n – 1 ) – 2p1 –1p2 = 3 (5 – 1 ) – 2*6 – 1*0 = 0 ( 1.3 )

El mecanismo de la figura 1. 14 realmente no puede moverse ( W = 0 ) y representa una

armadura.

13
 FIG. 1.14 Mecanismo con grado de libertad 0

En la figura 1.15 se presenta un mecanismo espacial de cuatro eslabones, en el cual el

elemento cuatro es inmóvil. El número de pares cinemáticos de un movimiento (giratorio)
es igual a dos. Los pares están formados por los elementos 1 - 4 y 1 - 2.
Los elementos 2 – 3 forman un par esférico, es decir, un par de tres movimientos.
Los elementos 3 – 4 forman un par cilíndrico, es decir, un par cinemático de dos
movimientos, por lo que:

n = 4 , p1 = 2 , p2 = 1 , p3 = 1 , p4 = p5 = 0

sustituyendo en la formula ( 1.1 ) se tiene que:

W = 6 (4 – 1) – 5*2 – 4*1 – 3*1 = 1

Por lo que el número de grados de libertad del mecanismo es uno. Esto significa que para
una posición dada de uno de sus elementos, los demás también ocuparán una posición
determinada.
Si uno de los elementos del mecanismo es obligado a desplazarse, entonces, todos sus
elementos restantes, realizarán también movimientos completamente determinados que son
funciones del movimiento dado.

14
 FIG. 1.15 Mecanismo espacial de 4 eslabones

1.6 MECANISMO:
Un mecanismo es aquella parte de una máquina que contiene dos o más piezas (eslabones)
dispuestas de tal manera que el movimiento de una de ellas obliga a moverse a las otras, los
eslabones pueden ser de cualquier forma, mientras no obstaculicen el movimiento deseado.
La forma más simple es la de cuatro barras, A, B, C, D, unidas en sus extremos por
pasadores cilíndricos y que se mueven todas en planos paralelos. Si los eslabones son de
longitudes diferentes y se fija cada uno sucesivamente, se tendrán cuatro combinaciones
posibles; pero como dos de estas son semejantes, se producirán tres mecanismos con
movimientos distintos [1.6].
Así, en la figura 1.16a , si se fija D, A puede girar y C oscilar, y se tiene un mecanismo
balancín y manivela. Si se fija B se obtendrá el mismo movimiento; si se fija A figura
1.16b, los eslabones B y D pueden girar, dando el mecanismo de eslabón de arrastre usado
para mover los flotadores montados sobre ruedas de paletas. Al fijar el eslabón C, ver
figura 1.16c, D y B sólo pueden oscilar, y se obtiene un mecanismo de oscilador, usado a
veces en donde se producen movimientos en línea recta.
A un eslabón conductor se le acostumbra llamar manivela; a uno oscilante palanca o
balancín, y al eslabón de conexión, biela. Con frecuencia se agranda el eslabón fijo y se
usa como bastidor de soporte.

15
 a b

FIG. 1.16 Mecanismo plano de 4 eslabones

1.7 SISTEMA DE COORDENADAS.
Al especificar cantidades físicas se supone que existe un marco de referencia con respecto
al cual se efectúan las medidas. Se califica de “referencia inercial” a un sistema de
coordenadas ligado a una estrella fija en el espacio. Cualquier otro sistema que se mueva
uniformemente y sin rotación con respecto a la estrella fija, también se puede utilizar como
referencia inercial. No obstante, en la mayoría de los trabajos de ingeniería, el sistema de
coordenadas se fija a la superficie de la tierra, pues el error en que se incurre, debido a la
rotación de esta y por la variación de su movimiento alrededor del sol, es muy pequeño
[1.7].
El sistema de coordenadas, utilizados con mayor frecuencia, es el sistema de coordenadas
cartesianas, ver figura 1.17 .
Existen además otros métodos, igualmente válidos como son las coordenadas polares, las
coordenadas cilíndricas y el sistema de coordenadas esférico [1.8].

16
 FIG. 1.17 Sistema de coordenadas

1.8 POSICIÓN ABSOLUTA.

Cuando un problema en particular obliga a considerar varios sistemas de coordenadas, la

aplicación conducirá a la identificación de un solo sistema de coordenadas fundamental. En
el que queda expresado el resultado final; conociéndose como sistema absoluto de
coordenadas.
La posición absoluta de un punto se define como su posición aparente vista por un
observador en el sistema absoluto de coordenadas.
1.9 POSICIÓN APARENTE.

A menudo resulta conveniente hacer observaciones en un sistema de coordenadas

secundario, es decir, tal y como lo ve un segundo observador en un sistema de coordenadas
distinto, y luego llevar esta información hacia el sistema de coordenadas básico.
Si se pidiera obtener la ubicación del punto P ver figura 1.18 utilizando sistemas de
coordenadas diferentes se obtendría un vector de posición RP02 para localizar el vector P en
el sistema de coordenadas x2, y2, z2, y con respecto al sistema x1, y1, z1, se tiene el vector
de posición RP01 .
Los vectores RP01 y RP02 están relacionados por medio de la expresión:
RP01 = R0201 + RP02

17
 FIG. 1.18 Posición aparente

1.10 SUMARIO:

Un mecanismo es un dispositivo mecánico que tiene el propósito de transferir el

movimiento de una fuente a una salida, el cual consiste en eslabones (barras) considerados
como cuerpos rígidos conectados por juntas o pares para formar cadenas o lazos que
pueden ser abiertos o cerrados. Los pares son formas geométricas mediante las cuales se
unen dos miembros (eslabones) de un mecanismo, de manera que el movimiento relativo
entre ambos sea consistente. Si la unión o articulación mediante la cual se conectan dos
miembros tiene un contacto superficial, tal como una unión de perno, la conexión se llama
par inferior. Si la conexión ocurre en un punto a lo largo de una línea tal como en un
rodamiento de bolas o entre dos dientes de engranes en contacto, se le conoce como par
superior. Un par que solo permite rotación relativa es un par deslizante. Un par de giro
puede ser inferior o superior, dependiendo de que se emplee un perno y buje o un
rodamiento de bolas para la conexión. Un par deslizante es un par inferior, como ocurre
entre un pistón y la pared del cilindro.

18
1.11 REFERENCIAS
[1.1] Robert L. Norton.
Diseño de Maquinaria
Mc. Graw-Hill, México 1995.
[1.2] Richard S. Hartenberg, Jacques Denavit.
Kinematic Synthesis of Linkages
Mc. Graw-Hill 1964
[1.3] Hamilton H. Mabie, Charles F. Reinholtz.
Mecanismos y Dinámica de Máquinas.
Limusa 1998.
[1.4] G. G. Baránov
Curso de la Teoría de Mecanismos y Máquinas.
MIR. Moscú 1985.
[1.5] M. en C. Cándido Palacios Montúfar.
Análisis y Síntesis de Mecanismos.
México D.F. 1997.
[1.6] Erdman, Arthur G. Sandor, George N.
Diseño de Mecanismos, Análsis y Síntesis.
Prentice Hall 1998.
[1.7] Anthony Bedford , Wallace Fowler
Ingeniería Mecánica, Dinámica.
Addison-Wesley Iberoamericana,1996.
[1.8] Luciano Chiang S.
Análisis Dinámico de Sistemas Mecánicos
Alfaomega 1994.

19
CAPITULO

II

SÍNTESIS DE
ESLABONAMIENTOS

SÍNTESIS DE ESLABONAMIENTOS

20
Para que un mecanismo sea útil, los movimientos entre los eslabones no pueden ser
completamente arbitrarios, estos también deben restringirse para producir los movimientos
relativos adecuados para el trabajo particular que se deba desarrollar.
En el estudio de los mecanismos, en muchas ocasiones se presenta la necesidad de generar
un movimiento preestablecido y con base en este movimiento determinar las proporciones
del mecanismo que produzcan dicho movimiento. Este procedimiento se conoce como
síntesis dimensional del mecanismo [2.1].
En este capítulo se describen cada una de las tareas para las cuales se realiza la síntesis
dimensional y se obtienen las ecuaciones de diseño que rigen el movimiento en un
mecanismo plano de cuatro barras conectado por pasadores.

2.1 SÍNTESIS DE TIPO:

La síntesis de tipo se refiere a la selección del mecanismo adecuado. El cual podría

ser un eslabonamiento, tren de engranajes, bandas, poleas, o un sistema de levas;
además de el número de eslabones que deberá tener el mecanismo, cuantos grados
de libertad se requieren, que configuración es deseable [2.2].
Las deliberaciones relacionadas con el número de eslabones y grados de libertad a veces se
consideran dentro de una subcategoría de la síntesis de tipo llamada síntesis de número.

2.2 SÍNTESIS DIMENSIONAL:

El objetivo fundamental de la síntesis dimensional es determinar las dimensiones

significativas, así como determinar la posición inicial de un mecanismo de un tipo
preconcebido para una tarea especifica.
El término de dimensiones significativas o principales se refiere a las longitudes de los
eslabonamientos.
La configuración o posición inicial suele especificarse mediante una posición angular de
un eslabonamiento de entrada (manivela impulsora) con respecto al eslabón fijo o marco

21
de referencia, o la distancia lineal de un bloque deslizante a partir de un punto en su
eslabonamiento guía.
Un mecanismo de tipo preconcebido puede ser un deslizador-manivela, un eslabonamiento
de cuatro barras, una leva con un seguidor plano, o un mecanismo más complejo con cierta
configuración definida topológica, pero no dimensionalmente.
Dependiendo de las tareas que se realicen con los mecanismos, la síntesis dimensional se
clasifica en tres categorías.
1. Generación de función.
2. Generación de trayectoria.
3. Generación de movimiento.
En un generador de función, lo que interesa es el movimiento relativo entre los eslabones
conectados a tierra, es decir, el movimiento relativo entre el eslabón conductor y el
conducido.
En la generación de trayectoria lo que interesa es la trayectoria de un punto trazador y no la
rotación del eslabón acoplador.
En la generación de movimiento lo que es de interés es el movimiento total del eslabón
acoplador.

2.2.1 GENERACIÓN DE FUNCIÓN.

En la síntesis de un mecanismo destinado a la generación de función es preciso

correlacionar la rotación o el movimiento deslizante de los eslabones de entrada y salida;
es conceptualmente una “caja negra” que suministra una salida predecible en respuesta a
una entrada conocida.
El diseño de un mecanismo que correlacione la entrada y la salida, de modo tal, que cuando
la entrada se mueva una distancia x la salida se mueva y = f(x), para un intervalo
x0 ≤ x ≤ xn +1.

Los valores del parámetro independiente , x1 , x2 ,....xn , corresponden a puntos de precisión

prescritos , p1 , p2 ,... pn en la función y = f(x) en un intervalo de x entre xo y x n +1 .

En el caso de entrada y salida giratorios (figura 2.1b), los ángulos de rotación φ y ϕ

son los análogos de x y y (figura 2.1a).

22
 ( a) (b)

FIG. 2. 1 Analogía entre desplazamiento lineal y ángulo de rotación

∆x = xn +1 − x0 ∆φ = φ n +1 − φ0

∆y = y n +1 − y 0 ∆ϕ = ϕ n +1 − ϕ 0

Puesto que existe una relación directa entre los cambios angular y lineal se tiene que:

φ j − φ1 ∆φ
= (2.1)
x j − x1 ∆x

donde φ1 es el nivel de referencia para φ j y por tanto, φ1 = 0 por lo que de (2.1) se obtiene

que:

∆φ
φj = (x j − x1 ) (2.2)
∆x

∆ϕ
ϕj = (y j − y1 ) (2.3)
∆y

Estas relaciones también pueden escribirse de la siguiente manera:

φ j = Rφ (x j − x1 ) y ϕ j = Rϕ ( y j − y1 )

23
donde Rφ y Rϕ son los factores de escala en grados por unidad de variable, definidos

por:
∆φ ∆ϕ
Rφ = y Rϕ =
∆x ∆y

Los mecanismos o eslabonamientos, no pueden generar sin errores una función

arbitraria, y solo pueden coincidir con la función en un número limitado de puntos
llamados puntos de precisión.

2.2.1.1 ESPACIAMIENTO DE CHEBYSHEV.

Chebyshev determinó que la mejor aproximación de un eslabonamiento a una función,

ocurre cuando se ecualiza el valor absoluto del error estructural máximo entre los puntos
de precisión y en ambos extremos del intervalo [2.3].
En la figura 2.2 se determina el espaciamiento de Chebyshev de un eslabonamiento de
cuatro barras que genera la función y = 4 x 2 − 1, en un intervalo 1 ≤ x ≤ 3 considerando
cuatro puntos de precisión.
Primeramente se traza un circulo cuyo diámetro es proporcional al intervalo del parámetro
independiente ∆x.

∆x = xn +1 − x0 = 3 − 1 = 2

Se inscribe un polígono equilátero regular de 2n lados ( n= número de puntos de

precisión prescritos) en el círculo, de modo que dos lados del polígono queden
verticales.

2 ( n ) = 2 ( 4 ) = 8 lados

Se dibujan líneas perpendiculares al diámetro horizontal que pasen por cada esquina del
polígono, estas líneas intersecarán el diámetro en puntos espaciados a distancias
proporcionales al espaciamiento de Chebyshev de los puntos de precisión figura 2.2 [2.4].

24
 FIG. 2.2 Espaciamiento de Chebyshev

La construcción gráfica anterior para el espaciamiento de Chebyshev es equivalente a las

siguientes expresiones:

1 ⎡ ⎡ ⎛ 2 j − 1 ⎞⎤ ⎤
∆x j = x j − x 0 = ∆x ⎢1 − cos ⎢π ⎜ ⎟⎥ ⎥ ; j= 1,2,...n (2.4)
2 ⎣ ⎣ ⎝ 2n ⎠ ⎦ ⎦

x j = x0 + ∆x j (2.5)

Donde ∆x j es la distancia desde el principio del intervalo x hasta el j-ésimo punto de

precisión.

∆x j = x n +1 − x0 = intervalo en x (2.6)

j= número de puntos de precisión

n = número total de puntos de precisión.
Por lo tanto para la resolución de el eslabonamiento de cuatro barras para generar función
( y = 4 x 2 − 1 ) se tendrá:

1 ⎡ ⎡ ⎛ 2 j − 1 ⎞⎤ ⎤
∆x j = x j − x 0 = ∆x ⎢1 − cos ⎢π ⎜ ⎟⎥ ⎥
2 ⎣ ⎣ ⎝ 2n ⎠ ⎦ ⎦

25
sustituyendo:

(2)⎢1 − cos ⎡⎢π (2(1) − 1)⎤⎥ ⎥

1 ⎡ ⎤
∆x1 =
2 ⎣ ⎣ 2(4 ) ⎦ ⎦
⎛π ⎞
∆x1 = 1 − cos⎜ ⎟ = 0.076
⎝8⎠
⎛ 3π ⎞
∆x 2 = 1 − cos⎜ ⎟ = 0.617
⎝ 8 ⎠

∆x3 = 1.382

∆x 4 = 1.923

Por lo que al sustituir ∆x1 en (2.5) se tendrá:

x j = x0 + ∆x j

x1 = x 0 + ∆x1

x1 = 1 + 0.076 = 1.076

x 2 = x0 + ∆x 2

x 2 = 1.617

x 3 = 2.382

x 4 = 2.923

Los valores correspondientes de y j se calculan sustituyendo los valores de las x j en la

función que se quiera generar, en este caso sustituyendo en la función y = 4 x 2 − 1

y0 = 3

y1 = 4(x1 ) − 1 = 4(1.076) 2 − 1 = 3.631

2

y 2 = 4( x 2 ) 2 − 1 = 9.458

26
 y 3 = 21.695

y 4 = 33.175

y 5 = 35

Una vez calculados los puntos de precisión de Chebyshev, utilizaremos los siguientes
intervalos de las rotaciones de entrada y de salida:

Intervalo de entrada ∆φ = 60 ° intervalo de salida ∆ϕ = 80 °

Esto con la finalidad de calcular los ángulos de entrada y sus respectivos ángulos de salida
en el mecanismo ( φ 2 , φ 3 , φ 4 , ϕ 2 , ϕ 3 , ϕ 4 ).

Para obtener tales ángulos se sustituye en (2.2) , (2.3) y se le suma el ángulo

correspondiente a la posición inicial de la barra conductora como de la conducida
donde:

∆x = 2 ; ∆y = 32 ; ∆φ = 60° ; ∆ϕ = 80° ;

φ 0 = 20 0 ϕ 0 = 10 0

∆φ
φ j = φ0 + (x j − x1 )
∆x
φ 2 = φ 0 + (1.617 − 1.076 ) = 16.23°
60
2

φ 3 = 39.18°

φ 4 = 55.41°

∆ϕ
ϕ j = ϕ0 + (y j − y1 )
∆y

ϕ2 = ϕ0 +
80
(9.458 − 3.631) = 14.56°
32

27
 ϕ3 =
80
(21.595 − 3.631) = 45.16°
32

ϕ 4 = 73.86°

Uno de los métodos analíticos mas conocidos en el desarrollo de la síntesis para la

generación de función , se basa en las ecuaciones de desplazamiento de
Freudenstein. Estas ecuaciones se deducen de la ecuación de cierre de circuito
figura 2.3, en donde

FIG. 2.3 Cierre de circuito de un mecanismo plano

R1 + R2 + R3 + R4 = 0 (2.7)

Si se utiliza una notación compleja polar la ecuación (2.7) queda expresada de la

siguiente manera.

jθ 4
R1 e jθ1 + R 2 e jθ 2 + R3 e jθ 3 + R4 e =0 (2.8)

Pero e jθ = cos θ + isenθ lo que permite expresar la ecuación anterior en su forma

compleja rectangular; la cual al separarse en su parte real e imaginaria permite obtener las
dos ecuaciones siguientes [2.1].

R1 cos θ 1 + R2 cos θ 2 + R3 cos θ 3 + R4 cos θ 4 = 0 (2.9)

28
 R1 senθ 1 + R2 senθ 2 + R3 senθ 3 + R4 senθ 4 = 0 (2.10)

Partiendo de la figura 2.3 se tiene que el ángulo correspondiente al eslabón 1 es de

180o con lo cual las expresiones (2.8) y (2.9) ahora serán:
− R1 + R2 cosθ 2 + R3 cosθ 3 + R4 cosθ 4 = 0 (2.11)

R2 sen θ 2 + R3 sen θ 3 + R4 sen θ 4 = 0 (2.12)

Puesto que se desea sintetizar un eslabonamiento para la generación de función, el ángulo

del eslabón acoplador θ 3 se eliminará de las ecuaciones, ya que el ángulo de nuestro
interés es el ángulo de la barra de salida.
Para ello, se pasan todos los términos excepto los que comprendan a R3 al segundo
miembro, y se elevan ambos miembros al cuadrado.

R32 cos 2 θ 3 = (R1 − R 2 cos θ 2 − R 4 cos θ 4 )

2

R32 sen 2 θ 3 = (− R2 sen θ 2 − R4 sen θ 4 )

2

Desarrollando el segundo miembro de ambas ecuaciones y sumándose la ecuación

que resulta es:

R32 = R12 + R22 + R42 − 2 R1 R2 cos θ 2 − 2 R1 R4 cos θ 4 + 2 R2 R4 (cos θ 2 cos θ 4 + senθ 2 senθ 4 )

Dividiéndola entre 2 R 2 R 4 y realizando la siguiente sustitución

cos θ 2 cos θ 4 + senθ 2 senθ 4 = cos(θ 2 − θ 4 ) la ecuación se expresa como:

R32 − R12 − R32 − R42 R1 R

+ cos θ 2 + 1 cos θ 4 = cos(θ 2 − θ 4 )
2 R2 R4 R4 R2

Freudenstein escribe esta ecuación como:

K 1 cos θ 2 + K 2 cos θ 4 + K 3 = cos(θ 2 − θ 4 ) (2.13)

Donde :

29
 R1 R R 2 − R12 − R22 − R 42
K1 = ; K2 = 1 ; K3 = 3 ; (2.14)
R4 R2 2 R2 R4
Las K son expresiones algebraicas independientes, las cuales contienen las longitudes
desconocidas de los eslabones.
La ecuación de Freudenstein es una ecuación de desplazamiento para eslabonamientos, la
cual se cumple para cada punto de precisión. Así, si se desea sintetizar un mecanismo para
la generación de función con tres puntos de precisión, lo que se tiene que hacer es generar
una ecuación por cada punto de precisión y sustituir θ 2 por φ y θ 4 por ϕ que son las
posiciones angulares de entrada y salida en cada una de las ecuaciones.
De esta forma las ecuaciones de Freudenstein para tres puntos de precisión son:

K 1 cos φ1 + K 2 cos ϕ 1 + K 3 = − cos(φ1 − ϕ 1 )

K 1 cos φ 2 + K 2 cos ϕ 2 + K 3 = − cos(φ 2 − ϕ 2 )

K 1 cos φ 3 + K 2 cos ϕ 3 + K 3 = − cos(φ 3 − ϕ 3 )

Estas ecuaciones se resuelven simultáneamente para las tres incógnitas K 1 , K 2 , K 3 , se

selecciona una longitud arbitraria para el eslabón R1 y se resuelve el conjunto de

ecuaciones para determinar las dimensiones de los otros tres eslabones.
Retomando la función y = 4 x 2 − 1 que se ha estado utilizando y para la cual ya se han
calculado los valores de x j , y j , φ j y ϕ j que se muestran en la tabla 2.1.

Tabla 2.1 Parámetros de un mecanismo plano de cuatro eslabones para generar función

Posición x φ y ϕ

1 1.076 32.28 3.631 11.577

2 1.617 36.23 9.458 24.56

3 2.382 59.18 21.695 55.16

4 2.923 75.41 33.175 83.86

30
Tomando los valores de φ y ϕ para sustituirlos en la ecuaciones con lo que se tiene el
siguiente sistema de ecuaciones:

K 1 cos(32.28) + K 2 cos(11.577) + K 3 = cos(32.28 − 11.577)

K 1 cos(36.23) + K 2 cos(24.56) + K 3 = cos(36.23 − 24.56)

K 1 cos(59.18) + K 2 cos(55.16) + K 3 = cos(59.18 − 55.16)

K 1 cos(75.41) + K 2 cos(83.86) + K 3 = cos(75.41 − 83.86)

Se resuelve el sistema de ecuaciones anterior para obtener los valores de las K

K 1 = 0.3619
K 2 = -0.2063
K 3 = -1.0615

Utilizando R1 = 1 y las identidades 2.13, se obtienen las dimensiones de los eslabones.

R4 = 2.7632
R2 = −4.8473
R3 = 7.7825

FIG. 2. 4 Posición inicial de un mecanismo de 4 eslabones

31
 2.2.1.2 MODELADO CON DIADAS.

Como se ha visto, los mecanismos pueden modelarse utilizando vectores para la

representación de los eslabones. Las diversas combinaciones de los vectores representan
alternativas para la resolución de la síntesis dimensional. Una de estas alternativas para
realizar la síntesis dimensional es la utilización de díadas; las cuales son combinaciones de
pares de vectores [2.5].
El eslabonamiento de cuatro barras de la figura ( 2.5 ) está modelado por medio de díadas.
El lado izquierdo del eslabonamiento es modelado con el par de vectores ( Wa y Z a ) y el

lado derecho con ( Wb y Z b )

FIG. 2.5 Representación por medio de diadas de un mecanismo plano

2.2.2 GENERACIÓN DE TRAYECTORIA

En el desarrollo de la síntesis dimensional de un mecanismo para generar trayectoria, un

punto del eslabón acoplador debe trazar una trayectoria definida respecto al marco de
referencia fijo.En la figura 2.6 se muestra un mecanismo plano de cuatro eslabones, el cual
es

32
representado por medio de vectores; el lado izquierdo del eslabonamiento es modelado por
un par de vectores ( Wa y Z a ) y el lado derecho por el par de vectores ( Wb y Z b ).
Además se muestra la ubicación de un punto arbitrario (P), el cual se mueve de la posición
1 a una posición j; los puntos P1 y Pj son llamados puntos de precisión.

δj denota el vector desde P1 hasta Pj por lo que se tiene que δj = Pj – P1

αj es el ángulo del vector Z a desde la primera hasta la j-ésima posición. αj P1

βj es el ángulo que forma el vector Wa desde su primera posición hasta la j-ésima posición.

FIG. 2.6 Generación de trayectoria

Los vectores Wa y Wb se originan en el pivote de tierra y terminan en el pivote móvil; los

vectores Za y Zb salen del pivote móvil y terminan en el punto trazador de la trayectoria que
se desea generar.
Considerando la primera y la j-ésima posiciones de la díada izquierda del mecanismo de
cuatro barras figura 2.6. Se busca relacionar los vectores Wa y Za con las posiciones de
los puntos de precisión P1 y Pj y con las rotaciones angulares αj y βj.

33
La ecuación de cierre de circuito para la díada izquierda es:

Wae ( βj + β1 ) + Zae (αj +α1 ) − Wae β1 − Zae α1 − Pj + P1 = 0

utilizando la identidad de Euler y separando la parte real de la imaginaria, se obtienen las

siguientes ecuaciones de diseño:

Wa cos( β j + β 1 ) + Z a cos(α j + α 1 ) − Wa cos( β 1 ) − Z a cos(α 1 ) − Pjx + P1x = 0

Wa sen( β j + β 1 ) + Z a sen(α j + α 1 ) − Wa sen( β 1 ) − Z a sen(α 1 ) − Pjy + P1 y = 0

La díada derecha del mecanismo está representada por los vectores Wb y Z b , en la cual el

vector Wb se origina en un pivote de tierra y termina en el pivote móvil y el vector Z b sale

del pivote móvil y termina en el punto trazador de la trayectoria (figura 2.7):
φ1 es el ángulo del vector Z b en la primera posición.
ϕ 1 es el ángulo del vector Wb en la posición de inicio.
ϕ j es el ángulo del vector Wb desde la primera hasta la j-ésima posición.

FIG. 2.7 Diada derecha de un mecanismo plano

La ecuación de cierre de circuito para la díada derecha es:

34
 (ϕ j +ϕ1 ) (α j +φ1 )
Wb e + Zbe − Wb e ϕ1 − Z b eφ1 − Pj + P1 = 0

Al igual que para la díada izquierda en la díada derecha también se separan la parte real de
la imaginaria y se obtienen la siguientes ecuaciones de diseño:

Wb cos(ϕ j + ϕ 1 ) + Z b cos(α j + φ1 ) − Wb cos(ϕ 1 ) − Z b cos(φ1 ) − Pjx + P1x = 0

Wb sen(ϕ j + ϕ 1 ) + Z b sen(α j + φ1 ) − Wb sen(ϕ 1 ) − Z b sen(φ1 ) − Pjy + P1 y = 0

Por ejemplo, si se tuvieran que satisfacer tres puntos de precisión se tendría el siguiente
sistema de ecuaciones a resolver:

Díada izquierda:

Wa cos( β 2 + β 1 ) + Z a cos(α 2 + α 1 ) − Wa cos( β 1 ) − Z a cos(α 1 ) − P2 x + P1x = 0

Wa sen( β 2 + β 1 ) + Z a sen(α 2 + α 1 ) − Wa sen( β 1 ) − Z a sen(α 1 ) − P2 y + P1 y = 0

Wa cos( β 3 + β 1 ) + Z a cos(α 3 + α 1 ) − Wa cos( β 1 ) − Z a cos(α 1 ) − P3 x + P1x = 0

Wa sen( β 3 + β 1 ) + Z a sen(α 3 + α 1 ) − Wa sen( β 1 ) − Z a sen(α 1 ) − P3 y + P1 y = 0

De tal manera que conforme se incremente el número de puntos de precisión se verá

aumentado el número de ecuaciones.

Las ecuaciones correspondientes a la díada derecha son:

Wb cos(ϕ 2 + ϕ 1 ) + Z b cos(α 2 + φ1 ) − Wb cos(ϕ 1 ) − Z b cos(φ1 ) − P2 x + P1x = 0

35
 Wb sen(ϕ 2 + ϕ 1 ) + Z b sen(α 2 + φ1 ) − Wb sen(ϕ 1 ) − Z b sen(φ1 ) − P2 y + P1 y = 0

Wb cos(ϕ 3 + ϕ 1 ) + Z b cos(α 3 + φ1 ) − Wb cos(ϕ 1 ) − Z b cos(φ1 ) − P3 x + P1x = 0

Wb sen(ϕ 3 + ϕ 1 ) + Z b sen(α 3 + φ1 ) − Wb sen(ϕ 1 ) − Z b sen(φ1 ) − P3 y + P1 y = 0

Es importante mencionar que en la generación de trayectoria el ángulo prescrito es el

ángulo βj.

A manera de ejemplo se desea sintetizar un mecanismo de cuatro barras para generar

trayectoria, el cual debe satisfacer los siguientes puntos de precisión.
P1= (-8,11) P2= (-6,14)
P3 = (2,16) P4 = (8,15)
P5 = (12,11) P6 = (8,7)
β 2 = 160 β 3 = 140
β 4 = 120 β 5 = 100
β 6 = 80
valores iniciales (son arbitrarios):
Wa = 0.1 α1 = 0.2618 rad= 150
Za = 0.1 α2 = 0.3491 rad= 200
β1 = 0.1745 rad. = 100 α3 = 0.4363 rad= 250
α4 = 0.5236 rad=300
α5 = 0.6109 rad= 350
α6 = 0.9599 rad= 550

valores iniciales (son arbitrarios):

Wb = 0.1 ϕ1 = 0.2618 rad= 150 ϕ5 = 0.6109 rad= 350
Zb = 0.1 ϕ2 = 0.3491 rad= 200 ϕ6 = 0.9599 rad= 550
x =1 ϕ3 = 0.4363 rad= 250 φ1 = 0.1745 rad=100
y=1 ϕ4 = 0.5236 rad= 300

36
La síntesis dimensional para este mecanismo se puede realizar utilizando dos díadas
independientes, en el cual cada díada conectará un pivote de tierra.
Las ecuaciones que describen los desplazamientos de la díada izquierda son:

Wa cos( β 2 + β 1 ) + Z a cos(α 2 + α 1 ) − Wa cos( β 1 ) − Z a cos(α 1 ) − P2 x + P1x = 0

Wa sen( β 2 + β 1 ) + Z a sen(α 2 + α 1 ) − Wa sen( β 1 ) − Z a sen(α 1 ) − P2 y + P1 y = 0

Las ecuaciones que describen el movimiento para la díada derecha son:

Wb cos(ϕ 2 + ϕ 1 ) + Z b cos(α 2 + φ1 ) − Wb cos(ϕ 1 ) − Z b cos(φ1 ) − P2 x + P1x = 0

Wb sen(ϕ 2 + ϕ 1 ) + Z b sen(α 2 + φ1 ) − Wb sen(ϕ 1 ) − Z b sen(φ1 ) − P2 y + P1 y = 0

Al resolver los sistemas de ecuaciones para ambas díadas se obtienen las dimensiones para
los eslabones, así como sus respectivos ángulos para satisfacer cada uno de los puntos
prescritos, esos valores son los que se muestran en la tabla (2.2):

Tabla 2.2 Parámetros del mecanismo plano de cuatro eslabones para generar trayectoria

Posición β α wa za ϕ φ wb zb

1 -321.58 254.52 -17.532 -23.11 -159.26 31.176 13.193 16.624

2 160 -276.23 -231.52 -276.23

3 140 -302.68 131.688 -302.68

4 120 -325 128.952 -325

5 100 -342.83 122.292 -342.83

6 180 15.264 97.092 15.264

37
Las coordenadas de localización del pivote b son:
x = -9.7035
y = 6.5539

FIG. 2.8 Mecanismo plano de 4 eslabones

para generar trayectoria

2.2.3 GENERACIÓN DE MOVIMIENTO O CONDUCCIÓN DE CUERPO

RIGIDO.

En este tipo de tarea, lo que se requiere es que todo un cuerpo sea guiado a través de una
serie de puntos llamados puntos de precisión .
La síntesis dimensional para la conducción de un cuerpo rígido consiste en encontrar las
dimensiones del mecanismo, el cual con la ayuda de la barra acopladora, mueva un objeto
siguiendo una trayectoria deseada o prescrita.
La deducción de las ecuaciones de diseño que permitan determinar las dimensiones de los
eslabones son obtenidas de manera similar a como se hizo par obtener las ecuaciones para
la generación de trayectoria.

38
 FIG. 2.9 Mecanismo de cuatro eslabones para conducción de cuerpo rígido.

De tal manera que si se requiere sintetizar un mecanismo de cuatro barras (figura 2.9) para
conducción de cuerpo rígido su ecuación de diseño para la díada izquierda (figura 2.10) del
mecanismo quedará expresada de la siguiente manera:

( β j + β1 ) (θ j +θ1 )
Wa e + Zae − Wa e β1 − Z a eθ1 − Pj + P1 = 0

FIG. 2.10 Cierre de circuito diada izquierda para conducción de cuerpo rígido

39
Expresando esa ecuación en su forma rectangular y separando las componentes real e
imaginaria, se obtienen las siguientes ecuaciones de diseño:
Wa cos( β j + β 1 ) + Z a cos(θ j + θ 1 ) − Wa cos( β 1 ) − Z a cos(θ 1 ) − Pjx + P1x = 0

Wa sen( β j + β 1 ) + Z a sen(θ j + θ 1 ) − Wa sen( β 1 ) − Z a sen(θ 1 ) − Pjy + P1 y = 0

La ecuación de cierre de circuito para la díada derecha (figura 2.11) es:

FIG. 2.11 Cierre de circuito díada derecha para conducción de cuerpo rígido

(ϕ j +ϕ1 ) (θ j +φ 1 )
Wb e + Zbe − Wb e ϕ1 − Z b e φ1 − Pj + P1 = 0

Al igual que se hizo para la díada izquierda se separan la parte real de la imaginaria para
obtener:

Wb cos(ϕ j + ϕ 1 ) + Z b cos(θ j + φ1 ) − Wb cos(ϕ 1 ) − Z b cos(φ1 ) − Pjx + P1x = 0

Wb sen(ϕ j + ϕ 1 ) + Z b sen(θ j + φ1 ) − Wb sen(ϕ 1 ) − Z b sen(φ1 ) − Pjy + P1 y = 0

40
 Si se deseara sintetizar un mecanismo para satisfacer tres puntos de precisión
se tendría el siguiente sistema de ecuaciones:

Díada izquierda:

Wa cos( β 2 + β 1 ) + Z a cos(θ 2 + θ 1 ) − Wa cos( β 1 ) − Z a cos(θ 1 ) − P2 x + P1x = 0

Wa sen( β 2 + β 1 ) + Z a sen(θ 2 + θ 1 ) − Wa sen( β 1 ) − Z a sen(θ 1 ) − P2 y + P1 y = 0

Wa cos( β 3 + β 1 ) + Z a cos(θ 3 + θ 1 ) − Wa cos( β 1 ) − Z a cos(θ 1 ) − P3 x + P1x = 0

Wa sen( β 3 + β 1 ) + Z a sen(θ 3 + θ 1 ) − Wa sen( β 1 ) − Z a sen(θ 1 ) − P3 y + P1 y = 0

díada derecha:

Wb cos(ϕ 2 + ϕ 1 ) + Z b cos(θ 2 + φ1 ) − Wb cos(ϕ 1 ) − Z b cos(φ1 ) − P2 x + P1x = 0

Wb sen(ϕ 2 + ϕ 1 ) + Z b sen(θ 2 + φ1 ) − Wb sen(ϕ 1 ) − Z b sen(φ1 ) − P2 y + P1 y = 0

Wb cos(ϕ 3 + ϕ 1 ) + Z b cos(θ 3 + φ1 ) − Wb cos(ϕ 1 ) − Z b cos(φ1 ) − P3 x + P1x = 0

Wb sen(ϕ 3 + ϕ 1 ) + Z b sen(θ 3 + φ1 ) − Wb sen(ϕ 1 ) − Z b sen(φ1 ) − P3 y + P1 y = 0

En el estudio de la síntesis dimensional para la conducción de un cuerpo rígido, el ángulo

prescrito es el θ j .

41
2.3 SUMARIO
El tratar de adecuar un mecanismo para que produzca un movimiento deseado es un
problema al que se le conoce como síntesis cinemática del mecanismo.
La síntesis cinemática se clasifica en tres categorías:
Generación de función. La generación de función implica la coordinación de las
orientaciones angulares de los eslabones dentro de un mecanismo.
Un mecanismo comúnmente empleado es el de cuatro barras. Aquí, el problema de la
síntesis consiste en encontrar las dimensiones del mecanismo de eslabones articulados
requerido para producir una relación funcional específica entre el ángulo de entrada y el
ángulo de salida.
Generación de trayectoria: en este tipo de tarea se requiere un mecanismo para orientar un
punto (denominado punto trazador) a lo largo de una trayectoria específica. En este tipo de
tarea se debe coordinar el movimiento del punto a lo largo de su trayectoria con el
movimiento del eslabón de entrada.
En otras palabras, para valores específicos del ángulo de entrada se requiere que el punto
trazador esté en ubicaciones específicas a lo largo de su trayectoria.
Conducción de cuerpos: en esta tarea se requiere que todo un cuerpo sea guiado a través de
una secuencia de movimiento prescrita.

42
2.4 REFERENCIAS
[2.1] Robert L. Norton
Diseño de Maquinaria
Mc Graw-Hill, México 1995
[2.2] Arthur G. Erdman, George N. Sandor
Diseño de Mecanismos
Prentice Hall, México 1998
[2.3] Joseph Edward Shigley
Teoría de Máquinas y Mecanismos
McGraw-Hill México D.F. 1988
[2.4] Richard S. Hartenberg, Jacques Denavit
Kinematic Synthesis of Linkages
Mc Graw-Hill 1964
[2.5] C.H. Suh, C.W. Radeliffe
Kinematics and Mechanisms Design

43
CAPITULO

III

SÍNTESIS
PARA GENERACIÓN
DE TRAYECTORIA
DEL MECANISMO RSSR-SS

44
 SÍNTESIS PARA GENERACIÓN DE TRAYECTORIA DEL
MECANISMO RSSR-SS.

Los mecanismos se pueden clasificar de diversas maneras haciendo hincapié en sus

similitudes y diferencias. Uno de estos agrupamientos divide los mecanismos en planos y
espaciales.
Un mecanismo plano es aquel en el que todas sus partículas describen curvas planas en el
espacio y todas estas se encuentran en planos paralelos, en otras palabras, los lugares
geométricos de todos los puntos son curvas planas paralelas a un solo plano común.
Los mecanismos espaciales no incluyen restricción alguna en los movimientos relativos de
las partículas. La transformación del movimiento no es necesariamente coplanar.
En los mecanismos espaciales al igual que en los mecanismos planos, las tareas básicas de
la síntesis cinemática (generación de función, generación de trayectoria y guía de cuerpos)
siguen siendo las mismas.
En este capítulo se desarrollan las ecuaciones de diseño de la síntesis dimensional del
mecanismo espacial con pares cinemáticos RSSR-SS para generar una trayectoria prescrita.

3.1 MATRICES DE ROTACIÓN:

Una matriz de rotación se define como una matriz de transformación que opera sobre un
vector de posición y transforma sus coordenadas expresadas en un sistema de coordenadas
rotado OUVW a un sistema de coordenadas de referencia OXYZ [3.1].
Considerando que se tiene en el plano dos sistemas de referencia OXY y OUV con un
mismo origen O figura 3.1a, siendo el sistema OXY el de referencia (estático) y el sistema
OUV el móvil unido al objeto.
Los vectores unitarios de los ejes coordenados del sistema OXY son ix , jy, mientras que los
del sistema OUV son iu, jv .
Un vector p en el plano se puede representar en ambos sistemas como:

Pxy = [ px , py ]T =px.ix + py . jy ( 3.1 )

T
Puv = [ pu , pv] = pu.iu + pv . jv

45
 (a) (b)
FIG. 3.1 Orientación de un sistema OUV respecto a otro OXY en un plano

La matriz de rotación R que transforma las coordenadas de pu,v a las coordenadas

expresadas con respecto al sistemas de coordenadas OXY, después de que el sistema de
coordenadas OUV ha sido girado (figura 3.1b ) se obtiene de las siguientes expresiones
[3.2].

Px = ix . p = ix .iu .pu+ ix . jv .pv ( 3.2 )

Py = jy . p = jy .iu .pu+ jy . jv .pv

Expresadas en forma matricial se obtiene:

⎡ px ⎤ ⎡i x .iu i x . j v ⎤ ⎡ pu ⎤
⎢p ⎥ = ⎢ j .i j y . j v ⎥⎦ ⎢⎣ p v ⎥⎦
( 3.3 )
⎣ y⎦ ⎣ y u

donde :

⎡i x .iu i x . jv ⎤
R= ⎢
j y . j v ⎥⎦
(3. 4)
⎣ j y .iu

46
Es la matriz de rotación que define la orientación del sistema OUV con respecto al sistema
OXY, y que sirve para transformar las coordenadas de un vector de un sistema a las de el
otro. También recibe el nombre de matriz de cosenos directores.
En un espacio tridimensional, la matriz de rotación se obtiene de manera similar.

Px = ix . p = ix .iu.pu + ix .jv.pv + ix .kw.pw

Py = jy . p = jy .iu.pu + jy .jv.pv + jy .kw.pw ( 3.5 )
Pz = kz . p = kz .iu.pu + kz .jv.pv + kz .kw.pw

Expresadas en forma matricial:

⎡ p x ⎤ ⎡i x .iu i x . jv i x .k w ⎤ ⎡ pu ⎤
⎢ ⎥ ⎢ ⎥ ⎢p ⎥
⎢ p y ⎥ = ⎢ j y .iu j y . jv j y .k w ⎥ ⎢ v⎥ ( 3.6 )
⎢ p ⎥ ⎢k .i k z .k w ⎥⎦ ⎢⎣ p w ⎥⎦
⎣ z⎦ ⎣ z u k z . jv

La matriz de rotación es:

⎡i x .iu i x . jv i x .k w ⎤
⎢ ⎥
R = ⎢ j y .iu j y . jv j y .k w ⎥ ( 3.7 )
⎢k .i k z .k w ⎥⎦
⎣ z u k z . jv

El interés en obtener la matriz de rotación anterior, es encontrar las matrices de rotación

que representan rotaciones del sistema de coordenadas OUVW respecto a cada uno de los
ejes principales del sistema de coordenadas de referencia OXYZ. Si el sistema de
coordenadas OUVW se gira un ángulo α respecto al eje OX para llegar a una nueva
posición en el espacio, entonces el punto puvw, que tiene coordenadas ( pu, pv, pw)T con
respecto al sistema OUVW, tendrá coordenadas diferentes (px , py , pz)T con respecto al

47
sistema de referencia OXYZ [3.3]. La matriz de transformación Rα,x es la matriz de
rotación respecto al eje OX con ángulo α (figura 3.2), por lo que pxyz = Rα,x puvw

⎡i x .iu i x . jv i x .k w ⎤ ⎡1 0 0 ⎤
⎢ ⎥ ⎢0
R α ,x = ⎢ j y .iu j y . jv j y .k w ⎥ = ⎢ cos α − senα ⎥⎥ ( 3.8 )
⎢k .i k z .k w ⎥⎦ ⎢⎣0 senα cos α ⎥⎦
⎣ z u k z . jv

FIG. 3.2 Rotación del sistema uvw con respecto al eje OX

Análogamente, las matrices de rotación para rotaciones con respecto al eje OY figura 3.3a
con ángulo φ y respecto al eje OZ figura 3.3b un ángulo θ son:

⎡cos φ 0 senφ ⎤
Rφ,y = ⎢⎢0 1 0 ⎥
⎥ ( 3.9 )
⎢⎣− senφ 0 cos φ ⎥⎦

⎡cosθ − senθ 0⎤
Rθ,z = ⎢⎢ senθ cos θ 0⎥⎥ ( 3.10 )
⎢⎣0 0 1 ⎥⎦

48
 (a) (b)

FIG. 3.3 Rotación del sistema uvw con respecto a los ejes OY y OZ

Las matrices Rα,x , Rφ,y , Rθ,z se llaman matrices de rotación básicas. En la figura. 3.4 se
representa la rotación con respecto al eje z de un vector v1 el cual antes de la rotación tiene
componentes v1x , v1y , una vez realizada la rotación las componentes del vector v2 se
obtienen de la siguiente manera [3.4]:

v2x = v1x cosα - v1ysenα

v2y = v1x senα + v1ycosα
v2z = v1z

o escritas en forma matricial

49
 ⎡v 2 x ⎤ ⎡cos α − senα 0⎤ ⎡v1x ⎤
⎢ ⎥ ⎢ ⎢ ⎥
⎢v 2 y ⎥ = ⎢ senα cos α 0⎥⎥ ⎢v1 y ⎥
⎢v ⎥ ⎢⎣0 0 1 ⎥⎦ ⎢v ⎥
⎣ 2z ⎦ ⎣ 1z ⎦

FIG. 3..4 Rotación con respecto al eje z

3.2 MATRICES DE ROTACIÓN COMPUESTA.

Las matrices de rotación básicas se pueden multiplicar entre sí para representar una
secuencia de rotación finita respecto del eje principal del sistema de coordenadas OXYZ.
Como las multiplicaciones de matrices no conmutan, es importante el orden o secuencia de
realización de las rotaciones.
Por ejemplo, para desarrollar una matriz de rotación que represente una rotación de ángulo
α respecto del eje OX seguida por una rotación del ángulo θ respecto del eje OZ, seguida
por una rotación del ángulo φ respecto del eje OY, la matriz de rotación resultante que
representa estas rotaciones es:

R = Rφ,y , Rθ,z , Rα,x ( 3.11 )

⎡cos φ 0 senφ ⎤ ⎡cosθ − senθ 0⎤ ⎡1 0 0 ⎤

R = ⎢⎢0 1 0 ⎥
⎥
⎢ senθ
⎢ cosθ 0⎥⎥ ⎢0
⎢ cos α − senα ⎥⎥
⎢⎣− senφ 0 cos φ ⎥⎦ ⎢⎣0 0 1 ⎥⎦ ⎢⎣0 senα cos α ⎥⎦

50
 ⎡cφcθ sφsα − cφsθcα cφsθsα + sφcα ⎤
R= ⎢⎢ sθ cθcα − cθsα ⎥
⎥ ( 3.12 )
⎢⎣− sφcθ sφsθcα + cφsα cφcα − sφsθsα ⎥⎦

Donde: c φ = cos φ ; sφ = senφ ; cθ = cos θ ; sθ = senθ ; cα = cos α ; sα = senα

3.3 MATRIZ DE ROTACIÓN RESPECTO DE UN EJE ARBITRARIO.

Algunas veces el sistema de coordenadas rotante OUVW puede girar un ángulo φ respecto
de un eje arbitrario r, que es un vector unitario que tiene de componentes rx, ry, rz, y que pasa
a través del origen O. La ventaja es que para ciertos movimientos angulares del sistema
OUVW puede realizar una rotación alrededor del eje r en lugar de algunas rotaciones
respecto de los ejes principales de los sistemas de coordenadas OUVW y OXYZ [3.2].

51
Para obtener esta matriz de rotación Rφ,r se realizan primero algunas rotaciones respecto
de los ejes principales del sistema OXYZ para alinear el eje r con el eje OZ. Luego se
realiza la
rotación respecto de r con ángulo φ , se gira el eje principal del sistema OXYZ para volver
el eje r otra vez a su posición original.
Con respecto a la figura 3.5 el alineamiento del eje OZ con el eje r se puede hacer girando
respecto del eje OX un ángulo α (el eje r está en el plano XZ), seguido por una rotación de
ángulo − β respecto del eje OY (el eje r ahora se alinea con el eje OZ o eje r, invertir las
secuencias de rotaciones anteriores con sus respectivos ángulos opuestos. La matriz de
rotación resultante es:

Rφ,r = R-α,x Rβ,y Rφ,z R-β,y Rα,x ( 3.13 )

⎡1 0 0 ⎤ ⎡cβ 0 sβ ⎤ ⎡cφ − sφ 0⎤
Rφ,r= ⎢⎢0 cα sα ⎥⎥ ⎢0
⎢ 1 0 ⎥⎥ ⎢ sφ
⎢ cφ 0⎥⎥
⎢⎣0 − sα cα ⎥⎦ ⎢⎣− sβ 0 cβ ⎥⎦ ⎢⎣0 0 1 ⎥⎦

⎡cβ 0 − sβ ⎤ ⎡1 0 0 ⎤
x ⎢⎢0 1 0 ⎥⎥ ⎢0
⎢ cα − sα ⎥⎥
⎢⎣ sβ 0 cβ ⎥⎦ ⎢⎣0 sα cα ⎥⎦

pero se tienen las siguientes relaciones:

ry rz
senα = cos α =
ry2 + rz2 ry2 + rz2

senβ = rx cos β = ry2 + rz2

sustituyendo estas relaciones en las matrices de rotación anteriores y realizando las

multiplicaciones matriciales se obtiene la matriz de rotación siguiente:

52
 ⎡rx2Vφ + cφ rx ryVφ − rz sφ rx rzVφ + ry sφ ⎤
⎢ ⎥
Rφ ,r = ⎢rx ryVφ + rz sφ ry2Vφ + cφ ry rzVφ − rx sφ ⎥ ( 3.14 )
⎢r r Vφ − r sφ ry rzVφ + rx sφ ⎥
⎣x z y rx2Vφ + cφ ⎦

donde:

Vφ = versφ = 1 − cos φ
3.4. INTERPRETACIÓN GEOMÉTRICA DE LAS MATRICES DE
ROTACIÓN:

Para interpretar las matrices de rotación en forma geométrica se considerará un punto p fijo
en el sistema de coordenadas OUVW que sea (1,0,0)T , esto es, puvw = iu. Entonces la
primera columna de la matriz de rotación representa las coordenadas de este punto con
respecto al sistema de coordenadas OXYZ.
Análogamente, escogiendo p como (0,1,0)T y (0,0,1)T, se puede identificar que los
elementos de la segunda y tercera columna de una matriz de rotación representan los ejes
OV y OW , respectivamente, del sistema de coordenadas OUVW con respecto al sistema
de coordenadas OXYZ. Así, dado un sistema de referencia OXYZ y una matriz de rotación,
los vectores columna de la matriz de rotación representan los ejes principales del sistema de
coordenadas OUVW con respecto al sistema de referencia y se puede deducir la
localización de todos los ejes principales del sistema de coordenadas OUVW con respecto
al sistema de referencia. En otras palabras, una matriz de rotación geométricamente
representa los ejes principales del sistema de coordenadas rotado con respecto al sistema de
coordenadas de referencia.
Como la inversa de una matriz de rotación es equivalente a su traspuesta, los vectores fila
de la matriz de rotación representan los ejes principales del sistema de referencia OXYZ con
respecto al sistema de coordenadas rotado OUVW. Esta interpretación geométrica de las
matrices de rotación es un concepto importante que proporciona indicaciones en muchos

53
problemas cinemáticos del brazo del robot. Algunas propiedades de las matrices de rotación
son:
1. Cada vector columna de la matriz de rotación es una representación del vector
unitario del eje rotado expresado en términos de los vectores unitarios de los ejes
del sistema de referencia, y cada vector fila es una representación del vector unitario
de los ejes de referencia expresado en función de los vectores unitarios de los ejes
rotados del sistema OUVW.
2. Como cada fila y columna es una representación de un vector unitario, la magnitud
de cada una de ellas debería ser igual a 1. Esta es una propiedad directa de un
sistema de coordenadas ortonormal. Más aún, el determinante de una matriz de
rotación es 1 para un sistema de coordenadas dextrógiro y –1 para un sistema de
coordenadas levógiro.
3. Como cada fila es una representación vectorial de vectores ortonormales, el
producto interno (producto escalar) de cada fila por cualquier otra fila es igual a
cero. Análogamente, el producto interno de cada columna por cualquier otra
columna también es igual a cero.
4. La inversa de una matriz de rotación es la traspuesta de la matriz de rotación.

R-1 = RT y RRT = I3

Donde I3 es una matriz de identidad 3 × 3.

En un mecanismo espacial cada eslabón tendrá seis grados de libertad (tres traslaciones y
tres rotaciones). En consecuencia, la conexión de dos eslabones espaciales con una unión
que tenga un grado de libertad, como una unión de revoluta, tiene el efecto de quitarle cinco
grados de libertad. De manera similar, la conexión de dos eslabones con una unión de dos
grados de libertad tiene el efecto de quitarle cuatro grados de libertad, y así sucesivamente.
A un eslabón de un mecanismo espacial se le quitarán todos sus seis grados de libertad
cuando se encuentre fijo al piso o base.
La movilidad total de un sistema de n eslabones espaciales interconectados está dada por lo
tanto por la siguiente ecuación, conocida como la ecuación de Kutzbach.

54
 W = 6 ( n – 1 ) – 5 p1 – 4 p2 –3 p3 – 2 p4 – p5 ( 3.15 )
En donde:

W = movilidad o número de grados de libertad.

n = número total de eslabones, incluyendo el piso o base.
p1 = número de uniones de un grado de libertad
p2 = número de uniones de dos grados de libertad
p3 = número de uniones de tres grados de libertad
p4 = número de uniones de cuatro grados de libertad
p5 = número de uniones de cinco grados de libertad
En los mecanismos planos por lo común sólo se emplean cuatro tipos de uniones o pares:
1) la unión de revoluta.
2) La unión prismática
3) La unión de contacto rodante
4) La unión de leva o engrane.
En el caso de las uniones de revoluta, prismática y de contacto rodante, cada una tiene un
grado de libertad, en tanto que la unión de leva o engrane tiene dos grados de libertad. En
los mecanismos espaciales es posible tener muchos otros tipos de uniones. Entre los más
comunes se encuentran la unión de tornillo o helicoidal (un grado de libertad), la unión
cilíndrica (dos grados de libertad), la unión esférica o de rótula (tres grados de libertad) y
la unión de leva espacial (cinco grados de libertad).
Los mecanismos espaciales al igual que los mecanismos planos, realizan las mismas tareas
básicas (generación de función, generación de trayectoria y guía de cuerpos o conducción
de cuerpos rígidos).
Como ya se había mencionado anteriormente, en la síntesis para generar una trayectoria, un
punto de el eslabón acoplador debe trazar una trayectoria definida con respecto al marco de
referencia. Por lo que al sintetizar un mecanismo para realizar esta tarea, lo que se tiene
que hacer es desarrollar una ecuación que relacione los puntos de precisión con las
dimensiones de los eslabones.

3.5 SÍNTESIS DIMENSIONAL PARA EL MECANISMO CON PARES

CINEMÁTICOS RSSR-SS.

55
La síntesis cinemática es el proceso de diseñar un mecanismo para desempeñar una tarea
deseada. La síntesis cinemática tiene tres tareas usuales que son: generación de función,
generación de trayectoria y generación de movimiento, las cuales fueron descritas en el
capítulo II, aquí solo se sintetizará el mecanismo RSSR-SS figura 3.6 para generar
trayectoria. Este mecanismo consiste e cinco eslabones, uno fijo y cuatro en movimiento. El
mecanismo primario es el RSSR y está extensamente estudiado para generar función. Para
generar trayectoria y conducir un cuerpo rígido con la barra acopladora, se hizo necesario
resolver el problema del eslabón flotante, fué Suh el primero en proponer la topología
RSSR-SS para poder generar trayectoria.
La síntesis dimensional es parte de la síntesis cinemática y consiste en la determinación de
las longitudes (tamaños) de los eslabones que conforman un mecanismo para realizar los
movimientos deseados [3.5].
En la figura 3.6 se muestra el mecanismo con pares cinemáticos RSSR-SS, además se
muestra la ubicación de un punto arbitrario p que se mueve de la posición 1 a la posición j
representado por p1 y pj, estos puntos son llamados puntos de precisión o dicho de otra
manera son puntos que se quieren generar o satisfacer con el mecanismo analizado.

FIG. 3.6 Mecanismo espacial con pares RSSR-SS

La síntesis dimensional del mecanismo RSSR-SS será realizada analizando el mecanismo

por secciones; en estas secciones se relacionan los puntos de precisión con la magnitud de
los eslabones figura 3.7.

56
 FIG. 3.7 Secciones que relacionan los puntos de precisión
con los eslabones del mecanismo

De la figura 3.7 se analiza una sección que es la que se muestra en la figura 3.8 de la cual se
deduce la ecuación 3.16.

FIG. 3.8 Circuito que relaciona los puntos de precisión con el eslabón W1

(Wj) + (Zj) - W1-Z1 = pj – p1 (3.16)

en la cual Wj y Zj representan a los vectores W1 y Z1 rotados:

57
 Wj =Rβ W1
Zj = (R) Z1
Rβ es una matriz de rotación compuesta que va a permitir transformar las coordenadas del
vector W2 del sistema uvw al sistema xyz.

Rβ = ( Rγ 1 y )( Rλ1u )( Rβw ) (3.17)

R representa la matriz de rotación compuesta, resultado del producto de matrices de

rotación básicas con la siguiente secuencia: rotación del ángulo φ con respecto al eje z,
rotación de un ángulo θ con respecto al eje rotado u y la rotación del ángulo ϕ con respecto
al eje rotado w .

R = ( Rφ,z ) ( Rθ,u ) (Rϕ,w) ( 3.18 )

Z1= posición inicial del eslabón conductor

Zj = j-ésima posición del eslabón conductor
P1 = primer punto de la trayectoria a generar
Pj = j-ésima posición de la trayectoria a generar
Rφ , z = matriz de rotación respecto del eje z

Rθ ,u = matriz de rotación respecto del eje rotado u

Rϕ , w = matriz de rotación respecto del eje rotado w

expresando matricialmente Rβ = ( Rγ 1 y )( Rλ1u )( Rβw ) se obtiene:

⎡cos γ 1 0 senγ 1 ⎤ ⎡1 0 0 ⎤ ⎡cos β − senβ 0⎤

⎢ ⎥ ⎢ ⎥⎢ ⎥
Rβ = ⎢0 1 0 ⎥ ⎢0 cos λ1 − senλ1 ⎥ ⎢ senβ cos β 0⎥
⎢⎣− senγ 1 0 cos γ 1 ⎥⎦ ⎢⎣0 senλ1 cos λ1 ⎥⎦ ⎢⎣0 0 1 ⎥⎦

58
 ⎡cos γ 1 cos β + senγ 1senλ1senβ − senβ cos γ 1 + senγ 1senλ1 cos β senγ 1 cos λ1 ⎤
Rβ = ⎢⎢cos λ1senβ cos λ1 cos β − senλ1 ⎥
⎥
⎢⎣− senγ 1 cos β + cos γ 1senλ1senβ senγ 1senβ + cos γ 1senλ1 cos β cos γ 1 cos λ1 ⎥⎦

expresando la ecuación (3.16) en forma matricial y realizando la equivalencia anterior se

tiene:

⎡cos γ 1 cos β + senγ 1senλ1senβ − senβ cos γ 1 + senγ 1senλ1 cos β senγ 1 cos λ1 ⎤ ⎡W1u ⎤
⎢cos λ senβ cos λ1 cos β − senλ1 ⎥ ⎢W ⎥ +
⎢ 1 ⎥ ⎢ 1v ⎥
⎢⎣− senγ 1 cos β + cos γ 1senλ1senβ senγ 1senβ + cos γ 1senλ1 cos β cos γ 1 cos λ1 ⎥⎦ ⎢⎣W1w ⎥⎦

⎡cos φ cos ϕ − senφ cosθsenϕ − cos φsenϕ − senφ cosθ cos ϕ senφsenθ ⎤ ⎡ Z1x ⎤
⎢ senφ cos ϕ + cos φ cosθsenϕ ⎢ ⎥
⎢ − senφsenϕ + cos φ cosθ cos ϕ − cos φsenθ ⎥⎥ ⎢ Z1 y ⎥ -
⎢⎣ senθsenϕ senθ cos ϕ cosθ ⎥⎦ ⎢Z ⎥
⎣ 1z ⎦

⎡cos γ 1 0 senγ 1 ⎤ ⎡1 0 0 ⎤ ⎡W1u ⎤ ⎡ Z1x ⎤ ⎡ p jx − p1x ⎤

⎢0 ⎥ ⎢0 cos λ − senλ ⎥ ⎢W ⎥ - ⎢ Z ⎥ = ⎢ p − p ⎥
⎢ 1 0 ⎥⎢ 1 1 ⎥ ⎢ 1v ⎥ ⎢ 1 y ⎥ ⎢ jy 1y ⎥

⎢⎣− senγ 1 0 cos γ 1 ⎥⎦ ⎢⎣0 senλ1 cos λ1 ⎥⎦ ⎢⎣W1w ⎥⎦ ⎢⎣ Z1z ⎥⎦ ⎢ p jz − p1z ⎥

⎣ ⎦

Debido a que el vector W1 se mueve en el plano uv su componente W1w = 0, desarrollando

las operaciones matriciales indicadas se obtiene el siguiente sistema de ecuaciones:

W1u (cos γ 1 cos β + senγ 1senλ1senβ ) + W1v (− senβ cos γ 1 + senγ 1senλ1 cos β )
+ W1w ( senγ 1 cos λ1 ) + ( Z1x )(cosφ cos ϕ − senφ cosθsenϕ )
(3.19)
+ ( Z1 y )(− cos φsenϕ − senφ cosθ cos ϕ )
+ ( Z1z )( senφsenθ ) − (W1u (cos γ 1 ) + W1v ( senγ 1senλ1 )) − Z1x = p jx − p1x

W1u (cos λ1senβ ) + W1v (cos λ1 cos β ) + ( Z1x )( senφ cosϕ + cosφ cosθsenϕ )
+ ( Z1 y )(− senφsenϕ + cosφ cosθ cosϕ ) + ( Z1z )(− cosφsenθ ) − W1v (cos λ1 ) − Z1 y = p jy − p1 y

W1u (− senγ 1 cos β + cos γ 1senλ1senβ ) + W1v ( senγ 1senβ + cos γ 1senλ1 cos β )
+ ( Z1x )( senθsenϕ ) + ( Z1 y )( senθ cos ϕ )
+ ( Z1z ) cosθ − (W1u (− senγ 1 ) + W1v (cos γ 1senλ1 )) − Z1z = p jz − p1z

59
 FIG. 3.9 localización del pivote del eslabón W1

De la figura 3.9 se deduce la siguiente expresión para la posición de inicio del mecanismo:

⎡ Z 2⎤ ⎡cos γ 1 0 senγ 1 ⎤ ⎡1 0 0 ⎤ ⎡W1u ⎤ ⎡ Z1x ⎤ ⎡ p1x ⎤

⎢ Z 3 ⎥ + ⎢0 ⎥ ⎢ ⎥⎢ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ 1 0 ⎥ ⎢0 cos λ1 − senλ1 ⎥ ⎢W1v ⎥ + ⎢ Z1 y ⎥ - ⎢ p1 y ⎥
⎢⎣ Z 4⎥⎦ ⎢⎣− senγ 1 0 cos γ 1 ⎥⎦ ⎢⎣0 senλ1 cos λ1 ⎥⎦ ⎢⎣W1w ⎥⎦ ⎢⎣ Z1z ⎥⎦ ⎢⎣ p1z ⎥⎦

Z2+(W1u(cosγ1)+W1v(sinγ1sinλ1))+Z1x - p1x = 0
Z3+W1v(cosλ1)+Z1y-p1y = 0 (3.20)
Z4+(W1u(-sinγ1)+W1v(cosγ1sinλ1))+Z1z - p1z = 0

Por lo tanto para dos puntos de precisión se obtendrá un sistema de seis ecuaciones, tres
generadas por el sistema (3.19) y tres por el (3.20) el número de ecuaciones se incrementará
dependiendo del número de puntos de precisión que se quieran satisfacer .
Para obtener la dimensión del eslabón W2 se relacionan los puntos de precisión que se
desean satisfacer con el eslabón W2 como se muestra en la figura 3.10 , la expresión que se
obtiene es la siguiente:

W2j+ Z2j – W2 – Z2 = pj – p1 (3.21)

W2j es el resultado de rotar W2 con respecto al eje w en el sistema uvw.
Z2j es el resultado de rotar Z2 en el sistema xyz.
Por lo que la expresión (3.21 ) puede escribirse de la siguiente manera:

Rα (W2) + R (Z2) –W2–Z2= pj –p1 (3. 22)

60
 FIG. 3.10 Circuito para relacionar los puntos
de precisión con el eslabón W2

Rα es una matriz de rotación compuesta que va a permitir transformar las coordenadas del
vector W2 del sistema uvw al sistema xyz.

FIG. 3.11 Representación del sistema de coordenadas uvw

En la figura 3.11 se puede observar que el sistema de coordenadas uvw está rotado con
respecto al sistema xyz, por lo tanto, la matriz de rotación que permite pasar las
coordenadas del vector W2 de un sistema a otro resulta de una rotación de un ángulo γ con
respecto al eje y seguida de una rotación de un ángulo λ con respecto al eje u rotado, pero
el vector W2 va a estar rotando en el sistema uvw por lo que se tiene que agregar una
rotación de un ángulo α con respecto al eje w figura 3.12 por lo que :

Rα = (Rγ,y)(Rλ,u)(Rα,w) (3.23)

61
 FIG. 3.12 Rotación del sistema uvw respecto al sistema xyz

Expresando ( 3.23 ) de manera matricial se obtiene:

⎡cos γ 0 senγ ⎤ ⎡1 0 0 ⎤ ⎡cos α − senα 0⎤
Rα = ⎢⎢0 1 0 ⎥
⎥
⎢0
⎢ cos λ − senλ ⎥⎥ ⎢ senα
⎢ cos α 0⎥⎥
⎢⎣− senγ 0 cos γ ⎥⎦ ⎢⎣0 senλ cos λ ⎥⎦ ⎢⎣0 0 1 ⎥⎦

⎡cos γ cos α + senγsenλsenα − senα cos γ + senγsenλ cos α senγ cos λ ⎤

⎢
Rα = ⎢cos λsenα cos λ cos α − senλ ⎥
⎥
⎢⎣− senγ cos α + cos γsenλsenα senγsenα + cos γsenλ cos α cos γ cos λ ⎥⎦

⎡cos γ cosα + senγsenλsenα − senα cos γ + senγsenλ cos α senγ cos λ ⎤ ⎡W2u ⎤
⎢cos λsenα cos λ cos α − senλ ⎥ ⎢W ⎥
⎢ ⎥ ⎢ 2v ⎥
⎢⎣− senγ cosα + cos γsenλsenα senγsenα + cos γsenλ cos α cos γ cos λ ⎥⎦ ⎢⎣W2 w ⎥⎦

en este caso la coordenada correspondiente a W2w es cero debido a que en ese eje el vector
W2 no tiene componente ya que rota en el plano uv.

Escribiendo (3.21) de manera matricial:

⎡cos γ cos α + senγsenλsenα − senα cos γ + senγsenλ cos α senγ cos λ ⎤ ⎡W2u ⎤
⎢cos λsenα cos λ cos α − senλ ⎥ ⎢W ⎥ +
⎢ ⎥ ⎢ 2v ⎥
⎢⎣− senγ cos α + cos γsenλsenα senγsenα + cos γsenλ cos α cos γ cos λ ⎥⎦ ⎢⎣W2 w ⎥⎦

62
⎡cos φ cos ϕ − senφ cosθsenϕ − cos φsenϕ − senφ cosθ cos ϕ senφsenθ ⎤ ⎡Z 2 x ⎤
⎢ senφ cos ϕ + cos φ cosθsenϕ − senφsenϕ + cos φ cosθ cos ϕ ⎢ ⎥
⎢ − cos φsenθ ⎥⎥ ⎢Z 2 y ⎥ -
⎢⎣ senθsenϕ senθ cos ϕ cosθ ⎥⎦ ⎢Z ⎥
⎣ 2z ⎦

⎡cos γ 0 senγ ⎤ ⎡1 0 0 ⎤ ⎡W2u ⎤ ⎡ Z 2 x ⎤ ⎡ p jx − p1x ⎤

⎢0 ⎥ ⎢0 cos λ − senλ ⎥ ⎢W ⎥ - Z ⎢ ⎥ ⎢ ⎥
⎢ 1 0 ⎥ ⎢ ⎥ ⎢ 2 v ⎥ ⎢ 2 y ⎥ = ⎢ p jy − p1 y ⎥
⎢⎣− senγ 0 cos γ ⎥⎦ ⎢⎣0 senλ cos λ ⎥⎦ ⎢⎣W2 w ⎥⎦ ⎢⎣ Z 2 z ⎥⎦ ⎢p − p ⎥
⎣ jz 1z ⎦

Al realizar las operaciones matriciales que se indican se obtiene el sistema de tres

ecuaciones siguiente:

W2u (cos γ cosα + senγsenλsenα ) + W2v (− senα cos γ + senγsenλ cosα )

+ ( Z 2 x )(cosφ cos ϕ − senφ cosθsenϕ )
+ ( Z 2 y )(− cos φsenϕ − senφ cosθ cos ϕ )
+ ( Z 2 z )( senφsenθ ) − (W2u (cos γ ) + W2v ( senγsenλ )) − Z 2 x = p jx − p1x

(3.24)
W2u (cos λsenα ) + W2 v (cos λ cos α )
+ ( Z 2 x )( senφ cos ϕ + cos φ cosθsenϕ )
+ ( Z 2 y )( − senφsenϕ + cos φ cosθ cos ϕ ) + ( Z 2 z )( − cos φsenθ ) − W2 v cos λ − Z 2 y = p jy − p1 y

W2u (− senγ cosα + cos γsenλsenα ) + W2 v ( senγsenα + cos γsenλ cosα )

+ ( Z 2 x )( senθsenϕ )
+ ( Z 2 y )( senθ cos ϕ ) + ( Z 2 z ) cosθ − (W2u (− senγ ) + W2v (cos γsenλ )) − Z 2 z = p jz − p1z

63
 FIG. 3.13 cadena cerrada para relacionar W1 y W2

De la figura 3.13 se obtiene la siguiente expresión:

⎡ Z 2⎤ ⎡cos γ 1 0 senγ 1 ⎤ ⎡1 0 0 ⎤ ⎡W1u ⎤ ⎡ Z1x ⎤ ⎡cos γ 0 senγ ⎤ ⎡1 0

⎢ Z 3 ⎥ + ⎢0 1 0 ⎥ ⎢ 0 cos λ − senλ ⎥ ⎢W ⎥ + ⎢ Z ⎥ = ⎢0 1 0 ⎥ ⎢0 cos λ
⎢ ⎥ ⎢ ⎥⎢ 1 1 ⎥ ⎢ 1v ⎥ ⎢ 1 y ⎥ ⎢ ⎥ ⎢
⎢⎣ Z 4⎥⎦ ⎢⎣− senγ 1 0 cos γ 1 ⎥⎦ ⎢⎣0 senλ1 cos λ1 ⎥⎦ ⎢⎣W1w ⎥⎦ ⎢⎣ Z1z ⎥⎦ ⎢⎣− senγ 0 cos γ ⎥⎦ ⎢⎣0 senλ

0 ⎤
− senλ ⎥⎥
cos λ ⎦⎥

⎡W2u ⎤ ⎡ Z 2 x ⎤ ⎡ Z 3 x ⎤
⎢W ⎥ + ⎢ Z ⎥ + ⎢ Z 3 y ⎥
⎢ 2v ⎥ ⎢ 2 y ⎥ ⎢ ⎥
⎢⎣W2 w ⎥⎦ ⎢⎣ Z 2 z ⎥⎦ ⎢⎣ Z 3 z ⎥⎦

Z 2 + W1u (cos γ 1 ) + W1v ( senγ 1senλ1 ) + Z1x = W2u (cos γ 1 ) + W2 v ( senγ 1senλ1 ) + Z 2 x + Z 3 x

Z 3 + W1v (cos λ1 ) + z1z = W2v (cos λ ) + Z 2 z + Z 3 y

(3.25)
Z 4 + W1u (− senγ 1 ) + W1v (cos γ 1senλ1 ) = W2u (− senγ ) + W2v (cos γsenλ ) + Z 2 x + Z 3z

64
Para dos puntos de precisión se tiene un sistema de seis ecuaciones; como ya se mencionó
el número de ecuaciones se incrementará dependiendo del número de puntos de precisión
que se quieran satisfacer .
La siguiente sección por analizar es la que contiene al eslabón C1.

FIG. 3.14 Cadena cerrada para relacionar el eslabón C1 con el

eslabón W1

De la figura 3.14 se obtiene la expresión que relaciona al eslabón C1 con el eslabón de

entrada W1 (cadena cerrada) :
Z2+ ( Rγ 1 y )( Rλ1u )( Rβw ) W1 + ( Rφ,z ) ( Rθ,u )(Rϕ,w ) (Z1) =
( Rφ,z ) ( Rθ,u )(Rϕ,w ) Z5+ (Rµ,z )(Rρ,u )(Rη,w )C1 +C0 (3. 26)

De la figura 3.15 se obtiene la expresión (3.27) que permite relacionar al eslabón C1 con los
puntos de precisión:

FIG. 3.15 Circuito para relacionar el eslabón C1 con los

puntos de precisión

65
 (Rµ,z )(Rρ,u )(Rη,w ) (C1 )+ ( Rφ,z ) ( Rθ,u )(Rϕ,w ) (Z5 )-Z5-C1 =pj –p1 (3.27)

Escribiendo 3.26 en forma matricial:

⎡ Z 2⎤ ⎡cos γ 1 cos β + senγ 1senλ1senβ − senβ cos γ 1 + senγ 1senλ1 cos β senγ 1 cos λ1 ⎤
⎢ Z 3 ⎥ + ⎢cos λ senβ cos λ1 cos β − senλ1 ⎥
⎢ ⎥ ⎢ 1 ⎥
⎢⎣ Z 4⎥⎦ ⎢⎣− senγ 1 cos β + cos γ 1senλ1senβ senγ 1senβ + cos γ 1senλ1 cos β cos γ 1 cos λ1 ⎥⎦
⎡W1u ⎤
⎢W ⎥
⎢ 1v ⎥
⎢⎣W1w ⎥⎦
⎡cos φ cos ϕ − senφ cosθsenϕ − cos φsenϕ − senφ cosθ cos ϕ senφsenθ ⎤ ⎡ Z1x ⎤
⎢ senφ cos ϕ + cos φ cosθsenϕ − senφsenϕ + cos φ cosθ cos ϕ − cos φsenθ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ Z1 y ⎥ =
⎢⎣ senθsenϕ senθ cos ϕ cosθ ⎥⎦ ⎢Z ⎥
⎣ 1z ⎦
⎡cos φ cos ϕ − senφ cosθsenϕ − cos φsenϕ − senφ cosθ cos ϕ senφsenθ ⎤ ⎡Z5 x ⎤
⎢ senφ cos ϕ + cos φ cosθsenϕ − senφsenϕ + cos φ cosθ cos ϕ − cos φsenθ ⎥ ⎢ ⎥
⎢ ⎥ ⎢Z5 y ⎥
⎢⎣ senθsenϕ senθ cos ϕ cosθ ⎥⎦ ⎢Z ⎥
⎣ 5z ⎦
⎡cos µ cosη − senµ cos ρsenη − cos µsenη − senµ cos ρ cosη senµsenρ ⎤ ⎡C1x ⎤
⎢ senµ cosη + cos µ cos ρsenη − senµsenη + cos µ cos ρ cosη − cos µsenρ ⎥ ⎢C ⎥
⎢ ⎥ ⎢ 1y ⎥
⎢⎣ senρsenη senρ cosη cos ρ ⎥⎦ ⎢⎣C1z ⎥⎦

⎡C0 x ⎤
⎢ ⎥
+ ⎢C0 y ⎥
⎢C ⎥
⎣ 0z ⎦

Z 2 + W1u (cos γ 1 cos β + senγ 1senλ1senβ ) + W1v (− senβ cos γ 1 + senγ 1senλ1 cos β )
+ ( Z1x )(cosφ cos ϕ − senφ cosθsenϕ )
+ ( Z1 y )(− cos φsenϕ − senφ cosθ cos ϕ ) + ( Z1z )( senφsenθ ) =
( Z 5 x )(cosφ cos ϕ − senφ cosθsenϕ ) + ( Z 5 y )(− cos φsenϕ − senφ cosθ cos ϕ )
+ ( Z 5 z )( senφsenθ ) + C1x (cos µ cosη − senµ cos ρsenη ) +
C1 y (− cos µsenη − senµ cos ρ cosη ) + C1z ( senµsenρ ) + C0 x
(3.28)

66
Z 3 + W1u (cos λ1senβ ) + W1v (cos λ1 cos β ) + ( Z1x )( senφ cos ϕ + cos φ cosθsenϕ ) +
( Z1 y )(− senφsenϕ + cos φ cosθ cos ϕ ) + ( Z1z )(− cos φsenθ ) = ( Z 5 x )( senφ cos ϕ + cos φ cosθsenϕ ) +
( Z 5 y )(− senφsenϕ + cos φ cosθ cos ϕ ) + ( Z 5 z )(− cos φsenθ ) + C1x ( senµ cosη + cos µ cos ρsenη )
+ C1 y (− senµsenη + cos µ cos ρ cosη ) + C1z (− cos µsenρ ) + C0 y

Z 4 + W1u (− senγ 1 cos β + cos γ 1senλ1senβ ) + W1v ( senγ 1senβ + cos γ 1senλ1 cos β )
+ ( Z1x )( senθsenϕ ) + ( Z1 y )( senθ cos ϕ ) + ( Z1z )(cosθ ) =
( Z 5 x )( senθsenϕ ) + ( Z 5 y )( senθ cos ϕ ) + ( Z 5 z )(cosθ )
+ C1x ( senρsenη ) + C1 y ( senρ cosη ) + C1z (cos ρ ) + C0 z
Escribiendo 3.27 de manera matricial:

⎡cos µ cosη − senµ cos ρsenη − cos µsenη − senµ cos ρ cosη senµsenρ ⎤ ⎡C1x ⎤
⎢ senµ cosη + cos µ cos ρsenη − senµsenη + cos µ cos ρ cosη − cos µsenρ ⎥ ⎢ ⎥
⎢ ⎥ ⎢C1 y ⎥ +
⎢⎣ senρsenη senρ cosη cos ρ ⎥⎦ ⎢C ⎥
⎣ 1z ⎦

⎡cos φ cos ϕ − senφ cosθsenϕ − cos φsenϕ − senφ cosθ cos ϕ senφsenθ ⎤ ⎡Z5 x ⎤ ⎡Z5 x ⎤
⎢ senφ cos ϕ + cos φ cosθsenϕ − senφsenϕ + cos φ cosθ cos ϕ − cos φsenθ ⎥ ⎢ ⎥ ⎢ ⎥
⎢ ⎥ ⎢ z5 y ⎥ - ⎢ z5 y ⎥ -
⎢⎣ senθsenϕ senθ cos ϕ cosθ ⎥⎦ ⎢Z ⎥ ⎢Z ⎥
⎣ 5z ⎦ ⎣ 5z ⎦

⎡C1x ⎤ ⎡ p jx − p1x ⎤
⎢ ⎥ ⎢ ⎥
⎢C1 y ⎥ = ⎢ p jy − p1 y ⎥
⎢C ⎥ ⎢ p − p ⎥
⎣ 1z ⎦ ⎣ jz 1z ⎦

C1x (cos µ cosη − senµ cos ρsenη ) + C1 y (− cos µsenη − senµ cos ρ cosη )
+ C1z ( senµsenρ ) + ( Z 5 x )(cosφ cos ϕ − senφ cosθsenϕ )
+ ( Z 5 y )(− cos φsenϕ − senφ cosθ cos ϕ )
+ ( Z 5 z )( senφsenθ ) − Z 5 x − C1x = p jx − p1x

C1x ( senµ cosη + cos µ cos ρsenη ) + C1 y (− senµsenη + cos µ cos ρ cosη )
+ C1z (− cos µsenρ ) + ( Z 5 x )( senφ cos ϕ + cos φ cosθsenϕ ) (3.29)
+ ( Z 5 y )(− senφsenϕ + cos φ cosθ cos ϕ ) + ( Z 5 z )(− cos φsenθ ) − Z 5 y − C1 y = p jy − p1 y

C1x ( senρsenη ) − C1 y ( senρ cosη ) − C1z cos ρ − ( Z 5 x )( senρsenη )

− ( Z 5 y )( senρ cosη ) − ( Z 5 z )(cos ρ ) − Z 5 z − C1z = p jz − p1z

67
Los puntos de precisión a satisfacer con el mecanismo RSSR-SS son puntos tomados de un
elipsoide:

PUNTOS DE PRECISION:
PUNTO Px Py Pz
1 10 8 27.1293
2 11 10 26.0576
3 12 11 25.1992
4 13 12 24.2281
5 14 13 23.1301
6 15 14 21.8861

La síntesis dimensional del mecanismo con topología RSSR-SS figura 3.6 se realiza
resolviendo primeramente los sistemas de ecuaciones (3.19) y (3.20), con lo que se
determinarán las magnitudes W1 , Z1, W2, Z2 estos sistemas de ecuaciones son los
siguientes:

W1u (cos γ 1 cos β + senγ 1senλ1senβ ) + W1v (− senβ cos γ 1 + senγ 1senλ1 cos β )
+ ( Z1x )(cosφ cos ϕ − senφ cosθsenϕ )
(3.30)
+ ( Z1 y )(− cos φsenϕ − senφ cosθ cos ϕ )
+ ( Z1z )( senφsenθ ) − (W1u (cos γ 1 ) + W1v ( senγ 1senλ1 )) − Z1x = p jx − p1x

W1u (cos λ1senβ ) + W1v (cos λ1 cos β )

+ ( Z1x )( senφ cos ϕ + cos φ cosθsenϕ )
+ ( Z1 y )(− senφsenϕ + cos φ cosθ cos ϕ ) + ( Z1z )(− cos φsenθ ) − W1v (cos λ1 ) − Z1z = p jy − p1 y

W1u (− senγ 1 cos β + cos γ 1senλ1senβ ) + W1v ( senγ 1senβ + cos γ 1senλ1 cos β )
+ ( Z1x )( senθsenϕ )
+ ( Z1 y )( senθ cos ϕ ) + ( Z1z ) cosθ − (W1u (− senγ 1 ) + W1v (cos γ 1senλ1 )) − Z1z = p jz − p1z

Z2+(W1u(cosγ1)+W1v(sinγ1sinλ1))+Z1x - p1x

68
Z3+W1v(cosλ1)+Z1y-p1y
Z4+(W1u(-sinγ1)+W1v(cosγ1sinλ1))+Z1z - p1z
El sistema de ecuaciones (3.30) se verá incrementado dependiendo del numero de puntos de
precisión.
En este caso se resolverá para seis puntos de precisión, por lo que se originará un sistema
de dieciocho ecuaciones con diez incógnitas; estás incógnitas serán W1u,W1v, Z1x Z1y, Z1z,
Z2,Z3,Z4, α2, α3, α4, α5, α6,

Los valores iniciales y valores prescritos son los siguientes:

Los valores iniciales son arbitrarios y son utilizados para iniciar el proceso de cálculo por el
método de Newton-Raphson:

W 1u = 0.01 W 1v= 0.01 Z1x = 0.01 Z1y = 0.01 Z1z = 0.01 Z2 = 0.01 Z3=0.01
Z4=0.01 γ1=100 λ1=150

valores prescritos:

φ2 = 600 θ2 = 300 ϕ2 = 700 β2 = 350

φ3 = 700 θ3 = 500 ϕ3 = 900 β3 = 650
φ4 = 1000 θ4 = 800 ϕ4 = 1200 β4 = 950
φ5 = 1200 θ5 = 1150 ϕ5 = 1400 β5 = 1250
φ6 = 1300 θ6 = 1250 ϕ6 = 1500 β6 = 1550

Los resultados que se obtienen de la resolución de este sistema de ecuaciones son:

W 1u = -1.4689 Z1x = 1.2409 Z1z = 2.986 Z3 = 11.2006 γ1=-310.70

W 1v= -1.4745 Z1y = -1.8844 Z2 = 10.2209 Z4 = 23.4632 λ1=270

Al igual que en el cálculo de W1 y Z1 el procedimiento utilizado para calcular W2 y Z2 se

desarrolla considerando dos secciones del mecanismo figura 3.10 y figura 3.13 de las
cuales se dedujeron los sistemas de ecuaciones (3.24) y (3.25) que son los siguientes:

69
W2u (cos γ cosα + senγsenλsenα ) + W2 v (− senα cos γ + senγsenλ cosα )
+ ( Z 2 x )(cosφ cos ϕ − senφ cosθsenϕ ) + ( Z 2 y )(− cos φsenϕ − senφ cosθ cos ϕ )
+ ( Z 2 z )( senφsenθ ) − (W2u (cos γ ) + W2 v ( senγsenλ )) − Z 2 x = p jx − p1x

W2u (cos λsenα ) + W2v (cos λ cosα ) + ( Z 2 x )( senφ cosϕ + cosφ cosθsenϕ )
+ ( Z 2 y )(− senφsenϕ + cosφ cosθ cosϕ ) + ( Z 2 z )(− cosφsenθ ) − W2v cos λ − Z 2 y = p jy − p1 y

W2u (− senγ cosα + cos γsenλsenα ) + W2 v ( senγsenα + cos γsenλ cosα )

+ ( Z 2 x )( senθsenϕ ) (3.31)
+ ( Z 2 y )( senθ cos ϕ ) + ( Z 2 z ) cosθ − (W2u (− senγ ) + W2v (cos γsenλ )) − Z 2 z = p jz − p1z

Z 2 + W1u (cos γ 1 ) + W1 y ( senγ 1senλ1 ) + Z1x = W2u (cos γ 1 ) + W2v ( senγ 1senλ1 ) + Z 2 x + Z 3x
Z 3 + W1v (cos λ1 ) + z1z = W2v (cos λ ) + Z 2 z + Z 3 y
Z 4 + W1u (− senγ 1 ) + W1v (cos γ 1senλ1 ) = W2u (− senγ ) + W2v (cos γsenλ ) + Z 2 x + Z 3z
En este caso Z2,Z3,Z4,W1u,W1v,γ1,λ1, son conocidos del cálculo anterior por lo que los
valores iniciales y los prescritos son:
Valores iniciales:
W 2u = 0.01 W 2v= 0.01 Z2x = 0.01 Z2y = 0.01 Z2z = 0.01 Z3x = 0.04 Z3y = 0.03
Z3z = 0.05
α2= 13.530 γ= 1.0472 rad= 600
α3= 18.540 λ= .6981 rad= 400
α4= 23.540
α5= 43.530
α6= 53.540
valores prescritos:
φ2 = 600 θ2 = 300 ϕ2 = 700 β2 = 350
φ3 = 700 θ3 = 500 ϕ3 = 900 β3 = 650
φ4 = 1000 θ4 = 800 ϕ4 = 1200 β4 = 950
φ5 = 1200 θ5 = 1150 ϕ5 = 1400 β5 = 1250
φ6 = 1300 θ6 = 1250 ϕ6 = 1500 β6 = 1550
Los resultados que se obtienen de la resolución de este sistema de ecuaciones son:

70
W 2u = .9017 Z2x = 1.3197 Z2y = -3.1508 Z2z = 7.4053 γ=129.740
W 1v= 2.2336 Z3x = 8.1156 Z3y = 9.4815 Z3z = 21.3661 λ=-318.340
α2= 4.8337 rad. = 276.950
α3= 5.129 rad. = 293.870
α4= 4.6703 rad. = 267.58850
α5= 3.0605 rad. = 175.35370
α6= 2.3983 rad. = 137.41250

El procedimiento utilizado para calcular C1 y Z5 se desarrolla considerando dos secciones

del mecanismo figura 3.14 y figura 3.15 de las cuales se dedujeron los sistemas de
ecuaciones (3.28) y (3.29) que son los siguientes.

Z 2 + W1u (cos γ 1 cos β + senγ 1senλ1senβ ) + W1v (− senβ cos γ 1 + senγ 1senλ1 cos β )
+ ( Z1x )(cosφ cos ϕ − senφ cosθsenϕ )
+ ( Z1 y )(− cos φsenϕ − senφ cosθ cos ϕ ) + ( Z1z )( senφsenθ ) =
( Z 5 x )(cosφ cos ϕ − senφ cosθsenϕ ) + ( Z 5 y )(− cos φsenϕ − senφ cosθ cos ϕ )
+ ( Z 5 z )( senφsenθ ) + C1x (cos µ cosη − senµ cos ρsenη ) +
C1 y (− cos µsenη − senµ cos ρ cosη ) + C1z ( senµsenρ ) + C0 x

Z 3 + W1u (cos λ1senβ ) + W1v (cos λ1 cos β ) + ( Z1x )( senφ cos ϕ + cos φ cosθsenϕ ) +
( Z1 y )(− senφsenϕ + cos φ cosθ cos ϕ ) + ( Z1z )(− cos φsenθ ) = ( Z 5 x )( senφ cos ϕ + cos φ cosθsenϕ ) +
( Z 5 y )(− senφsenϕ + cos φ cosθ cos ϕ ) + ( Z 5 z )(− cos φsenθ ) + C1x ( senµ cosη + cos µ cos ρsenη )
+ C1 y (− senµsenη + cos µ cos ρ cosη ) + C1z (− cos µsenρ ) + C0 y

Z 4 + W1u (− senγ 1 cos β + cos γ 1senλ1senβ ) + W1v ( senγ 1senβ + cos γ 1senλ1 cos β )
+ ( Z1x )( senθsenϕ ) + ( Z1 y )( senθ cos ϕ ) + ( Z1z )(cosθ ) =
(3.32)
( Z 5 x )( senθsenϕ ) + ( Z 5 y )( senθ cos ϕ ) + ( Z 5 z )(cosθ )
+ C1x ( senρsenη ) + C1 y ( senρ cosη ) + C1z (cos ρ ) + C0 z

71
C1x (cos µ cosη − senµ cos ρsenη ) + C1 y (− cos µsenη − senµ cos ρ cosη )
+ C1z ( senµsenρ ) + ( Z 5 x )(cosφ cos ϕ − senφ cosθsenϕ )
+ ( Z 5 y )(− cos φsenϕ − senφ cosθ cos ϕ )
+ ( Z 5 z )( senφsenθ ) − Z 5 x − C1x = p jx − p1x

C1x ( senµ cosη + cos µ cos ρsenη ) + C1 y (− senµsenη + cos µ cos ρ cosη )
+ C1z (− cos µsenρ ) + ( Z 5 x )( senφ cos ϕ + cos φ cosθsenϕ )
+ ( Z 5 y )(− senφsenϕ + cos φ cosθ cos ϕ ) + ( Z 5 z )(− cos φsenθ ) − Z 5 y − C1 y = p jy − p1 y

C1x ( senρsenη ) − C1 y ( senρ cosη ) − C1z cos ρ − ( Z 5 x )( senρsenη )

− ( Z 5 y )( senρ cosη ) − ( Z 5 z )(cos ρ ) − Z 5 z − C1z = p jz − p1z

Debido a que la síntesis es resuelta para seis puntos de precisión se origina un sistema de
treinta ecuaciones con veinticuatro incógnitas; estás incógnitas serán los ángulos µ2 , µ3 ,
µ4, µ5 , µ6 , ρ2, ρ3, ρ4, ρ5, ρ6, η2, η3, η4, η5, η6, las coordenadas del pivote de tierra C0x,
C0y, C0z , las magnitudes C1x, C1y, C1z, Z5x, Z5y, Z5z.

Los valores a utilizar ahora son los que se muestran a continuación:

W 1u = -1.4689 Z1x = 3.792 Z1z = -2.761 Z3 = 11.2006

W 1v= -1.4745 Z1y = 2.625 Z2 = 10.2209 Z4 = 23.4632

Valores iniciales
C0x = 0.1 C1x = 0.1 Z5x = 0.1
C0y = 0.1 C1y = 0.1 Z5y = 0.1
C0z = 0.1 C1z = 0.1 Z5z = 0.1
µ2 = 300 ρ2 = 350 η2 = 700
µ3 = 350 ρ3 = 450 η3 = 850
µ4 = 400 ρ4 = 500 η4 = 950
µ5 = 450 ρ5 = 600 η5 = 1000
µ6 = 500 ρ6 = 650 η6 = 1050

72
Los resultados obtenidos son:
C0x = 12.56 C1x = -3.7 Z5x = 1.24
C0y = 8.26 C1y = 1.2 Z5y = -1.46
C0z = 20.54 C1z = 1.83 Z5z = 4.74

µ2 = 3.4507 rad = 197.710 ρ2 =4.0168 rad = 230.140 η2 = -2.3706 rad= -135.80

µ3 = 1.7153 rad = 98.27950 ρ3 = 4.264 rad =244.360 η3 = 5.9 rad = 338.290
µ4 = 6.1205 rad = 350.670 ρ4 = .3713 rad = 21.270 η4 = 5.2559=301.140
µ5 = -2.8117 rad = -161.090 ρ5 = 2.3832 rad = 136.50 η5 = -.2293 rad=-13.130
µ6 = -2.227 rad =-127.590 ρ6 = 5.34 rad=306.50 η6 = -4.3241rad=-247.750

En la figura 3.16 se muestra el diagrama esquemático del mecanismo RSSR-SS.

FIG. 3.16 Mecanismo RSSR-SS para generar trayectoria

73
Localización del pivote de tierra respecto al sistema de referencia xo , yo , zo
x y z
Ro1 10.22 11.2 23.46
Ro2 8.11 9.48 21.36
So4 12.56 8.26 20.54
Longitud de los eslabones:
W1 = 2.07 Z1 = 3.73 a = 4.68
W2 = 2.4 Z2 = 8.14 b = 2.88
C1 = 4.29 Z5 = 5.11 c = 5.47

3.6. SUMARIO
El dimensionar los eslabones de un mecanismo no es un proceso arbitrario, ya que un
mecanismo para que sea útil debe satisfacer un conjunto deseado de características de
movimiento, el cual puede ser el de generar una función, una trayectoria o conducir un
cuerpo.
La gran variedad de mecanismos espaciales hace difícil desarrollar un método unificado de
análisis que se aplique a todos los casos, sin embargo, es posible analizarlos utilizando
vectores, mediante los cuales se obtienen las ecuaciones de diseño del mecanismo. El
mecanismo con pares cinemáticos RSSR-SS está constituido por cinco eslabones, uno fijo y
cuatro en movimiento, las ecuaciones de diseño necesarias para obtener la síntesis
dimensional de este mecanismo se obtuvieron utilizando la metodología de Suh que
consiste en el modelado del mecanismo por medio de vectores formando diadas.

74
3.7 REFERENCIAS

[3.1] Antonio Barrientos, Luis Felipe Peñín , Carlos Balaguer, Rafael Aracil.
Fundamentos de Robótica.
Mc Graw-Hill 1997

[3.2] K.S. Fu, R.C. Gonzalez, C.S.G. Lee

Robótica ,control, detección, visión e inteligencia
Mc Graw-Hill 1988

[3.3] D. McClay, D.M.J. Harris

Robótica, una introducción
Limusa 1993

[3.4] C.H. Suh, C.W. Radeliffe

Kinematics and Mechanisms Design
John Wiley & sons 1978

[3.5] C. H. Suh, Ph. D. Chairman

Computer-Aided Design of Mechanisms 1973

75
CAPITULO

IV

ANÁLISIS CINEMATICO
DEL MECANISMO RSSR-SS

76
 ANÁLISIS CINEMATICO DEL MECANISMO RSSR-SS

El análisis cinemático de mecanismos se refiere al estudio del movimiento de los mismos,

se trata por consiguiente de un análisis de movimiento, por lo que una vez conocidos el
número de eslabones, número de pares, dimensión de eslabones y conocido el movimiento
del eslabón conductor, se determina el estado de movimiento (velocidades y aceleraciones)
del eslabón de salida y el eslabón conductor, así como de puntos de particular interés.
En este capítulo se calculan las velocidades y aceleraciones del mecanismo RSSR-SS.

4.1 VELOCIDAD.

Una vez efectuado el análisis de posición, el siguiente paso es determinar las velocidades
de todos los eslabones y puntos de interés del mecanismo.
La velocidad se define como la tasa de variación de la posición, con respecto al tiempo. La
posición (R) corresponde a un vector, y por tanto, la velocidad es también vectorial.
La velocidad puede ser angular o lineal. La velocidad angular se representa por ω, y la
velocidad lineal por υ [4.1].

dθ dR
ω= ; υ= ; ( 4.1 )
dt dt

4.2 MOVIMIENTO LINEAL.

En los mecanismos, las partículas de los eslabones están restringidas a moverse en

trayectorias determinadas, muchas de las cuales son obvias como los círculos y las líneas
rectas [4.2].
La velocidad lineal Vp de una partícula P es la razón instantánea de cambio de posición de
la partícula, o desplazamiento, con respecto al tiempo. Haciendo referencia a la figura 4.1,
en un intervalo pequeño de tiempo ∆t, la partícula se desplaza ∆S a lo largo de la
trayectoria curva desde la posición P hasta la posición P’. Al mismo tiempo, el radio
vector de la partícula cambia de R a R+ ∆R y sufre un desplazamiento angular ∆θ. Por lo

77
tanto, el desplazamiento ∆S está formado por dos componentes: uno debido al
desplazamiento angular ∆θ de radio R y el otro debido al cambio en longitud ∆R [4.3].
De la figura 4.1.

∆S = R∆θp + ∆Rr ( 4.2 )

en donde p y r son vectores unitarios perpendiculares y paralelos a R, respectivamente.

FIG. 4.1 Movimiento de una partícula

La ecuación para la velocidad de P se determina como sigue:

⎛ ∆S ⎞ ⎛ R∆θ ∆R ⎞
V p = lím ∆t →0 ⎜ ⎟ = lím ∆t →0 ⎜ p+ r⎟ (4.3)
⎝ ∆t ⎠ ⎝ ∆t ∆t ⎠

78
Por lo tanto.
dR dθ
V p = Rω r p + r en donde ωr = (4.4)
dt dt

o empleando el producto vectorial:

dR
Vp = ωr × R + r (4.5)
dt

Cuando el origen del sistema de coordenadas coincide con el centro de curvatura, dR/dt es
igual a cero, de manera que d2R/dt2 también es igual a cero. Bajo esta condición, la
ecuación (4.5) puede simplificarse para obtener

Vp = ωr × R (4.6)

4.3 MOVIMIENTO RELATIVO.

El movimiento relativo es muy importante en el análisis cinemático de los mecanismos. En

la figura 4.2a, P y Q son partículas que se mueven con relación a un plano fijo de
referencia, dadas las velocidades respectivas de Vp y VQ se requiere determinar la
velocidad relativa VPQ entre las dos partículas. Al determinar VPQ se hará uso del hecho
que la adición de velocidades iguales a cada partícula no cambia la velocidad relativa de las
dos partículas. Por lo tanto, si a P como a Q se les da una velocidad igual y opuesta a VQ,
la partícula Q se vuelve estacionaria en el plano fijo y P adquiere una componente adicional
de velocidad –VQ relativa al plano fijo. Por lo que la nueva velocidad absoluta de (Vp-VQ )
se convierte en la velocidad relativa VPQ debido a que Q ahora está fija con relación al
plano de referencia. Esto se demuestra mediante el diagrama vectorial de la figura 4.2b, de
donde la ecuación para VPQ resulta:

VPQ = VP - VQ

79
 (b)
FIG. 4.2 Movimiento relativo

De manera similar, VQP puede obtenerse mediante la adición de – VP a cada partícula. Esto
se muestra en la figura 4.3 y VQP está dada mediante la ecuación

VQP = VQ - VP

FIG. 4.3 Velocidad relativa

4.4 ANÁLISIS DE VELOCIDAD.

En la figura 4.4 se conoce el movimiento del punto P con respecto al sistema de

coordenadas xyz el cual, a su vez, se mueve con relación al sistema fijo de coordenadas
XYZ. La posición del punto P con relación al sistema XYZ se puede expresar como

RP = R0 + R (4.7)

80
 FIG. 4.4 Sistemas de coordenadas

Si los vectores unitarios i, j y k se fijan en los ejes x, y, z, respectivamente:

R = xi + yj + zk (4.8)

La velocidad absoluta del punto P con relación al sistema XYZ, Vp, puede obtenerse
diferenciando la ecuación (4.7) con respecto al tiempo para dar

• • •
VP = R P = R 0 + R (4.9)

Diferenciando la ecuación (4.9) con respecto al tiempo se obtiene

• • • • • • •
R = ( x i + y j + z k ) + ( x i+ y j+ z k ) (4.10)

• • •
El término ( x i + y j + z k ) es la velocidad del punto P con relación al sistema móvil de
coordenadas xyz, siendo

• • •
(x i + y j + z k) = V (4.11)

Considerando los términos del segundo paréntesis de la ecuación (4.10) y del hecho de que
se puede demostrar que la velocidad de la punta de un vector R que pasa por un punto base

81
fijo y gira alrededor de este punto base con una velocidad angular ω, es igual a ω × R, las
velocidades de las puntas de los vectores unitarios i, j y k se pueden expresar como

•
i =ωxi
•
j=ωxj
•
k =ωxk
en donde ω es la velocidad angular del sistema móvil de coordenadas xyz con respecto al
sistema fijo XYZ. Efectuando las sustituciones anteriores:
• • •
x i + y j + z k = x(ω x i) + y(ω x j) + z(ω x k) = ω x ( xi + yj + zk)
y usando la relación expresada en la ecuación (4.8)

• • •
x i+ y j + z k = ω x R (4.12)

La ecuación (4.10) se convierte entonces en

•
R =V+ωxR (4.13)

•
La ecuación (4.9) ahora puede escribirse como sigue haciendo V0 = R 0 y sustituyendo el
valor de R de la ecuación (4.13)

VP = V0 + V + ω x R (4.14)

En donde :

VP = velocidad del punto P en el sistema XYZ

V0 = velocidad del origen del sistema xyz con respecto al sistema XYZ
V = velocidad del punto P con relación al sistema xyz
ω = velocidad angular del sistema xyz con respecto al sistema XYZ

82
 R = distancia desde el origen del sistema xyz hasta el punto P

El uso de la representación vectorial de un eslabonamiento resulta de mucha utilidad en el

análisis cinemático de un mecanismo, ya que las velocidades y aceleraciones del mismo
pueden obtenerse derivando con respecto al tiempo los vectores de posición [4.5].
De la figura 4.5 se obtiene la siguiente ecuación:

W1j + Z1j = W2j + Z2j (4.15)

Donde:
W1j = (Rγ1,y)(Rλ1,u)( Rβ,w ) W1
W2j = (Rγ,y)(Rλ,u)(Rα,w)W2

FIG.4.5 Circuito para determinar las velocidades del eslabón W2 y de la barra acopladora

Al introducir las matrices de rotación en la expresión (4.15) para obtener las componentes
de los vectores de posición en el sistema de coordenadas xyz se tiene:

(Rγ1,y)(Rλ1,u)(Rβ,w)W1+(Rφ,z)(Rθ,u)(Rϕ,w)Z1= (Rγ,y)(Rλ,u)(Rα,w)W2+(Rφ,z)(Rθ,u)(Rϕ,w)Z2 (4.16)

Para obtener la expresión de la velocidad se deriva la ecuación (4.16) con respecto al

tiempo en donde:
dβ dφ dθ dϕ dα
= ωβ = ωφ = ωθ = ωϕ = ωα (4.17)
dt dt dt dt dt

83
La velocidad angular ωβ es dato así como la longitud de los eslabones, por lo tanto, se tiene
un sistema de 3 ecuaciones con cuatro incógnitas que son las velocidades angulares ωφ
(velocidad angular de la barra acopladora con respecto al eje z), ωθ (velocidad angular de la
barra acopladora con respecto al eje rotado u), ωϕ (velocidad angular de la barra acopladora
respecto al eje rotado w) y ωα( velocidad angular del eslabón W2).
De la figura 4.6 se obtiene la ecuación de siguiente:
W1j + Z1j = C1j + Z5j (4.18)

FIG. 4.6 Circuito para determinar lass velocidades del eslabón C1

Al introducir las matrices de rotación para poder calcular la velocidad en cada uno de los
puntos deseados se tiene:

(Rγ1,y)(Rλ1,u)(Rβ,w)W1+(Rφ,z)(Rθ,u)(Rϕ,w)Z1=(Rφ,z)(Rθ,u)(Rϕ,w)Z5+(Rµ,z)(Rρ,u)(Rη,w)C1 (4.19)

Para obtener la expresión de la velocidad se deriva con respecto al tiempo la expresión

(4.19) en donde:

dρ dµ dη
= ωρ = ωµ = ωη (4.20)
dt dt dt
en este caso se tendrá un sistema de tres ecuaciones con tres incógnitas, ya que ωφ ,ωθ ,ωϕ
son conocidas del cálculo realizado para determinar el valor de la velocidad del eslabón W2
.

84
 4.5 ANALISIS DE ACELERACIONES.

Para calcular las aceleraciones, lo que se hace es obtener la segunda derivada con respecto
al tiempo de las ecuaciones 4.16 y 4.19, o lo que es lo mismo derivar con respecto al
tiempo las expresiones de la velocidad:
Primeramente son calculadas las aceleraciones angulares αφ , αθ , αϕ
d 2θ d 2ϕ d 2φ d 2β d 2α
= αθ = αϕ = αφ = αβ = αα (4.21)
dt 2 dt 2 dt 2 dt 2 dt 2
ya que son calculadas αθ , αφ ,αϕ se calculan las aceleraciones αµ , αρ ,αη:
d 2µ d 2ρ d 2η
= αµ = αρ = αη (4.22)
dt 2 dt 2 dt 2

4.6 DESARROLLO DE LAS ECUACIONES CINEMÁTICAS PARA EL

CÁLCULO DE LAS VELOCIDADES Y ACELERACIONES DEL
MECANISMO CON PARES RSSR-SS.

Al desarrollar las expresiones (4.16) y (4.19) se obtiene el siguiente sistema de ecuaciones

ωφ ( (Z1x- Z2x)*(-cϕ*sφ-sϕ*cθ*cφ)+(Z1y- Z2y)*(sϕ*sφ-cϕ*cθ*cφ)+(Z1z- Z2z)*sθ*cφ ) +

ωϕ ((Z1x- Z2x)*(-cφ*sϕ-sφ*cθ*cϕ)+(Z1y- Z2y)*(-cφ*cϕ+sφ*cθ*sϕ) )+
ωθ ((Z1x- Z2x)*(sϕ*sφ*sθ)+(Z1y- Z2y)*(cϕ*sφ*sθ)+(Z1z- Z2z)*(sφ*cθ) –
ωα (W2u*(-sα*cγ+sγ*sλ*cα)-W2v*(cγ*cα+sγ*sλ*sα)) = -ωβ*(W1u*(-sβ*cγ+sγ*sλ*cβ)-
W1v*(cγ*cβ+sγ*sλ*sβ))

ωφ ((Z1x- Z2x)*(cϕ*cφ-sϕ*cθ*sφ)-(Z1y- Z2y)*(sϕ*cφ+cϕ*cθ*sφ)+(Z1z- Z2z)*sθ*sφ )+

ωϕ ((Z1x- Z2x)*(-sφ*sϕ+cφ*cθ*cϕ)-(Z1y- Z2y)*(sφ*cϕ+cφ*cθ*sϕ)) +
ωθ ((Z1x- Z2x)*(-sϕ*cφ*sθ)-(Z1y- Z2y)*(cϕ*cφ*sθ)-(Z1z-Z2z)*cφ*cθ) -
ωa (W2u*(cλ*cα)-W2v*(cλ*sα))= -ωβ*(w2u*(cλ*cβ)-w2v*(cλ*sβ))

ωϕ ((Z1x- Z2x)*(sθ*cϕ)-(Z1y- Z2y)*(sθ*sϕ)) +

ωθ ((Z1x- Z2x)*(sϕ*cθ)+(Z1y- Z2y)*(cϕ*cθ)-(Z1z-Z2z)*sθ) -
ωa (W2u*(-sγ*sα+cγ*sλ*cα)+W2v*(sγ*cα-cγ*sλ*sα))=-ωβ*(W1u*(-sγ*sβ+cγ*sλ*cβ)+W1v*(sγ*cβ-
cγ*sλ*sβ))

Sistema de ecuaciones generado con la expresión (4.19)

ωµ ( (c1x)*(-cρ*sµ-sρ*cη*cµ)+(c1y)*(sρ*sµ-cρ*cη*cµ)+(c1z)*sη*cµ ) +
ωρ (c1x)*(-cµ*sρ-sµ*cη*cρ)+(c1y)*(-cµ*cρ+sµ*cη*sρ) +

85
ωη (c1x)*(sρ*sµ*sη)+(c1y)*(cρ*sµ*sη)+
(c1z)*(sµ*cη) =ωβ*(W1u*(-sβ*cγ+sγ*sλ*cβ)-W1v*(cγ*cβ+sγ*sλ*sβ))+h1+h2+h3

ωµ (c1x)*(cρ*cµ-sρ*cη*sµ)-(c1y)*(sρ*cµ+cρ*cη*sµ)+(c1z)*sη*sµ +
ωρ (c1x)*(-sµ*sρ+cµ*cη*cρ)-(c1y)*(sµ*cρ+cµ*cη*sρ) +
ωη (c1x)*(-sρ*cµ*sη)-(c1y)*(cρ*cµ*sη)-(c1z)*cµ*cη = ωβ*(W1u*(cλ*cβ)-W1v*(cλ*sβ))+h4+h5+h6

ωρ (c1x)*(sη*cρ)-(c1y)*(sη*sρ) +
ωη (c1x)*(sρ*cη)+(c1y)*(cρ*cη)-(c1z)*sη = ωβ*(W1u*(-sγ*sβ+cγ*sλ*cβ)+W1v*(sγ*cβ-
cγ*sλ*sβ))+h7+h8

h1=ωφ( (Z1x- Z5x)*(-cϕ*sφ-sϕ*cθ*cφ)+(Z1y- Z5y)*(sϕ*sφ-cϕ*cθ*cφ)+(Z1z- Z5z)*sθ*cφ )

h2=ωϕ ((Z1x- Z5x)*(-cφ*sϕ-sφ*cθ*cϕ)+ (Z1y- Z5y)*(-cφ*cϕ+sφ*cθ*sϕ) )
h3=ωθ (Z1x- Z5x)*(sϕ*sφ*sθ)+(Z1y- Z5y)*(cϕ*sφ*sθ)+(Z1z- Z5z)*(sφ*cθ)

h4=ωφ (Z1x- Z5x)*(cϕ*cφ-sϕ*cθ*sφ)- (Z1y- Z5y)*(sϕ*cφ+cϕ*cθ*sφ)+(Z1z- Z5z)*sθ*sφ )

h5=ωϕ (Z1x- Z5x)*(-sφ*sϕ+cφ*cθ*cϕ)-(Z1y- Z5y)*(sφ*cϕ+cφ*cθ*sϕ))
h6=ωθ (Z1x- Z5x)*(-sϕ*cφ*sθ)-(Z1y- Z5y)*(cϕ*cφ*sθ)- (Z1z-Z5z)*cφ*cθ)
h7=ωϕ (Z1x- Z5x)*(sθ*cϕ)-(Z1y- Z5y)*(sθ*sϕ))
h8=ωθ (Z1x- Z5x)*(sϕ*cθ)+(Z1y- Z5y)*(cϕ*cθ)-(Z1z-Z5z)))*sθ)

TABLA 4.1 Velocidades angulares de los eslabones del mecanismo RSSR-SS

Punto de ωβ ωφ ωϕ ωθ ωα ωµ ωρ ωη
Precisión r.p.m rad./seg. rad/seg rad./seg. rad./seg. rad./seg. rad./seg. rad./seg.

1 10 1.2917 1.2917 -0.2756 -1.4775 0.2479 0.2479 -0.3726

2 10 0.8871 -0.1438 0.0722 1.4219 0.1579 0.1791 0.1591
3 10 -0.9642 0.0488 -0.0894 1.4254 -0.3468 -0.3428 0.1535
4 10 -0.8117 0.2487 -0.4634 1.4897 -0.8891 0.2635 0.0545

5 10 -0.3053 0.1561 -0.5579 0.2473 -0.2236 -0.2841 -0.1495

6 10 -0.1384 0.1028 -0.6087 0.6079 -0.037 0.0164 -0.0749

ωβ = velocidad angular del eslabón de entrada

ωφ = velocidad angular del eslabón acoplador respecto al eje z
ωθ = velocidad angular del eslabón acoplador respecto al eje u
ωϕ = velocidad angular del eslabón acoplador respecto al eje rotado w
ωα = velocidad angular del eslabón W2
ωµ = velocidad angular del eslabón C1 respecto al eje z

86
ωρ = velocidad angular del eslabón C1 respecto al eje rotado u
ωη = velocidad angular del eslabón C1 respecto al eje rotado w

1.5

0.5
ra d/s eg.

-0.5

-1

-1.5
0 20 40 60 80 1 00 120 140 1 60
A n gulo de e ntrada (gra dos)

FIG. 4.7 Gráfica de velocidades angulares

ωφ ωθ
ωϕ ωα
ωρ ωη
ωµ

En esta gráfica se muestran las variaciones de las velocidades angulares de los eslabones
con respecto a cada uno de los ejes de giro cuando se alcanzan los puntos de la trayectoria
que se quieren satisfacer.

87
Las aceleraciones se obtienen derivando con respecto al tiempo el sistema de ecuaciones
generado por la ecuación ( 4.15 ) así como de la derivada con respecto al tiempo del
sistema de ecuaciones generado por la ecuación (4.18 ).
Una vez resuelto el sistema de ecuaciones generado por la derivada con respecto al tiempo
de la ecuación ( 4.15 ) se resuelve el sistema de ecuaciones generado por la derivada con
respecto al tiempo de la ecuación (4. 18 ) y los resultados se presentan en la tabla ( 4.2 ):
αf(Z1x- Z2x)*(-sφ*cϕ-cθ*cφ*sϕ)+(Z1y- Z2y)*(sϕ*sφ-cϕ*cθ*cφ)+(Z1z-Z2z)*sθ*cφ)
αp( Z1x- Z2x)*(-cφ*sϕ-sφ*cθ*cϕ)+(Z1y- Z2y)*(sφ*cθ*sϕ-cφ*cϕ) )+
αt( (Z1x- Z2x)*(sϕ*sφ*sθ)+(Z1y- Z2y)*(cϕ*sφ*sθ)+(Z1z-Z2z)*(sφ*cθ) )-
αa (W2u*(-cγ*sα+cα*sγ*sλ)-W2v*(cγ*cα+sγ*sλ*sα))=
-(αβ*(W1u*(-cγ*sβ+cβ*sγ*sλ)-W1v*(cγ*cβ+sγ*sλ*sβ))+h7)-h1+h2

αf( (Z1x- Z2x)*(cϕ*cφ-cθ*sφ*sϕ)+(Z1y- Z2y)*(sθ*sφ-sϕ*cφ)-

(Z1z-Z2z)*cϕ*cθ*sφ )
αp( (Z1x- Z2x)*(-sφ*sϕ+cϕ*cφ*cθ)-(Z1y- Z2y)*(sφ*cϕ-cφ*cθ*sϕ) )
αt( (Z1x- Z2x)*(-sϕ*cφ*sθ)-(Z1y- Z2y)*(cϕ*cφ*sθ)-(Z1z –Z2z))*(cφ*cθ) )-
αa (W2u*(cλ*cα)-W2v*(cλ*sα))=-(αβ*(W1u*(cλ*cβ)-W1v*(cλ*sβ))+h8)-h3+h4

αp( (Z1x- Z2x)*(sθ*cϕ)-(Z1y- Z2y)*(sθ*sϕ) )

αt( (Z1x- Z2x)*(sϕ*cθ)+(Z1y- Z2y)*(cϕ*cθ)-(Z1z-Z2z))*(sθ) )-
αa (W2u*(-sα*sγ+cγ*sλ*cα)+W2v*(sγ*cα-cγ*sλ*sα))=-(αβ*(W1u*(-sβ*sγ+cγ*sλ*cβ)+W1v*(sγ*cβ-
cγ*sλ*sβ))+h9)-h5+h6

k1=(Z1x- Z2x)*(cφ*(-cϕ)*ωϕ+sϕ*sφ*ωφ)
k2=(Z1x- Z2x)*(cϕ*(-cφ)*ωφ+sφ*sϕ*ωϕ)
k3=(Z1x- Z2x)*( ωϕ*(-sφ*cθ*sϕ)+cϕ*(sφ*(-sθ)*ωθ+ωφ*cθ*cφ))
k4=(Z1x- Z2x)*( ωθ*(sφ*(-cθ)*ωθ-sθ*cφ*ωφ)+ωφ*(cθ*(-sφ)*ωφ+cφ*(-sθ)*ωθ))
k5=(Z1x- Z2x)*(sφ*(-sθ)*ωθ+cθ*cφ*ωφ)
k6=(Z1x- Z2x))*(cφ*(-sϕ)*ωϕ+cϕ*(-sφ)*ωφ)
k7=(Z1y- Z2y)*(-sφ*cϕ*ωϕ-sϕ*cφ*ωφ)
k8=(Z1y- Z2y)*(-sφ*cθ*ωϕ*cϕ*ωϕ-sϕ*ωϕ*(sφ*(-sθ)*ωθ+cθ*cφ*ωφ))
k9=(Z1y- Z2y))*(sφ*(-sθ)*ωθ+cθ*cφ*ωφ)
k10=(Z1z-Z2z)*( ωθ*(sφ*(-sθ)*ωθ+cθ*cφ*ωφ))
k11=(Z1z-Z2z)*( ωφ*(sθ*(-sφ)*ωφ+cφ*cθ*ωθ))
k12=(Z1x- Z2x)*(sφ*(-cϕ)*ωϕ+(-sϕ)*cφ*ωφ)
k13=(Z1x- Z2x)*(-cϕ*sφ*ωφ-cφ*sϕ*ωϕ)
k14=(Z1x- Z2x)*(cφ*cθ*(-sϕ)*ωϕ+cϕ*(cφ*(-sθ)*ωθ-cθ*sφ*ωφ))
k15=(Z1x- Z2x)*( ωθ*(cφ*(-cθ)*ωθ+sθ*sφ*ωφ)+ ωφ*(cθ*(-cφ)*ωφ+sφ*sθ*ωθ))
k16=(Z1x- Z2x)*(cφ*(-sθ)*ωθ-cθ*sφ*ωφ)
k17=(Z1y- Z2y)*(-sφ*sϕ*ωϕ+cϕ*cφ*ωφ)
k18=(Z1y- Z2y)*(-sϕ*sφ*ωφ+cφ*cϕ*ωϕ)
k19=(Z1y- Z2y)*(-cφ*cθ*cϕ*ωϕ-sϕ*(-cφ*sθ*ωθ+cθ*(-sφ)*ωφ))
k20=(Z1y- Z2y)*( ωθ*(cφ*(-cθ)*ωθ+sθ*sφ*ωφ)+ ωφ*(cθ*(-cφ)*ωφ+sφ*sθ*ωθ))
k21=(Z1y- Z2y)*(-cφ*sθ*ωθ-cθ*sφ*ωφ)

88
k22=(Z1z-Z2z)*(-cφ*sθ*ωθ-cθ*sφ*ωφ)
k23=(Z1z-Z2z)*(-sθ*cφ*ωφ-sφ*cθ*ωθ)
k24=(Z1x- Z2x)*(-sθ*sϕ*ωϕ+cϕ*sθ*ωθ)
k25=(Z1x- Z2x)*(-sϕ*sθ*ωθ+cθ*cϕ*ωϕ)
k26=(Z1y- Z2y)*(-sθ*cϕ*ωϕ-sϕ*cθ*ωθ)
k27=(Z1y- Z2y)*(-cϕ*sθ*ωθ-cθ*sϕ*ωϕ)
k28=(Z1z-Z2z)*(- ωθ*cθ*ωθ)
h1=ωϕ*k1+ωφ*k2-ωϕ*k3-sϕ*k4-cϕ*ωϕ*k5-ωϕ*k6-ωφ*k7-k8-cϕ*k4-(-sϕ)*ωϕ*k9+k10+k11
h2=W2u*(-ωa*cγ*cα*ωa)+W2u*(ωa*sγ*sλ*(-sα)*ωa)-W2v*(ωa*cγ*(-sα)*ωa)+W2v*(ωa*sγ*sλ*(-
cα)*ωa)
h3=ωϕ*k12+ωφ*k13+ωϕ*k14+sϕ*k15+cϕ*ωϕ*k16-ωϕ*k17-ωφ*k18+ωϕ*k9+cϕ*k20+(-
sϕ)*ωϕ*k21-ωθ*k22-ωφ*k23
h4=W2u*(ωa*cλ*(-sα)*ωa)+W2v*(ωa*cλ*(-cα)*ωa)
h5=ωϕ*k24+ωθ*k25+ωϕ*k26+ωθ*k27+k28
h6=W2u*(ωa*sγ*(-cα)*ωa)+W2u*(ωa*cγ*sλ*(-sα)*ωa)+W2v*(ωa*sγ*(-sα)*ωa)+W2v*(ωa*cγ*sλ*(-
cα)*ωa)
h7=W1u*(-ωβ*cγ1*cβ*ωβ)+W1u*(ωβ*sγ1*sλ1*(-sβ)* ωβ)-W1v*(ωβ*cγ1*(-sβ)*
ωβ)+W1v*(ωβ*sγ1*sλ1*(-cβ)* ωβ);
h8=W1u*(ωβ*cγ1*(-sβ)* ωβ)+W1v*(ωβ*cγ1*(-cβ)* ωβ);
h9=W1u*(ωβ*sγ1*(-cβ)* ωβ)+W1u*(ωβ*cγ1*sλ1*(-sβ)* ωβ)+W1v*(ωβ*sγ1*(-sβ)*
ωβ)+W1v*(ωβ*cγ1*sλ1*(-cβ)* ωβ);

αµ((c1x)*(-sµ*cρ-cη*cµ*sρ)+(c1y)*(sρ*sµ-cρ*cη*cµ)+(c1z)*sη*cµ)
αρ( (c1x)*(-cµ*sρ-sµ*cη*cρ)+(c1y)*(sµ*cη*sρ-cµ*cρ) )
αη( (c1x)*(sρ*sµ*sη)+(c1y)*(cρ*sµ*sη)+(c1z)*(sµ*cη) )=
(αβ*(W1u*(-cγ*sβ+cβ*sγ*sλ)-W1v*(cγ*cβ+sγ*sλ*sβ))+h7)-g1+f1+m1+m2+m3

αµ( (c1x)*(cρ*cµ-cη*sµ*sρ)+(c1y)*(sη*sµ-sρ*cµ)-(c1z)*cρ*cη*sµ )
αρ( (c1x)*(-sµ*sρ+cρ*cµ*cη)-(c1y)*(sµ*cρ-cµ*cη*sρ) )
αη( (c1x)*(-sρ*cµ*sη)-(c1y)*(cρ*cµ*sη)-(c1z)*(cµ*cη) )=
(αβ*(W1u*(cλ*cβ)-W1v*(cλ*sβ))+h8)-g2+f2+m4+m5+m6

αρ( (c1x)*(sη*cρ)-(c1y)*(sη*sρ) )
αη( (c1x)*(sρ*cη)+(c1y)*(cρ*cη)-(c1z)*(sη) )= (αβ*(W1u*(-sβ*sγ+cγ*sλ*cβ)+W1v*(sγ*cβ-
cγ*sλ*sβ))+h9)+f3+m7+m8-g3

m1=αf((Z1x- Z5x)*(-sφ*cϕ-cθ*cφ*sϕ)+(Z1y- Z1y)*(sϕ*sφ-cϕ*cθ*cφ)+(Z1z-Z5z)*sθ*cφ)+

αp( (Z1x- Z5x)*(-cφ*sϕ-sφ*cθ*cϕ)+(Z1y- Z5y)*(sφ*cθ*sϕ-cφ*cϕ) )+
αt( (Z1x- Z5x)*(sϕ*sφ*sθ)+(Z1y)- Z5y)*(cϕ*sφ*sθ)+(Z1z-Z5z)*(sφ*cθ) )+f1-d1

m2=αf( (Z1x- Z5x)*(cϕ*cφ-cθ*sφ*sϕ)+(Z1y- Z5y)*(sθ*sφ-sϕ*cφ)-(Z1z-Z5z)*cϕ*cθ*sφ )+

αp( (Z1x- Z1x)*(-sφ*sϕ+cϕ*cφ*cθ)-(Z1y- Z5y)*(sφ*cϕ-cφ*cθ*sϕ) )+
αt( (Z1x- Z5x)*(-sϕ*cφ*sθ)-(Z1y- Z5y)*(cϕ*cφ*sθ)-(Z1z -Z5z)*(cφ*cθ) )+f2-d2

m3=αp( (Z1x- Z5x)*(sθ*cϕ)-(Z1y- Z5y)*(sθ*sϕ) )+αt( (Z1x- Z5x)*(sϕ*cθ)+(Z1y- Z5y)*(cϕ*sθ)-

(Z1z-Z5z)*(sθ) )+f3-d3
m4=αφ*((Z1x-Z5x)*(cϕ*cφ-cθ*sφ*sϕ)+(Z1y-Z5y)*(sθ*sφ-sϕ*cφ)-(Z1z-Z5z)*cϕ*cθ*sφ );

89
m5=αϕ*((Z1x-Z5x)*(-sφ*sϕ+cϕ*cφ*cθ)-(Z1y-Z5y)*(sφ*cϕ-cφ*cθ*sϕ));
m6=αθ*((Z1x-Z5x)*(-sϕ*cφ*sθ)-(Z1y-Z5y)*(cϕ*cφ*sθ)-(Z1z-Z5z)*(cφ*cθ));
m7=αϕ*((Z1x-Z5x)*(sθ*cϕ)-(Z1y-Z5y)*(sθ*sϕ));
m8=αθ*((Z1x-Z5x)*(sϕ*cθ)+(Z1y-Z5y)*(cϕ*cθ)-(Z1z-Z5z)*(sθ));

q1=(Z1x- Z5x)*(cφ*(-cϕ)*ωϕ+sϕ*sφ*ωφ)
q2=(Z1x- Z5x)*(cϕ*(-cφ)*ωφ+sφ*sϕ*ωϕ)
q3=(Z1x- Z5x)*( ωϕ*(-sφ*cθ*sϕ)+cϕ*(sφ*(-sθ)*ωθ+ωφ*cθ*cφ))
q4=(Z1x- Z5x)*( ωθ*(sφ*(-cθ)*ωθ-sθ*cφ*ωφ)+ ωφ*(cθ*(-sφ)*ωφ+cφ*(-sθ)*ωθ))
q5=(Z1x- Z5x)*(sφ*(-sθ)*ωθ+cθ*cφ*ωφ)
q6=(Z1x- Z5x)*(cφ*(-sϕ)*ωϕ+cϕ*(-sφ)*ωφ)
q7=(Z1y- Z5y)*(-sφ*cϕ*ωϕ-sϕ*cφ*ωφ)
q8=(Z1y- Z5y)*(-sφ*cθ*ωϕ*cϕ*ωϕ-sϕ*ωϕ*(sφ*(-sθ)*ωθ+cθ*cφ*ωφ))
q9=(Z1y- Z5y)*(sφ*(-sθ)*ωθ+cθ*cφ*ωφ)
q10=(Z1z-Z5z)*( ωθ*(sφ*(-sθ)*ωθ+cθ*cφ*ωφ))
q11=(Z1z-Z5z)*( ωφ*(sθ*(-sφ)*ωφ+cφ*cθ*ωθ))
q12=(Z1x- Z5x)*(sφ*(-cϕ)*ωϕ+(-sϕ)*cφ*ωφ)
q13=(Z1x- Z5x)*(-cϕ*sφ*ωφ-cφ*sϕ*ωϕ)
q14=(Z1x- Z5x)*(cφ*cθ*(-sϕ)*ωϕ+cϕ*(cφ*(-sθ)*ωθ-cθ*sφ*ωφ))
q15=(Z1x- Z5x)*( ωθ*(cφ*(-cθ)*ωt+sθ*sφ*ωφ)+ ωφ*(cθ*(-cφ)*ωφ+sφ*sθ*ωθ))
q16=(Z1x- Z5x)*(cφ*(-sθ)*ωθ-cθ*sφ*ωφ)
q17=(Z1y- Z5y)*(-sφ*sϕ*ωϕ+cϕ*cφ*ωφ)
q18=(Z1y- Z5y)*(-sϕ*sφ*ωφ+cφ*cϕ*ωϕ)
q19=(Z1y- Z5y)*(-cφ*cθ*cϕ*ωϕ-sϕ*(-cφ*sθ*ωθ+cθ*(-sφ)*ωφ))
q20=(Z1y- Z5y)*( ωθ*(cφ*(-cθ)*ωθ+sθ*sφ*ωφ)+ ωφ*(cθ*(-cφ)*ωφ+sφ*sθ*ωθ))
q21=(Z1y- Z5y)*(-cφ*sθ*ωθ-cθ*sφ*ωφ)
q22=(Z1z-Z5z)*(-cφ*sθ*ωθ-cθ*sφ*ωφ)
q23=(Z1z-Z5z)*(-sθ*cφ*ωφ-sφ*cθ*ωθ)
q24=(Z1x- Z5x)*(-sθ*sϕ*ωϕ+cϕ*sθ*ωθ)
q25=(Z1x- Z5x)*(-sϕ*sθ*ωθ+cθ*cϕ*ωϕ)
q26=(Z1y- Z5y)*(-sθ*cϕ*ωϕ-sϕ*cθ*ωθ)
q27=(Z1y- Z1y)*(-cϕ*sθ*ωθ-cθ*sϕ*ωϕ)
q28=(Z1z-Z5z)*(- ωθ*cθ*ωθ)
f1=ωϕ*q1+ωφ*q2-ωϕ*q3-sϕ*q4-cϕ*ωϕ*q5-ωϕ*q6-ωφ*q7-q8-cϕ*q4-(-sϕ)*ωϕ*q9+q10+q11
f3=ωϕ*q12+ωφ*q13+ωϕ*q14+sϕ*q15+cϕ*ωϕ*q16-ωϕ*q17-ωφ*q18+ωϕ*q19+cϕ*q20+(-
sϕ)*ωϕ*q21-ωθ*q22-ωφ*q23
f5=ωϕ*q24+ωθ*q25+ωϕ*q26+ωθ*q27+q28

s1=(c1x)*(cµ*(-cρ)*ωρ+sρ*sµ*ωµ)
s2=(c1x)*(cρ*(-cµ)*ωµ+sµ*sρ*ωρ)
s3=(c1x)*( ωρ*(-sµ*cη*sρ)+cρ*(sµ*(-sη)*ωη+ωµ*cη*cµ))
s4=(c1x)*( ωη*(sµ*(-cη)*ωη-sη*cµ*ωµ)+ ωµ*(cη*(-sµ)*ωµ+cµ*(-sη)*ωη))
s5=(c1x)*(sµ*(-sη)*ωη+cη*cµ*ωµ)
s6=(c1x)*(cµ*(-sρ)*ωρ+cρ*(-sµ)*ωµ)
s7=(c1y)*(-sµ*cρ*ωρ-sρ*cµ*ωµ)
s8=(c1y)*(-sµ*cη*ωρ*cρ*ωρ-sρ*ωρ*(sµ*(-sη)*ωη+cη*cµ*ωµ))
s9=(c1y)*(sµ*(-sη)*ωη+cη*cµ*ωµ)

90
s10=(c1z)*(ωη*(sµ*(-sη)*ωη+cη*cµ*ωµ))
s11=(c1z)*( ωµ*(sη*(-sµ)*ωµ+cµ*cη*ωn))
s12=(c1x)*(sµ*(-cρ)*ωρ+(-sρ)*cµ*ωµ)
s13=(c1x)*(-cρ*sµ*ωµ-cµ*sρ*ωρ)
s14=(c1x)*(cµ*cη*(-sρ)*ωρ+cρ*(cµ*(-sη)*ωη-cη*sµ*ωµ))
s15=(c1x)*( ωη*(cµ*(-cη)*ωη+sη*sµ*ωµ)+ ωµ*(cη*(-cµ)*ωµ+sµ*sη*ωη))
s16=(c1x)*(cµ*(-sη)*ωη-cη*sµ*ωµ)
s17=(c1y)*(-sµ*sρ*ωρ+cρ*cµ*ωµ)
s18=(c1y)*(-sρ*sµ*ωµ+cµ*cρ*ωρ)
s19=(c1y)*(-cµ*cη*cρ*ωρ-sρ*(-cµ*sη*ωη+cη*(-sµ)*ωµ))
s20=(c1y)*( ωη*(cµ*(-cη)*ωη+sη*sµ*ωµ)+ ωµ*(cη*(-cµ)*ωµ+sµ*sη*ωη))
s21=(c1y)*(-cµ*sη*ωη-cη*sµ*ωµ)
s22=(c1z)*(-cµ*sη*ωη-cη*sµ*ωµ)
s23=(c1z)*(-sη*cµ*ωµ-sµ*cη*ωη)
s24=(c1x)*(-sη*sρ*ωρ+cρ*sη*ωη)
s25=(c1x)*(-sρ*sη*ωη+cη*cρ*ωρ)
s26=(c1y)*(-sη*cρ*ωρ-sρ*cη*ωη)
s27=(c1y)*(-cρ*sη*ωη-cη*sρ*ωρ)
s28=(c1z)*(- ωη*cη*ωη)
d1=ωρ*s1+ωµ*s2-ωρ*s3-sρ*s4-cρ*ωρ*s5-ωρ*s6-ωµ*s7-s8-cρ*s4-(-sρ)*ωρ*s9+s10+s11
d2=ωρ*s12+ωµ*s13+ωρ*s14+sρ*s15+cρ*ωρ*s16-ωρ*s17-ωµ*s18+ωρ*s19+cρ*s20+(-sρ)*ωρ*s21-
ωη*s22-ωµ*s23
d3=ωρ*s24+ωη*s25+ωρ*s26+ωη*s27+s28

TABLA 4.2Aceleraciones angulares de los eslabones del mecanismo RSSR-SS

Punto de αβ αφ αϕ αθ αα αµ αρ αη
2 2 2 2 2 2 2
Precisión rad./seg rad./seg rad./seg rad./seg rad./seg rad./seg rad./seg rad./seg2
1 0 0.8424 0.8424 1.2543 2.3164 -1.1603 -1.1603 1.0937
2 0 -1.0172 0.6026 0.2952 0.0586 -75.4577 -9.1976 87.681
3 0 -1.0971 0.4902 0.31 0.509 20.2195 -23.1769 -6.1137
4 0 0.4444 -0.1144 0.2086 -0.0188 2.4707 12.705 -27.8494
5 0 -0.347 -0.6909 -0.1971 0.2249 4.663 -3.1956 0.5462
6 0 -0.0487 -0.636 0.1614 0.0781 1.7073 1.2482 -0.0796

αβ = aceleración angular del eslabón de entrada

αφ = aceleración angular del eslabón acoplador respecto al eje z
αθ = aceleración angular del eslabón acoplador respecto al eje u
αϕ = aceleración angular del eslabón acoplador respecto al eje rotado w
αα = aceleración angular del eslabón W2
αµ = aceleración angular del eslabón c respecto al eje z
αρ = aceleración angular del eslabón c respecto al eje rotado u
αη = aceleración angular del eslabón c respecto al eje rotado w

91
 100

80

60

40

20
rad./s eg 2

-20

-40

-60

-80
0 20 40 60 80 100 120 140 160
A ngulo de entrada (grados)

Fig. 4.9 Gráfica de aceleraciones angulares

αφ αθ
αϕ αα
αρ αη
αµ

En esta gráfica se muestran las aceleraciones angulares de cada uno de los eslabones con
respecto a sus ejes de giro, esto como una información complementaria de la síntesis de
mecanismos.

92
 4.7 SUMARIO.

El análisis cinemático de mecanismos se refiere al estudio del movimiento de los mismos,

por lo que una vez conocidos el número de eslabones, número de pares, dimensión de
eslabones y conocido el movimiento del eslabón conductor, se determina el estado de
movimiento (velocidades y aceleraciones angulares) de los eslabones que constituyen el
mecanismo.
En la investigación cinemática de los mecanismos, la velocidad angular (cantidad vectorial
cuya dirección es la misma que la del eje de rotación) y aceleración angular (rapidez de
cambio de la velocidad angular) de los eslabones es factible obtenerlas a partir de la
primera y segunda derivada de las ecuaciones obtenidas de los circuitos que sirven para
relacionar los eslabones con cada uno de los puntos de precisión, de esta manera se pueden
obtener las velocidades y aceleraciones angulares instantáneas de los eslabones cuando se
alcanzan cada uno de los puntos de la trayectoria a satisfacer.

93
4.8 REFERENCIAS

[4.1] Anthony Bedford y Wallace Fowler

Ingeniería Mecánica, Dinámica
Addison-Wesley Iberoamericana, 1996
[4.2] Robert L.Norton
Diseño de Maquinaria
McGraw-Hill/ México 1995
[4.3] Hamilton H. Mabie, Charles F. Reinholtz
Mecanismos y Dinámica de Máquinas
Limusa 1998
[4.4] Ferdinand P. Beer & E. Russell Johnston Jr.
Mecánica Vectorial para Ingenieros, Dinámica
McGraw-Hill, México D.F. 1998
[4.5] C.H. Suh, C.W. Radeliffe
Kinematics and Mechanisms Design
John Wiley & sons 1978

94
CONCLUSIONES Y RECOMENDACONES.

Al término del desarrollo de este trabajo de tesis, pueden exponerse las conclusiones que se
derivaron del mismo, teniendo en cuenta su objetivo principal: establecer una metodología
para el cálculo de la síntesis dimensional de mecanismos espaciales.

El dimensionar los eslabones de un mecanismo no es un proceso arbitrario, ya que un

mecanismo para que sea útil debe satisfacer un conjunto deseado de características de
movimiento. La gran variedad de mecanismos espaciales hace difícil desarrollar un método
unificado de análisis que se aplique a todos los casos, sin embargo, es posible analizarlos
utilizando vectores, mediante los cuales se obtienen las ecuaciones de diseño del
mecanismo.

Se cumplió con el objetivo del trabajo al realizar una metodología de análisis que puede
emplearse en otros mecanismos espaciales, las matrices de rotación son una herramienta
muy útil en el desarrollo de la síntesis dimensional ya que facilitan en gran medida la
obtención de las ecuaciones de diseño además de tenerse una mayor claridad en la
comprensión de los movimientos de los eslabonamientos.

Un aspecto importante en el cálculo de la síntesis dimensional del mecanismo así como del
análisis cinemático del mismo es la utilización del paquete computacional empleado
(MATLAB 5.2), el cual es una herramienta que facilita en gran medida los cálculos
realizados en este trabajo.

Se recomienda para trabajos futuros realizar la síntesis utilizando otros métodos para la
localización espacial, como pueden ser la utilización de números duales, cuaterniones, par
de rotación, ya que aunque las matrices de rotación facilitan la comprensión de los
movimientos de los eslabones presentan cierta dificultad en el manejo de la composición de
las rotaciones.

95
En los programas se presentan los siguientes cambios en la utilización de variables:
f2 = φ2 t2 = θ2 p2 = ϕ2 i2 = µ2 l2 = ρ2 n2 = η2 b2 = β2
f3 = φ3 t3 = θ3 p3 = ϕ3 i3 = µ3 l3 = ρ3 n3 = η3 b3 = β3
f4 = φ4 t4 = θ4 p4 = ϕ4 i4 = µ4 l4 = ρ4 n4 = η4 b4 = β4
f5 = φ5 t5 = θ5 p5 = ϕ5 i5 = µ5 l5 = ρ5 n5 = η5 b5 = β5
f6 = φ6 t6 = θ6 p6 = ϕ6 i6 = µ6 l6 = ρ6 n6 = η6 b6 = β6
z5 = γ1 z6 = λ1 m=γ s=λ
wb = ωβ wf = ωφ wp = ωϕ wt = ωθ wa = ωα wi = ωµ wl = ωρ wn = ωη
ab = αβ af = αφ ap = αϕ at = αθ aa = αα ai = αµ ap = αρ an = αη

PROGRAMA PARA EL CALCULO DE LAS COMPONENTES DEL ESLABÓN W1 Y DE SU PIVOTE

DE TIERRA
clear all %LIMPIA TODAS LAS VARIABLES DEL WORKSPACE
clc %LIMPIA LA PANTALLA
damp=.01; %INCREMENTO PARA DERIVAR
PARCIALMENTE
%******************************** %ESTOS VALORES SON ARBITRARIOS Y SE
%ENTRADA DE VALORES INICIALES %UTILIZAN PARA INICIAR EL PROCESO DEL
%******************************** %CALCULO.
w1u = input ('componente en el eje u del vector w1 ');
w1v = input ('componente en el eje v del vector w1 ');
z1x= input ('componente en el eje x del vector z1 ');
z1y= input ('componente en el eje y del vector z1 ');
z1z= input ('componente en el eje z del vector z1 ');
z2= input ('componente en el eje x del pivote de tierra de w1 ');
z3= input ('componente en el eje y del pivote de tierra de w1 ');
z4= input ('componente en el eje z del pivote de tierra de w1 ');
z5= input ('ángulo de rotación sobre el eje y');
z6= input ('ángulo de rotación sobre el eje x rotado');
f2 = input ('ángulo de rotación f2 ');
f3 = input ('ángulo de rotación f3 ');
f4 = input ('ángulo de rotación f4 ');
f5 = input ('ángulo de rotación f5 ');
f6= input ('ángulo de rotación f6 ');
p2= input ('ángulo de rotación p2 ');
p3= input ('ángulo de rotación p3 ');
p4= input ('ángulo de rotación p4');
p5= input ('ángulo de rotación p5');
p6= input ('ángulo de rotación p6 ');
t2= input ('ángulo de rotación t2 ');
t3= input ('ángulo de rotación t3 ');
t4= input ('ángulo de rotación t4 ');
t5= input ('ángulo de rotación t5 ');
t6= input ('ángulo de rotación t6 ');

%***********************************
%ENTRADA DE VALORES PRESCRITOS %ESTOS VALORES SON CONSTANTES
%***********************************

b2 = input ('ángulo del eslabón de entrada b2 ');

b3 = input ('ángulo del eslabón de entrada b3 ');
b4 = input ('ángulo del eslabón de entrada b4 ');
b5 = input ('ángulo del eslabón de entrada b5 ');
b6= input ('ángulo del eslabón de entrada b6 ');
p1x= input ('componente en x del primer punto de precisión ');

96
p1y= input ('componente en y del primer punto de precisión ');
p1z= input ('componente en z del primer punto de precisión ');
p2x= input ('componente en x del segundo punto de precisión ');
p2y= input ('componente en y del segundo punto de precisión ');
p2z= input ('componente en z del segundo punto de precisión ');
p3x= input ('componente en x del tercer punto de precisión ');
p3y= input ('componente en y del tercer punto de precisión ');
p3z= input ('componente en z del tercer punto de precisión ');
p4x= input ('componente en x del cuarto punto de precisión ');
p4y= input ('componente en y del cuarto punto de precisión ');
p4z= input ('componente en z del cuarto punto de precisión ');
p5x= input ('componente en x del quinto punto de precisión ');
p5y= input ('componente en y del quinto punto de precisión ');
p5z= input ('componente en z del quinto punto de precisión ');
p6x= input ('componente en x del sexto punto de precisión ');
p6y= input ('componente en y del sexto punto de precisión ');
p6z= input ('componente en x del sexto punto de precisión ');
r = 400; % NUMERO DE ITERACIONES
%**********************
%CICLO DE ITERACIONES
%**********************
for n=1:r;
w1up=w1u+damp;
w1vp=w1v+damp;
z1xp=z1x+damp;
z1yp=z1y+damp;
z1zp=z1z+damp;
z2p=z2+damp;
z3p=z3+damp;
z4p=z4+damp;
z5p=z5+damp;
z6p=z6+damp;
************************
%CAMBIO DE VARIABLES
************************
k1=(z1x)*(cos(f2)*cos(p2)-sin(f2)*cos(t2)*sin(p2))+(z1y)*(-cos(f2)*sin(p2)-
sin(f2)*cos(t2)*cos(p2))+(z1z)*(sin(f2)*sin(t2));
k2=(z1x)*(sin(f2)*cos(p2)+cos(f2)*cos(t2)*sin(p2))+(z1y)*(-sin(f2)*sin(p2)+
cos(f2)*cos(t2)*cos(p2))+(z1z)*(-cos(f2)*sin(t2));
k3=(z1x)*sin(t2)*sin(p2)+(z1y)*sin(t2)*cos(p2)+(z1z)*cos(t2);
k4=(z1x)*(cos(f3)*cos(p3)-sin(f3)*cos(t3)*sin(p3))+(z1y)*(-cos(f3)*sin(p3)-
sin(f3)*cos(t3)*cos(p3))+(z1z)*(sin(f3)*sin(t3));
k5=(z1x)*(sin(f3)*cos(p3)+cos(f3)*cos(t3)*sin(p3))+(z1y)*(-sin(f3)*sin(p3)+
cos(f3)*cos(t3)*cos(p3))+(z1z)*(-cos(f3)*sin(t3));
k6=(z1x)*sin(t3)*sin(p3)+(z1y)*sin(t3)*cos(p3)+(z1z)*cos(t3);
k7=(z1x)*(cos(f4)*cos(p4)-sin(f4)*cos(t4)*sin(p4))+(z1y)*(-cos(f4)*sin(p4)-
sin(f4)*cos(t4)*cos(p4))+(z1z)*(sin(f4)*sin(t4));
k8=(z1x)*(sin(f4)*cos(p4)+cos(f4)*cos(t4)*sin(p4))+(z1y)*(-sin(f4)*sin(p4)+
cos(f4)*cos(t4)*cos(p4))+(z1z)*(-cos(f4)*sin(t4));
k9=(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4);
k10=(z1x)*(cos(f5)*cos(p5)-sin(f5)*cos(t5)*sin(p5))+(z1y)*(-cos(f5)*sin(p5)-
sin(f5)*cos(t5)*cos(p5))+(z1z)*(sin(f5)*sin(t5));
k11=(z1x)*(sin(f5)*cos(p5)+cos(f5)*cos(t5)*sin(p5))+(z1y)*(-sin(f5)*sin(p5)+
cos(f5)*cos(t5)*cos(p5))+(z1z)*(-cos(f5)*sin(t5));
k12=(z1x)*sin(t5)*sin(p5)+(z1y)*sin(t5)*cos(p5)+(z1z)*cos(t5);

97
k13=(z1x)*(cos(f6)*cos(p6)-sin(f6)*cos(t6)*sin(p6))+(z1y)*(-cos(f6)*sin(p6)-
sin(f6)*cos(t6)*cos(p6))+(z1z)*(sin(f6)*sin(t6));
k14=(z1x)*(sin(f6)*cos(p6)+cos(f6)*cos(t6)*sin(p6))+(z1y)*(-
sin(f6)*sin(p6)+cos(f6)*cos(t6)*cos(p6))+(z1z)*(-cos(f6)*sin(t6));
k15=(z1x)*sin(t6)*sin(p6)+(z1y)*sin(t6)*cos(p6)+(z1z)*cos(t6);
d1=(cos(f2)*cos(p2)-sin(f2)*cos(t2)*sin(p2));
d2=(-cos(f2)*sin(p2)-sin(f2)*cos(t2)*cos(p2));
d3=(sin(f2)*sin(t2));
d4=(sin(f2)*cos(p2)+cos(f2)*cos(t2)*sin(p2));
d5=(-sin(f2)*sin(p2)+cos(f2)*cos(t2)*cos(p2));
d6=(-cos(f2)*sin(t2));
d7=sin(t2)*sin(p2);
d8=sin(t2)*cos(p2);
d9=cos(t2);
d10=(cos(f3)*cos(p3)-sin(f3)*cos(t3)*sin(p3));
d11=(-cos(f3)*sin(p3)-sin(f3)*cos(t3)*cos(p3));
d12=(sin(f3)*sin(t3));
d13=(sin(f3)*cos(p3)+cos(f3)*cos(t3)*sin(p3));
d14=(-sin(f3)*sin(p3)+cos(f3)*cos(t3)*cos(p3));
d15=(-cos(f3)*sin(t3));
d16=sin(t3)*sin(p3);
d17=sin(t3)*cos(p3);
d18=cos(t3);
d19=(cos(f4)*cos(p4)-sin(f4)*cos(t4)*sin(p4));
d20=(-cos(f4)*sin(p4)-sin(f4)*cos(t4)*cos(p4));
d21=(sin(f4)*sin(t4));
d22=(sin(f4)*cos(p4)+cos(f4)*cos(t4)*sin(p4));
d23=(-sin(f4)*sin(p4)+cos(f4)*cos(t4)*cos(p4));
d24=(-cos(f4)*sin(t4));
d25=sin(t4)*sin(p4);
d26=sin(t4)*cos(p4);
d27=cos(t4);
d28=(cos(f5)*cos(p5)-sin(f5)*cos(t5)*sin(p5));
d29=(-cos(f5)*sin(p5)-sin(f5)*cos(t5)*cos(p5));
d30=(sin(f5)*sin(t5));
d31=(sin(f5)*cos(p5)+cos(f5)*cos(t5)*sin(p5));
d32=(-sin(f5)*sin(p5)+cos(f5)*cos(t5)*cos(p5));
d33=(-cos(f5)*sin(t5));
d34=sin(t5)*sin(p5);
d35=sin(t5)*cos(p5);
d36=cos(t5);
d37=(cos(f6)*cos(p6)-sin(f6)*cos(t6)*sin(p6));
d38=(-cos(f6)*sin(p6)-sin(f6)*cos(t6)*cos(p6));
d39=(sin(f6)*sin(t6));
d40=(sin(f6)*cos(p6)+cos(f6)*cos(t6)*sin(p6));
d41=(-sin(f6)*sin(p6)+cos(f6)*cos(t6)*cos(p6));
d42=(-cos(f6)*sin(t6));
d43=sin(t6)*sin(p6);
d44=sin(t6)*cos(p6);
d45=cos(t6);
bx=w1u*cos(z5)+w1v*sin(z5)*sin(z6);
b1x=w1up*cos(z5)+w1v*sin(z5)*sin(z6);
b2x=w1u*cos(z5)+w1vp*sin(z5)*sin(z6);
b3x=w1u*cos(z5p)+w1v*sin(z5p)*sin(z6);
b4x=w1u*cos(z5)+w1v*sin(z5)*sin(z6p);
by=w1v*cos(z6);

98
b2y=w1vp*cos(z6);
b4y=w1v*cos(z6p);
bz=-w1u*sin(z5)+w1v*cos(z5)*sin(z6);
b1z=-w1up*sin(z5)+w1v*cos(z5)*sin(z6);
b2z=-w1u*sin(z5)+w1vp*cos(z5)*sin(z6);
b3z=-w1u*sin(z5p)+w1v*cos(z5p)*sin(z6);
b4z=-w1u*sin(z5)+w1v*cos(z5)*sin(z6p);

h1=w1u*(cos(b2)*cos(z5)+sin(z5)*sin(z6)*sin(b2))+w1v*(-sin(b2)*cos(z5)+sin(z5)*sin(z6)*cos(b2))+k1-
(bx+z1x+p2x-p1x);
h2=w1u*(sin(b2)*cos(z6))+w1v*cos(b2)*cos(z6)+k2-(by+z1y+p2y-p1y);
h3=w1u*(-sin(z5)*cos(b2)+cos(z5)*sin(z6)*sin(b2))+w1v*(sin(z5)*sin(b2)+cos(z5)*sin(z6)*cos(b2))+k3-
(bz+z1z+p2z-p1z);
h4=w1u*(cos(b3)*cos(z5)+sin(z5)*sin(z6)*sin(b3))+w1v*(-sin(b3)*cos(z5)+sin(z5)*sin(z6)*cos(b3))+k4-
(bx+z1x+p3x-p1x);
h5=w1u*(sin(b3)*cos(z6))+w1v*cos(b3)*cos(z6)+k5-(by+z1y+p3y-p1y);
h6=w1u*(-sin(z5)*cos(b3)+cos(z5)*sin(z6)*sin(b3))+w1v*(sin(z5)*sin(b3)+cos(z5)*sin(z6)*cos(b3))+k6-
(bz+z1z+p3z-p1z);
h7=w1u*(cos(b4)*cos(z5)+sin(z5)*sin(z6)*sin(b4))+w1v*(-sin(b4)*cos(z5)+sin(z5)*sin(z6)*cos(b4))+k7-
(bx+z1x+p4x-p1x);
h8=w1u*(sin(b4)*cos(z6))+w1v*cos(b4)*cos(z6)+k8-(by+z1y+p4y-p1y);
h9=w1u*(-sin(z5)*cos(b4)+cos(z5)*sin(z6)*sin(b4))+w1v*(sin(z5)*sin(b4)+cos(z5)*sin(z6)*cos(b4))+k9-
(bz+z1z+p4z-p1z);
h10=w1u*(cos(b5)*cos(z5)+sin(z5)*sin(z6)*sin(b5))+w1v*(-
sin(b5)*cos(z5)+sin(z5)*sin(z6)*cos(b5))+k10-(bx+z1x+p5x-p1x);
h11=w1u*(sin(b5)*cos(z6))+w1v*cos(b5)*cos(z6)+k11-(by+z1y+p5y-p1y);
h12=w1u*(-sin(z5)*cos(b5)+cos(z5)*sin(z6)*sin(b5))+
w1v*(sin(z5)*sin(b5)+cos(z5)*sin(z6)*cos(b5))+k12-(bz+z1z+p5z-p1z);
h13=w1u*(cos(b6)*cos(z5)+sin(z5)*sin(z6)*sin(b6))+w1v*(-
sin(b6)*cos(z5)+sin(z5)*sin(z6)*cos(b6))+k13-(bx+z1x+p6x-p1x);
h14=w1u*(sin(b6)*cos(z6))+w1v*cos(b6)*cos(z6)+k14-(by+z1y+p6y-p1y);
h15=w1u*(-
sin(z5)*cos(b6)+cos(z5)*sin(z6)*sin(b6))+w1v*(sin(z5)*sin(b6)+cos(z5)*sin(z6)*cos(b6))+k15-
(bz+z1z+p6z-p1z);
h16=z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-p1x;
h17=z3+w1v*cos(z6)+z1y-p1y;
h18=z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-p1z;

%****************************
%FORMACION DEL JACOBIANO
%****************************
J(1,1)=(w1up*(cos(b2)*cos(z5)+sin(z5)*sin(z6)*sin(b2))+w1v*(-sin(b2)*cos(z5)+
sin(z5)*sin(z6)*cos(b2))+(z1x)*d1+(z1y)*d2+(z1z)*d3-(b1x+z1x+p2x-p1x)-h1)/damp;
J(1,2)=(w1u*(cos(b2)*cos(z5)+sin(z5)*sin(z6)*sin(b2))+w1vp*(-sin(b2)*cos(z5)+
sin(z5)*sin(z6)*cos(b2))+(z1x)*d1+(z1y)*d2+(z1z)*d3-(b2x+z1x+p2x-p1x)-h1)/damp;
J(1,3)=(w1u*(cos(b2)*cos(z5)+sin(z5)*sin(z6)*sin(b2))+w1v*(-sin(b2)*cos(z5)+
sin(z5)*sin(z6)*cos(b2))+(z1xp)*d1+(z1y)*d2+(z1z)*d3-(bx+z1xp+p2x-p1x)-h1)/damp;
J(1,4)=(w1u*(cos(b2)*cos(z5)+sin(z5)*sin(z6)*sin(b2))+w1v*(-sin(b2)*cos(z5)+
sin(z5)*sin(z6)*cos(b2))+(z1x)*d1+(z1yp)*d2+(z1z)*d3-(bx+z1x+p2x-p1x)-h1)/damp;
J(1,5)=(w1u*(cos(b2)*cos(z5)+sin(z5)*sin(z6)*sin(b2))+w1v*(-sin(b2)*cos(z5)+
sin(z5)*sin(z6)*cos(b2))+(z1x)*d1+(z1y)*d2+(z1zp)*d3-(bx+z1x+p2x-p1x)-h1)/damp;
J(1,6)=0;
J(1,7)=0;

99
J(1,8)=0;
J(1,9)=(w1u*(cos(b2)*cos(z5p)+sin(z5p)*sin(z6)*sin(b2))+w1v*(-sin(b2)*cos(z5p)+
sin(z5p)*sin(z6)*cos(b2))+(z1x)*d1+(z1y)*d2+(z1z)*d3-(b3x+z1x+p2x-p1x)-h1)/damp;
J(1,10)=(w1u*(cos(b2)*cos(z5)+sin(z5)*sin(z6p)*sin(b2))+w1v*(-sin(b2)*cos(z5)+
sin(z5)*sin(z6p)*cos(b2))+(z1x)*d1+(z1y)*d2+(z1z)*d3-(b4x+z1x+p2x-p1x)-h1)/damp;
J(2,1)=(w1up*(sin(b2)*cos(z6))+w1v*cos(b2)*cos(z6)+(z1x)*d4+(z1y)*d5+(z1z)*d6-(by+z1y+p2y-p1y)-
h2)/damp;
J(2,2)=(w1u*(sin(b2)*cos(z6))+w1vp*cos(b2)*cos(z6)+(z1x)*d4+(z1y)*d5+(z1z)*d6-(b2y+z1y+p2y-p1y)-
h2)/damp;
J(2,3)=(w1u*(sin(b2)*cos(z6))+w1v*cos(b2)*cos(z6)+(z1xp)*d4+(z1y)*d5+(z1z)*d6-(by+z1y+p2y-p1y)-
h2)/damp;
J(2,4)=(w1u*(sin(b2)*cos(z6))+w1v*cos(b2)*cos(z6)+(z1x)*d4+(z1yp)*d5+(z1z)*d6-(by+z1yp+p2y-p1y)-
h2)/damp;
J(2,5)=(w1u*(sin(b2)*cos(z6))+w1v*cos(b2)*cos(z6)+(z1x)*d4+(z1y)*d5+(z1zp)*d6-(by+z1y+p2y-p1y)-
h2)/damp;
J(2,6)=0;
J(2,7)=0;
J(2,8)=0;J(2,9)=0;
J(2,10)=(w1u*(sin(b2)*cos(z6p))+w1v*cos(b2)*cos(z6p)+(z1x)*d4+(z1y)*d5+(z1z)*d6-(b4y+z1y+p2y-
p1y)-h2)/damp;
J(3,1)=(w1up*(-sin(z5)*cos(b2)+cos(z5)*sin(z6)*sin(b2))+w1v*(sin(z5)*sin(b2)+
cos(z5)*sin(z6)*cos(b2))+(z1x)*sin(t2)*sin(p2)+(z1y)*sin(t2)*cos(p2)+(z1z)*cos(t2)-(b1z+z1z+p2z-p1z)-
h3)/damp;
J(3,2)=(w1u*(-sin(z5)*cos(b2)+cos(z5)*sin(z6)*sin(b2))+w1vp*(sin(z5)*sin(b2)+
cos(z5)*sin(z6)*cos(b2))+(z1x)*sin(t2)*sin(p2)+(z1y)*sin(t2)*cos(p2)+(z1z)*cos(t2)-(b2z+z1z+p2z-p1z)-
h3)/damp;
J(3,3)=(w1u*(-sin(z5)*cos(b2)+cos(z5)*sin(z6)*sin(b2))+w1v*(sin(z5)*sin(b2)+
cos(z5)*sin(z6)*cos(b2))+(z1xp)*sin(t2)*sin(p2)+(z1y)*sin(t2)*cos(p2)+(z1z)*cos(t2)-(bz+z1z+p2z-p1z)-
h3)/damp;
J(3,4)=(w1u*(-sin(z5)*cos(b2)+cos(z5)*sin(z6)*sin(b2))+w1v*(sin(z5)*sin(b2)+
cos(z5)*sin(z6)*cos(b2))+(z1x)*sin(t2)*sin(p2)+(z1yp)*sin(t2)*cos(p2)+(z1z)*cos(t2)-(bz+z1z+p2z-p1z)-
h3)/damp;
J(3,5)=(w1u*(-
sin(z5)*cos(b2)+cos(z5)*sin(z6)*sin(b2))+w1v*(sin(z5)*sin(b2)+cos(z5)*sin(z6)*cos(b2))+(z1x)*sin(t2)*sin
(p2)+(z1y)*sin(t2)*cos(p2)+(z1zp)*cos(t2)-(bz+z1zp+p2z-p1z)-h3)/damp;
J(3,6)=0;J(3,7)=0;J(3,8)=0;
J(3,9)=(w1u*(-
sin(z5p)*cos(b2)+cos(z5p)*sin(z6)*sin(b2))+w1v*(sin(z5p)*sin(b2)+cos(z5p)*sin(z6)*cos(b2))+(z1x)*sin(t2
)*sin(p2)+(z1y)*sin(t2)*cos(p2)+(z1z)*cos(t2)-(b3z+z1z+p2z-p1z)-h3)/damp;
J(3,10)=(w1u*(-sin(z5)*cos(b2)+cos(z5)*sin(z6p)*sin(b2))+w1v*(sin(z5)*sin(b2)+
cos(z5)*sin(z6p)*cos(b2))+(z1x)*sin(t2)*sin(p2)+(z1y)*sin(t2)*cos(p2)+(z1z)*cos(t2)-(b4z+z1z+p2z-p1z)-
h3)/damp;
J(4,1)=(w1up*(cos(b3)*cos(z5)+sin(z5)*sin(z6)*sin(b3))+w1v*(-sin(b3)*cos(z5)+sin(z5)*sin(z6)*cos(b3))+
(z1x)*d10+(z1y)*d11+(z1z)*d12-(b1x+z1x+p3x-p1x)-h4)/damp;
J(4,2)=(w1u*(cos(b3)*cos(z5)+sin(z5)*sin(z6)*sin(b3))+w1vp*(-sin(b3)*cos(z5)+sin(z5)*sin(z6)*cos(b3))+
(z1x)*d10+(z1y)*d11+(z1z)*d12-(b2x+z1x+p3x-p1x)-h4)/damp;
J(4,3)=(w1u*(cos(b3)*cos(z5)+sin(z5)*sin(z6)*sin(b3))+w1v*(-sin(b3)*cos(z5)+sin(z5)*sin(z6)*cos(b3))+
(z1xp)*d10+(z1y)*d11+(z1z)*d12-(bx+z1xp+p3x-p1x)-h4)/damp;
J(4,4)=(w1u*(cos(b3)*cos(z5)+sin(z5)*sin(z6)*sin(b3))+w1v*(-sin(b3)*cos(z5)+sin(z5)*sin(z6)*cos(b3))+
(z1x)*d10+(z1yp)*d11+(z1z)*d12-(bx+z1x+p3x-p1x)-h4)/damp;
J(4,5)=(w1u*(cos(b3)*cos(z5)+sin(z5)*sin(z6)*sin(b3))+w1v*(-sin(b3)*cos(z5)+sin(z5)*sin(z6)*cos(b3))+
(z1x)*d10+(z1y)*d11+(z1zp)*d12-(bx+z1x+p3x-p1x)-h4)/damp;
J(4,6)=0;
J(4,7)=0;J(4,8)=0;

J(4,9)=(w1u*(cos(b3)*cos(z5p)+sin(z5p)*sin(z6)*sin(b3))+w1v*(sin(b3)*cos(z5p)+

100
sin(z5p)*sin(z6)*cos(b3))+(z1x)*d10+(z1y)*d11+(z1z)*d12-(b3x+z1x+p3x-p1x)-h4)/damp;
J(4,10)=(w1u*(cos(b3)*cos(z5)+sin(z5)*sin(z6p)*sin(b3))+w1v*(-sin(b3)*cos(z5)+
sin(z5)*sin(z6p)*cos(b3))+(z1x)*d10+(z1y)*d11+(z1z)*d12-(b4x+z1x+p3x-p1x)-h4)/damp;
J(5,1)=(w1up*(sin(b3)*cos(z6))+w1v*cos(b3)*cos(z6)+(z1x)*d13+(z1y)*d14+(z1z)*d15-(by+z1y+p3y-
p1y)-h5)/damp;
J(5,2)=(w1u*(sin(b3)*cos(z6))+w1vp*cos(b3)*cos(z6)+(z1x)*d13+(z1y)*d14+(z1z)*d15-(b2y+z1y+p3y-
p1y)-h5)/damp;
J(5,3)=(w1u*(sin(b3)*cos(z6))+w1v*cos(b3)*cos(z6)+(z1xp)*d13+(z1y)*d14+(z1z)*d15-(by+z1y+p3y-
p1y)-h5)/damp;
J(5,4)=(w1u*(sin(b3)*cos(z6))+w1v*cos(b3)*cos(z6)+(z1x)*d13+(z1yp)*d14+(z1z)*d15-(by+z1yp+p3y-
p1y)-h5)/damp;
J(5,5)=(w1u*(sin(b3)*cos(z6))+w1v*cos(b3)*cos(z6)+(z1x)*d13+(z1y)*d14+(z1zp)*d15-(by+z1y+p3y-
p1y)-h5)/damp;
J(5,6)=0;
J(5,7)=0;
J(5,8)=0;
J(5,9)=0;
J(5,10)=(w1u*(sin(b3)*cos(z6p))+w1v*cos(b3)*cos(z6p)+(z1x)*d13+(z1y)*d14+(z1z)*d15-(b4y+z1y+p3y-
p1y)-h5)/damp;
J(6,1)=(w1up*(-sin(z5)*cos(b3)+cos(z5)*sin(z6)*sin(b3))+w1v*(sin(z5)*sin(b3)+cos(z5)*sin(z6)*cos(b3))+
(z1x)*sin(t3)*sin(p3)+(z1y)*sin(t3)*cos(p3)+(z1z)*cos(t3)-(b1z+z1z+p3z-p1z)-h6)/damp;
J(6,2)=(w1u*(-sin(z5)*cos(b3)+cos(z5)*sin(z6)*sin(b3))+w1vp*(sin(z5)*sin(b3)+
cos(z5)*sin(z6)*cos(b3))+(z1x)*sin(t3)*sin(p3)+(z1y)*sin(t3)*cos(p3)+(z1z)*cos(t3)-(b2z+z1z+p3z-p1z)-
h6)/damp;
J(6,3)=(w1u*(-sin(z5)*cos(b3)+cos(z5)*sin(z6)*sin(b3))+w1v*(sin(z5)*sin(b3)+
cos(z5)*sin(z6)*cos(b3))+(z1xp)*sin(t3)*sin(p3)+(z1y)*sin(t3)*cos(p3)+(z1z)*cos(t3)-(bz+z1z+p3z-p1z)-
h6)/damp;
J(6,4)=(w1u*(-sin(z5)*cos(b3)+cos(z5)*sin(z6)*sin(b3))+w1v*(sin(z5)*sin(b3)+
cos(z5)*sin(z6)*cos(b3))+(z1x)*sin(t3)*sin(p3)+(z1yp)*sin(t3)*cos(p3)+(z1z)*cos(t3)-(bz+z1z+p3z-p1z)-
h6)/damp;
J(6,5)=(w1u*(-sin(z5)*cos(b3)+cos(z5)*sin(z6)*sin(b3))+w1v*(sin(z5)*sin(b3)+
cos(z5)*sin(z6)*cos(b3))+(z1x)*sin(t3)*sin(p3)+(z1y)*sin(t3)*cos(p3)+(z1zp)*cos(t3)-(bz+z1z+p3z-p1z)-
h6)/damp;
J(6,6)=0;
J(6,7)=0;
J(6,8)=0;
J(6,9)=(w1u*(-sin(z5p)*cos(b3)+cos(z5p)*sin(z6)*sin(b3))+w1v*(sin(z5p)*sin(b3)+
cos(z5p)*sin(z6)*cos(b3))+(z1x)*sin(t3)*sin(p3)+(z1y)*sin(t3)*cos(p3)+(z1z)*cos(t3)-(b3z+z1z+p3z-p1z)-
h6)/damp;
J(6,10)=(w1u*(-sin(z5)*cos(b3)+cos(z5)*sin(z6p)*sin(b3))+w1v*(sin(z5)*sin(b3)+
cos(z5)*sin(z6p)*cos(b3))+(z1x)*sin(t3)*sin(p3)+(z1y)*sin(t3)*cos(p3)+(z1z)*cos(t3)-(b4z+z1z+p3z-p1z)-
h6)/damp;
J(7,1)=(w1up*(cos(b4)*cos(z5)+sin(z5)*sin(z6)*sin(b4))+w1v*(-sin(b4)*cos(z5)+sin(z5)*sin(z6)*cos(b4))+
(z1x)*d19+(z1y)*d20+(z1z)*d21-(b1x+z1x+p4x-p1x)-h7)/damp;
J(7,2)=(w1u*(cos(b4)*cos(z5)+sin(z5)*sin(z6)*sin(b4))+w1vp*(-sin(b4)*cos(z5)+sin(z5)*sin(z6)*cos(b4))+
(z1x)*d19+(z1y)*d20+(z1z)*d21-(b2x+z1x+p4x-p1x)-h7)/damp;
J(7,3)=(w1u*(cos(b4)*cos(z5)+sin(z5)*sin(z6)*sin(b4))+w1v*(-sin(b4)*cos(z5)+sin(z5)*sin(z6)*cos(b4))+
(z1xp)*d19+(z1y)*d20+(z1z)*d21-(bx+z1xp+p4x-p1x)-h7)/damp;
J(7,4)=(w1u*(cos(b4)*cos(z5)+sin(z5)*sin(z6)*sin(b4))+w1v*(-sin(b4)*cos(z5)+sin(z5)*sin(z6)*cos(b4))+
(z1x)*d19+(z1yp)*d20+(z1z)*d21-(bx+z1x+p4x-p1x)-h7)/damp;
J(7,5)=(w1u*(cos(b4)*cos(z5)+sin(z5)*sin(z6)*sin(b4))+w1v*(-sin(b4)*cos(z5)+sin(z5)*sin(z6)*cos(b4))+
(z1x)*d19+(z1y)*d20+(z1zp)*d21-(bx+z1x+p4x-p1x)-h7)/damp;

J(7,6)=0;

101
J(7,7)=0;
J(7,8)=0;
J(7,9)=(w1u*(cos(b4)*cos(z5p)+sin(z5p)*sin(z6)*sin(b4))+w1v*(-sin(b4)*cos(z5p)+
sin(z5p)*sin(z6)*cos(b4))+(z1x)*d19+(z1y)*d20+(z1z)*d21-(b3x+z1x+p4x-p1x)-h7)/damp;
J(7,10)=(w1u*(cos(b4)*cos(z5)+sin(z5)*sin(z6p)*sin(b4))+w1v*(sin(b4)*cos(z5)+
sin(z5)*sin(z6p)*cos(b4))+(z1x)*d19+(z1y)*d20+(z1z)*d21-(b4x+z1x+p4x-p1x)-h7)/damp;
J(8,1)=(w1up*(sin(b4)*cos(z6))+w1v*cos(b4)*cos(z6)+(z1x)*d22+(z1y)*d23+(z1z)*d24-(by+z1y+p4y-
p1y)-h8)/damp;
J(8,2)=(w1u*(sin(b4)*cos(z6))+w1vp*cos(b4)*cos(z6)+(z1x)*d22+(z1y)*d23+(z1z)*d24-(b2y+z1y+p4y-
p1y)-h8)/damp;
J(8,3)=(w1u*(sin(b4)*cos(z6))+w1v*cos(b4)*cos(z6)+(z1xp)*d22+(z1y)*d23+(z1z)*d24-(by+z1y+p4y-
p1y)-h8)/damp;
J(8,4)=(w1u*(sin(b4)*cos(z6))+w1v*cos(b4)*cos(z6)+(z1x)*d22+(z1yp)*d23+(z1z)*d24-(by+z1yp+p4y-
p1y)-h8)/damp;
J(8,5)=(w1u*(sin(b4)*cos(z6))+w1v*cos(b4)*cos(z6)+(z1x)*d22+(z1y)*d23+(z1zp)*d24-(by+z1y+p4y-
p1y)-h8)/damp;
J(8,6)=0;
J(8,7)=0;
J(8,8)=0;
J(8,9)=0;
J(8,10)=(w1u*(sin(b4)*cos(z6p))+w1v*cos(b4)*cos(z6p)+(z1x)*d22+(z1y)*d23+(z1z)*d24-(b4y+z1y+p4y-
p1y)-h8)/damp;
J(9,1)=(w1up*(-sin(z5)*cos(b4)+cos(z5)*sin(z6)*sin(b4))+w1v*(sin(z5)*sin(b4)+cos(z5)*sin(z6)*cos(b4))+
(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4)-(b1z+z1z+p4z-p1z)-h9)/damp;
J(9,2)=(w1u*(-sin(z5)*cos(b4)+cos(z5)*sin(z6)*sin(b4))+w1vp*(sin(z5)*sin(b4)+
cos(z5)*sin(z6)*cos(b4))+(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4)-(b2z+z1z+p4z-p1z)-
h9)/damp;
J(9,3)=(w1u*(-sin(z5)*cos(b4)+cos(z5)*sin(z6)*sin(b4))+w1v*(sin(z5)*sin(b4)+cos(z5)*sin(z6)*cos(b4))+
(z1xp)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4)-(bz+z1z+p4z-p1z)-h9)/damp;
J(9,4)=(w1u*(-sin(z5)*cos(b4)+cos(z5)*sin(z6)*sin(b4))+w1v*(sin(z5)*sin(b4)+cos(z5)*sin(z6)*cos(b4))+
(z1x)*sin(t4)*sin(p4)+(z1yp)*sin(t4)*cos(p4)+(z1z)*cos(t4)-(bz+z1z+p4z-p1z)-h9)/damp;
J(9,5)=(w1u*(-sin(z5)*cos(b4)+cos(z5)*sin(z6)*sin(b4))+w1v*(sin(z5)*sin(b4)+cos(z5)*sin(z6)*cos(b4))+
(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1zp)*cos(t4)-(bz+z1zp+p4z-p1z)-h9)/damp;
J(9,6)=0;
J(9,7)=0;
J(9,8)=0;
J(9,9)=(w1u*(-
sin(z5p)*cos(b4)+cos(z5p)*sin(z6)*sin(b4))+w1v*(sin(z5p)*sin(b4)+cos(z5p)*sin(z6)*cos(b4))+
(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4)-(b3z+z1z+p4z-p1z)-h9)/damp;
J(9,10)=(w1u*(-sin(z5)*cos(b4)+cos(z5)*sin(z6p)*sin(b4))+w1v*(sin(z5)*sin(b4)+
cos(z5)*sin(z6p)*cos(b4))+(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4)-(b4z+z1z+p4z-p1z)-
h9)/damp;
J(10,1)=(w1up*(cos(b5)*cos(z5)+sin(z5)*sin(z6)*sin(b5))+w1v*(-sin(b5)*cos(z5)+
sin(z5)*sin(z6)*cos(b5))+(z1x)*d28+(z1y)*d29+(z1z)*d30-(b1x+z1x+p5x-p1x)-h10)/damp;
J(10,2)=(w1u*(cos(b5)*cos(z5)+sin(z5)*sin(z6)*sin(b5))+w1vp*(-sin(b5)*cos(z5)+
sin(z5)*sin(z6)*cos(b5))+(z1x)*d28+(z1y)*d29+(z1z)*d30-(b2x+z1x+p5x-p1x)-h10)/damp;
J(10,3)=(w1u*(cos(b5)*cos(z5)+sin(z5)*sin(z6)*sin(b5))+w1v*(-sin(b5)*cos(z5)+
sin(z5)*sin(z6)*cos(b5))+(z1xp)*d28+(z1y)*d29+(z1z)*d30-(bx+z1xp+p5x-p1x)-h10)/damp;
J(10,4)=(w1u*(cos(b5)*cos(z5)+sin(z5)*sin(z6)*sin(b5))+w1v*(-sin(b5)*cos(z5)+
sin(z5)*sin(z6)*cos(b5))+(z1x)*d28+(z1yp)*d29+(z1z)*d30-(bx+z1x+p5x-p1x)-h10)/damp;

102
J(10,5)=(w1u*(cos(b5)*cos(z5)+sin(z5)*sin(z6)*sin(b5))+w1v*(-sin(b5)*cos(z5)+sin(z5)*sin(z6)*cos(b5))+
(z1x)*d28+(z1y)*d29+(z1zp)*d30-(bx+z1x+p5x-p1x)-h10)/damp;
J(10,6)=0;
J(10,7)=0;
J(10,8)=0;
J(10,9)=(w1u*(-sin(z5p)*cos(b4)+cos(z5p)*sin(z6)*sin(b4))+w1v*(sin(z5p)*sin(b4)+
cos(z5p)*sin(z6)*cos(b4))+(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4)-(b3x+z1x+p4x-p1x)-
h10)/damp;
J(10,10)=(w1u*(-sin(z5)*cos(b4)+cos(z5)*sin(z6p)*sin(b4))+w1v*(sin(z5)*sin(b4)+
cos(z5)*sin(z6p)*cos(b4))+(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4)-(b4x+z1x+p4x-p1x)-
h10)/damp;
J(11,1)=(w1up*(sin(b5)*cos(z6))+w1v*cos(b5)*cos(z6)+(z1x)*d31+(z1y)*d32+(z1z)*d33-(by+z1y+p5y-
p1y)-h11)/damp;
J(11,2)=(w1u*(sin(b5)*cos(z6))+w1vp*cos(b5)*cos(z6)+(z1x)*d31+(z1y)*d32+(z1z)*d33-(b2y+z1y+p5y-
p1y)-h11)/damp;
J(11,3)=(w1u*(sin(b5)*cos(z6))+w1v*cos(b5)*cos(z6)+(z1xp)*d31+(z1y)*d32+(z1z)*d33-(by+z1y+p5y-
p1y)-h11)/damp;
J(11,4)=(w1u*(sin(b5)*cos(z6))+w1v*cos(b5)*cos(z6)+(z1x)*d31+(z1yp)*d32+(z1z)*d33-(by+z1yp+p5y-
p1y)-h11)/damp;
J(11,5)=(w1u*(sin(b5)*cos(z6))+w1v*cos(b5)*cos(z6)+(z1x)*d31+(z1y)*d32+(z1zp)*d33-(by+z1y+p5y-
p1y)-h11)/damp;
J(11,6)=0;
J(11,7)=0;
J(11,8)=0;
J(11,9)=0;
J(11,10)=(w1u*(sin(b5)*cos(z6p))+w1v*cos(b5)*cos(z6p)+(z1x)*d31+(z1y)*d32+(z1z)*d33-(b4y+z1y+p5y-
p1y)-h11)/damp;
J(12,1)=(w1up*(-
sin(z5)*cos(b5)+cos(z5)*sin(z6)*sin(b5))+w1v*(sin(z5)*sin(b5)+cos(z5)*sin(z6)*cos(b5))+(z1x)*sin(t5)*sin
(p5)+(z1y)*sin(t5)*cos(p5)+(z1z)*cos(t5)-(b1z+z1z+p5z-p1z)-h12)/damp;
J(12,2)=(w1u*(-sin(z5)*cos(b5)+cos(z5)*sin(z6)*sin(b5))+w1vp*(sin(z5)*sin(b5)+
cos(z5)*sin(z6)*cos(b5))+(z1x)*sin(t5)*sin(p5)+(z1y)*sin(t5)*cos(p5)+(z1z)*cos(t5)-(b2z+z1z+p5z-p1z)-
h12)/damp;
J(12,3)=(w1u*(-sin(z5)*cos(b5)+cos(z5)*sin(z6)*sin(b5))+w1v*(sin(z5)*sin(b5)+cos(z5)*sin(z6)*cos(b5))+
(z1xp)*sin(t5)*sin(p5)+(z1y)*sin(t5)*cos(p5)+(z1z)*cos(t5)-(bz+z1z+p5z-p1z)-h12)/damp;
J(12,4)=(w1u*(-sin(z5)*cos(b5)+cos(z5)*sin(z6)*sin(b5))+w1v*(sin(z5)*sin(b5)+
cos(z5)*sin(z6)*cos(b5))+(z1x)*sin(t5)*sin(p5)+(z1yp)*sin(t5)*cos(p5)+(z1z)*cos(t5)-(bz+z1z+p5z-p1z)-
h12)/damp;
J(12,5)=(w1u*(-sin(z5)*cos(b5)+cos(z5)*sin(z6)*sin(b5))+w1v*(sin(z5)*sin(b5)+
cos(z5)*sin(z6)*cos(b5))+(z1x)*sin(t5)*sin(p5)+(z1y)*sin(t5)*cos(p5)+(z1zp)*cos(t5)-(bz+z1zp+p5z-p1z)-
h12)/damp;
J(12,6)=0;
J(12,7)=0;
J(12,8)=0;
J(12,9)=(w1u*(-sin(z5p)*cos(b5)+cos(z5p)*sin(z6)*sin(b5))+w1v*(sin(z5p)*sin(b5)+
cos(z5p)*sin(z6)*cos(b5))+(z1x)*sin(t5)*sin(p5)+(z1y)*sin(t5)*cos(p5)+(z1z)*cos(t5)-(b3z+z1z+p5z-p1z)-
h12)/damp;
J(12,10)=(w1u*(-sin(z5)*cos(b5)+cos(z5)*sin(z6p)*sin(b5))+w1v*(sin(z5)*sin(b5)+
cos(z5)*sin(z6p)*cos(b5))+(z1x)*sin(t5)*sin(p5)+(z1y)*sin(t5)*cos(p5)+(z1z)*cos(t5)-(b4z+z1z+p5z-p1z)-
h12)/damp;
J(13,1)=(w1up*(cos(b6)*cos(z5)+sin(z5)*sin(z6)*sin(b6))+w1v*(-sin(b6)*cos(z5)+
sin(z5)*sin(z6)*cos(b6))+(z1x)*d37+(z1y)*d38+(z1z)*d39-(b1x+z1x+p6x-p1x)-h13)/damp;
J(13,2)=(w1u*(cos(b6)*cos(z5)+sin(z5)*sin(z6)*sin(b6))+w1v*(-sin(b6)*cos(z5)+
sin(z5)*sin(z6)*cos(b6))+(z1x)*d37+(z1y)*d38+(z1z)*d39-(b2x+z1x+p6x-p1x)-h13)/damp;

103
J(13,3)=(w1u*(cos(b6)*cos(z5)+sin(z5)*sin(z6)*sin(b6))+w1v*(-
sin(b6)*cos(z5)+sin(z5)*sin(z6)*cos(b6))+(z1xp)*d37+(z1y)*d38+(z1z)*d39-(bx+z1xp+p6x-p1x)-
h13)/damp;
J(13,4)=(w1u*(cos(b6)*cos(z5)+sin(z5)*sin(z6)*sin(b6))+w1v*(-
sin(b6)*cos(z5)+sin(z5)*sin(z6)*cos(b6))+(z1x)*d37+(z1yp)*d38+(z1z)*d39-(bx+z1x+p6x-p1x)-h13)/damp;
J(13,5)=(w1u*(cos(b6)*cos(z5)+sin(z5)*sin(z6)*sin(b6))+w1v*(-
sin(b6)*cos(z5)+sin(z5)*sin(z6)*cos(b6))+(z1x)*d37+(z1y)*d38+(z1zp)*d39-(bx+z1x+p6x-p1x)-h13)/damp;
J(13,6)=0;
J(13,7)=0;
J(13,8)=0;
J(13,9)=(w1u*(cos(b6)*cos(z5p)+sin(z5p)*sin(z6)*sin(b6))+w1v*(-sin(b6)*cos(z5p)+
sin(z5p)*sin(z6)*cos(b6))+(z1x)*d37+(z1y)*d38+(z1z)*d39-(b3x+z1x+p6x-p1x)-h13)/damp;
J(13,10)=(w1u*(cos(b6)*cos(z5)+sin(z5)*sin(z6p)*sin(b6))+w1v*(-sin(b6)*cos(z5)+
sin(z5)*sin(z6p)*cos(b6))+(z1x)*d37+(z1y)*d38+(z1z)*d39-(b4x+z1x+p6x-p1x)-h13)/damp;
J(14,1)=(w1up*(sin(b6)*cos(z6))+w1v*cos(b6)*cos(z6)+(z1x)*d40+(z1y)*d41+(z1z)*d42-(by+z1y+p6y-
p1y)-h14)/damp;
J(14,2)=(w1u*(sin(b6)*cos(z6))+w1vp*cos(b6)*cos(z6)+(z1x)*d40+(z1y)*d41+(z1z)*d42-(b2y+z1y+p6y-
p1y)-h14)/damp;
J(14,3)=(w1u*(sin(b6)*cos(z6))+w1v*cos(b6)*cos(z6)+(z1xp)*d40+(z1y)*d41+(z1z)*d42-(by+z1y+p6y-
p1y)-h14)/damp;
J(14,4)=(w1u*(sin(b6)*cos(z6))+w1v*cos(b6)*cos(z6)+(z1x)*d40+(z1yp)*d41+(z1z)*d42-(by+z1yp+p6y-
p1y)-h14)/damp;
J(14,5)=(w1u*(sin(b6)*cos(z6))+w1v*cos(b6)*cos(z6)+(z1x)*d40+(z1y)*d41+(z1zp)*d42-(by+z1y+p6y-
p1y)-h14)/damp;
J(14,6)=0;J(14,7)=0;
J(14,8)=0;
J(14,9)=0;
J(14,10)=(w1u*(sin(b6)*cos(z6p))+w1v*cos(b6)*cos(z6p)+(z1x)*d40+(z1y)*d41+(z1z)*d42-(b4y+z1y+p6y-
p1y)-h14)/damp;
J(15,1)=(w1up*(-sin(z5)*cos(b6)+cos(z5)*sin(z6)*sin(b6))+w1v*(sin(z5)*sin(b6)+
cos(z5)*sin(z6)*cos(b6))+(z1x)*sin(t6)*sin(p6)+(z1y)*sin(t6)*cos(p6)+(z1z)*cos(t6)-(b1z+z1z+p6z-p1z)-
h15)/damp;
J(15,2)=(w1u*(-sin(z5)*cos(b6)+cos(z5)*sin(z6)*sin(b6))+w1vp*(sin(z5)*sin(b6)+
cos(z5)*sin(z6)*cos(b6))+(z1x)*sin(t6)*sin(p6)+(z1y)*sin(t6)*cos(p6)+(z1z)*cos(t6)-(b2z+z1z+p6z-p1z)-
h15)/damp;
J(15,3)=(w1u*(-sin(z5)*cos(b6)+cos(z5)*sin(z6)*sin(b6))+w1v*(sin(z5)*sin(b6)+
cos(z5)*sin(z6)*cos(b6))+(z1xp)*sin(t6)*sin(p6)+(z1y)*sin(t6)*cos(p6)+(z1z)*cos(t6)-(bz+z1z+p6z-p1z)-
h15)/damp;
J(15,4)=(w1u*(-sin(z5)*cos(b6)+cos(z5)*sin(z6)*sin(b6))+w1v*(sin(z5)*sin(b6)+
cos(z5)*sin(z6)*cos(b6))+(z1x)*sin(t6)*sin(p6)+(z1yp)*sin(t6)*cos(p6)+(z1z)*cos(t6)-(bz+z1z+p6z-p1z)-
h15)/damp;
J(15,5)=(w1u*(-sin(z5)*cos(b6)+cos(z5)*sin(z6)*sin(b6))+w1v*(sin(z5)*sin(b6)+
cos(z5)*sin(z6)*cos(b6))+(z1x)*sin(t6)*sin(p6)+(z1y)*sin(t6)*cos(p6)+(z1zp)*cos(t6)-(bz+z1zp+p6z-p1z)-
h15)/damp;
J(15,6)=0;
J(15,7)=0;
J(15,8)=0;
J(15,9)=(w1u*(-sin(z5p)*cos(b6)+cos(z5p)*sin(z6)*sin(b6))+w1v*(sin(z5p)*sin(b6)+
cos(z5p)*sin(z6)*cos(b6))+(z1x)*sin(t6)*sin(p6)+(z1y)*sin(t6)*cos(p6)+(z1z)*cos(t6)-(b3z+z1z+p6z-p1z)-
h15)/damp;
J(15,10)=(w1u*(-sin(z5)*cos(b6)+cos(z5)*sin(z6p)*sin(b6))+w1v*(sin(z5)*sin(b6)+
cos(z5)*sin(z6p)*cos(b6))+(z1x)*sin(t6)*sin(p6)+(z1y)*sin(t6)*cos(p6)+(z1z)*cos(t6)-(b4z+z1z+p6z-p1z)-
h15)/damp;
J(16,1)=(z2+(w1up*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-p1x-h16)/damp;
J(16,2)=(z2+(w1u*cos(z5)+w1vp*sin(z5)*sin(z6))+z1x-p1x-h16)/damp;
J(16,3)=(z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1xp-p1x-h16)/damp;

104
J(16,4)=0;J(16,5)=0;
J(16,6)=(z2p+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-p1x-h16)/damp;
J(16,7)=0;J(16,8)=0;
J(16,9)=(z2+(w1u*cos(z5p)+w1v*sin(z5p)*sin(z6))+z1x-p1x-h16)/damp;
J(16,10)=(z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6p))+z1x-p1x-h16)/damp;
J(17,1)=0;
J(17,2)=(z3+w1vp*cos(z6)+z1y-p1y-h17)/damp;
J(17,3)=0;
J(17,4)=(z3+w1v*cos(z6)+z1yp-p1y-h17)/damp;
J(17,5)=0;J(17,6)=0;
J(17,7)=(z3p+w1v*cos(z6)+z1y-p1y-h17)/damp;
J(17,8)=0;J(17,9)=0;
J(17,10)=(z3+w1v*cos(z6p)+z1y-p1y-h17)/damp;
J(18,1)=(z4+(w1up*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-p1z-h18)/damp;
J(18,2)=(z4+(w1u*(-sin(z5))+w1vp*cos(z5)*sin(z6))+z1z-p1z-h18)/damp;
J(18,3)=0;J(18,4)=0;
J(18,5)=(z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1zp-p1z-h18)/damp;
J(18,6)=0;J(18,7)=0;
J(18,8)=(z4p+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-p1z-h18)/damp;
J(18,9)=(z4+(w1u*(-sin(z5p))+w1v*cos(z5p)*sin(z6))+z1z-p1z-h18)/damp;
J(18,10)=(z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6p))+z1z-p1z-h18)/damp;
h(1)=h1;
h(2)=h2;
h(3)=h3;
h(4)=h4;
h(5)=h5;
h(6)=h6;
h(7)=h7;
h(8)=h8;
h(9)=h9;
h(10)=h10;
h(11)=h11;
h(12)=h12;
h(13)=h13;
h(14)=h14;
h(15)=h15;
h(16)=h16;
h(17)=h17;
h(18)=h18;
ds=-J\h';
%*******************************
%ACTUALIZACION DE VARIABLES
%*******************************
w1u=w1u+ds(1);
w1v=w1v+ds(2);
z1x=z1x+ds(3);
z1y=z1y+ds(4);
z1z=z1z+ds(5);
z2=z2+ds(6);
z3=z3+ds(7);
z4=z4+ds(8);
z5=z5+ds(9);

z6=z6+ds(10);
%***************************

105
 %IMPRESION DE RESULTADOS
%***************************
fprintf('n=%2.0f, w1x=%12.5e, w1y=%12.5e,z1x=%12.5e,
z1y=%12.5e,z1z=%12.5e,z2=%12.5e,z3=%12.5e,z4=%12.5e,z5=%12.5e,z6=%12.5e',
n,w1u,w1v,z1x,z1y,z1z,z2,z3,z4,z5,z6)
end;
end

PROGRAMA PARA EL CALCULO DE LAS COMPONENTES DEL ESLABÓN W2 Y DE SU PIVOTE

DE TIERRA

clear all
clc
damp=.001; %INCREMENTO PARA DERIVAR PARCIALMENTE
%******************************** %ESTOS VALORES SON ARBITRARIOS Y SE
%ENTRADA DE VALORES INICIALES %UTILIZAN PARA INICIAR EL PROCESO DEL
%******************************** %CALCULO.

w2u = input ('componente en el eje u del vector w2 ');

w2v = input ('componente en el eje v del vector w2 ');
z2x = input ('componente en el eje x del vector z2 ');
z2y = input ('componente en el eje y del vector z2 ');
z2z = input ('componente en el eje z del vector z2 ');
z3x = input ('componente en el eje x del pivote de tierra de w2 ');
z3y = input ('componente en el eje y del pivote de tierra de w2 ');
z3z = input ('componente en el eje z del pivote de tierra de w2 ');
a2 = input ('ángulo de rotación a2 ');
a3 = input ('ángulo de rotación a3 ');
a4 = input ('ángulo de rotación a4 ');
a5 = input ('ángulo de rotación a5 ');
a6 = input ('ángulo de rotación a6 ');
m = input ('ángulo de rotación m sobre el eje y ');
s = input ('ángulo de rotación s sobre el eje x rotado');

%**********************************
%ENTRADA DE VALORES PRESCRITOS %ESTOS VALORES SON CONSTANTES
%**********************************
w1u = input ('componente en el eje u del vector w1 ');
w1v = input ('componente en el eje v del vector w1 ');
z1x = input ('componente en el eje x del vector z1 ');
z1y = input ('componente en el eje y del vector z1 ');
z1z = input ('componente en el eje z del vector z1 ');
z2 = input ('componente en el eje x del pivote de tierra de w1 ');
z3 = input ('componente en el eje y del pivote de tierra de w1 ');
z4 = input ('componente en el eje z del pivote de tierra de w1 ');
z5 = input ('ángulo de rotación z5 sobre el eje y');
z6 = input ('ángulo de rotación z6 sobre el eje x rotado');
f2 = input ('ángulo de rotación f2 ');
f3 = input ('ángulo de rotación f3 ');
f4 = input ('ángulo de rotación f4 ');
f5 = input ('ángulo de rotación f5 ');
f6 = input ('ángulo de rotación f6 ');
p2 = input ('ángulo de rotación p2 ');
p3 = input ('ángulo de rotación p3 ');

106
p4 = input ('ángulo de rotación p4');
p5 = input ('ángulo de rotación p5');
p6 = input ('ángulo de rotación p6 ');
t2 = input ('ángulo de rotación t2 ');
t3 = input ('ángulo de rotación t3 ');
t4 = input ('ángulo de rotación t4 ');
t5 = input ('ángulo de rotación t5 ');
t6 = input ('ángulo de rotación t6 ');
b2 = input ('ángulo del eslabón de entrada b2 ');
b3 = input ('ángulo del eslabón de entrada b3 ');
b4 = input ('ángulo del eslabón de entrada b4 ');
b5 = input ('ángulo del eslabón de entrada b5 ');
b6 = input ('ángulo del eslabón de entrada b6 ');
p1x = input ('componente en x del primer punto de precisión ');
p1y = input ('componente en y del primer punto de precisión ');
p1z = input ('componente en z del primer punto de precisión ');
p2x = input ('componente en x del segundo punto de precisión ');
p2y = input ('componente en y del segundo punto de precisión ');
p2z = input ('componente en z del segundo punto de precisión ');
p3x = input ('componente en x del tercer punto de precisión ');
p3y = input ('componente en y del tercer punto de precisión ');
p3z = input ('componente en z del tercer punto de precisión ');
p4x = input ('componente en x del cuarto punto de precisión ');
p4y = input ('componente en y del cuarto punto de precisión ');
p4z = input ('componente en z del cuarto punto de precisión ');
p5x = input ('componente en x del quinto punto de precisión ');
p5y = input ('componente en y del quinto punto de precisión ');
p5z = input ('componente en z del quinto punto de precisión ');
p6x = input ('componente en x del sexto punto de precisión ');
p6y = input ('componente en y del sexto punto de precisión ');
p6z = input ('componente en x del sexto punto de precisión ');
r = 350; %NUMERO DE ITERACIONES
%**********************
%CICLO DE ITERACIONES
%**********************
for n=1:r;
a2p=a2+damp;
a3p=a3+damp;
a4p=a4+damp;
a5p=a5+damp;
a6p=a6+damp;
mp=m+damp;
sp=s+damp;
w2up=w2u+damp;
w2vp=w2v+damp;
z2xp=z2x+damp;
z2yp=z2y+damp;
z2zp=z2z+damp;
z3xp=z3x+damp;
z3yp=z3y+damp;
z3zp=z3z+damp;

************************
%CAMBIO DE VARIABLES
************************

107
k16=(z2x)*(cos(f2)*cos(p2)-sin(f2)*cos(t2)*sin(p2))+(z2y)*(-cos(f2)*sin(p2)-
sin(f2)*cos(t2)*cos(p2))+(z2z)*(sin(f2)*sin(t2));
k17=(z2x)*(sin(f2)*cos(p2)+cos(f2)*cos(t2)*sin(p2))+(z2y)*(-sin(f2)*sin(p2)+
cos(f2)*cos(t2)*cos(p2))+(z2z)*(-cos(f2)*sin(t2));
k18=(z2x)*sin(t2)*sin(p2)+(z2y)*sin(t2)*cos(p2)+(z2z)*cos(t2);
k19=(z2x)*(cos(f3)*cos(p3)-sin(f3)*cos(t3)*sin(p3))+(z2y)*(-cos(f3)*sin(p3)-
sin(f3)*cos(t3)*cos(p3))+(z2z)*(sin(f3)*sin(t3));
k20=(z2x)*(sin(f3)*cos(p3)+cos(f3)*cos(t3)*sin(p3))+(z2y)*(-sin(f3)*sin(p3)+
cos(f3)*cos(t3)*cos(p3))+(z2z)*(-cos(f3)*sin(t3));
k21=(z2x)*sin(t3)*sin(p3)+(z2y)*sin(t3)*cos(p3)+(z2z)*cos(t3);
k22=(z2x)*(cos(f4)*cos(p4)-sin(f4)*cos(t4)*sin(p4))+(z2y)*(-cos(f4)*sin(p4)-
sin(f4)*cos(t4)*cos(p4))+(z2z)*(sin(f4)*sin(t4));
k23=(z2x)*(sin(f4)*cos(p4)+cos(f4)*cos(t4)*sin(p4))+(z2y)*(-sin(f4)*sin(p4)+
cos(f4)*cos(t4)*cos(p4))+(z2z)*(-cos(f4)*sin(t4));
k24=(z2x)*sin(t4)*sin(p4)+(z2y)*sin(t4)*cos(p4)+(z2z)*cos(t4);
k25=(z2x)*(cos(f5)*cos(p5)-sin(f5)*cos(t5)*sin(p5))+(z2y)*(-cos(f5)*sin(p5)-
sin(f5)*cos(t5)*cos(p5))+(z2z)*(sin(f5)*sin(t5));
k26=(z2x)*(sin(f5)*cos(p5)+cos(f5)*cos(t5)*sin(p5))+(z2y)*(-sin(f5)*sin(p5)+
cos(f5)*cos(t5)*cos(p5))+(z2z)*(-cos(f5)*sin(t5));
k27=(z2x)*sin(t5)*sin(p5)+(z2y)*sin(t5)*cos(p5)+(z2z)*cos(t5);
k28=(z2x)*(cos(f6)*cos(p6)-sin(f6)*cos(t6)*sin(p6))+(z2y)*(-cos(f6)*sin(p6)-
sin(f6)*cos(t6)*cos(p6))+(z2z)*(sin(f6)*sin(t6));
k29=(z2x)*(sin(f6)*cos(p6)+cos(f6)*cos(t6)*sin(p6))+(z2y)*(-sin(f6)*sin(p6)+
cos(f6)*cos(t6)*cos(p6))+(z2z)*(-cos(f6)*sin(t6));
k30=(z2x)*sin(t6)*sin(p6)+(z2y)*sin(t6)*cos(p6)+(z2z)*cos(t6);
d1=(cos(f2)*cos(p2)-sin(f2)*cos(t2)*sin(p2));
d2=(-cos(f2)*sin(p2)-sin(f2)*cos(t2)*cos(p2));
d3=(sin(f2)*sin(t2));
d4=(sin(f2)*cos(p2)+cos(f2)*cos(t2)*sin(p2));
d5=(-sin(f2)*sin(p2)+cos(f2)*cos(t2)*cos(p2));
d6=(-cos(f2)*sin(t2));
d7=sin(t2)*sin(p2);
d8=sin(t2)*cos(p2);
d9=cos(t2);
d10=(cos(f3)*cos(p3)-sin(f3)*cos(t3)*sin(p3));
d11=(-cos(f3)*sin(p3)-sin(f3)*cos(t3)*cos(p3));
d12=(sin(f3)*sin(t3));
d13=(sin(f3)*cos(p3)+cos(f3)*cos(t3)*sin(p3));
d14=(-sin(f3)*sin(p3)+cos(f3)*cos(t3)*cos(p3));
d15=(-cos(f3)*sin(t3));
d16=sin(t3)*sin(p3);
d17=sin(t3)*cos(p3);
d18=cos(t3);
d19=(cos(f4)*cos(p4)-sin(f4)*cos(t4)*sin(p4));
d20=(-cos(f4)*sin(p4)-sin(f4)*cos(t4)*cos(p4));
d21=(sin(f4)*sin(t4));
d22=(sin(f4)*cos(p4)+cos(f4)*cos(t4)*sin(p4));
d23=(-sin(f4)*sin(p4)+cos(f4)*cos(t4)*cos(p4));
d24=(-cos(f4)*sin(t4));
d25=sin(t4)*sin(p4);
d26=sin(t4)*cos(p4);
d27=cos(t4);
d28=(cos(f5)*cos(p5)-sin(f5)*cos(t5)*sin(p5));
d29=(-cos(f5)*sin(p5)-sin(f5)*cos(t5)*cos(p5));
d30=(sin(f5)*sin(t5));
d31=(sin(f5)*cos(p5)+cos(f5)*cos(t5)*sin(p5));

108
d32=(-sin(f5)*sin(p5)+cos(f5)*cos(t5)*cos(p5));
d33=(-cos(f5)*sin(t5));
d34=sin(t5)*sin(p5);
d35=sin(t5)*cos(p5);
d36=cos(t5);
d37=(cos(f6)*cos(p6)-sin(f6)*cos(t6)*sin(p6));
d38=(-cos(f6)*sin(p6)-sin(f6)*cos(t6)*cos(p6));
d39=(sin(f6)*sin(t6));
d40=(sin(f6)*cos(p6)+cos(f6)*cos(t6)*sin(p6));
d41=(-sin(f6)*sin(p6)+cos(f6)*cos(t6)*cos(p6));
d42=(-cos(f6)*sin(t6));
d43=sin(t6)*sin(p6);
d44=sin(t6)*cos(p6);
d45=cos(t6);
bx=w2u*cos(m)+w2v*sin(m)*sin(s);
b1x=w2up*cos(m)+w2v*sin(m)*sin(s);
b2x=w2u*cos(m)+w2vp*sin(m)*sin(s);
b3x=w2u*cos(mp)+w2v*sin(mp)*sin(s);
b4x=w2u*cos(m)+w2v*sin(m)*sin(sp);
by=w2v*cos(s);
b2y=w2vp*cos(s);
b4y=w2v*cos(sp);
bz=-w2u*sin(m)+w2v*cos(m)*sin(s);
b1z=-w2up*sin(m)+w2v*cos(m)*sin(s);
b2z=-w2u*sin(m)+w2vp*cos(m)*sin(s);
b3z=-w2u*sin(mp)+w2v*cos(mp)*sin(s);
b4z=-w2u*sin(m)+w2v*cos(m)*sin(sp);
g1= (w2u*(cos(m)*cos(a2)+sin(m)*sin(s)*sin(a2))+w2v*(-sin(a2)*cos(m)+sin(m)*sin(s)*cos(a2)));
g2= (w2u*cos(s)*sin(a2)+w2v*cos(s)*cos(a2));
g3= (w2u*(-sin(m)*cos(a2)+cos(m)*sin(s)*sin(a2))+w2v*(sin(m)*sin(a2)+cos(m)*sin(s)*cos(a2)));
g4= (w2u*(cos(m)*cos(a2p)+sin(m)*sin(s)*sin(a2p))+w2v*(-sin(a2p)*cos(m)+sin(m)*sin(s)*cos(a2p)));
g5= (w2up*(cos(m)*cos(a2)+sin(m)*sin(s)*sin(a2))+w2v*(-sin(a2)*cos(m)+sin(m)*sin(s)*cos(a2)));
g6= (w2u*(cos(m)*cos(a2)+sin(m)*sin(s)*sin(a2))+w2vp*(-sin(a2)*cos(m)+sin(m)*sin(s)*cos(a2)));
g7=(w2u*cos(s)*sin(a2p)+w2v*cos(s)*cos(a2p));
g8=(w2up*cos(s)*sin(a2)+w2v*cos(s)*cos(a2));
g9=(w2u*cos(s)*sin(a2)+w2vp*cos(s)*cos(a2));
g10=(w2u*(-sin(m)*cos(a2p)+cos(m)*sin(s)*sin(a2p))+w2v*(sin(m)*sin(a2p)+cos(m)*sin(s)*cos(a2p)));
g11=(w2up*(-sin(m)*cos(a2)+cos(m)*sin(s)*sin(a2))+w2v*(sin(m)*sin(a2)+cos(m)*sin(s)*cos(a2)));
g12=(w2u*(-sin(m)*cos(a2)+cos(m)*sin(s)*sin(a2))+w2vp*(sin(m)*sin(a2)+cos(m)*sin(s)*cos(a2)));
dm1=(w2u*(cos(mp)*cos(a2)+sin(mp)*sin(s)*sin(a2))+w2v*(-sin(a2)*cos(mp)+sin(mp)*sin(s)*cos(a2)));
ds1=(w2u*(cos(m)*cos(a2)+sin(m)*sin(sp)*sin(a2))+w2v*(-sin(a2)*cos(m)+sin(m)*sin(sp)*cos(a2)));
ds2=(w2u*cos(sp)*sin(a2)+w2v*cos(sp)*cos(a2));
dm3=(w2u*(-sin(mp)*cos(a2)+cos(mp)*sin(s)*sin(a2))+w2v*(sin(mp)*sin(a2)+cos(mp)*sin(s)*cos(a2)));
ds3=(w2u*(-sin(m)*cos(a2)+cos(m)*sin(sp)*sin(a2))+w2v*(sin(m)*sin(a2)+cos(m)*sin(sp)*cos(a2)));

g13= (w2u*(cos(m)*cos(a3)+sin(m)*sin(s)*sin(a3))+w2v*(-sin(a3)*cos(m)+sin(m)*sin(s)*cos(a3)));
g14= (w2u*cos(s)*sin(a3)+w2v*cos(s)*cos(a3));
g15= (w2u*(-sin(m)*cos(a3)+cos(m)*sin(s)*sin(a3))+w2v*(sin(m)*sin(a3)+cos(m)*sin(s)*cos(a3)));
g16= (w2u*(cos(m)*cos(a3p)+sin(m)*sin(s)*sin(a3p))+w2v*(-sin(a3p)*cos(m)+sin(m)*sin(s)*cos(a3p)));
g17= (w2up*(cos(m)*cos(a3)+sin(m)*sin(s)*sin(a3))+w2v*(-sin(a3)*cos(m)+sin(m)*sin(s)*cos(a3)));
g18= (w2u*(cos(m)*cos(a3)+sin(m)*sin(s)*sin(a3))+w2vp*(-sin(a3)*cos(m)+sin(m)*sin(s)*cos(a3)));
g19= (w2u*cos(s)*sin(a3p)+w2v*cos(s)*cos(a3p));
g20= (w2up*cos(s)*sin(a3)+w2v*cos(s)*cos(a3));
g21= (w2u*cos(s)*sin(a3)+w2vp*cos(s)*cos(a3));
g22= (w2u*(-sin(m)*cos(a3p)+cos(m)*sin(s)*sin(a3p))+w2v*(sin(m)*sin(a3p)+cos(m)*sin(s)*cos(a3p)));
g23= (w2up*(-sin(m)*cos(a3)+cos(m)*sin(s)*sin(a3))+w2v*(sin(m)*sin(a3)+cos(m)*sin(s)*cos(a3)));

109
g24= (w2u*(-sin(m)*cos(a3)+cos(m)*sin(s)*sin(a3))+w2vp*(sin(m)*sin(a3)+cos(m)*sin(s)*cos(a3)));
dm13=(w2u*(cos(mp)*cos(a3)+sin(mp)*sin(s)*sin(a3))+w2v*(-sin(a3)*cos(mp)+sin(mp)*sin(s)*cos(a3)));
ds13=(w2u*(cos(m)*cos(a3)+sin(m)*sin(sp)*sin(a3))+w2v*(-sin(a3)*cos(m)+sin(m)*sin(sp)*cos(a3)));
ds14=(w2u*cos(sp)*sin(a3)+w2v*cos(sp)*cos(a3));
dm15=(w2u*(-sin(mp)*cos(a3)+cos(mp)*sin(s)*sin(a3))+w2v*(sin(mp)*sin(a3)+cos(mp)*sin(s)*cos(a3)));
ds15=(w2u*(-sin(m)*cos(a3)+cos(m)*sin(sp)*sin(a3))+w2v*(sin(m)*sin(a3)+cos(m)*sin(sp)*cos(a3)));

g25= (w2u*(cos(m)*cos(a4)+sin(m)*sin(s)*sin(a4))+w2v*(-sin(a4)*cos(m)+sin(m)*sin(s)*cos(a4)));
g26= (w2u*cos(s)*sin(a4)+w2v*cos(s)*cos(a4));
g27= (w2u*(-sin(m)*cos(a4)+cos(m)*sin(s)*sin(a4))+w2v*(sin(m)*sin(a4)+cos(m)*sin(s)*cos(a4)));
g28= (w2u*(cos(m)*cos(a4p)+sin(m)*sin(s)*sin(a4p))+w2v*(-sin(a4p)*cos(m)+sin(m)*sin(s)*cos(a4p)));
g29= (w2up*(cos(m)*cos(a4)+sin(m)*sin(s)*sin(a4))+w2v*(-sin(a4)*cos(m)+sin(m)*sin(s)*cos(a4)));
g30= (w2u*(cos(m)*cos(a4)+sin(m)*sin(s)*sin(a4))+w2vp*(-sin(a4)*cos(m)+sin(m)*sin(s)*cos(a4)));
g31= (w2u*cos(s)*sin(a4p)+w2v*cos(s)*cos(a4p));
g32= (w2up*cos(s)*sin(a4)+w2v*cos(s)*cos(a4));
g33= (w2u*cos(s)*sin(a4)+w2vp*cos(s)*cos(a4));
g34= (w2u*(-sin(m)*cos(a4p)+cos(m)*sin(s)*sin(a4p))+w2v*(sin(m)*sin(a4p)+cos(m)*sin(s)*cos(a4p)));
g35= (w2up*(-sin(m)*cos(a4)+cos(m)*sin(s)*sin(a4))+w2v*(sin(m)*sin(a4)+cos(m)*sin(s)*cos(a4)));
g36= (w2u*(-sin(m)*cos(a4)+cos(m)*sin(s)*sin(a4))+w2vp*(sin(m)*sin(a4)+cos(m)*sin(s)*cos(a4)));
dm25=(w2u*(cos(mp)*cos(a4)+sin(mp)*sin(s)*sin(a4))+w2v*(-sin(a4)*cos(mp)+sin(mp)*sin(s)*cos(a4)));
ds25=(w2u*(cos(m)*cos(a4)+sin(m)*sin(sp)*sin(a4))+w2v*(-sin(a4)*cos(m)+sin(m)*sin(sp)*cos(a4)));
ds26=(w2u*cos(sp)*sin(a4)+w2v*cos(sp)*cos(a4));
dm27=(w2u*(-sin(mp)*cos(a4)+cos(mp)*sin(s)*sin(a4))+w2v*(sin(mp)*sin(a4)+cos(mp)*sin(s)*cos(a4)));
ds27=(w2u*(-sin(m)*cos(a4)+cos(m)*sin(sp)*sin(a4))+w2v*(sin(m)*sin(a4)+cos(m)*sin(sp)*cos(a4)));

g37= (w2u*(cos(m)*cos(a5)+sin(m)*sin(s)*sin(a5))+w2v*(-sin(a5)*cos(m)+sin(m)*sin(s)*cos(a5)));
g38= (w2u*cos(s)*sin(a5)+w2v*cos(s)*cos(a5));
g39= (w2u*(-sin(m)*cos(a5)+cos(m)*sin(s)*sin(a5))+w2v*(sin(m)*sin(a5)+cos(m)*sin(s)*cos(a5)));
g40= (w2u*(cos(m)*cos(a5p)+sin(m)*sin(s)*sin(a5p))+w2v*(-sin(a5p)*cos(m)+sin(m)*sin(s)*cos(a5p)));
g41= (w2up*(cos(m)*cos(a5)+sin(m)*sin(s)*sin(a5))+w2v*(-sin(a5)*cos(m)+sin(m)*sin(s)*cos(a5)));
g42= (w2u*(cos(m)*cos(a5)+sin(m)*sin(s)*sin(a5))+w2vp*(-sin(a5)*cos(m)+sin(m)*sin(s)*cos(a5)));
g43= (w2u*cos(s)*sin(a5p)+w2v*cos(s)*cos(a5p));
g44= (w2up*cos(s)*sin(a5)+w2v*cos(s)*cos(a5));
g45= (w2u*cos(s)*sin(a5)+w2vp*cos(s)*cos(a5));
g46= (w2u*(-sin(m)*cos(a5p)+cos(m)*sin(s)*sin(a5p))+w2v*(sin(m)*sin(a5p)+cos(m)*sin(s)*cos(a5p)));
g47= (w2up*(-sin(m)*cos(a5)+cos(m)*sin(s)*sin(a5))+w2v*(sin(m)*sin(a5)+cos(m)*sin(s)*cos(a5)));
g48= (w2u*(-sin(m)*cos(a5)+cos(m)*sin(s)*sin(a5))+w2vp*(sin(m)*sin(a5)+cos(m)*sin(s)*cos(a5)));
dm37=(w2u*(cos(mp)*cos(a5)+sin(mp)*sin(s)*sin(a5))+w2v*(-sin(a5)*cos(mp)+sin(mp)*sin(s)*cos(a5)));
ds37=(w2u*(cos(m)*cos(a5)+sin(m)*sin(sp)*sin(a5))+w2v*(-sin(a5)*cos(m)+sin(m)*sin(sp)*cos(a5)));
ds38=(w2u*cos(sp)*sin(a5)+w2v*cos(sp)*cos(a5));
dm39=(w2u*(-sin(mp)*cos(a5)+cos(mp)*sin(s)*sin(a5))+w2v*(sin(mp)*sin(a5)+cos(mp)*sin(s)*cos(a5)));
ds39=(w2u*(-sin(m)*cos(a5)+cos(m)*sin(sp)*sin(a5))+w2v*(sin(m)*sin(a5)+cos(m)*sin(sp)*cos(a5)));

g49= (w2u*(cos(m)*cos(a6)+sin(m)*sin(s)*sin(a6))+w2v*(-sin(a6)*cos(m)+sin(m)*sin(s)*cos(a6)));
g50= (w2u*cos(s)*sin(a6)+w2v*cos(s)*cos(a6));
g51= (w2u*(-sin(m)*cos(a6)+cos(m)*sin(s)*sin(a6))+w2v*(sin(m)*sin(a6)+cos(m)*sin(s)*cos(a6)));
g52= (w2u*(cos(m)*cos(a6p)+sin(m)*sin(s)*sin(a6p))+w2v*(-sin(a6p)*cos(m)+sin(m)*sin(s)*cos(a6p)));
g53= (w2up*(cos(m)*cos(a6)+sin(m)*sin(s)*sin(a6))+w2v*(-sin(a6)*cos(m)+sin(m)*sin(s)*cos(a6)));
g54= (w2u*(cos(m)*cos(a6)+sin(m)*sin(s)*sin(a6))+w2vp*(-sin(a6)*cos(m)+sin(m)*sin(s)*cos(a6)));
g55= (w2u*cos(s)*sin(a6p)+w2v*cos(s)*cos(a6p));
g56= (w2up*cos(s)*sin(a6)+w2v*cos(s)*cos(a6));
g57= (w2u*cos(s)*sin(a6)+w2vp*cos(s)*cos(a6));
g58= (w2u*(-sin(m)*cos(a6p)+cos(m)*sin(s)*sin(a6p))+w2v*(sin(m)*sin(a6p)+cos(m)*sin(s)*cos(a6p)));
g59= (w2up*(-sin(m)*cos(a6)+cos(m)*sin(s)*sin(a6))+w2v*(sin(m)*sin(a6)+cos(m)*sin(s)*cos(a6)));
g60= (w2u*(-sin(m)*cos(a6)+cos(m)*sin(s)*sin(a6))+w2vp*(sin(m)*sin(a6)+cos(m)*sin(s)*cos(a6)));
dm49=(w2u*(cos(mp)*cos(a6)+sin(mp)*sin(s)*sin(a6))+w2v*(-sin(a6)*cos(mp)+sin(mp)*sin(s)*cos(a6)));

110
 ds49=(w2u*(cos(m)*cos(a6)+sin(m)*sin(sp)*sin(a6))+w2v*(-sin(a6)*cos(m)+sin(m)*sin(sp)*cos(a6)));
ds50=(w2u*cos(sp)*sin(a6)+w2v*cos(sp)*cos(a6));
dm51=(w2u*(-sin(mp)*cos(a6)+cos(mp)*sin(s)*sin(a6))+w2v*(sin(mp)*sin(a6)+cos(mp)*sin(s)*cos(a6)));
ds51=(w2u*(-sin(m)*cos(a6)+cos(m)*sin(sp)*sin(a6))+w2v*(sin(m)*sin(a6)+cos(m)*sin(sp)*cos(a6)));
h1=k16+g1-bx-z2x-(p2x-p1x);
h2=k17+g2-by-z2y-(p2y-p1y);
h3=k18+g3-bz-z2z-(p2z-p1z);
h4=k19+g13-bx-z2x-(p3x-p1x);
h5=k20+g14-by-z2y-(p3y-p1y);
h6=k21+g15-bz-z2z-(p3z-p1z);
h7=k22+g25-bx-z2x-(p4x-p1x);
h8=k23+g26-by-z2y-(p4y-p1y);
h9=k24+g27-bz-z2z-(p4z-p1z);
h10=k25+g37-bx-z2x-(p5x-p1x);
h11=k26+g38-by-z2y-(p5y-p1y);
h12=k27+g39-bz-z2z-(p5z-p1z);
h13=k28+g49-bx-z2x-(p6x-p1x);
h14=k29+g50-by-z2y-(p6y-p1y);
h15=k30+g51-bz-z2z-(p6z-p1z);
h16=z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-z3x-(w2u*cos(m)+w2v*sin(m)*sin(s))-z2x;
h17=z3+w1v*cos(z6)+z1y-z3y-w2v*cos(s)-z2y;
h18=z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-z3z-(w2u*(-sin(m))+w2v*cos(m)*sin(s))-z2z;
%**************************
%FORACION DEL JACOBIANO
%**************************
J(1,1)=(k16+g4-bx-z2x-(p2x-p1x)-h1)/damp;
J(1,2)=0;J(1,3)=0;J(1,4)=0;J(1,5)=0;
J(1,6)=(dm1+k16-b3x-z2x-(p2x-p1x)-h1)/damp;
J(1,7)=(ds1+k16-b4x-z2x-(p2x-p1x)-h1)/damp;
J(1,8)=(g5+((z2x)*d1+(z2y)*d2+(z2z)*d3)-b1x-z2x-(p2x-p1x)-h1)/damp;
J(1,9)=(g6+((z2x)*d1+(z2y)*d2+(z2z)*d3)-b2x-z2x-(p2x-p1x)-h1)/damp;
J(1,10)=(g1+((z2xp)*d1+(z2y)*d2+(z2z)*d3)-bx-z2xp-(p2x-p1x)-h1)/damp;
J(1,11)=(g1+((z2x)*d1+(z2yp)*d2+(z2z)*d3)-bx-z2x-(p2x-p1x)-h1)/damp;
J(1,12)=(g1+((z2x)*d1+(z2y)*d2+(z2zp)*d3)-bx-z2x-(p2x-p1x)-h1)/damp;
J(1,13)=0;J(1,14)=0;J(1,15)=0;
J(2,1)=(k17+g7-by-z2y-(p2y-p1y)-h2)/damp;
J(2,2)=0;J(2,3)=0;J(2,4)=0;J(2,5)=0;
J(2,6)=(g2+k17-by-z2y-(p2y-p1y)-h2)/damp;
J(2,7)=(ds2+k17-b4y-z2y-(p2y-p1y)-h2)/damp;
J(2,8)=(g8+((z2x)*d4+(z2y)*d5+(z2z)*d6)-by-z2y-(p2y-p1y)-h2)/damp;
J(2,9)=(g9+((z2x)*d4+(z2y)*d5+(z2z)*d6)-b2y-z2y-(p2y-p1y)-h2)/damp;
J(2,10)=(g2+((z2xp)*d4+(z2y)*d5+(z2z)*d6)-by-z2y-(p2y-p1y)-h2)/damp;
J(2,11)=(g2+((z2x)*d4+(z2yp)*d5+(z2z)*d6)-by-z2yp-(p2y-p1y)-h2)/damp;
J(2,12)=(g2+((z2x)*d4+(z2y)*d5+(z2zp)*d6)-by-z2y-(p2y-p1y)-h2)/damp;
J(2,13)=0;J(2,14)=0;J(2,15)=0;
J(3,1)=(k18+g10-bz-z2z-(p2z-p1z)-h3)/damp;
J(3,2)=0;J(3,3)=0;J(3,4)=0;J(3,5)=0;
J(3,6)=(dm3+k18-b3z-z2z-(p2z-p1z)-h3)/damp;
J(3,7)=(ds3+k18-b4z-z2z-(p2z-p1z)-h3)/damp;
J(3,8)=(g11+((z2x)*d7+(z2y)*d8+(z2z)*d9)-b1z-z2z-(p2z-p1z)-h3)/damp;
J(3,9)=(g12+((z2x)*d7+(z2y)*d8+(z2z)*d9)-b2z-z2z-(p2z-p1z)-h3)/damp;
J(3,10)=(g3+((z2xp)*d7+(z2y)*d8+(z2z)*d9)-bz-z2z-(p2z-p1z)-h3)/damp;
J(3,11)=(g3+((z2x)*d7+(z2yp)*d8+(z2z)*d9)-bz-z2z-(p2z-p1z)-h3)/damp;
J(3,12)=(g3+((z2x)*d7+(z2y)*d8+(z2zp)*d9)-bz-z2zp-(p2z-p1z)-h3)/damp;
J(3,13)=0;J(3,14)=0;J(3,15)=0;
J(4,1)=0;

111
J(4,2)=(k19+g16-bx-z2x-(p3x-p1x)-h4)/damp;
J(4,3)=0;J(4,4)=0;J(4,5)=0;
J(4,6)=(dm13+k19-b3x-z2x-(p3x-p1x)-h4)/damp;
J(4,7)=(ds13+k19-b4x-z2x-(p3x-p1x)-h4)/damp;
J(4,8)=(g17+((z2x)*d10+(z2y)*d11+(z2z)*d12)-b1x-z2x-(p3x-p1x)-h4)/damp;
J(4,9)=(g18+((z2x)*d10+(z2y)*d11+(z2z)*d12)-b2x-z2x-(p3x-p1x)-h4)/damp;
J(4,10)=(g13+((z2xp)*d10+(z2y)*d11+(z2z)*d12)-bx-z2xp-(p3x-p1x)-h4)/damp;
J(4,11)=(g13+((z2x)*d10+(z2yp)*d11+(z2z)*d12)-bx-z2x-(p3x-p1x)-h4)/damp;
J(4,12)=(g13+((z2x)*d10+(z2y)*d11+(z2zp)*d12)-bx-z2x-(p3x-p1x)-h4)/damp;
J(4,13)=0;J(4,14)=0;J(4,15)=0;
J(5,1)=0;
J(5,2)=(k20+g19-by-z2y-(p3y-p1y)-h5)/damp;
J(5,3)=0;J(5,4)=0;J(5,5)=0;
J(5,6)=(g14+k20-by-z2y-(p3y-p1y)-h5)/damp;
J(5,7)=(ds14+k20-b4y-z2y-(p3y-p1y)-h5)/damp;
J(5,8)=(g20+((z2x)*d13+(z2y)*d14+(z2z)*d15)-by-z2y-(p3y-p1y)-h5)/damp;
J(5,9)=(g21+((z2x)*d13+(z2y)*d14+(z2z)*d15)-b2y-z2y-(p3y-p1y)-h5)/damp;
J(5,10)=(g14+((z2xp)*d13+(z2y)*d14+(z2z)*d15)-by-z2y-(p3y-p1y)-h5)/damp;
J(5,11)=(g14+((z2x)*d13+(z2yp)*d14+(z2z)*d15)-by-z2yp-(p3y-p1y)-h5)/damp;
J(5,12)=(g14+((z2x)*d13+(z2y)*d14+(z2zp)*d15)-by-z2y-(p3y-p1y)-h5)/damp;
J(5,13)=0;J(5,14)=0;J(5,15)=0;
J(6,1)=0;
J(6,2)=(k21+g22-bz-z2z-(p3z-p1z)-h6)/damp;
J(6,3)=0;J(6,4)=0;J(6,5)=0;
J(6,6)=(dm15+k21-b3z-z2z-(p3z-p1z)-h6)/damp;
J(6,7)=(ds15+k21-b4z-z2z-(p3z-p1z)-h6)/damp;
J(6,8)=(g23+((z2x)*d16+(z2y)*d17+(z2z)*d18)-b1z-z2z-(p3z-p1z)-h6)/damp;
J(6,9)=(g24+((z2x)*d16+(z2y)*d17+(z2z)*d18)-b2z-z2z-(p3z-p1z)-h6)/damp;
J(6,10)=(g15+((z2xp)*d16+(z2y)*d17+(z2z)*d18)-bz-z2z-(p3z-p1z)-h6)/damp;
J(6,11)=(g15+((z2x)*d16+(z2yp)*d17+(z2z)*d18)-bz-z2z-(p3z-p1z)-h6)/damp;
J(6,12)=(g15+((z2x)*d16+(z2y)*d17+(z2zp)*d18)-bz-z2zp-(p3z-p1z)-h6)/damp;
J(6,13)=0;J(6,14)=0;J(6,15)=0;
J(7,1)=0;J(7,2)=0;
J(7,3)=(k22+g28-bx-z2x-(p4x-p1x)-h7)/damp;
J(7,4)=0;J(7,5)=0;
J(7,6)=(dm25+k22-b3x-z2x-(p4x-p1x)-h7)/damp;
J(7,7)=(ds25+k22-b4x-z2x-(p4x-p1x)-h7)/damp;
J(7,8)=(g29+((z2x)*d19+(z2y)*d20+(z2z)*d21)-b1x-z2x-(p4x-p1x)-h7)/damp;
J(7,9)=(g30+((z2x)*d19+(z2y)*d20+(z2z)*d21)-b2x-z2x-(p4x-p1x)-h7)/damp;
J(7,10)=(g25+((z2xp)*d19+(z2y)*d20+(z2z)*d21)-bx-z2xp-(p4x-p1x)-h7)/damp;
J(7,11)=(g25+((z2x)*d19+(z2yp)*d20+(z2z)*d21)-bx-z2x-(p4x-p1x)-h7)/damp;
J(7,12)=(g25+((z2x)*d19+(z2y)*d20+(z2zp)*d21)-bx-z2x-(p4x-p1x)-h7)/damp;
J(7,13)=0;J(7,14)=0;J(7,15)=0;
J(8,1)=0;J(8,2)=0;
J(8,3)=(k23+g31-by-z2y-(p4y-p1y)-h8)/damp;
J(8,4)=0;J(8,5)=0;
J(8,6)=(g26+k23-by-z2y-(p4y-p1y)-h8)/damp;
J(8,7)=(ds26+k23-b4y-z2y-(p4y-p1y)-h8)/damp;
J(8,8)=(g32+((z2x)*d22+(z2y)*d23+(z2z)*d24)-by-z2y-(p4y-p1y)-h8)/damp;
J(8,9)=(g33+((z2x)*d22+(z2y)*d23+(z2z)*d24)-b2y-z2y-(p4y-p1y)-h8)/damp;
J(8,10)=(g26+((z2xp)*d22+(z2y)*d23+(z2z)*d24)-by-z2y-(p4y-p1y)-h8)/damp;
J(8,11)=(g26+((z2x)*d22+(z2yp)*d23+(z2z)*d24)-by-z2yp-(p4y-p1y)-h8)/damp;
J(8,12)=(g26+((z2x)*d22+(z2y)*d23+(z2zp)*d24)-by-z2y-(p4y-p1y)-h8)/damp;
J(8,13)=0;J(8,14)=0;J(8,15)=0;
J(9,1)=0;J(9,2)=0;
J(9,3)=(k24+g34-bz-z2z-(p4z-p1z)-h9)/damp;

112
J(9,4)=0;J(9,5)=0;
J(9,6)=(dm27+k24-b3z-z2z-(p4z-p1z)-h9)/damp;
J(9,7)=(ds27+k24-b4z-z2z-(p4z-p1z)-h9)/damp;
J(9,8)=(g35+((z2x)*d25+(z2y)*d26+(z2z)*d27)-b1z-z2z-(p4z-p1z)-h9)/damp;
J(9,9)=(g36+((z2x)*d25+(z2y)*d26+(z2z)*d27)-b2z-z2z-(p4z-p1z)-h9)/damp;
J(9,10)=(g27+((z2xp)*d25+(z2y)*d26+(z2z)*d27)-bz-z2z-(p4z-p1z)-h9)/damp;
J(9,11)=(g27+((z2x)*d25+(z2yp)*d26+(z2z)*d27)-bz-z2z-(p4z-p1z)-h9)/damp;
J(9,12)=(g27+((z2x)*d25+(z2y)*d26+(z2zp)*d27)-bz-z2zp-(p4z-p1z)-h9)/damp;
J(9,13)=0;J(9,14)=0;J(9,15)=0;
J(10,1)=0;J(10,2)=0;J(10,3)=0;
J(10,4)=(k25+g40-bx-z2x-(p5x-p1x)-h10)/damp;
J(10,5)=0;
J(10,6)=(dm37+k25-b3x-z2x-(p5x-p1x)-h10)/damp;
J(10,7)=(ds37+k25-b4x-z2x-(p5x-p1x)-h10)/damp;
J(10,8)=(g41+((z2x)*d28+(z2y)*d29+(z2z)*d30)-b1x-z2x-(p5x-p1x)-h10)/damp;
J(10,9)=(g42+((z2x)*d28+(z2y)*d29+(z2z)*d30)-b2x-z2x-(p5x-p1x)-h10)/damp;
J(10,10)=(g37+((z2xp)*d28+(z2y)*d29+(z2z)*d30)-bx-z2xp-(p5x-p1x)-h10)/damp;
J(10,11)=(g37+((z2x)*d28+(z2yp)*d29+(z2z)*d30)-bx-z2x-(p5x-p1x)-h10)/damp;
J(10,12)=(g37+((z2x)*d28+(z2y)*d29+(z2zp)*d30)-bx-z2x-(p5x-p1x)-h10)/damp;
J(10,13)=0;J(10,14)=0;J(10,15)=0;
J(11,1)=0;J(11,2)=0;J(11,3)=0;
J(11,4)=(k26+g43-by-z2y-(p5y-p1y)-h11)/damp;
J(11,5)=0;
J(11,6)=(g38+k26-by-z2y-(p5y-p1y)-h11)/damp;
J(11,7)=(ds38+k26-b4y-z2y-(p5y-p1y)-h11)/damp;
J(11,8)=(g44+((z2x)*d31+(z2y)*d32+(z2z)*d33)-by-z2y-(p5y-p1y)-h11)/damp;
J(11,9)=(g45+((z2x)*d31+(z2y)*d32+(z2z)*d33)-b2y-z2y-(p5y-p1y)-h11)/damp;
J(11,10)=(g38+((z2xp)*d31+(z2y)*d32+(z2z)*d33)-by-z2y-(p5y-p1y)-h11)/damp;
J(11,11)=(g38+((z2x)*d31+(z2yp)*d32+(z2z)*d33)-by-z2yp-(p5y-p1y)-h11)/damp;
J(11,12)=(g38+((z2x)*d31+(z2y)*d32+(z2zp)*d33)-by-z2y-(p5y-p1y)-h11)/damp;
J(11,13)=0;J(11,14)=0;J(11,15)=0;
J(12,1)=0;J(12,2)=0;J(12,3)=0;
J(12,4)=(k27+g46-bz-z2z-(p5z-p1z)-h12)/damp;
J(12,5)=0;
J(12,6)=(dm39+k27-b3z-z2z-(p5z-p1z)-h12)/damp;
J(12,7)=(ds39+k27-b4z-z2z-(p5z-p1z)-h12)/damp;
J(12,8)=(g47+((z2x)*d34+(z2y)*d35+(z2z)*d36)-b1z-z2z-(p5z-p1z)-h12)/damp;
J(12,9)=(g48+((z2x)*d34+(z2y)*d35+(z2z)*d36)-b2z-z2z-(p5z-p1z)-h12)/damp;
J(12,10)=(g39+((z2xp)*d34+(z2y)*d35+(z2z)*d36)-bz-z2z-(p5z-p1z)-h12)/damp;
J(12,11)=(g39+((z2x)*d34+(z2yp)*d35+(z2z)*d36)-bz-z2z-(p5z-p1z)-h12)/damp;
J(12,12)=(g39+((z2x)*d34+(z2y)*d35+(z2zp)*d36)-bz-z2zp-(p5z-p1z)-h12)/damp;
J(12,13)=0;J(12,14)=0;J(12,15)=0;
J(13,1)=0;J(13,2)=0;J(13,3)=0;J(13,4)=0;
J(13,5)=(k28+g52-bx-z2x-(p6x-p1x)-h13)/damp;
J(13,6)=(dm49+k28-b3x-z2x-(p6x-p1x)-h13)/damp;
J(13,7)=(ds49+k28-b4x-z2x-(p6x-p1x)-h13)/damp;
J(13,8)=(g53+((z2x)*d37+(z2y)*d38+(z2z)*d39)-b1x-z2x-(p6x-p1x)-h13)/damp;
J(13,9)=(g54+((z2x)*d37+(z2y)*d38+(z2z)*d39)-b2x-z2x-(p6x-p1x)-h13)/damp;
J(13,10)=(g49+((z2xp)*d37+(z2y)*d38+(z2z)*d39)-bx-z2xp-(p6x-p1x)-h13)/damp;
J(13,11)=(g49+((z2x)*d37+(z2yp)*d38+(z2z)*d39)-bx-z2x-(p6x-p1x)-h13)/damp;
J(13,12)=(g49+((z2x)*d37+(z2y)*d38+(z2zp)*d39)-bx-z2x-(p6x-p1x)-h13)/damp;
J(13,13)=0;J(13,14)=0;J(13,15)=0;
J(14,1)=0;J(14,2)=0;J(14,3)=0;J(14,4)=0;
J(14,5)=(k29+g55-by-z2y-(p6y-p1y)-h14)/damp;
J(14,6)=(g50+k29-by-z2y-(p6y-p1y)-h14)/damp;
J(14,7)=(ds50+k29-b4y-z2y-(p6y-p1y)-h14)/damp;

113
J(14,8)=(g56+((z2x)*d40+(z2y)*d41+(z2z)*d42)-by-z2y-(p6y-p1y)-h14)/damp;
J(14,9)=(g57+((z2x)*d40+(z2y)*d41+(z2z)*d42)-b2y-z2y-(p6y-p1y)-h14)/damp;
J(14,10)=(g50+((z2xp)*d40+(z2y)*d41+(z2z)*d42)-by-z2y-(p6y-p1y)-h14)/damp;
J(14,11)=(g50+((z2x)*d40+(z2yp)*d41+(z2z)*d42)-by-z2yp-(p6y-p1y)-h14)/damp;
J(14,12)=(g50+((z2x)*d40+(z2y)*d41+(z2zp)*d42)-by-z2y-(p6y-p1y)-h14)/damp;
J(14,13)=0;J(14,14)=0;J(14,15)=0;
J(15,1)=0;J(15,2)=0;J(15,3)=0;J(15,4)=0;
J(15,5)=(k30+g58-bz-z2z-(p6z-p1z)-h15)/damp;
J(15,6)=(dm51+k30-b3z-z2z-(p6z-p1z)-h15)/damp;
J(15,7)=(ds51+k30-b4z-z2z-(p6z-p1z)-h15)/damp;
J(15,8)=(g59+((z2x)*d43+(z2y)*d44+(z2z)*d45)-b1z-z2z-(p6z-p1z)-h15)/damp;
J(15,9)=(g60+((z2x)*d43+(z2y)*d44+(z2z)*d45)-b2z-z2z-(p6z-p1z)-h15)/damp;
J(15,10)=(g51+((z2xp)*d43+(z2y)*d44+(z2z)*d45)-bz-z2z-(p6z-p1z)-h15)/damp;
J(15,11)=(g51+((z2x)*d43+(z2yp)*d44+(z2z)*d45)-bz-z2z-(p6z-p1z)-h15)/damp;
J(15,12)=(g51+((z2x)*d43+(z2y)*d44+(z2zp)*d45)-bz-z2zp-(p6z-p1z)-h15)/damp;
J(15,13)=0;J(15,14)=0;J(15,15)=0;

J(16,1)=0;J(16,2)=0;J(16,3)=0;J(16,4)=0;J(16,5)=0;
J(16,6)=(z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-z3x-(w2u*cos(mp)+w2v*sin(mp)*sin(s))-z2x-
h16)/damp;
J(16,7)=(z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-z3x-(w2u*cos(m)+w2v*sin(m)*sin(sp))-z2x-
h16)/damp;
J(16,8)=(z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-z3x-(w2up*cos(m)+w2v*sin(m)*sin(s))-z2x-
h16)/damp;
J(16,9)=(z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-z3x-(w2u*cos(m)+w2vp*sin(m)*sin(s))-z2x-
h16)/damp;
J(16,10)=(z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-z3x-(w2u*cos(m)+w2v*sin(m)*sin(s))-z2xp-
h16)/damp;
J(16,11)=0;J(16,12)=0;
J(16,13)=(z2+(w1u*cos(z5)+w1v*sin(z5)*sin(z6))+z1x-z3xp-(w2u*cos(m)+w2v*sin(m)*sin(s))-z2x-
h16)/damp;
J(16,14)=0;J(16,15)=0;
J(17,1)=0;J(17,2)=0;J(17,3)=0;J(17,4)=0;J(17,5)=0;
J(17,6)=0;
J(17,7)=(z3+w1v*cos(z6)+z1y-z3y-(w2v*cos(sp))-z2y-h17)/damp;
J(17,8)=0;
J(17,9)=(z3+w1v*cos(z6)+z1y-z3y-(w2vp*cos(s))-z2y-h17)/damp;
J(17,10)=0;
J(17,11)=(z3+w1v*cos(z6)+z1y-z3y-(w2v*cos(s))-z2yp-h17)/damp;
J(17,12)=0;J(17,13)=0;
J(17,14)=(z3+w1v*cos(z6)+z1y-z3yp-(w2v*cos(s))-z2y-h17)/damp;
J(17,15)=0;
J(18,1)=0;J(18,2)=0;J(18,3)=0;J(18,4)=0;J(18,5)=0;
J(18,6)=(z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-z3z-(w2u*(-sin(mp))+w2v*cos(mp)*sin(s))-z2z-
h18)/damp;
J(18,7)=(z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-z3z-(w2u*(-sin(m))+w2v*cos(m)*sin(sp))-z2z-
h18)/damp;
J(18,8)=(z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-z3z-(w2up*(-sin(m))+w2v*cos(m)*sin(s))-z2z-
h18)/damp;
J(18,9)=(z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-z3z-(w2u*(-sin(m))+w2vp*cos(m)*sin(s))-z2z-
h18)/damp;
J(18,10)=0;J(18,11)=0;
J(18,12)=(z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-z3z-(w2u*(-sin(m))+w2v*cos(m)*sin(s))-z2zp-
h18)/damp;
J(18,13)=0;J(18,14)=0;

114
J(18,15)=(z4+(w1u*(-sin(z5))+w1v*cos(z5)*sin(z6))+z1z-z3zp-(w2u*(-sin(m))+w2v*cos(m)*sin(s))-z2z-
h18)/damp;
h(1)=h1;
h(2)=h2;
h(3)=h3;
h(4)=h4;
h(5)=h5;
h(6)=h6;
h(7)=h7;
h(8)=h8;
h(9)=h9;
h(10)=h10;
h(11)=h11;
h(12)=h12;
h(13)=h13;
h(14)=h14;
h(15)=h15;
h(16)=h16;
h(17)=h17;
h(18)=h18;
ds=-J\h';
%*******************************
%ACTUALIZACION DE VARIABLES
%*******************************
a2=a2+ds(1);
a3=a3+ds(2);
a4=a4+ds(3);
a5=a5+ds(4);
a6=a6+ds(5);
m=m+ds(6);
s=s+ds(7);
w2u=w2u+ds(8);
w2v=w2v+ds(9);
z2x=z2x+ds(10);
z2y=z2y+ds(11);
z2z=z2z+ds(12);
z3x=z3x+ds(13);
z3y=z3y+ds(14);
z3z=z3z+ds(15);
%***************************
%IMPRESION DE RESULTADOS
%***************************
fprintf('n=%2.0f,a2=%12.5e, a3=%12.5e,a4=%12.5e,a5=%12.5e,a6=%12.5e,m=%12.5e,
s=%12.5e,w2u=%12.5e,w2v=%12.5e,z2x=%12.5e,z2y=%12.5e, z2z=%12.5e,z3x=%12.5e,z3y=%12.5e,
z3z=%12.5e,', n,a2,a3,a4,a5,a6,m,s,w2u,w2v,z2x,z2y,z2z,z3x,z3y,z3z)
end; end

PROGRAMA PARA EL CALCULO DE LAS COMPONENTES DEL ESLABÓN C1 Y DE SU PIVOTE

DE TIERRA
Clear all
clc

115
damp=.1; %INCREMENTO PARA DERIVAR PARCIALMENTE
%******************************** %ESTOS VALORES SON ARBITRARIOS Y SE
%ENTRADA DE VALORES INICIALES %UTILIZAN PARA INICIAR EL PROCESO DEL
%******************************** %CALCULO.

c1x = input ('componente en el eje x del vector c1 ');

c1y = input ('componente en el eje y del vector c1 ');
c1z = input ('componente en el eje z del vector c1 ');
z5x = input ('componente en el eje x del vector z5 ');
z5y = input ('componente en el eje y del vector z5 ');
z5z = input ('componente en el eje z del vector z5 ');
c0x = input ('componente en el eje x del pivote de tierra de c1 ');
c0z = input ('componente en el eje y del pivote de tierra de c1 ');
c0y = input ('componente en el eje z del pivote de tierra de c1 ');
z5 = input ('ángulo de rotación sobre el eje y');
z6 = input ('ángulo de rotación sobre el eje x rotado');
i2 = input ('ángulo de rotación i2 ');
i3 = input ('ángulo de rotación i3 ');
i4 = input ('ángulo de rotación i4 ');
i5 = input ('ángulo de rotación i5 ');
i6 = input ('ángulo de rotación i6 ');
l2 = input ('ángulo de rotación l2 ');
l3 = input ('ángulo de rotación l3 ');
l4 = input ('ángulo de rotación l4');
l5 = input ('ángulo de rotación l5');
l6 = input ('ángulo de rotación l6 ');
n2 = input ('ángulo de rotación n2 ');
n3 = input ('ángulo de rotación n3 ');
n4 = input ('ángulo de rotación n4 ');
n5 = input ('ángulo de rotación n5 ');
n6 = input ('ángulo de rotación n6 ');

%*********************
%VALORES PRESCRITOS %ESTOS VALORES SON CONSTANTES
%*********************
z1x = input ('componente en el eje x del vector z1 ');
z1y = input ('componente en el eje y del vector z1 ');
z1z = input ('componente en el eje z del vector z1 ');
z2 = input ('componente en el eje x del pivote de tierra de w1 ');
z3 = input ('componente en el eje y del pivote de tierra de w1 ');
z4 = input ('componente en el eje z del pivote de tierra de w1 ');
z5 = input ('ángulo de rotación sobre el eje y');
z6 = input ('ángulo de rotación sobre el eje x rotado');
f2 = input ('ángulo de rotación f2 ');
f3 = input ('ángulo de rotación f3 ');
f4 = input ('ángulo de rotación f4 ');
f5 = input ('ángulo de rotación f5 ');
f6 = input ('ángulo de rotación f6 ');
p2 = input ('ángulo de rotación p2 ');
p3 = input ('ángulo de rotación p3 ');
p4 = input ('ángulo de rotación p4');
p5 = input ('ángulo de rotación p5');
p6 = input ('ángulo de rotación p6 ');
t2 = input ('ángulo de rotación t2 ');
t3 = input ('ángulo de rotación t3 ');
t4 = input ('ángulo de rotación t4 ');

116
t5 = input ('ángulo de rotación t5 ');
t6 = input ('ángulo de rotación t6 ');
b2 = input ('ángulo del eslabón de entrada b2 ');
b3 = input ('ángulo del eslabón de entrada b3 ');
b4 = input ('ángulo del eslabón de entrada b4 ');
b5 = input ('ángulo del eslabón de entrada b5 ');
b6 = input ('ángulo del eslabón de entrada b6 ');
p1x = input ('componente en x del primer punto de precisión ');
p1y = input ('componente en y del primer punto de precisión ');
p1z = input ('componente en z del primer punto de precisión ');
p2x = input ('componente en x del segundo punto de precisión ');
p2y = input ('componente en y del segundo punto de precisión ');
p2z = input ('componente en z del segundo punto de precisión ');
p3x = input ('componente en x del tercer punto de precisión ');
p3y = input ('componente en y del tercer punto de precisión ');
p3z = input ('componente en z del tercer punto de precisión ');
p4x = input ('componente en x del cuarto punto de precisión ');
p4y = input ('componente en y del cuarto punto de precisión ');
p4z = input ('componente en z del cuarto punto de precisión ');
p5x = input ('componente en x del quinto punto de precisión ');
p5y = input ('componente en y del quinto punto de precisión ');
p5z = input ('componente en z del quinto punto de precisión ');
p6x = input ('componente en x del sexto punto de precisión ');
p6y = input ('componente en y del sexto punto de precisión ');
p6z = input ('componente en x del sexto punto de precisión ');
r = 1500; %NUMERO DE ITERACIONES

%**********************
%CICLO DE ITERACIONES
%**********************
for n=1:r
c0xp=c0x+damp;
c0yp=c0y+damp;
c0zp=c0z+damp;
c1xp=c1x+damp;
c1yp=c1y+damp;
c1zp=c1z+damp;
i2p=i2+damp;
i3p=i3+damp;
i4p=i4+damp;
i5p=i5+damp;
i6p=i6+damp;
l2p=l2+damp;
l3p=l3+damp;
l4p=l4+damp;
l5p=l5+damp;
l6p=l6+damp;
n2p=n2+damp;
n3p=n3+damp;
n4p=n4+damp;
n5p=n5+damp;
n6p=n6+damp;
z5xp=z5x+damp;
z5yp=z5y+damp;
z5zp=z5z+damp;

117
k1=(z5x)*(cos(f2)*cos(p2)-sin(f2)*cos(t2)*sin(p2))+(z5y)*(-cos(f2)*sin(p2)-sin(f2)*cos(t2)*cos(p2))+
(z5z)*(sin(f2)*sin(t2));
k2=(z5x)*(sin(f2)*cos(p2)+cos(f2)*cos(t2)*sin(p2))+(z5y)*(-sin(f2)*sin(p2)+cos(f2)*cos(t2)*cos(p2))+
(z5z)*(-cos(f2)*sin(t2));
k3=(z5x)*sin(t2)*sin(p2)+(z5y)*sin(t2)*cos(p2)+(z5z)*cos(t2);
k4=(z5x)*(cos(f3)*cos(p3)-sin(f3)*cos(t3)*sin(p3))+(z5y)*(-cos(f3)*sin(p3)-sin(f3)*cos(t3)*cos(p3))+
(z5z)*(sin(f3)*sin(t3));
k5=(z5x)*(sin(f3)*cos(p3)+cos(f3)*cos(t3)*sin(p3))+(z5y)*(-sin(f3)*sin(p3)+cos(f3)*cos(t3)*cos(p3))+
(z5z)*(-cos(f3)*sin(t3));
k6=(z5x)*sin(t3)*sin(p3)+(z5y)*sin(t3)*cos(p3)+(z5z)*cos(t3);
k7=(z5x)*(cos(f4)*cos(p4)-sin(f4)*cos(t4)*sin(p4))+(z5y)*(-cos(f4)*sin(p4)-sin(f4)*cos(t4)*cos(p4))+
(z5z)*(sin(f4)*sin(t4));
k8=(z5x)*(sin(f4)*cos(p4)+cos(f4)*cos(t4)*sin(p4))+(z5y)*(-sin(f4)*sin(p4)+cos(f4)*cos(t4)*cos(p4))+
(z5z)*(-cos(f4)*sin(t4));
k9=(z5x)*sin(t4)*sin(p4)+(z5y)*sin(t4)*cos(p4)+(z5z)*cos(t4);
k10=(z5x)*(cos(f5)*cos(p5)-sin(f5)*cos(t5)*sin(p5))+(z5y)*(-cos(f5)*sin(p5)-sin(f5)*cos(t5)*cos(p5))+
(z5z)*(sin(f5)*sin(t5));
k11=(z5x)*(sin(f5)*cos(p5)+cos(f5)*cos(t5)*sin(p5))+(z5y)*(-sin(f5)*sin(p5)+cos(f5)*cos(t5)*cos(p5))+
(z5z)*(-cos(f5)*sin(t5));
k12=(z5x)*sin(t5)*sin(p5)+(z5y)*sin(t5)*cos(p5)+(z5z)*cos(t5);
k13=(z5x)*(cos(f6)*cos(p6)-sin(f6)*cos(t6)*sin(p6))+(z5y)*(-cos(f6)*sin(p6)-sin(f6)*cos(t6)*cos(p6))+
(z5z)*(sin(f6)*sin(t6));
k14=(z5x)*(sin(f6)*cos(p6)+cos(f6)*cos(t6)*sin(p6))+(z5y)*(-sin(f6)*sin(p6)+cos(f6)*cos(t6)*cos(p6))+
(z5z)*(-cos(f6)*sin(t6));
k15=(z5x)*sin(t6)*sin(p6)+(z5y)*sin(t6)*cos(p6)+(z5z)*cos(t6);
k16=(c1x)*(cos(i2)*cos(l2)-sin(i2)*cos(n2)*sin(l2))+(c1y)*(-cos(i2)*sin(l2)-sin(i2)*cos(n2)*cos(l2))+
(c1z)*(sin(i2)*sin(n2));
k17=(c1x)*(sin(i2)*cos(l2)+cos(i2)*cos(n2)*sin(l2))+(c1y)*(-sin(i2)*sin(l2)+cos(i2)*cos(n2)*cos(l2))+
(c1z)*(-cos(i2)*sin(n2));
k18=(c1x)*sin(n2)*sin(l2)+(c1y)*sin(n2)*cos(l2)+(c1z)*cos(n2);
k19=(c1x)*(cos(i3)*cos(l3)-sin(i3)*cos(n3)*sin(l3))+(c1y)*(-cos(i3)*sin(l3)-sin(i3)*cos(n3)*cos(l3))+
(c1z)*(sin(i3)*sin(n3));
k20=(c1x)*(sin(i3)*cos(l3)+cos(i3)*cos(n3)*sin(l3))+(c1y)*(-sin(i3)*sin(l3)+cos(i3)*cos(n3)*cos(l3))+
(c1z)*(-cos(i3)*sin(n3));
k21=(c1x)*sin(n3)*sin(l3)+(c1y)*sin(n3)*cos(l3)+(c1z)*cos(n3);
k22=(c1x)*(cos(i4)*cos(l4)-sin(i4)*cos(n4)*sin(l4))+(c1y)*(-cos(i4)*sin(l4)-sin(i4)*cos(n4)*cos(l4))+
(c1z)*(sin(i4)*sin(n4));
k23=(c1x)*(sin(i4)*cos(l4)+cos(i4)*cos(n4)*sin(l4))+(c1y)*(-sin(i4)*sin(l4)+cos(i4)*cos(n4)*cos(l4))+
(c1z)*(-cos(i4)*sin(n4));
k24=(c1x)*sin(n4)*sin(l4)+(c1y)*sin(n4)*cos(l4)+(c1z)*cos(n4);
k25=(c1x)*(cos(i5)*cos(l5)-sin(i5)*cos(n5)*sin(l5))+(c1y)*(-cos(i5)*sin(l5)-sin(i5)*cos(n5)*cos(l5))+
(c1z)*(sin(i5)*sin(n5));
k26=(c1x)*(sin(i5)*cos(l5)+cos(i5)*cos(n5)*sin(l5))+(c1y)*(-sin(i5)*sin(l5)+cos(i5)*cos(n5)*cos(l5))+
(c1z)*(-cos(i5)*sin(n5));
k27=(c1x)*sin(n5)*sin(l5)+(c1y)*sin(n5)*cos(l5)+(c1z)*cos(n5);
k28=(c1x)*(cos(i6)*cos(l6)-sin(i6)*cos(n6)*sin(l6))+(c1y)*(-cos(i6)*sin(l6)-sin(i6)*cos(n6)*cos(l6))+
(c1z)*(sin(i6)*sin(n6));
k29=(c1x)*(sin(i6)*cos(l6)+cos(i6)*cos(n6)*sin(l6))+(c1y)*(-sin(i6)*sin(l6)+
cos(i6)*cos(n6)*cos(l6))+
(c1z)*(-cos(i6)*sin(n6));
k30=(c1x)*sin(n6)*sin(l6)+(c1y)*sin(n6)*cos(l6)+(c1z)*cos(n6);
k31=(z1x)*(cos(f2)*cos(p2)-sin(f2)*cos(t2)*sin(p2))+(z1y)*(-cos(f2)*sin(p2)-sin(f2)*cos(t2)*cos(p2))+
(z1z)*(sin(f2)*sin(t2));
k32=(z1x)*(sin(f2)*cos(p2)+cos(f2)*cos(t2)*sin(p2))+(z1y)*(-sin(f2)*sin(p2)+cos(f2)*cos(t2)*cos(p2))+
(z1z)*(-cos(f2)*sin(t2));
k33=(z1x)*sin(t2)*sin(p2)+(z1y)*sin(t2)*cos(p2)+(z1z)*cos(t2);

118
k34=(z1x)*(cos(f3)*cos(p3)-sin(f3)*cos(t3)*sin(p3))+(z1y)*(-cos(f3)*sin(p3)-sin(f3)*cos(t3)*cos(p3))+
(z1z)*(sin(f3)*sin(t3));
k35=(z1x)*(sin(f3)*cos(p3)+cos(f3)*cos(t3)*sin(p3))+(z1y)*(-sin(f3)*sin(p3)+cos(f3)*cos(t3)*cos(p3))+
(z1z)*(-cos(f3)*sin(t3));
k36=(z1x)*sin(t3)*sin(p3)+(z1y)*sin(t3)*cos(p3)+(z1z)*cos(t3);
k37=(z1x)*(cos(f4)*cos(p4)-sin(f4)*cos(t4)*sin(p4))+(z1y)*(-cos(f4)*sin(p4)-sin(f4)*cos(t4)*cos(p4))+
(z1z)*(sin(f4)*sin(t4));
k38=(z1x)*(sin(f4)*cos(p4)+cos(f4)*cos(t4)*sin(p4))+(z1y)*(-sin(f4)*sin(p4)+cos(f4)*cos(t4)*cos(p4))+
(z1z)*(-cos(f4)*sin(t4));
k39=(z1x)*sin(t4)*sin(p4)+(z1y)*sin(t4)*cos(p4)+(z1z)*cos(t4);
k40=(z1x)*(cos(f5)*cos(p5)-sin(f5)*cos(t5)*sin(p5))+(z1y)*(-cos(f5)*sin(p5)-sin(f5)*cos(t5)*cos(p5))+
(z1z)*(sin(f5)*sin(t5));
k41=(z1x)*(sin(f5)*cos(p5)+cos(f5)*cos(t5)*sin(p5))+(z1y)*(-sin(f5)*sin(p5)+cos(f5)*cos(t5)*cos(p5))+
(z1z)*(-cos(f5)*sin(t5));
k42=(z1x)*sin(t5)*sin(p5)+(z1y)*sin(t5)*cos(p5)+(z1z)*cos(t5);
k43=(z1x)*(cos(f6)*cos(p6)-sin(f6)*cos(t6)*sin(p6))+(z1y)*(-cos(f6)*sin(p6)-sin(f6)*cos(t6)*cos(p6))+
(z1z)*(sin(f6)*sin(t6));
k44=(z1x)*(sin(f6)*cos(p6)+cos(f6)*cos(t6)*sin(p6))+(z1y)*(-sin(f6)*sin(p6)+cos(f6)*cos(t6)*cos(p6))+
(z1z)*(-cos(f6)*sin(t6));
k45=(z1x)*sin(t6)*sin(p6)+(z1y)*sin(t6)*cos(p6)+(z1z)*cos(t6);

d1=(cos(f2)*cos(p2)-sin(f2)*cos(t2)*sin(p2));
d2=(-cos(f2)*sin(p2)-sin(f2)*cos(t2)*cos(p2));
d3=(sin(f2)*sin(t2));
d4=(sin(f2)*cos(p2)+cos(f2)*cos(t2)*sin(p2));
d5=(-sin(f2)*sin(p2)+cos(f2)*cos(t2)*cos(p2));
d6=(-cos(f2)*sin(t2));
d7=sin(t2)*sin(p2);
d8=sin(t2)*cos(p2);
d9=cos(t2);
d10=(cos(f3)*cos(p3)-sin(f3)*cos(t3)*sin(p3));
d11=(-cos(f3)*sin(p3)-sin(f3)*cos(t3)*cos(p3));
d12=(sin(f3)*sin(t3));
d13=(sin(f3)*cos(p3)+cos(f3)*cos(t3)*sin(p3));
d14=(-sin(f3)*sin(p3)+cos(f3)*cos(t3)*cos(p3));
d15=(-cos(f3)*sin(t3));
d16=sin(t3)*sin(p3);
d17=sin(t3)*cos(p3);
d18=cos(t3);
d19=(cos(f4)*cos(p4)-sin(f4)*cos(t4)*sin(p4));
d20=(-cos(f4)*sin(p4)-sin(f4)*cos(t4)*cos(p4));
d21=(sin(f4)*sin(t4));
d22=(sin(f4)*cos(p4)+cos(f4)*cos(t4)*sin(p4));
d23=(-sin(f4)*sin(p4)+cos(f4)*cos(t4)*cos(p4));
d24=(-cos(f4)*sin(t4));
d25=sin(t4)*sin(p4);
d26=sin(t4)*cos(p4);
d27=cos(t4);
d28=(cos(f5)*cos(p5)-sin(f5)*cos(t5)*sin(p5));
d29=(-cos(f5)*sin(p5)-sin(f5)*cos(t5)*cos(p5));
d30=(sin(f5)*sin(t5));
d31=(sin(f5)*cos(p5)+cos(f5)*cos(t5)*sin(p5));
d32=(-sin(f5)*sin(p5)+cos(f5)*cos(t5)*cos(p5));
d33=(-cos(f5)*sin(t5));
d34=sin(t5)*sin(p5);
d35=sin(t5)*cos(p5);

119
d36=cos(t5);
d37=(cos(f6)*cos(p6)-sin(f6)*cos(t6)*sin(p6));
d38=(-cos(f6)*sin(p6)-sin(f6)*cos(t6)*cos(p6));
d39=(sin(f6)*sin(t6));
d40=(sin(f6)*cos(p6)+cos(f6)*cos(t6)*sin(p6));
d41=(-sin(f6)*sin(p6)+cos(f6)*cos(t6)*cos(p6));
d42=(-cos(f6)*sin(t6));
d43=sin(t6)*sin(p6);
d44=sin(t6)*cos(p6);
d45=cos(t6);
d46=(cos(i2)*cos(l2)-sin(i2)*cos(n2)*sin(l2));
d47=(-cos(i2)*sin(l2)-sin(i2)*cos(n2)*cos(l2));
d48=(sin(i2)*sin(n2));
d49=(sin(i2)*cos(l2)+cos(i2)*cos(n2)*sin(l2));
d50=(-sin(i2)*sin(l2)+cos(i2)*cos(n2)*cos(l2));
d51=(-cos(i2)*sin(n2));
d52=sin(n2)*sin(l2);
d53=sin(n2)*cos(l2);
d54=cos(n2);
d55=(cos(i3)*cos(l3)-sin(i3)*cos(n3)*sin(l3));
d56=(-cos(i3)*sin(l3)-sin(i3)*cos(n3)*cos(l3));
d57=(sin(i3)*sin(n3));
d58=(sin(i3)*cos(l3)+cos(i3)*cos(n3)*sin(l3));
d59=(-sin(i3)*sin(l3)+cos(i3)*cos(n3)*cos(l3));
d60=(-cos(i3)*sin(n3));
d61=sin(n3)*sin(l3);
d62=sin(n3)*cos(l3);
d63=cos(n3);
d64=(cos(i4)*cos(l4)-sin(i4)*cos(n4)*sin(l4));
d65=(-cos(i4)*sin(l4)-sin(i4)*cos(n4)*cos(l4));
d66=(sin(i4)*sin(n4));
d67=(sin(i4)*cos(l4)+cos(i4)*cos(n4)*sin(l4));
d68=(-sin(i4)*sin(l4)+cos(i4)*cos(n4)*cos(l4));
d69=(-cos(i4)*sin(n4));
d70=sin(n4)*sin(l4);
d71=sin(n4)*cos(l4);
d72=cos(n4);
d73=(cos(i5)*cos(l5)-sin(i5)*cos(n5)*sin(l5));
d74=(-cos(i5)*sin(l5)-sin(i5)*cos(n5)*cos(l5));
d75=(sin(i5)*sin(n5));
d76=(sin(i5)*cos(l5)+cos(i5)*cos(n5)*sin(l5));
d77=(-sin(i5)*sin(l5)+cos(i5)*cos(n5)*cos(l5));
d78=(-cos(i5)*sin(n5));
d79=sin(n5)*sin(l5);
d80=sin(n5)*cos(l5);
d81=cos(n5);
d82=(cos(i6)*cos(l6)-sin(i6)*cos(n6)*sin(l6));
d83=(-cos(i6)*sin(l6)-sin(i6)*cos(n6)*cos(l6));
d84=(sin(i6)*sin(n6));
d85=(sin(i6)*cos(l6)+cos(i6)*cos(n6)*sin(l6));
d86=(-sin(i6)*sin(l6)+cos(i6)*cos(n6)*cos(l6));
d87=(-cos(i6)*sin(n6));
d88=sin(n6)*sin(l6);
d89=sin(n6)*cos(l6);
d90=cos(n6);

120
e1=(cos(i2p)*cos(l2)-sin(i2p)*cos(n2)*sin(l2));
e2=(-cos(i2p)*sin(l2)-sin(i2p)*cos(n2)*cos(l2));
e3=(sin(i2p)*sin(n2));
e4=(cos(i2)*cos(l2p)-sin(i2)*cos(n2)*sin(l2p));
e5=(-cos(i2)*sin(l2p)-sin(i2)*cos(n2)*cos(l2p));
e6=(sin(i2)*sin(n2));
e7=(cos(i2)*cos(l2)-sin(i2)*cos(n2p)*sin(l2));
e8=(-cos(i2)*sin(l2)-sin(i2)*cos(n2p)*cos(l2));
e9=(sin(i2)*sin(n2p));
e10=(sin(i2p)*cos(l2)+cos(i2p)*cos(n2)*sin(l2));
e11=(-sin(i2p)*sin(l2)+cos(i2p)*cos(n2)*cos(l2));
e12=(-cos(i2p)*sin(n2));
e13=(sin(i2)*cos(l2p)+cos(i2)*cos(n2)*sin(l2p));
e14=(-sin(i2)*sin(l2p)+cos(i2)*cos(n2)*cos(l2p));
e15=(-cos(i2)*sin(n2));
e16=(sin(i2)*cos(l2)+cos(i2)*cos(n2p)*sin(l2));
e17=(-sin(i2)*sin(l2)+cos(i2)*cos(n2p)*cos(l2));
e18=(-cos(i2)*sin(n2p));
e19=sin(n2)*sin(l2p);
e20=sin(n2)*cos(l2p);
e21=cos(n2);
e22=sin(n2p)*sin(l2);
e23=sin(n2p)*cos(l2);
e24=cos(n2p);
e25=(cos(i3p)*cos(l3)-sin(i3p)*cos(n3)*sin(l3));
e26=(-cos(i3p)*sin(l3)-sin(i3p)*cos(n3)*cos(l3));
e27=(sin(i3p)*sin(n3));
e28=(cos(i3)*cos(l3p)-sin(i3)*cos(n3)*sin(l3p));
e29=(-cos(i3)*sin(l3p)-sin(i3)*cos(n3)*cos(l3p));
e30=(sin(i3)*sin(n3));
e31=(cos(i3)*cos(l3)-sin(i3)*cos(n3p)*sin(l3));
e32=(-cos(i3)*sin(l3)-sin(i3)*cos(n3p)*cos(l3));
e33=(sin(i3)*sin(n3p));
e34=(sin(i3p)*cos(l3)+cos(i3p)*cos(n3)*sin(l3));
e35=(-sin(i3p)*sin(l3)+cos(i3p)*cos(n3)*cos(l3));
e36=(-cos(i3p)*sin(n3));
e37=(sin(i3)*cos(l3p)+cos(i3)*cos(n3)*sin(l3p));
e38=(-sin(i3)*sin(l3p)+cos(i3)*cos(n3)*cos(l3p));
e39=(-cos(i3)*sin(n3));
e40=(sin(i3)*cos(l3)+cos(i3)*cos(n3p)*sin(l3));
e41=(-sin(i3)*sin(l3)+cos(i3)*cos(n3p)*cos(l3));
e42=(-cos(i3)*sin(n3p));
e43=sin(n3)*sin(l3p);
e44=sin(n3)*cos(l3p);
e45=cos(n3);
e46=sin(n3p)*sin(l3);
e47=sin(n3p)*cos(l3);
e48=cos(n3p);
e49=(cos(i4p)*cos(l4)-sin(i4p)*cos(n4)*sin(l4));
e50=(-cos(i4p)*sin(l4)-sin(i4p)*cos(n4)*cos(l4));
e51=(sin(i4p)*sin(n4));
e52=(cos(i4)*cos(l4p)-sin(i4)*cos(n4)*sin(l4p));
e53=(-cos(i4)*sin(l4p)-sin(i4)*cos(n4)*cos(l4p));
e54=(sin(i4)*sin(n4));
e55=(cos(i4)*cos(l4)-sin(i4)*cos(n4p)*sin(l4));
e56=(-cos(i4)*sin(l4)-sin(i4)*cos(n4p)*cos(l4));

121
e57=(sin(i4)*sin(n4p));
e58=(sin(i4p)*cos(l4)+cos(i4p)*cos(n4)*sin(l4));
e59=(-sin(i4p)*sin(l4)+cos(i4p)*cos(n4)*cos(l4));
e60=(-cos(i4p)*sin(n3));
e61=(sin(i4)*cos(l4p)+cos(i4)*cos(n4)*sin(l4p));
e62=(-sin(i4)*sin(l4p)+cos(i4)*cos(n4)*cos(l4p));
e63=(-cos(i4)*sin(n4));
e64=(sin(i4)*cos(l4)+cos(i4)*cos(n4p)*sin(l4));
e65=(-sin(i4)*sin(l4)+cos(i4)*cos(n4p)*cos(l4));
e66=(-cos(i4)*sin(n4p));
e67=sin(n4)*sin(l4p);
e68=sin(n4)*cos(l4p);
e69=cos(n4);
e70=sin(n4p)*sin(l4);
e71=sin(n4p)*cos(l4);
e72=cos(n4p);
e73=(cos(i5p)*cos(l5)-sin(i5p)*cos(n5)*sin(l5));
e74=(-cos(i5p)*sin(l5)-sin(i5p)*cos(n5)*cos(l5));
e75=(sin(i5p)*sin(n5));
e76=(cos(i5)*cos(l5p)-sin(i5)*cos(n5)*sin(l5p));
e77=(-cos(i5)*sin(l5p)-sin(i5)*cos(n5)*cos(l5p));
e78=(sin(i5)*sin(n5));
e79=(cos(i5)*cos(l5)-sin(i5)*cos(n5p)*sin(l5));
e80=(-cos(i5)*sin(l5)-sin(i5)*cos(n5p)*cos(l5));
e81=(sin(i5)*sin(n5p));
e82=(sin(i5p)*cos(l5)+cos(i5p)*cos(n5)*sin(l5));
e83=(-sin(i5p)*sin(l5)+cos(i5p)*cos(n5)*cos(l5));
e84=(-cos(i5p)*sin(n5));
e85=(sin(i5)*cos(l5p)+cos(i5)*cos(n5)*sin(l5p));
e86=(-sin(i5)*sin(l5p)+cos(i5)*cos(n5)*cos(l5p));
e87=(-cos(i5)*sin(n5));
e88=(sin(i5)*cos(l5)+cos(i5)*cos(n5p)*sin(l5));
e89=(-sin(i5)*sin(l5)+cos(i5)*cos(n5p)*cos(l5));
e90=(-cos(i5)*sin(n5p));
e91=sin(n5)*sin(l5p);
e92=sin(n5)*cos(l5p);
e93=cos(n5);
e94=sin(n5p)*sin(l5);
e95=sin(n5p)*cos(l5);
e96=cos(n5p);
e97=(cos(i6p)*cos(l6)-sin(i6p)*cos(n6)*sin(l6));
e98=(-cos(i6p)*sin(l6)-sin(i6p)*cos(n6)*cos(l6));
e99=(sin(i6p)*sin(n6));
e100=(cos(i6)*cos(l6p)-sin(i6)*cos(n6)*sin(l6p));
e101=(-cos(i6)*sin(l6p)-sin(i6)*cos(n6)*cos(l6p));
e102=(sin(i6)*sin(n6));
e103=(cos(i6)*cos(l6)-sin(i6)*cos(n6p)*sin(l6));
e104=(-cos(i6)*sin(l6)-sin(i6)*cos(n6p)*cos(l6));
e105=(sin(i6)*sin(n6p));
e106=(sin(i6p)*cos(l6)+cos(i6p)*cos(n6)*sin(l6));
e107=(-sin(i6p)*sin(l6)+cos(i6p)*cos(n6)*cos(l6));
e108=(-cos(i6p)*sin(n6));
e109=(sin(i6)*cos(l6p)+cos(i6)*cos(n6)*sin(l6p));
e110=(-sin(i6)*sin(l6p)+cos(i6)*cos(n6)*cos(l6p));
e111=(-cos(i6)*sin(n6));
e112=(sin(i6)*cos(l6)+cos(i6)*cos(n6p)*sin(l6));

122
e113=(-sin(i6)*sin(l6)+cos(i6)*cos(n6p)*cos(l6));
e114=(-cos(i6)*sin(n6p));
e115=sin(n6)*sin(l6p);
e116=sin(n6)*cos(l6p);
e117=cos(n6);
e118=sin(n6p)*sin(l6);
e119=sin(n6p)*cos(l6);
e120=cos(n6p);
v1=z2+w1x*(cos(b2)*cos(z5)+sin(z5)*sin(z6)*sin(b2))+w1y*(-sin(b2)*cos(z5)+sin(z5)*sin(z6)*cos(b2));
v2=z3+w1x*(sin(b2)*cos(z6))+w1y*cos(b2)*cos(z6);
v3=z4+w1x*(-sin(z5)*cos(b2)+cos(z5)*sin(z6)*sin(b2))+w1y*(sin(z5)*sin(b2)+cos(z5)*sin(z6)*cos(b2));
v4=z2+w1x*(cos(b3)*cos(z5)+sin(z5)*sin(z6)*sin(b3))+w1y*(-sin(b3)*cos(z5)+sin(z5)*sin(z6)*cos(b3));
v5=z3+w1x*(sin(b3)*cos(z6))+w1y*cos(b3)*cos(z6);
v6=z4+w1x*(-sin(z5)*cos(b3)+cos(z5)*sin(z6)*sin(b3))+w1y*(sin(z5)*sin(b3)+cos(z5)*sin(z6)*cos(b3));
v7=z2+w1x*(cos(b4)*cos(z5)+sin(z5)*sin(z6)*sin(b4))+w1y*(-sin(b4)*cos(z5)+sin(z5)*sin(z6)*cos(b4));
v8=z3+w1x*(sin(b4)*cos(z6))+w1y*cos(b4)*cos(z6);
v9=z4+w1x*(-sin(z5)*cos(b4)+cos(z5)*sin(z6)*sin(b4))+w1y*(sin(z5)*sin(b4)+cos(z5)*sin(z6)*cos(b4));
v10=z2+w1x*(cos(b5)*cos(z5)+sin(z5)*sin(z6)*sin(b5))+w1y*(-sin(b5)*cos(z5)+sin(z5)*sin(z6)*cos(b5));
v11=z3+w1x*(sin(b5)*cos(z6))+w1y*cos(b5)*cos(z6);
v12=z4+w1x*(-sin(z5)*cos(b5)+cos(z5)*sin(z6)*sin(b5))+w1y*(sin(z5)*sin(b5)+cos(z5)*sin(z6)*cos(b5));
v13=z2+w1x*(cos(b6)*cos(z5)+sin(z5)*sin(z6)*sin(b6))+w1y*(-sin(b6)*cos(z5)+sin(z5)*sin(z6)*cos(b6));
v14=z3+w1x*(sin(b6)*cos(z6))+w1y*cos(b6)*cos(z6);
v15=z4+w1x*(-sin(z5)*cos(b6)+cos(z5)*sin(z6)*sin(b6))+w1y*(sin(z5)*sin(b6)+cos(z5)*sin(z6)*cos(b6));
h1=k1+k16-z5x-c1x-(p2x-p1x);
h2=k2+k17-z5y-c1y-(p2y-p1y);
h3=k3+k18-z5z-c1z-(p2z-p1z);
h4=k4+k19-z5x-c1x-(p3x-p1x);
h5=k5+k20-z5y-c1y-(p3y-p1y);
h6=k6+k21-z5z-c1z-(p3z-p1z);
h7=k7+k22-z5x-c1x-(p4x-p1x);
h8=k8+k23-z5y-c1y-(p4y-p1y);
h9=k9+k24-z5z-c1z-(p4z-p1z);
h10=k10+k25-z5x-c1x-(p5x-p1x);
h11=k11+k26-z5y-c1y-(p5y-p1y);
h12=k12+k27-z5z-c1z-(p5z-p1z);
h13=k13+k28-z5x-c1x-(p6x-p1x);
h14=k14+k29-z5y-c1y-(p6y-p1y);
h15=k15+k30-z5z-c1z-(p6z-p1z);
h16=z2+w1x*(cos(b2)*cos(z5)+sin(z5)*sin(z6)*sin(b2))+w1y*(-sin(b2)*cos(z5)+
sin(z5)*sin(z6)*cos(b2))+k31-k1-k16-c0x;
h17=z3+w1x*(sin(b2)*cos(z6))+w1y*cos(b2)*cos(z6)+k32-k2-k17-c0y;
h18=z4+w1x*(-sin(z5)*cos(b2)+cos(z5)*sin(z6)*sin(b2))+
w1y*(sin(z5)*sin(b2)+cos(z5)*sin(z6)*cos(b2))+k33-k3-k18-c0z;
h19=z2+w1x*(cos(b3)*cos(z5)+sin(z5)*sin(z6)*sin(b3))+w1y*(-sin(b3)*cos(z5)+
sin(z5)*sin(z6)*cos(b3))+k34-k4-k19-c0x;
h20=z3+w1x*(sin(b3)*cos(z6))+w1y*cos(b3)*cos(z6)+k35-k5-k20-c0y;
h21=z4+w1x*(-sin(z5)*cos(b3)+cos(z5)*sin(z6)*sin(b3))+
w1y*(sin(z5)*sin(b3)+cos(z5)*sin(z6)*cos(b3))+k36-k6-k21-c0z;
h22=z2+w1x*(cos(b4)*cos(z5)+sin(z5)*sin(z6)*sin(b4))+w1y*(-sin(b4)*cos(z5)+
sin(z5)*sin(z6)*cos(b4))+k37-k7-k22-c0x;
h23=z3+w1x*(sin(b4)*cos(z6))+w1y*cos(b4)*cos(z6)+k38-k8-k23-c0y;
h24=z4+w1x*(-sin(z5)*cos(b4)+cos(z5)*sin(z6)*sin(b4))+w1y*(sin(z5)*sin(b4)+
cos(z5)*sin(z6)*cos(b4))+k39-k9-k24-c0z;
h25=z2+w1x*(cos(b5)*cos(z5)+sin(z5)*sin(z6)*sin(b5))+w1y*(-sin(b5)*cos(z5)+
sin(z5)*sin(z6)*cos(b5))+k40-k10-k25-c0x;
h26=z3+w1x*(sin(b5)*cos(z6))+w1y*cos(b5)*cos(z6)+k41-k11-k26-c0y;

123
 h27=z4+w1x*(-sin(z5)*cos(b5)+cos(z5)*sin(z6)*sin(b5))+
w1y*(sin(z5)*sin(b5)+cos(z5)*sin(z6)*cos(b5))+k42-k12-k27-c0z;
h28=z2+w1x*(cos(b6)*cos(z5)+sin(z5)*sin(z6)*sin(b6))+w1y*(-sin(b6)*cos(z5)+
sin(z5)*sin(z6)*cos(b6))+k43-k13-k28-c0x;
h29=z3+w1x*(sin(b6)*cos(z6))+w1y*cos(b6)*cos(z6)+k44-k14-k29-c0y;
h30=z4+w1x*(-sin(z5)*cos(b6)+cos(z5)*sin(z6)*sin(b6))+
w1y*(sin(z5)*sin(b6)+cos(z5)*sin(z6)*cos(b6))+k45-k15-k30-c0z;
%****************************
%FORMACION DEL JACOBIANO
%****************************
J(1,1)=0;
J(1,2)=0;
J(1,3)=0;
J(1,4)=(k1+c1xp*d46+c1y*d47+c1z*d48-z5x-c1xp-(p2x-p1x)-h1)/damp;
J(1,5)=(k1+c1x*d46+c1yp*d47+c1z*d48-z5x-c1x-(p2x-p1x)-h1)/damp;
J(1,6)=(k1+c1x*d46+c1y*d47+c1zp*d48-z5x-c1x-(p2x-p1x)-h1)/damp;
J(1,7)=(k1+(c1x*e1+c1y*e2+c1z*e3)-z5x-c1x-(p2x-p1x)-h1)/damp;
J(1,8)=0;J(1,9)=0;J(1,10)=0;J(1,11)=0;
J(1,12)=(k1+(c1x*e4+c1y*e5+c1z*e6)-z5x-c1x-(p2x-p1x)-h1)/damp;
J(1,13)=0;J(1,14)=0;J(1,15)=0;J(1,16)=0;
J(1,17)=(k1+(c1x*e7+c1y*e8+c1z*e9)-z5x-c1x-(p2x-p1x)-h1)/damp;
J(1,18)=0;J(1,19)=0;J(1,20)=0;J(1,21)=0;
J(1,22)=(z5xp*d1+z5y*d2+z5z*d3+(c1x*d46+c1y*d47+c1z*d48)-z5xp-c1x-(p2x-p1x)-h1)/damp;
J(1,23)=(z5x*d1+z5yp*d2+z5z*d3+(c1x*d46+c1y*d47+c1z*d48)-z5x-c1x-(p2x-p1x)-h1)/damp;
J(1,24)=(z5x*d1+z5y*d2+z5zp*d3+(c1x*d46+c1y*d47+c1z*d48)-z5x-c1x-(p2x-p1x)-h1)/damp;
J(2,1)=0;
J(2,2)=0;
J(2,3)=0;
J(2,4)=(k2+(c1xp)*d49+(c1y)*d50+(c1z)*d51-z5y-c1y-(p2y-p1y)-h2)/damp;
J(2,5)=(k2+(c1x*d49+c1yp*d50+c1z*d51)-z5y-c1yp-(p2y-p1y)-h2)/damp;
J(2,6)=(k2+(c1x*d49+c1y*d50+c1zp*d51)-z5y-c1y-(p2y-p1y)-h2)/damp;
J(2,7)=(k2+(c1x*e10+c1y*e11+c1z*e12)-z5y-c1y-(p2y-p1y)-h2)/damp;
J(2,8)=0;J(2,9)=0;J(2,10)=0;J(2,11)=0;
J(2,12)=(k2+(c1x*e13+c1y*e14+c1z*e15)-z5y-c1y-(p2y-p1y)-h2)/damp;
J(2,13)=0;J(2,14)=0;J(2,15)=0;J(2,16)=0;
J(2,17)=(k2+(c1x*e16+c1y*e17+c1z*e18)-z5y-c1y-(p2y-p1y)-h2)/damp;
J(2,18)=0;J(2,19)=0;J(2,20)=0;J(2,21)=0;
J(2,22)=(z5xp*d4+z5y*d5+z5z*d6+(c1x*d49+c1y*d50+c1z*d51)-z5y-c1y-(p2y-p1y)-h2)/damp;
J(2,23)=(z5x*d4+z5yp*d5+z5z*d6+(c1x*d49+c1y*d50+c1z*d51)-z5yp-c1y-(p2y-p1y)-h2)/damp;
J(2,24)=(z5x*d4+z5y*d5+z5zp*d6+(c1x*d49+c1y*d50+c1z*d51)-z5y-c1y-(p2y-p1y)-h2)/damp;
J(3,1)=0;
J(3,2)=0;
J(3,3)=0;
J(3,4)=(k3+(c1xp*d52+c1y*d53+c1z*d54)-z5z-c1z-(p2z-p1z)-h3)/damp;
J(3,5)=(k3+(c1x*d52+c1yp*d53+c1z*d54)-z5z-c1z-(p2z-p1z)-h3)/damp;
J(3,6)=(k3+(c1x*d52+c1y*d53+c1zp*d54)-z5z-c1zp-(p2z-p1z)-h3)/damp;
J(3,7)=0;
J(3,8)=0;J(3,9)=0;J(3,10)=0;J(3,11)=0;
J(3,12)=(k3+(c1x*e19+c1y*e20+c1z*e21)-z5z-c1z-(p2z-p1z)-h3)/damp;
J(3,13)=0;J(3,14)=0;J(3,15)=0;J(3,16)=0;
J(3,17)=(k3+(c1x*e22+c1y*e23+c1z*e24)-z5z-c1z-(p2z-p1z)-h3)/damp;
J(3,18)=0;J(3,19)=0;J(3,20)=0;J(3,21)=0;
J(3,22)=(z5xp*d7+z5y*d8+z5z*d9+(c1x*d52+c1y*d53+c1z*d54)-z5z-c1z-(p2z-p1z)-h3)/damp;
J(3,23)=(z5x*d7+z5yp*d8+z5z*d9+(c1x*d52+c1y*d53+c1z*d54)-z5z-c1z-(p2z-p1z)-h3)/damp;
J(3,24)=(z5x*d7+z5y*d8+z5zp*d9+(c1x*d52+c1y*d53+c1z*d54)-z5zp-c1z-(p2z-p1z)-h3)/damp;
J(4,1)=0;

124
J(4,2)=0;
J(4,3)=0;
J(4,4)=(k4+(c1xp*d55+c1y*d56+c1z*d57)-z5x-c1xp-(p3x-p1x)-h4)/damp;
J(4,5)=(k4+(c1x*d55+c1yp*d56+c1z*d57)-z5x-c1x-(p3x-p1x)-h4)/damp;
J(4,6)=(k4+(c1x*d55+c1y*d56+c1zp*d57)-z5x-c1x-(p3x-p1x)-h4)/damp;
J(4,7)=0;
J(4,8)=(k4+(c1x*e25+c1y*e26+c1z*e27)-z5x-c1x-(p3x-p1x)-h4)/damp;
J(4,9)=0;J(4,10)=0;J(4,11)=0;
J(4,12)=0;
J(4,13)=(k4+(c1x*e28+c1y*e29+c1z*e30)-z5x-c1x-(p3x-p1x)-h4)/damp;
J(4,14)=0;J(4,15)=0;J(4,16)=0;
J(4,17)=0;
J(4,18)=(k4+(c1x*e31+c1y*e32+c1z*e33)-z5x-c1x-(p3x-p1x)-h4)/damp;
J(4,19)=0;J(4,20)=0;J(4,21)=0;
J(4,22)=(z5xp*d10+z5y*d11+z5z*d12+(c1x*d55+c1y*d56+c1z*d57)-z5xp-c1x-(p3x-p1x)-h4)/damp;
J(4,23)=(z5x*d10+z5yp*d11+z5z*d12+(c1x*d55+c1y*d56+c1z*d57)-z5x-c1x-(p3x-p1x)-h4)/damp;
J(4,24)=(z5x*d10+z5y*d11+z5zp*d12+(c1x*d55+c1y*d56+c1z*d57)-z5x-c1x-(p3x-p1x)-h4)/damp;
J(5,1)=0;
J(5,2)=0;
J(5,3)=0;
J(5,4)=(k5+(c1xp*d58+c1y*d59+c1z*d60)-z5y-c1y-(p3y-p1y)-h5)/damp;
J(5,5)=(k5+(c1x*d58+c1yp*d59+c1z*d60)-z5y-c1yp-(p3y-p1y)-h5)/damp;
J(5,6)=(k5+(c1x*d58+c1y*d59+c1zp*d60)-z5y-c1y-(p3y-p1y)-h5)/damp;
J(5,7)=0;
J(5,8)=(k5+(c1x*e34+c1y*e35+c1z*e36)-z5y-c1y-(p3y-p1y)-h5)/damp;
J(5,9)=0;J(5,10)=0;J(5,11)=0;J(5,12)=0;
J(5,13)=(k5+(c1x*e37+c1y*e38+c1z*e39)-z5y-c1y-(p3y-p1y)-h5)/damp;
J(5,14)=0;J(5,15)=0;J(5,16)=0;J(5,17)=0;
J(5,18)=(k5+(c1x*e40+c1y*e41+c1z*e42)-z5y-c1y-(p3y-p1y)-h5)/damp;
J(5,19)=0;J(5,20)=0;J(5,21)=0;
J(5,22)=(z5xp*d13+z5y*d14+z5z*d15+(c1x*d58+c1y*d59+c1z*d60)-z5y-c1y-(p3y-p1y)-h5)/damp;
J(5,23)=(z5x*d13+z5yp*d14+z5z*d15+(c1x*d58+c1y*d59+c1z*d60)-z5yp-c1y-(p3y-p1y)-h5)/damp;
J(5,24)=(z5x*d13+z5y*d14+z5zp*d15+(c1x*d58+c1y*d59+c1z*d60)-z5y-c1y-(p3y-p1y)-h5)/damp;
J(6,1)=0;
J(6,2)=0;
J(6,3)=0;
J(6,4)=(k6+(c1xp*d61+c1y*d62+c1z*d63)-z5z-c1z-(p3z-p1z)-h6)/damp;
J(6,5)=(k6+(c1x*d61+c1yp*d62+c1z*d63)-z5z-c1z-(p3z-p1z)-h6)/damp;
J(6,6)=(k6+(c1x*d61+c1y*d62+c1zp*d63)-z5z-c1zp-(p3z-p1z)-h6)/damp;
J(6,7)=0;
J(6,8)=0;
J(6,9)=0;
J(6,10)=0;
J(6,11)=0;
J(6,12)=0;
J(6,13)=(k6+(c1x*e43+c1y*e44+c1z*e45)-z5z-c1z-(p3z-p1z)-h6)/damp;
J(6,14)=0;J(6,15)=0;J(6,16)=0;J(6,17)=0;
J(6,18)=(k6+(c1x*e46+c1y*e47+c1z*e48)-z5z-c1z-(p3z-p1z)-h6)/damp;
J(6,19)=0;J(6,20)=0;J(6,21)=0;
J(6,22)=(z5xp*d16+z5y*d17+z5z*d18+(c1x*d61+c1y*d62+c1z*d63)-z5z-c1z-(p3z-p1z)-h6)/damp;
J(6,23)=(z5x*d16+z5yp*d17+z5z*d18+(c1x*d61+c1y*d62+c1z*d63)-z5z-c1z-(p3z-p1z)-h6)/damp;
J(6,24)=(z5x*d16+z5y*d17+z5zp*d18+(c1x*d61+c1y*d62+c1z*d63)-z5zp-c1z-(p3z-p1z)-h6)/damp;
J(7,1)=0;
J(7,2)=0;
J(7,3)=0;
J(7,4)=(k7+(c1xp*d64+c1y*d65+c1z*d66)-z5x-c1xp-(p4x-p1x)-h7)/damp;

125
J(7,5)=(k7+(c1x*d64+c1yp*d65+c1z*d66)-z5x-c1x-(p4x-p1x)-h7)/damp;
J(7,6)=(k7+(c1x*d64+c1y*d65+c1zp*d66)-z5x-c1x-(p4x-p1x)-h7)/damp;
J(7,7)=0;
J(7,8)=0;
J(7,9)=(k7+(c1x*e49+c1y*e50+c1z*e51)-z5x-c1x-(p4x-p1x)-h7)/damp;
J(7,10)=0;J(7,11)=0;J(7,12)=0;J(7,13)=0;
J(7,14)=(k7+(c1x*e52+c1y*e53+c1z*e54)-z5x-c1x-(p4x-p1x)-h7)/damp;
J(7,15)=0;J(7,16)=0;J(7,17)=0;J(7,18)=0;
J(7,19)=(k7+(c1x*e55+c1y*e56+c1z*e57)-z5x-c1x-(p4x-p1x)-h7)/damp;
J(7,20)=0;J(7,21)=0;
J(7,22)=(z5xp*d19+z5y*d20+z5z*d21+(c1x*d64+c1y*d65+c1z*d66)-z5xp-c1x-(p4x-p1x)-h7)/damp;
J(7,23)=(z5x*d19+z5yp*d20+z5z*d21+(c1x*d64+c1y*d65+c1z*d66)-z5x-c1x-(p4x-p1x)-h7)/damp;
J(7,24)=(z5x*d19+z5y*d20+z5zp*d21+(c1x*d64+c1y*d65+c1z*d66)-z5x-c1x-(p4x-p1x)-h7)/damp;
J(8,1)=0;
J(8,2)=0;
J(8,3)=0;
J(8,4)=(k8+(c1xp*d67+c1y*d68+c1z*d69)-z5y-c1y-(p4y-p1y)-h8)/damp;
J(8,5)=(k8+(c1x*d67+c1yp*d68+c1z*d69)-z5y-c1yp-(p4y-p1y)-h8)/damp;
J(8,6)=(k8+(c1x*d67+c1y*d68+c1zp*d69)-z5y-c1y-(p4y-p1y)-h8)/damp;
J(8,7)=0;
J(8,8)=0;
J(8,9)=(k8+(c1x*e58+c1y*e59+c1z*e60)-z5y-c1y-(p4y-p1y)-h8)/damp;
J(8,10)=0;J(8,11)=0;J(8,12)=0;J(8,13)=0;
J(8,14)=(k8+(c1x*e61+c1y*e62+c1z*e63)-z5y-c1y-(p4y-p1y)-h8)/damp;
J(8,15)=0;J(8,16)=0;J(8,16)=0;J(8,17)=0;J(8,18)=0;
J(8,19)=(k8+(c1x*e64+c1y*e65+c1z*e66)-z5y-c1y-(p4y-p1y)-h8)/damp;
J(8,20)=0;J(8,21)=0;
J(8,22)=(z5xp*d22+z5y*d23+z5z*d24+(c1x*d67+c1y*d68+c1z*d69)-z5y-c1y-(p4y-p1y)-h8)/damp;
J(8,23)=(z5x*d22+z5yp*d23+z5z*d24+(c1x*d67+c1y*d68+c1z*d69)-z5yp-c1y-(p4y-p1y)-h8)/damp;
J(8,24)=(z5x*d22+z5y*d23+z5zp*d24+(c1x*d67+c1y*d68+c1z*d69)-z5y-c1y-(p4y-p1y)-h8)/damp;
J(9,1)=0;
J(9,2)=0;
J(9,3)=0;
J(9,4)=(k9+(c1xp*d70+c1y*d71+c1z*d72)-z5z-c1z-(p4z-p1z)-h9)/damp;
J(9,5)=(k9+(c1x*d70+c1yp*d71+c1z*d72)-z5z-c1z-(p4z-p1z)-h9)/damp;
J(9,6)=(k9+(c1x*d70+c1y*d71+c1zp*d72)-z5z-c1zp-(p4z-p1z)-h9)/damp;
J(9,7)=0;
J(9,8)=0;
J(9,9)=0;
J(9,10)=0;J(9,11)=0;J(9,12)=0;J(9,13)=0;
J(9,14)=(k9+(c1x*e67+c1y*e68+c1z*e69)-z5z-c1z-(p4z-p1z)-h9)/damp;
J(9,15)=0;J(9,16)=0;J(9,17)=0;J(9,18)=0;
J(9,19)=(k9+(c1x*e70+c1y*e71+c1z*e72)-z5z-c1z-(p4z-p1z)-h9)/damp;
J(9,20)=0;J(9,21)=0;
J(9,22)=(z5xp*d25+z5y*d26+z5z*d27+(c1x*d70+c1y*d71+c1z*d72)-z5z-c1z-(p4z-p1z)-h9)/damp;
J(9,23)=(z5x*d25+z5yp*d26+z5z*d27+(c1x*d70+c1y*d71+c1z*d72)-z5z-c1z-(p4z-p1z)-h9)/damp;
J(9,24)=(z5x*d25+z5y*d26+z5zp*d27+(c1x*d70+c1y*d71+c1z*d72)-z5zp-c1z-(p4z-p1z)-h9)/damp;
J(10,1)=0;
J(10,2)=0;
J(10,3)=0;
J(10,4)=(k10+(c1xp*d73+c1y*d74+c1z*d75)-z5x-c1xp-(p5x-p1x)-h10)/damp;
J(10,5)=(k10+(c1x*d73+c1yp*d74+c1z*d75)-z5x-c1x-(p5x-p1x)-h10)/damp;
J(10,6)=(k10+(c1x*d73+c1y*d74+c1zp*d75)-z5x-c1x-(p5x-p1x)-h10)/damp;
J(10,7)=0;
J(10,8)=0;
J(10,9)=0;

126
J(10,10)=(k10+(c1x*e73+c1y*e74+c1z*e75)-z5x-c1x-(p5x-p1x)-h10)/damp;
J(10,11)=0;J(10,12)=0;J(10,13)=0;J(10,14)=0;
J(10,15)=(k10+(c1x*e76+c1y*e77+c1z*e78)-z5x-c1x-(p5x-p1x)-h10)/damp;
J(10,16)=0;J(10,17)=0;J(10,18)=0;J(10,19)=0;
J(10,20)=(k10+(c1x*e79+c1y*e80+c1z*e81)-z5x-c1x-(p5x-p1x)-h10)/damp;
J(10,21)=0;
J(10,22)=(z5xp*d28+z5y*d29+z5z*d30+(c1x*d73+c1y*d74+c1z*d75)-z5xp-c1x-(p5x-p1x)-h10)/damp;
J(10,23)=(z5x*d28+z5yp*d29+z5z*d30+(c1x*d73+c1y*d74+c1z*d75)-z5x-c1x-(p5x-p1x)-h10)/damp;
J(10,24)=(z5x*d28+z5y*d29+z5zp*d30+(c1x*d73+c1y*d74+c1z*d75)-z5x-c1x-(p5x-p1x)-h10)/damp;
J(11,1)=0;
J(11,2)=0;
J(11,3)=0;
J(11,4)=(k11+(c1xp*d76+c1y*d77+c1z*d78)-z5y-c1y-(p5y-p1y)-h11)/damp;
J(11,5)=(k11+(c1x*d76+c1yp*d77+c1z*d78)-z5y-c1yp-(p5y-p1y)-h11)/damp;
J(11,6)=(k11+(c1x*d76+c1y*d77+c1zp*d78)-z5y-c1y-(p5y-p1y)-h11)/damp;
J(11,7)=0;
J(11,8)=0;
J(11,9)=0;
J(11,10)=(k11+(c1x*e82+c1y*e83+c1z*e84)-z5y-c1y-(p5y-p1y)-h11)/damp;
J(11,11)=0;J(11,12)=0;J(11,13)=0;J(11,14)=0;
J(11,15)=(k11+(c1x*e85+c1y*e86+c1z*e87)-z5y-c1y-(p5y-p1y)-h11)/damp;
J(11,16)=0;J(11,17)=0;J(11,18)=0;J(11,19)=0;
J(11,20)=(k11+(c1x*e88+c1y*e89+c1z*e90)-z5y-c1y-(p5y-p1y)-h11)/damp;
J(11,21)=0;
J(11,22)=(z5xp*d31+z5y*d32+z5z*d33+(c1x*d76+c1y*d77+c1z*d78)-z5y-c1y-(p5y-p1y)-h11)/damp;
J(11,23)=(z5x*d31+z5yp*d32+z5z*d33+(c1x*d76+c1y*d77+c1z*d78)-z5yp-c1y-(p5y-p1y)-h11)/damp;
J(11,24)=(z5x*d31+z5y*d32+z5zp*d33+(c1x*d76+c1y*d77+c1z*d78)-z5y-c1y-(p5y-p1y)-h11)/damp;
J(12,1)=0;
J(12,2)=0;
J(12,3)=0;
J(12,4)=(k12+(c1xp*d79+c1y*d80+c1z*d81)-z5z-c1z-(p5z-p1z)-h12)/damp;
J(12,5)=(k12+(c1x*d79+c1yp*d80+c1z*d81)-z5z-c1z-(p5z-p1z)-h12)/damp;
J(12,6)=(k12+(c1x*d79+c1y*d80+c1zp*d81)-z5z-c1zp-(p5z-p1z)-h12)/damp;
J(12,7)=0;
J(12,8)=0;
J(12,9)=0;
J(12,10)=0;
J(12,11)=0;J(12,12)=0;J(12,13)=0;J(12,14)=0;
J(12,15)=(k12+(c1x*e91+c1y*e92+c1z*e93)-z5z-c1z-(p5z-p1z)-h12)/damp;
J(12,16)=0;J(12,17)=0;J(12,18)=0;J(12,19)=0;
J(12,20)=(k12+(c1x*e94+c1y*e95+c1z*e96)-z5z-c1z-(p5z-p1z)-h12)/damp;
J(12,21)=0;
J(12,22)=(z5xp*d34+z5y*d35+z5z*d36+(c1x*d79+c1y*d80+c1z*d81)-z5z-c1z-(p5z-p1z)-h12)/damp;
J(12,23)=(z5x*d34+z5yp*d35+z5z*d36+(c1x*d79+c1y*d80+c1z*d81)-z5z-c1z-(p5z-p1z)-h12)/damp;
J(12,24)=(z5x*d34+z5y*d35+z5zp*d36+(c1x*d79+c1y*d80+c1z*d81)-z5zp-c1z-(p5z-p1z)-h12)/damp;
J(13,1)=0;
J(13,2)=0;
J(13,3)=0;
J(13,4)=(k13+(c1xp*d82+c1y*d83+c1z*d84)-z5x-c1xp-(p6x-p1x)-h13)/damp;
J(13,5)=(k13+(c1x*d82+c1yp*d83+c1z*d84)-z5x-c1x-(p6x-p1x)-h13)/damp;
J(13,6)=(k13+(c1x*d82+c1y*d83+c1zp*d84)-z5x-c1x-(p6x-p1x)-h13)/damp;
J(13,7)=0;
J(13,8)=0;
J(13,9)=0;
J(13,10)=0;
J(13,11)=(k13+(c1x*e97+c1y*e98+c1z*e99)-z5x-c1x-(p6x-p1x)-h13)/damp;

127
J(13,12)=0;J(13,13)=0;J(13,14)=0;J(13,15)=0;
J(13,16)=(k13+(c1x*e100+c1y*e101+c1z*e102)-z5x-c1x-(p6x-p1x)-h13)/damp;
J(13,17)=0;J(13,18)=0;J(13,19)=0;J(13,20)=0;
J(13,21)=(k13+(c1x*e103+c1y*e104+c1z*e105)-z5x-c1x-(p6x-p1x)-h13)/damp;
J(13,22)=(z5xp*d37+z5y*d38+z5z*d39+(c1x*d82+c1y*d83+c1z*d84)-z5xp-c1x-(p6x-p1x)-h13)/damp;
J(13,23)=(z5x*d37+z5yp*d38+z5z*d39+(c1x*d82+c1y*d83+c1z*d84)-z5x-c1x-(p6x-p1x)-h13)/damp;
J(13,24)=(z5x*d37+z5y*d38+z5zp*d39+(c1x*d82+c1y*d83+c1z*d84)-z5x-c1x-(p6x-p1x)-h13)/damp;
J(14,1)=0;
J(14,2)=0;
J(14,3)=0;
J(14,4)=(k14+(c1xp*d85+c1y*d86+c1z*d87)-z5y-c1y-(p6y-p1y)-h14)/damp;
J(14,5)=(k14+(c1x*d85+c1yp*d86+c1z*d87)-z5y-c1yp-(p6y-p1y)-h14)/damp;
J(14,6)=(k14+(c1x*d85+c1y*d86+c1zp*d87)-z5y-c1y-(p6y-p1y)-h14)/damp;
J(14,7)=0;
J(14,8)=0;
J(14,9)=0;
J(14,10)=0;
J(14,11)=(k14+(c1x*e106+c1y*e107+c1z*e108)-z5y-c1y-(p6y-p1y)-h14)/damp;
J(14,12)=0;J(14,13)=0;J(14,14)=0;J(14,15)=0;
J(14,16)=(k14+(c1x*e109+c1y*e110+c1z*e111)-z5y-c1y-(p6y-p1y)-h14)/damp;
J(14,17)=0;J(14,18)=0;J(14,19)=0;J(14,20)=0;
J(14,21)=(k14+(c1x*e112+c1y*e113+c1z*e114)-z5y-c1y-(p6y-p1y)-h14)/damp;
J(14,22)=(z5xp*d40+z5y*d41+z5z*d42+(c1x*d85+c1y*d86+c1z*d87)-z5y-c1y-(p6y-p1y)-h14)/damp;
J(14,23)=(z5x*d40+z5yp*d41+z5z*d42+(c1x*d85+c1y*d86+c1z*d87)-z5yp-c1y-(p6y-p1y)-h14)/damp;
J(14,24)=(z5x*d40+z5y*d41+z5zp*d42+(c1x*d85+c1y*d86+c1z*d87)-z5y-c1y-(p6y-p1y)-h14)/damp;
J(15,1)=0;
J(15,2)=0;
J(15,3)=0;
J(15,4)=(k15+(c1xp*d88+c1y*d89+c1z*d90)-z5z-c1z-(p6z-p1z)-h15)/damp;
J(15,5)=(k15+(c1x*d88+c1yp*d89+c1z*d90)-z5z-c1z-(p6z-p1z)-h15)/damp;
J(15,6)=(k15+(c1x*d88+c1y*d89+c1zp*d90)-z5z-c1zp-(p6z-p1z)-h15)/damp;
J(15,7)=0;
J(15,8)=0;
J(15,9)=0;
J(15,10)=0;
J(15,11)=0;
J(15,12)=0;J(15,13)=0;J(15,14)=0;J(15,15)=0;
J(15,16)=(k15+(c1x*e115+c1y*e116+c1z*e117)-z5z-c1z-(p6z-p1z)-h15)/damp;
J(15,17)=0;J(15,18)=0;J(15,19)=0;J(15,20)=0;
J(15,21)=(k15+(c1x*e118+c1y*e119+c1z*e120)-z5z-c1z-(p6z-p1z)-h15)/damp;
J(15,22)=(z5xp*d43+z5y*d44+z5z*d45+(c1x*d88+c1y*d89+c1z*d90)-z5z-c1z-(p6z-p1z)-h15)/damp;
J(15,23)=(z5x*d43+z5yp*d44+z5z*d45+(c1x*d88+c1y*d89+c1z*d90)-z5z-c1z-(p6z-p1z)-h15)/damp;
J(15,24)=(z5x*d43+z5y*d44+z5zp*d45+(c1x*d88+c1y*d89+c1z*d90)-z5zp-c1z-(p6z-p1z)-h15)/damp;
J(16,1)=(v1+k31-k1-k6-c0xp-h16)/damp;
J(16,2)=0;
J(16,3)=0;
J(16,4)=(v1+k31-k1-(c1xp*d46+c1y*d47+c1z*d48)-c0x-h16)/damp;
J(16,5)=(v1+k31-k1-(c1x*d46+c1yp*d47+c1z*d48)-c0x-h16)/damp;
J(16,6)=(v1+k31-k1-(c1x*d46+c1y*d47+c1zp*d48)-c0x-h16)/damp;
J(16,7)=(v1+k31-k1-(c1x*e1+c1y*e2+c1z*e3)-c0x-h16)/damp;
J(16,8)=0;J(16,9)=0;J(16,10)=0;J(16,11)=0;
J(16,12)=(v1+k31-k1-(c1x*e4+c1y*e5+c1z*e6)-c0x-h16)/damp;
J(16,13)=0;J(16,14)=0;J(16,15)=0;J(16,16)=0;
J(16,17)=(v1+k31-k1-(c1x*e7+c1y*e8+c1z*e9)-c0x-h16)/damp;
J(16,18)=0;J(16,19)=0;J(16,20)=0;J(16,21)=0;
J(16,22)=(v1+k31-(z5xp*d1+z5y*d2+z5z*d3)-(c1x*d46+c1y*d47+c1z*d48)-c0x-h16)/damp;

128
J(16,23)=(v1+k31-(z5x*d1+z5yp*d2+z5z*d3)-(c1x*d46+c1y*d47+c1z*d48)-c0x-h16)/damp;
J(16,24)=(v1+k31-(z5x*d1+z5y*d2+z5zp*d3)-(c1x*d46+c1y*d47+c1z*d48)-c0x-h16)/damp;
J(17,1)=0;
J(17,2)=(v2+k32-k2-k17-c0yp-h17)/damp;
J(17,3)=0;
J(17,4)=(v2+k32-k2-(c1xp*d49+c1y*d50+c1z*d51)-c0y-h17)/damp;
J(17,5)=(v2+k32-k2-(c1x*d49+c1yp*d50+c1z*d51)-c0y-h17)/damp;
J(17,6)=(v2+k32-k2-(c1x*d49+c1y*d50+c1zp*d51)-c0y-h17)/damp;
J(17,7)=(v2+k32-k2-(c1x*e10+c1y*e11+c1z*e12)-c0y-h17)/damp;
J(17,8)=0;J(17,9)=0;J(17,10)=0;J(17,11)=0;
J(17,12)=(v2+k32-k2-(c1x*e13+c1y*e14+c1z*e15)-c0y-h17)/damp;
J(17,13)=0;J(2,14)=0;J(2,15)=0;J(2,16)=0;
J(17,17)=(v2+k32-k2-(c1x*e16+c1y*e17+c1z*e18)-c0y-h17)/damp;
J(17,18)=0;J(17,19)=0;J(17,20)=0;J(17,21)=0;
J(17,22)=(v2+k32-(z5xp*d4+z5y*d5+z5z*d6)-(c1x*d49+c1y*d50+c1z*d51)-c0y-h17)/damp;
J(17,23)=(v2+k32-(z5x*d4+z5yp*d5+z5z*d6)-(c1x*d49+c1y*d50+c1z*d51)-c0y-h17)/damp;
J(17,24)=(v2+k32-(z5x*d4+z5y*d5+z5zp*d6)-(c1x*d49+c1y*d50+c1z*d51)-c0y-h17)/damp;
J(18,1)=0;
J(18,2)=0;
J(18,3)=(v3+k33-k3-(c1x*d52+c1y*d53+c1z*d54)-c0zp-h18)/damp;
J(18,4)=(v3+k33-k3-(c1xp*d52+c1y*d53+c1z*d54)-c0z-h18)/damp;
J(18,5)=(v3+k33-k3-(c1x*d52+c1yp*d53+c1z*d54)-c0z-h18)/damp;
J(18,6)=(v3+k33-k3-(c1x*d52+c1y*d53+c1zp*d54)-c0z-h18)/damp;
J(18,7)=0;
J(18,8)=0;J(18,9)=0;J(18,10)=0;J(18,11)=0;
J(18,12)=(v3+k33-k3-(c1x*e19+c1y*e20+c1z*e21)-c0z-h18)/damp;
J(18,13)=0;J(18,14)=0;J(18,15)=0;J(18,16)=0;
J(18,17)=(v3+k33-k3-(c1x*e22+c1y*e23+c1z*e24)-c0z-h18)/damp;
J(18,18)=0;J(18,19)=0;J(18,20)=0;J(18,21)=0;
J(18,22)=(v3+k33-(z5xp*d7+z5y*d8+z5z*d9)-(c1x*d52+c1y*d53+c1z*d54)-c0z-h18)/damp;
J(18,23)=(v3+k33-(z5x*d7+z5yp*d8+z5z*d9)-(c1x*d52+c1y*d53+c1z*d54)-c0z-h18)/damp;
J(18,24)=(v3+k33-(z5x*d7+z5y*d8+z5zp*d9)-(c1x*d52+c1y*d53+c1z*d54)-c0z-h18)/damp;
J(19,1)=(v4+k34-k4-k19-c0xp-h19)/damp;
J(19,2)=0;
J(19,3)=0;
J(19,4)=(v4+k34-k4-(c1xp*d55+c1y*d56+c1z*d57)-c0x-h19)/damp;
J(19,5)=(v4+k34-k4-(c1x*d55+c1yp*d56+c1z*d57)-c0x-h19)/damp;
J(19,6)=(v4+k34-k4-(c1x*d55+c1y*d56+c1zp*d57)-c0x-h19)/damp;
J(19,7)=0;
J(19,8)=(v4+k34-k4-(c1x*e25+c1y*e26+c1z*e27)-c0x-h19)/damp;
J(19,9)=0;J(19,10)=0;J(19,11)=0;
J(19,12)=0;
J(19,13)=(v4+k34-k4-(c1x*e28+c1y*e29+c1z*e30)-c0x-h19)/damp;
J(19,14)=0;J(19,15)=0;J(19,16)=0;
J(19,17)=0;
J(19,18)=(v4+k34-k4-(c1x*e31+c1y*e32+c1z*e33)-c0x-h19)/damp;
J(19,19)=0;J(19,20)=0;J(19,21)=0;
J(19,22)=(v4+k34-(z5xp*d10+z5y*d11+z5z*d12)-(c1x*d55+c1y*d56+c1z*d57)-c0x-h19)/damp;
J(19,23)=(v4+k34-(z5x*d10+z5yp*d11+z5z*d12)-(c1x*d55+c1y*d56+c1z*d57)-c0x-h19)/damp;
J(19,24)=(v4+k34-(z5x*d10+z5y*d11+z5zp*d12)-(c1x*d55+c1y*d56+c1z*d57)-c0x-h19)/damp;
J(20,1)=0;
J(20,2)=(v5+k35-k5-k20-c0yp-h20)/damp;
J(20,3)=0;
J(20,4)=(v5+k35-k5-(c1xp*d58+c1y*d59+c1z*d60)-c0y-h20)/damp;
J(20,5)=(v5+k35-k5-(c1x*d58+c1yp*d59+c1z*d60)-c0y-h20)/damp;
J(20,6)=(v5+k35-k5-(c1x*d58+c1y*d59+c1zp*d60)-c0y-h20)/damp;

129
J(20,7)=0;
J(20,8)=(v5+k35-k5-(c1x*e34+c1y*e35+c1z*e36)-c0y-h20)/damp;
J(20,9)=0;J(20,10)=0;J(20,11)=0;J(20,12)=0;
J(20,13)=(v5+k35-k5-(c1x*e37+c1y*e38+c1z*e39)-c0y-h20)/damp;
J(20,14)=0;J(20,15)=0;J(20,16)=0;J(20,17)=0;
J(20,18)=(v5+k35-k5-(c1x*e40+c1y*e41+c1z*e42)-c0y-h20)/damp;
J(20,19)=0;J(20,20)=0;J(20,21)=0;
J(20,22)=(v5+k35-(z5xp*d13+z5y*d14+z5z*d15)-(c1x*d58+c1y*d59+c1z*d60)-c0y-h20)/damp;
J(20,23)=(v5+k35-(z5x*d13+z5yp*d14+z5z*d15)-(c1x*d58+c1y*d59+c1z*d60)-c0y-h20)/damp;
J(20,24)=(v5+k35-(z5x*d13+z5y*d14+z5zp*d15)-(c1x*d58+c1y*d59+c1z*d60)-c0y-h20)/damp;
J(21,1)=0;
J(21,2)=0;
J(21,3)=(v6+k36-k6-(c1x*d61+c1y*d62+c1z*d63)-c0zp-h21)/damp;
J(21,4)=(v6+k36-k6-(c1xp*d61+c1y*d62+c1z*d63)-c0z-h21)/damp;
J(21,5)=(v6+k36-k6-(c1x*d61+c1yp*d62+c1z*d63)-c0z-h21)/damp;
J(21,6)=(v6+k36-k6-(c1x*d61+c1y*d62+c1zp*d63)-c0z-h21)/damp;
J(21,7)=0;
J(21,8)=0;
J(21,9)=0;
J(21,10)=0;
J(21,11)=0;
J(21,12)=0;
J(21,13)=(v6+k36-k6-(c1x*e43+c1y*e44+c1z*e45)-c0z-h21)/damp;
J(21,14)=0;J(21,15)=0;J(21,16)=0;J(21,17)=0;
J(21,18)=(v6+k36-k6-(c1x*e46+c1y*e47+c1z*e48)-c0z-h21)/damp;
J(21,19)=0;J(21,20)=0;J(21,21)=0;
J(21,22)=(v6+k36-(z5xp*d16+z5y*d17+z5z*d18)-(c1x*d61+c1y*d62+c1z*d63)-c0z-h21)/damp;
J(21,23)=(v6+k36-(z5x*d16+z5yp*d17+z5z*d18)-(c1x*d61+c1y*d62+c1z*d63)-c0z-h21)/damp;
J(21,24)=(v6+k36-(z5x*d16+z5y*d17+z5zp*d18)-(c1x*d61+c1y*d62+c1z*d63)-c0z-h21)/damp;
J(22,1)=(v7+k37-k7-(c1x*d64+c1y*d65+c1z*d66)-c0xp-h22)/damp;
J(22,2)=0;
J(22,3)=0;
J(22,4)=(v7+k37-k7-(c1xp*d64+c1y*d65+c1z*d66)-c0x-h22)/damp;
J(22,5)=(v7+k37-k7-(c1x*d64+c1yp*d65+c1z*d66)-c0x-h22)/damp;
J(22,6)=(v7+k37-k7-(c1x*d64+c1y*d65+c1zp*d66)-c0x-h22)/damp;
J(22,7)=0;
J(22,8)=0;
J(22,9)=(v7+k37-k7-(c1x*e49+c1y*e50+c1z*e51)-c0x-h22)/damp;
J(22,10)=0;J(22,11)=0;J(22,12)=0;J(22,13)=0;
J(22,14)=(v7+k37-k7-(c1x*e52+c1y*e53+c1z*e54)-c0x-h22)/damp;
J(22,15)=0;J(22,16)=0;J(22,17)=0;J(22,18)=0;
J(22,19)=(v7+k37-k7-(c1x*e55+c1y*e56+c1z*e57)-c0x-h22)/damp;
J(22,20)=0;J(22,21)=0;
J(22,22)=(v7+k37-(z5xp*d19+z5y*d20+z5z*d21)-(c1x*d64+c1y*d65+c1z*d66)-c0x-h22)/damp;
J(22,23)=(v7+k37-(z5x*d19+z5yp*d20+z5z*d21)-(c1x*d64+c1y*d65+c1z*d66)-c0x-h22)/damp;
J(22,24)=(v7+k37-(z5x*d19+z5y*d20+z5zp*d21)-(c1x*d64+c1y*d65+c1z*d66)-c0x-h22)/damp;
J(23,1)=0;
J(23,2)=(v8+k38-k8-(c1x*d67+c1y*d68+c1z*d69)-c0yp-h23)/damp;
J(23,3)=0;
J(23,4)=(v8+k38-k8-(c1xp*d67+c1y*d68+c1z*d69)-c0y-h23)/damp;
J(23,5)=(v8+k38-k8-(c1x*d67+c1yp*d68+c1z*d69)-c0y-h23)/damp;
J(23,6)=(v8+k38-k8-(c1x*d67+c1y*d68+c1zp*d69)-c0y-h23)/damp;
J(23,7)=0;
J(23,8)=0;
J(23,9)=(v8+k38-k8-(c1x*e58+c1y*e59+c1z*e60)-c0y-h23)/damp;
J(23,10)=0;J(23,11)=0;J(23,12)=0;J(23,13)=0;

130
J(23,14)=(v8+k38-k8-(c1x*e61+c1y*e62+c1z*e63)-c0y-h23)/damp;
J(23,15)=0;J(23,16)=0;J(23,16)=0;J(23,17)=0;J(23,18)=0;
J(23,19)=(v8+k38-k8-(c1x*e64+c1y*e65+c1z*e66)-c0y-h23)/damp;
J(23,20)=0;J(23,21)=0;
J(23,22)=(v8+k38-(z5xp*d22+z5y*d23+z5z*d24)-(c1x*d67+c1y*d68+c1z*d69)-c0y-h23)/damp;
J(23,23)=(v8+k38-(z5x*d22+z5yp*d23+z5z*d24)-(c1x*d67+c1y*d68+c1z*d69)-c0y-h23)/damp;
J(23,24)=(v8+k38-(z5x*d22+z5y*d23+z5zp*d24)-(c1x*d67+c1y*d68+c1z*d69)-c0y-h23)/damp;
J(24,1)=0;
J(24,2)=0;
J(24,3)=(v9+k39-k9-(c1x*d70+c1y*d71+c1z*d72)-c0zp-h24)/damp;
J(24,4)=(v9+k39-k9-(c1xp*d70+c1y*d71+c1z*d72)-c0z-h24)/damp;
J(24,5)=(v9+k39-k9-(c1x*d70+c1yp*d71+c1z*d72)-c0z-h24)/damp;
J(24,6)=(v9+k39-k9-(c1x*d70+c1y*d71+c1zp*d72)-c0z-h24)/damp;
J(24,7)=0;
J(24,8)=0;
J(24,9)=0;
J(24,10)=0;J(9,11)=0;J(9,12)=0;J(9,13)=0;
J(24,14)=(v9+k39-k9-(c1x*e67+c1y*e68+c1z*e69)-c0z-h24)/damp;
J(24,15)=0;J(24,16)=0;J(24,17)=0;J(24,18)=0;
J(24,19)=(v9+k39-k9-(c1x*e70+c1y*e71+c1z*e72)-c0z-h24)/damp;
J(24,20)=0;J(24,21)=0;
J(24,22)=(v9+k39-(z5xp*d25+z5y*d26+z5z*d27)-(c1x*d70+c1y*d71+c1z*d72)-c0z-h24)/damp;
J(24,23)=(v9+k39-(z5x*d25+z5yp*d26+z5z*d27)-(c1x*d70+c1y*d71+c1z*d72)-c0z-h24)/damp;
J(24,24)=(v9+k39-(z5x*d25+z5y*d26+z5zp*d27)-(c1x*d70+c1y*d71+c1z*d72)-c0z-h24)/damp;
J(25,1)=(v10+k40-k10-(c1x*d73+c1y*d74+c1z*d75)-c0xp-h25)/damp;
J(25,2)=0;
J(25,3)=0;
J(25,4)=(v10+k40-k10-(c1xp*d73+c1y*d74+c1z*d75)-c0x-h25)/damp;
J(25,5)=(v10+k40-k10-(c1x*d73+c1yp*d74+c1z*d75)-c0x-h25)/damp;
J(25,6)=(v10+k40-k10-(c1x*d73+c1y*d74+c1zp*d75)-c0x-h25)/damp;
J(25,7)=0;
J(25,8)=0;
J(25,9)=0;
J(25,10)=(v10+k40-k10-(c1x*e73+c1y*e74+c1z*e75)-c0x-h25)/damp;
J(25,11)=0;J(25,12)=0;J(25,13)=0;J(25,14)=0;
J(25,15)=(v10+k40-k10-(c1x*e76+c1y*e77+c1z*e78)-c0x-h25)/damp;
J(25,16)=0;J(25,17)=0;J(25,18)=0;J(25,19)=0;
J(25,20)=(v10+k40-k10-(c1x*e79+c1y*e80+c1z*e81)-c0x-h25)/damp;
J(25,21)=0;
J(25,22)=(v10+k40-(z5xp*d28+z5y*d29+z5z*d30)-(c1x*d73+c1y*d74+c1z*d75)-c0x-h25)/damp;
J(25,23)=(v10+k40-(z5x*d28+z5yp*d29+z5z*d30)-(c1x*d73+c1y*d74+c1z*d75)-c0x-h25)/damp;
J(25,24)=(v10+k40-(z5x*d28+z5y*d29+z5zp*d30)-(c1x*d73+c1y*d74+c1z*d75)-c0x-h25)/damp;
J(26,1)=0;
J(26,2)=(v11+k41-k11-(c1x*d76+c1y*d77+c1z*d78)-c0yp-h26)/damp;
J(26,3)=0;
J(26,4)=(v11+k41-k11-(c1xp*d76+c1y*d77+c1z*d78)-c0y-h26)/damp;
J(26,5)=(v11+k41-k11-(c1x*d76+c1yp*d77+c1z*d78)-c0y-h26)/damp;
J(26,6)=(v11+k41-k11-(c1x*d76+c1y*d77+c1zp*d78)-c0y-h26)/damp;
J(26,7)=0;
J(26,8)=0;
J(26,9)=0;
J(26,10)=(v11+k41-k11-(c1x*e82+c1y*e83+c1z*e84)-c0y-h26)/damp;
J(26,11)=0;J(26,12)=0;J(26,13)=0;J(26,14)=0;
J(26,15)=(v11+k41-k11-(c1x*e85+c1y*e86+c1z*e87)-c0y-h26)/damp;
J(26,16)=0;J(26,17)=0;J(26,18)=0;J(26,19)=0;
J(26,20)=(v11+k41-k11-(c1x*e88+c1y*e89+c1z*e90)-c0y-h26)/damp;

131
J(26,21)=0;
J(26,22)=(v11+k41-(z5xp*d31+z5y*d32+z5z*d33)-(c1x*d76+c1y*d77+c1z*d78)-c0y-h26)/damp;
J(26,23)=(v11+k41-(z5x*d31+z5yp*d32+z5z*d33)-(c1x*d76+c1y*d77+c1z*d78)-c0y-h26)/damp;
J(26,24)=(v11+k41-(z5x*d31+z5y*d32+z5zp*d33)-(c1x*d76+c1y*d77+c1z*d78)-c0y-h26)/damp;
J(27,1)=0;
J(27,2)=0;
J(27,3)=(v12+k42-k12-(c1x*d79+c1y*d80+c1z*d81)-c0zp-h27)/damp;
J(27,4)=(v12+k42-k12-(c1xp*d79+c1y*d80+c1z*d81)-c0z-h27)/damp;
J(27,5)=(v12+k42-k12-(c1x*d79+c1yp*d80+c1z*d81)-c0z-h27)/damp;
J(27,6)=(v12+k42-k12-(c1x*d79+c1y*d80+c1zp*d81)-c0z-h27)/damp;
J(27,7)=0;
J(27,8)=0;
J(27,9)=0;
J(27,10)=0;
J(27,11)=0;J(27,12)=0;J(27,13)=0;J(27,14)=0;
J(27,15)=(v12+k42-k12-(c1x*e91+c1y*e92+c1z*e93)-c0z-h27)/damp;
J(27,16)=0;J(27,17)=0;J(27,18)=0;J(27,19)=0;
J(27,20)=(v12+k42-k12-(c1x*e94+c1y*e95+c1z*e96)-c0z-h27)/damp;
J(27,21)=0;
J(27,22)=(v12+k42-(z5xp*d34+z5y*d35+z5z*d36)-(c1x*d79+c1y*d80+c1z*d81)-c0z-h27)/damp;
J(27,23)=(v12+k42-(z5x*d34+z5yp*d35+z5z*d36)-(c1x*d79+c1y*d80+c1z*d81)-c0z-h27)/damp;
J(27,24)=(v12+k42-(z5x*d34+z5y*d35+z5zp*d36)-(c1x*d79+c1y*d80+c1z*d81)-c0z-h27)/damp;
J(28,1)=(v13+k43-k13-(c1x*d82+c1y*d83+c1z*d84)-c0xp-h28)/damp;
J(28,2)=0;
J(28,3)=0;
J(28,4)=(v13+k43-k13-(c1xp*d82+c1y*d83+c1z*d84)-c0x-h28)/damp;
J(28,5)=(v13+k43-k13-(c1x*d82+c1yp*d83+c1z*d84)-c0x-h28)/damp;
J(28,6)=(v13+k43-k13-(c1x*d82+c1y*d83+c1zp*d84)-c0x-h28)/damp;
J(28,7)=0;
J(28,8)=0;
J(28,9)=0;
J(28,10)=0;
J(28,11)=(v13+k43-k13-(c1x*e97+c1y*e98+c1z*e99)-c0x-h28)/damp;
J(28,12)=0;J(28,13)=0;J(28,14)=0;J(28,15)=0;
J(28,16)=(v13+k43-k13-(c1x*e100+c1y*e101+c1z*e102)-c0x-h28)/damp;
J(28,17)=0;J(13,18)=0;J(13,19)=0;J(13,20)=0;
J(28,21)=(v13+k43-k13-(c1x*e103+c1y*e104+c1z*e105)-c0x-h28)/damp;
J(28,22)=(v13+k43-(z5xp*d37+z5y*d38+z5z*d39)-(c1x*d82+c1y*d83+c1z*d84)-c0x-h28)/damp;
J(28,23)=(v13+k43-(z5x*d37+z5yp*d38+z5z*d39)-(c1x*d82+c1y*d83+c1z*d84)-c0x-h28)/damp;
J(28,24)=(v13+k43-(z5x*d37+z5y*d38+z5zp*d39)-(c1x*d82+c1y*d83+c1z*d84)-c0x-h28)/damp;
J(29,1)=0;
J(29,2)=(v14+k44-k14-(c1x*d85+c1y*d86+c1z*d87)-c0yp-h29)/damp;
J(29,3)=0;
J(29,4)=(v14+k44-k14-(c1xp*d85+c1y*d86+c1z*d87)-c0y-h29)/damp;
J(29,5)=(v14+k44-k14-(c1x*d85+c1yp*d86+c1z*d87)-c0y-h29)/damp;
J(29,6)=(v14+k44-k14-(c1x*d85+c1y*d86+c1zp*d87)-c0y-h29)/damp;
J(29,7)=0;
J(29,8)=0;
J(29,9)=0;
J(29,10)=0;
J(29,11)=(v14+k44-k14-(c1x*e106+c1y*e107+c1z*e108)-c0y-h29)/damp;
J(29,12)=0;J(29,13)=0;J(29,14)=0;J(29,15)=0;
J(29,16)=(v14+k44-k14-(c1x*e109+c1y*e110+c1z*e111)-c0y-h29)/damp;
J(29,17)=0;J(29,18)=0;J(29,19)=0;J(29,20)=0;
J(29,21)=(v14+k44-k14-(c1x*e112+c1y*e113+c1z*e114)-c0y-h29)/damp;
J(29,22)=(v14+k44-(z5xp*d40+z5y*d41+z5z*d42)-(c1x*d85+c1y*d86+c1z*d87)-c0y-h29)/damp;

132
J(29,23)=(v14+k44-(z5x*d40+z5yp*d41+z5z*d42)-(c1x*d85+c1y*d86+c1z*d87)-c0y-h29)/damp;
J(29,24)=(v14+k44-(z5x*d40+z5y*d41+z5zp*d42)-(c1x*d85+c1y*d86+c1z*d87)-c0y-h29)/damp;
J(30,1)=0;
J(30,2)=0;
J(30,3)=(v15+k45-k15-(c1x*d88+c1y*d89+c1z*d90)-c0zp-h30)/damp;
J(30,4)=(v15+k45-k15-(c1xp*d88+c1y*d89+c1z*d90)-c0z-h30)/damp;
J(30,5)=(v15+k45-k15-(c1x*d88+c1yp*d89+c1z*d90)-c0z-h30)/damp;
J(30,6)=(v15+k45-k15-(c1x*d88+c1y*d89+c1zp*d90)-c0z-h30)/damp;
J(30,7)=0;
J(30,8)=0;
J(30,9)=0;
J(30,10)=0;
J(30,11)=0;
J(30,12)=0;J(30,13)=0;J(30,14)=0;J(30,15)=0;
J(30,16)=(v15+k45-k15-(c1x*e115+c1y*e116+c1z*e117)-c0z-h30)/damp;
J(30,17)=0;J(30,18)=0;J(30,19)=0;J(30,20)=0;
J(30,21)=(v15+k45-k15-(c1x*e118+c1y*e119+c1z*e120)-c0z-h30)/damp;
J(30,22)=(v15+k45-(z5xp*d43+z5y*d44+z5z*d45)-(c1x*d88+c1y*d89+c1z*d90)-c0z-h30)/damp;
J(30,23)=(v15+k45-(z5x*d43+z5yp*d44+z5z*d45)-(c1x*d88+c1y*d89+c1z*d90)-c0z-h30)/damp;
J(30,24)=(v15+k45-(z5x*d43+z5y*d44+z5zp*d45)-(c1x*d88+c1y*d89+c1z*d90)-c0z-h30)/damp;
h(1)=h1;
h(2)=h2;
h(3)=h3;
h(4)=h4;
h(5)=h5;
h(6)=h6;
h(7)=h7;
h(8)=h8;
h(9)=h9;
h(10)=h10;
h(11)=h11;
h(12)=h12;
h(13)=h13;
h(14)=h14;
h(15)=h15;
h(16)=h16;
h(17)=h17;
h(18)=h18;
h(19)=h19;
h(20)=h20;
h(21)=h21;
h(22)=h22;
h(23)=h23;
h(24)=h24;
h(25)=h25;
h(26)=h26;
h(27)=h27;
h(28)=h28;
h(29)=h29;
h(30)=h30;
%*******************************
%ACTUALIZACION DE VARIABLES
%*******************************
ds=-J\h';
c0x=c0x+ds(1);
c0y=c0y+ds(2);

133
 c0z=c0z+ds(3);
c1x=c1x+ds(4);
c1y=c1y+ds(5);
c1z=c1z+ds(6);
i2=i2+ds(7);
i3=i3+ds(8);
i4=i4+ds(9);
i5=i5+ds(10);
i6=i6+ds(11);
l2=l2+ds(12);
l3=l3+ds(13);
l4=l4+ds(14);
l5=l5+ds(15);
l6=l6+ds(16);
n2=n2+ds(17);
n3=n3+ds(18);
n4=n4+ds(19);
n5=n5+ds(20);
n6=n6+ds(21);
z5x=z5x+ds(22);
z5y=z5y+ds(23);
z5z=z5z+ds(24);
%***************************
%IMPRESION DE RESULTADOS
%***************************
fprintf('n=%2.0f, c0x=%12.5e, c0y=%12.5e, c0z=%12.5e, c1x=%12.5e, c1y=%12.5e, c1z=%12.5e,
i2=%12.5e, i3=%12.5e, i4=%12.5e, i5=%12.5e,i6=%12.5e, l2=%12.5e, l3=%12.5e, l4=%12.5e,
l5=%12.5e,l6=%12.5e,n2=%12.5e, n3=%12.5e, n4=%12.5e,
n5=%12.5e,n6=%12.5e,z5x=%12.5e,z5y=%12.5e,z5z=%12.5e,',
n,c0x,c0y,c0z,c1x,c1y,c1z,i2,i3,i4,i5,i6,l2,l3,l4,l5,l6,n2,n3,n4,n5,n6,z5x,z5y,z5z)
end
end

PROGRAMA PARA EL CALCULO DE LAS VELOCIDADES ANGULARES DEL ESLABÓN

ACOPLADOR Y DEL ESLABÓN W2.
Clear all
clc
%*********************
%VALORES PRESCRITOS
%*********************
w1u = input ('componente en el eje u del vector w1 ');
w1v = input ('componente en el eje v del vector w1 ');
z1x = input ('componente en el eje x del vector z1 ');
z1y = input ('componente en el eje y del vector z1 ');
z1z = input ('componente en el eje z del vector z1 ');
w2u = input ('componente en el eje u del vector w2 ');
w2v = input ('componente en el eje v del vector w2 ');
z2x = input ('componente en el eje x del vector z2 ');
z2y = input ('componente en el eje y del vector z2 ');
z2z = input ('componente en el eje z del vector z2 ');
z5 = input ('ángulo de rotación z5 sobre el eje y');
z6 = input ('ángulo de rotación z6 sobre el eje x rotado');
m = input ('ángulo de rotación m sobre el eje y');
s = input ('ángulo de rotación s sobre el eje x rotado');
wb = input ('velocidad angular del eslabón de entrada');
b = input ('ángulo de rotación b ');

134
p = input ('ángulo de rotación p ');
f = input ('ángulo de rotación f ');
t = input ('ángulo de rotación t ');
a = input ('ángulo de rotación a ');
cb = cos(b);
sb = sin(b);
cp = cos(p);
sf = sin(f);
sp = sin(p);
ct = cos(t);
cf = cos(f);
st = sin(t);
sm = sin(m);
cm = cos(m);
sa = sin(a);
ca = cos(a);
ss = sin(s);
cs = cos(s);
cz5 = cos(z5);
sz5 = sin(z5);
cz6 = sin(z6);
sz6 = sin(z6);
A(1,1)=((z1x)-(z2x))*(-cp*sf-sp*ct*cf)+((z1y)-(z2y))*(sp*sf-cp*ct*cf)+((z1z)-(z2z))*st*cf;
A(1,2)=((z1x)-(z2x))*(-cf*sp-sf*ct*cp)+((z1y)-(z2y))*(-cf*cp+sf*ct*sp);
A(1,3)=((z1x)-(z2x))*(sp*sf*st)+((z1y)-(z2y))*(cp*sf*st)+((z1z)-(z2z))*(sf*ct);
A(1,4)=-(w2u*(-sa*cm+sm*ss*ca)-w2v*(cm*ca+sm*ss*sa));
A(2,1)=((z1x)-(z2x))*(cp*cf-sp*ct*sf)-((z1y)-(z2y))*(sp*cf+cp*ct*sf)+((z1z)-(z2z))*st*sf ;
A(2,2)=((z1x)-(z2x))*(-sf*sp+cf*ct*cp)-((z1y)-(z2y))*(sf*cp+cf*ct*sp) ;
A(2,3)=((z1x)-(z2x))*(-sp*cf*st)-((z1y)-(z2y))*(cp*cf*st)-((z1z)-(z2z))*cf*ct ;
A(2,4)=-(w2u*(cs*ca)-w2v*(cs*sa));
A(3,1)=0 ;
A(3,2)=((z1x)-(z2x))*(st*cp)-((z1y)-(z2y))*(st*sp) ;
A(3,3)=((z1x)-(z2x))*(sp*ct)+((z1y)-(z2y))*(cp*ct)-((z1z)-(z2z))*st ;
A(3,4)=-(w2u*(-sm*sa+cm*ss*ca)+w2v*(sm*ca-cm*ss*sa));
d=[-wb*(w1u*(-sb*cz5+sz5*sz6*cb)-w1v*(cz5*cb+sz5*sz6*sb));-wb*(w2u*(cz6*cb)-w2v*(cz6*sb));-
wb*(w1u*(-sz5*sb+cz5*sz6*cb)+w1v*(sz5*cb-cz5*sz6*sb))];
w= pinv (A)*d

PROGRAMA PARA EL CALCULO DE LAS VELOCIDADES ANGULARES DEL ESLABÓN C1

Clear all
clc

%*********************
%VALORES PRESCRITOS
%*********************
w1u = input ('componente en el eje u del vector w1 ');
w1v = input ('componente en el eje v del vector w1 ');
z1x = input ('componente en el eje x del vector z1 ');
z1y = input ('componente en el eje y del vector z1 ');
z1z = input ('componente en el eje z del vector z1 ');
z5 = input ('ángulo de rotación z5 sobre el eje y');
z6 = input ('ángulo de rotación z6 sobre el eje x rotado');
wb = input ('velocidad angular del eslabón de entrada');
b = input ('ángulo de rotación b ');
p = input ('ángulo de rotación p ');
f = input ('ángulo de rotación f ');

135
t = input ('ángulo de rotación t ');
a = input ('ángulo de rotación a ');
l = input ('ángulo de rotación l ');
i = input ('ángulo de rotación i ');
n = input ('ángulo de rotación n ');
c1x = input ('componente en el eje x del vector c1 ');
c1y = input ('componente en el eje y del vector c1 ');
c1z = input ('componente en el eje z del vector c1 ');
z5x = input ('componente en el eje x del vector z5 ');
z5y = input ('componente en el eje y del vector z5 ');
z5z = input ('componente en el eje z del vector z5 ');
wf = input ('velocidad angular wf de la barra acopladora respecto al eje z');
wt = input ('velocidad angular wt de la barra acopladora respecto al eje rotado u');
wp = input ('velocidad angular wp de la barra acopladora respecto al eje rotado w');

cl=cos(l);
si=sin(i);
sl=sin(l);
cn=cos(n);
ci=cos(i);
sn=sin(n);
cp=cos(p);
sf=sin(f);
sp=sin(p);
ct=cos(t);
cf=cos(f);
st=sin(t);
cz5=cos(z5);
sz5=sin(z5);
cz6=cos(z6);
sz6=sin(z6);
sb=sin(b);
cb=cos(b);
A(1,1)=(c1x)*(-cl*si-sl*cn*ci)+(c1y)*(sl*si-cl*cn*ci)+(c1z)*sn*ci;
A(1,2)=(c1x)*(-ci*sl-si*cn*cl)+(c1y)*(-ci*cl+si*cn*sl);
A(1,3)=(c1x)*(sl*si*sn)+(c1y)*(cl*si*sn)+(c1z)*(si*cn);
A(2,1)=(c1x)*(cl*ci-sl*cn*si)-(c1y)*(sl*ci+cl*cn*si)+(c1z)*sn*si ;
A(2,2)=(c1x)*(-si*sl+ci*cn*cl)-(c1y)*(si*cl+ci*cn*sl) ;
A(2,3)=(c1x)*(-sl*ci*sn)-(c1y)*(cl*ci*sn)-(c1z)*ci*cn ;
A(3,1)=0 ;
A(3,2)=(c1x)*(sn*cl)-(c1y)*(sn*sl) ;
A(3,3)=(c1x)*(sl*cn)+(c1y)*(cl*cn)-(c1z)*sn ;
h1=wf*((z1x-z5x)*(-cp*sf-sp*ct*cf)+(z1y-z5y)*(sp*sf-cp*ct*cf)+(z1z-z5z)*st*cf);
h2=wp*((z1x-z5x)*(-cf*sp-sf*ct*cp)+(z1y-z5y)*(-cf*cp+sf*ct*sp));
h3=wt*((z1x-z5x)*(sp*sf*st)+(z1y-z5y)*(cp*sf*st)+(z1z-z5z)*(sf*ct));

h4=wf*((z1x-z5x)*(cp*cf-sp*ct*sf)-(z1y-z5y)*(sp*cf+cp*ct*sf)+(z1z-z5z)*st*sf) ;
h5=wp*((z1x-z5x))*(-sf*sp+cf*ct*cp)-(z1y-z5y)*(sf*cp+cf*ct*sp) ;
h6=wt*((z1x-z5x)*(-sp*cf*st)-(z1y-z5y)*(cp*cf*st)-(z1z-z5z)*cf*ct) ;

h7=wp*((z1x-z5x)*(st*cp)-(z1y-z5y)*(st*sp)) ;
h8=wt*((z1x-z5x)*(sp*ct)+(z1y-z5y)*(cp*ct)-(z1z-z5z)*st) ;

136
d=[wb*(w1u*(-sb*cz5+sz5*sz6*cb)-w1v*(cz5*cb+sz5*sz6*sb))+h1+h2+h3;wb*(w1u*(cz6*cb)-
w1v*(cz6*sb))+h4+h5+h6;wb*(w1u*(-sz5*sb+cz5*sz6*cb)+w1v*(sz5*cb-cz5*sz6*sb))+h7+h8];
w= pinv(A)*d

PROGRAMA PARA EL CALCULO DE LAS ACELERAIONES ANGULARES DEL

ESLABÓN ACOPLADOR Y DEL ESLABÓN W2.
Clear all
clc
%*********************
%VALORES PRESCRITOS
%*********************
w1u = input ('componente en el eje u del vector w1 ');
w1v = input ('componente en el eje v del vector w1 ');
z1x = input ('componente en el eje x del vector z1 ');
z1y = input ('componente en el eje y del vector z1 ');
z1z = input ('componente en el eje z del vector z1 ');
w2u = input ('componente en el eje u del vector w2 ');
w2v = input ('componente en el eje v del vector w2 ');
z2x = input ('componente en el eje x del vector z2 ');
z2y = input ('componente en el eje y del vector z2 ');
z2z = input ('componente en el eje z del vector z2 ');
z5 = input ('ángulo de rotación z5 sobre el eje y');
z6 = input ('ángulo de rotación z6 sobre el eje x rotado');
m = input ('ángulo de rotación m sobre el eje y');
s = input ('ángulo de rotación s sobre el eje x rotado');
wb = input ('velocidad angular del eslabón de entrada');
ab=0;
wf = input ('velocidad angular wf de la barra acopladora respecto al eje z');
wt = input ('velocidad angular wt de la barra acopladora respecto al eje rotado u');
wp= input ('velocidad angular wp de la barra acopladora respecto al eje rotado w');
wa= input ('velocidad angular wa respecto al eje w del eslabón w2');
b = input ('ángulo de rotación b ');
p = input ('ángulo de rotación p ');
f = input ('ángulo de rotación f ');
t = input ('ángulo de rotación t ');
a = input ('ángulo de rotación a ');
cp=cos(p);
sf=sin(f);
sp=sin(p);
ct=cos(t);
cf=cos(f);
st=sin(t);
sm=sin(m);
cm=cos(m);
sa=sin(a);
ca=cos(a);
ss=sin(s);
cs=cos(s);
cb=cos(b);
sb=sin(b);
cz5=cos(z5);
sz5=sin(z5);
cz6=cos(z6);
sz6=sin(z6);

137
A(1,1)=((z1x)-(z2x))*(-sf*cp-ct*cf*sp)+((z1y)-(z2y))*(sp*sf-cp*ct*cf)+((z1z)-(z2z))*st*cf ;
A(1,2)=((z1x)-(z2x))*(-cf*sp-sf*ct*cp)+((z1y)-(z2y))*(sf*ct*sp-cf*cp);
A(1,3)=((z1x)-(z2x))*(sp*sf*st)+((z1y)-(z2y))*(cp*sf*st)+((z1z)-(z1z))*(sf*ct);
A(1,4)=-(w2u*(-cm*sa+ca*sm*ss)-w2v*(cm*ca+sm*ss*sa));
A(2,1)=((z1x)-(z2x))*(cp*cf-ct*sf*sp)+((z1y)-(z2y))*(st*sf-sp*cf)-((z1z)-(z2z))*cp*ct*sf ;
A(2,2)=((z1x)-(z2x))*(-sf*sp+cp*cf*ct)-((z1y)-(z2y))*(sf*cp-cf*ct*sp);
A(2,3)=((z1x)-(z2x))*(-sp*cf*st)-((z1y)-(z2y))*(cp*cf*st)-((z1z)-(z2z))*(cf*ct);
A(2,4)=-(w2u*(cs*ca)-w2v*(cs*sa));
A(3,1)=0;
A(3,2)=((z1x)-(z2x))*(st*cp)-((z1y)-(z2y))*(st*sp);
A(3,3)=((z1x)-(z2x))*(sp*ct)+((z1y)-(z2y))*(cp*ct)-((z1z)-(z2z))*(st);
A(3,4)=-(w2u*(-sa*sm+cm*ss*ca)+w2v*(sm*ca-cm*ss*sa));

k1=((z1x)-(z2x))*(cf*(-cp)*wp+sp*sf*wf);
k2=((z1x)-(z2x))*(cp*(-cf)*wf+sf*sp*wp);
k3=((z1x)-(z2x))*(wp*(-sf*ct*sp)+cp*(sf*(-st)*wt+wf*ct*cf));
k4=((z1x)-(z2x))*(wt*(sf*(-ct)*wt-st*cf*wf)+wf*(ct*(-sf)*wf+cf*(-st)*wt));
k5=((z1x)-(z2x))*(sf*(-st)*wt+ct*cf*wf);
k6=((z1y)-(z2y))*(cf*(-sp)*wp+cp*(-sf)*wf);
k7=((z1y)-(z2y))*(-sf*cp*wp-sp*cf*wf);
k8=((z1y)-(z2y))*(-sf*ct*wp*cp*wp-sp*wp*(sf*(-st)*wt+ct*cf*wf));
k9=((z1y)-(z2y))*(sf*(-st)*wt+ct*cf*wf);
k10=((z1z)-(z2z))*(wt*(sf*(-st)*wt+ct*cf*wf));
k11=((z1z)-(z2z))*(wf*(st*(-sf)*wf+cf*ct*wt));
k12=((z1x)-(z2x))*(sf*(-cp)*wp+(-sp)*cf*wf);
k13=((z1x)-(z2x))*(-cp*sf*wf-cf*sp*wp);
k14=((z1x)-(z2x))*(cf*ct*(-sp)*wp+cp*(cf*(-st)*wt-ct*sf*wf));
k15=((z1x)-(z2x))*(wt*(cf*(-ct)*wt+st*sf*wf)+wf*(ct*(-cf)*wf+sf*st*wt));
k16=((z1x)-(z2x))*(cf*(-st)*wt-ct*sf*wf);
k17=((z1y)-(z2y))*(-sf*sp*wp+cp*cf*wf);
k18=((z1y)-(z2y))*(-sp*sf*wf+cf*cp*wp);
k19=((z1y)-(z2y))*(-cf*ct*cp*wp-sp*(-cf*st*wt+ct*(-sf)*wf));
k20=((z1y)-(z2y))*(wt*(cf*(-ct)*wt+st*sf*wf)+wf*(ct*(-cf)*wf+sf*st*wt));
k21=((z1y)-(z2y))*(-cf*st*wt-ct*sf*wf);
k22=((z1z)-(z2z))*(-cf*st*wt-ct*sf*wf);
k23=((z1z)-(z2z))*(-st*cf*wf-sf*ct*wt);
k24=((z1x)-(z2x))*(-st*sp*wp+cp*st*wt);
k25=((z1x)-(z2x))*(-sp*st*wt+ct*cp*wp);
k26=((z1y)-(z2y))*(-st*cp*wp-sp*ct*wt);
k27=((z1y)-(z2y))*(-cp*st*wt-ct*sp*wp);
k28=((z1z)-(z2z))*(-wt*ct*wt);
h1=wp*k1+wf*k2-wp*k3-sp*k4-cp*wp*k5-wp*k6-wf*k7-k8-cp*k4-(-sp)*wp*k9+k10+k11;
h2=w2u*(-wa*cm*ca*wa)+w2u*(wa*sm*ss*(-sa)*wa)-w2v*(wa*cm*(-sa)*wa)+
w2v*(wa*sm*ss*(-ca)*wa);
h3=wp*k12+wf*k13+wp*k14+sp*k15+cp*wp*k16-wp*k17-wf*k18+wp*k19+cp*k20+(-sp)*wp*k21-
wt*k22-wf*k23;
h4=w2u*(wa*cs*(-sa)*wa)+w2v*(wa*cs*(-ca)*wa);
h5=wp*k24+wt*k25+wp*k26+wt*k27+k28;
h6=w2u*(wa*sm*(-ca)*wa)+w2u*(wa*cm*ss*(-sa)*wa)+w2v*(wa*sm*(-sa)*wa)+
w2v*(wa*cm*ss*(-ca)*wa);
h7=w1u*(-wb*cz5*cb*wb)+w1u*(wb*sz5*sz6*(-sb)*wb)-w1v*(wb*cz5*(-sb)*wb)+
w1v*(wb*sz5*sz6*(-cb)*wb);
h8=w1u*(wb*cz5*(-sb)*wb)+w1v*(wb*cz5*(-cb)*wb);
h9=w1u*(wb*sz5*(-cb)*wb)+w1u*(wb*cz5*sz6*(-sb)*wb)+w1v*(wb*sz5*(-sb)*wb)+
w1v*(wb*cz5*sz6*(-cb)*wb);

138
d=[-(ab*(w1u*(-cz5*sb+cb*sz5*sz6)-w1v*(cz5*cb+sz5*sz6*sb))+h7)-h1+h2;-(ab*(w1u*(cz6*cb)-
w1v*(cz6*sb))+h8)-h3+h4;-(ab*(w1u*(-sb*sz5+cz5*sz6*cb)+w1v*(sz5*cb-cz5*sz6*sb))+h9)-h5+h6];

r=pinv(A)*d
%RESULTADOS

PROGRAMA PARA EL CALCULO DE LAS ACELERACIONES ANGULARES DEL ESLABÓN C1

Clear all
clc

%*********************
%VALORES PRESCRITOS
%*********************
w1u = input ('componente en el eje u del vector w1 ');
w1v = input ('componente en el eje v del vector w1 ');
z1x = input ('componente en el eje x del vector z1 ');
z1y = input ('componente en el eje y del vector z1 ');
z1z = input ('componente en el eje z del vector z1 ');
z5 = input ('ángulo de rotación z5 sobre el eje y');
z6 = input ('ángulo de rotación z6 sobre el eje x rotado');
wb = input ('velocidad angular del eslabón de entrada');
b = input ('ángulo de rotación b ');
p = input ('ángulo de rotación p ');
f = input ('ángulo de rotación f ');
t = input ('ángulo de rotación t ');
a = input ('ángulo de rotación a ');
l = input ('ángulo de rotación l ');
i = input ('ángulo de rotación i ');
n = input ('ángulo de rotación n ');
c1x = input ('componente en el eje x del vector c1 ');
c1y = input ('componente en el eje y del vector c1 ');
c1z = input ('componente en el eje z del vector c1 ');
z5x = input ('componente en el eje x del vector z5 ');
z5y = input ('componente en el eje y del vector z5 ');
z5z = input ('componente en el eje z del vector z5 ');
wf = input ('velocidad angular wf de la barra acopladora respecto al eje z');
wt = input ('velocidad angular wt de la barra acopladora respecto al eje rotado u');
wp = input ('velocidad angular wp de la barra acopladora respecto al eje rotado w');
wi = input ('velocidad angular wi del eslabón c1 respecto al eje z ');
wl = input ('velocidad angular wl del eslabón c1 respecto al eje rotado u ');
wn = input ('velocidad angular wn del eslabón c1 respecto al eje rotado w ');
af = input ('aceleración angular af de la barra acopladora respecto al eje z');
at = input ('aceleración angular at de la barra acopladora respecto al eje rotado u');
ap = input ('aceleración angular ap de la barra acopladora respecto al eje rotado w');
ab=0;
cp=cos(p);
sp=sin(p);
sf=sin(f);
cf=cos(f);
ct=cos(t);
st=sin(t);
cl=cos(l);
sl=sin(l);
si=sin(i);
ci=cos(i);
cn=cos(n);

139
sn=sin(n);
cz5=cos(z5);
sz5=sin(z5);
cz6=cos(z6);
sz6=sin(z6);
cb=cos(b);
sb=sin(b);
A(1,1)=(c1x)*(-si*cl-cn*ci*sl)+(c1y)*(sl*si-cl*cn*ci)+(c1z)*sn*ci ;
A(1,2)=(c1x)*(-ci*sl-si*cn*cl)+(c1y)*(si*cn*sl-ci*cl);
A(1,3)=(c1x)*(sl*si*sn)+(c1y)*(cl*si*sn)+(c1z)*(si*cn);
A(2,1)=(c1x)*(cl*ci-cn*si*sl)+(c1y)*(sn*si-sl*ci)-(c1z)*cl*cn*si ;
A(2,2)=(c1x)*(-si*sl+cl*ci*cn)-(c1y)*(si*cl-ci*cn*sl);
A(2,3)=(c1x)*(-sl*ci*sn)-(c1y)*(cl*ci*sn)-(c1z)*(ci*cn);
A(3,1)=0;
A(3,2)=(c1x)*(sn*cl)-(c1y)*(sn*sl);
A(3,3)=(c1x)*(sl*cn)+(c1y)*(cl*cn)-(c1z)*(sn);

m1=af*((z1x-z5x)*(-sf*cp-ct*cf*sp)+(z1y-z5y)*(sp*sf-cp*ct*cf)+(z1z-z5z)*st*cf );
m2=ap*((z1x-z5x)*(-cf*sp-sf*ct*cp)+(z1y-z5y)*(sf*ct*sp-cf*cp));
m3=at*((z1x-z5x)*(sp*sf*st)+(z1y-z5y)*(cp*sf*st)+(z1z-z5z)*(sf*ct));
m4=af*((z1x-z5x)*(cp*cf-ct*sf*sp)+(z1y-z5y)*(st*sf-sp*cf)-(z1z-z5z)*cp*ct*sf );
m5=ap*((z1x-z5x)*(-sf*sp+cp*cf*ct)-(z1y-z5y)*(sf*cp-cf*ct*sp));
m6=at*((z1x-z5x)*(-sp*cf*st)-(z1y-z5y)*(cp*cf*st)-(z1z-z5z)*(cf*ct));
m7=ap*((z1x-z5x)*(st*cp)-(z1y-z5y)*(st*sp));
m8=at*((z1x-z5x)*(sp*ct)+(z1y-z5y)*(cp*ct)-(z1z-z5z)*(st));

q1=(z1x-z5x)*(cf*(-cp)*wp+sp*sf*wf);
q2=(z1x-z5x)*(cp*(-cf)*wf+sf*sp*wp);
q3=(z1x-z5x)*(wp*(-sf*ct*sp)+cp*(sf*(-st)*wt+wf*ct*cf));
q4=(z1x-z5x)*(wt*(sf*(-ct)*wt-st*cf*wf)+wf*(ct*(-sf)*wf+cf*(-st)*wt));
q5=(z1x-z5x)*(sf*(-st)*wt+ct*cf*wf);
q6=(z1y-z5y)*(cf*(-sp)*wp+cp*(-sf)*wf);
q7=(z1y-z5y)*(-sf*cp*wp-sp*cf*wf);
q8=(z1y-z5y)*(-sf*ct*wp*cp*wp-sp*wp*(sf*(-st)*wt+ct*cf*wf));
q9=(z1y-z5y)*(sf*(-st)*wt+ct*cf*wf);
q10=(z1z-z5z)*(wt*(sf*(-st)*wt+ct*cf*wf));
q11=(z1z-z5z)*(wf*(st*(-sf)*wf+cf*ct*wt));
q12=(z1x-z5x)*(sf*(-cp)*wp+(-sp)*cf*wf);
q13=(z1x-z5x)*(-cp*sf*wf-cf*sp*wp);
q14=(z1x-z5x)*(cf*ct*(-sp)*wp+cp*(cf*(-st)*wt-ct*sf*wf));
q15=(z1x-z5x)*(wt*(cf*(-ct)*wt+st*sf*wf)+wf*(ct*(-cf)*wf+sf*st*wt));
q16=(z1x-z5x)*(cf*(-st)*wt-ct*sf*wf);
q17=(z1y-z5y)*(-sf*sp*wp+cp*cf*wf);
q18=(z1y-z5y)*(-sp*sf*wf+cf*cp*wp);
q19=(z1y-z5y)*(-cf*ct*cp*wp-sp*(-cf*st*wt+ct*(-sf)*wf));
q20=(z1y-z5y)*(wt*(cf*(-ct)*wt+st*sf*wf)+wf*(ct*(-cf)*wf+sf*st*wt));
q21=(z1y-z5y)*(-cf*st*wt-ct*sf*wf);
q22=(z1z-z5z)*(-cf*st*wt-ct*sf*wf);
q23=(z1z-z5z)*(-st*cf*wf-sf*ct*wt);
q24=(z1x-z5x)*(-st*sp*wp+cp*st*wt);
q25=(z1x-z5x)*(-sp*st*wt+ct*cp*wp);
q26=(z1y-z5y)*(-st*cp*wp-sp*ct*wt);
q27=(z1y-z5y)*(-cp*st*wt-ct*sp*wp);
q28=(z1z-z5z)*(-wt*ct*wt);
f1=wp*q1+wf*q2-wp*q3-sp*q4-cp*wp*q5-wp*q6-wf*q7-q8-cp*q4-(-sp)*wp*q9+q10+q11;

140
f2=wp*q12+wf*q13+wp*q14+sp*q15+cp*wp*q16-wp*q17-wf*q18+wp*q19+cp*q20+(-sp)*wp*q21-
wt*q22-wf*q23;
f3=wp*q24+wt*q25+wp*q26+wt*q27+q28;
s1=(c1x)*(ci*(-cl)*wl+sl*si*wi);
s2=(c1x)*(cl*(-ci)*wi+si*sl*wl);
s3=(c1x)*(wl*(-si*cn*sl)+cl*(si*(-sn)*wn+wi*cn*ci));
s4=(c1x)*(wn*(si*(-cn)*wn-sn*ci*wi)+wi*(cn*(-si)*wi+ci*(-sn)*wn));
s5=(c1x)*(si*(-sn)*wn+cn*ci*wi);
s6=(c1y)*(ci*(-sl)*wl+cl*(-si)*wi);
s7=(c1y)*(-si*cl*wl-sl*ci*wi);
s8=(c1y)*(-si*cn*wl*cl*wl-sl*wl*(si*(-sn)*wn+cn*ci*wi));
s9=(c1y)*(si*(-sn)*wn+cn*ci*wi);
s10=(c1z)*(wn*(si*(-sn)*wn+cn*ci*wi));
s11=(c1z)*(wi*(sn*(-si)*wi+ci*cn*wn));
s12=(c1x)*(si*(-cl)*wl+(-sl)*ci*wi);
s13=(c1x)*(-cl*si*wi-ci*sl*wl);
s14=(c1x)*(ci*cn*(-sl)*wl+cl*(ci*(-sn)*wn-cn*si*wi));
s15=(c1x)*(wn*(ci*(-cn)*wn+sn*si*wi)+wi*(cn*(-ci)*wi+si*sn*wn));
s16=(c1x)*(ci*(-sn)*wn-cn*si*wi);
s17=(c1y)*(-si*sl*wl+cl*ci*wi);
s18=(c1y)*(-sl*si*wi+ci*cl*wl);
s19=(c1y)*(-ci*cn*cl*wl-sl*(-ci*sn*wn+cn*(-si)*wi));
s20=(c1y)*(wn*(ci*(-cn)*wn+sn*si*wi)+wi*(cn*(-ci)*wi+si*sn*wn));
s21=(c1y)*(-ci*sn*wn-cn*si*wi);
s22=(c1z)*(-ci*sn*wn-cn*si*wi);
s23=(c1z)*(-sn*ci*wi-si*cn*wn);
s24=(c1x)*(-sn*sl*wl+cl*sn*wn);
s25=(c1x)*(-sl*sn*wn+cn*cl*wl);
s26=(c1y)*(-sn*cl*wl-sl*cn*wn);
s27=(c1y)*(-cl*sn*wn-cn*sl*wl);
s28=(c1z)*(-wn*cn*wn);
g1=wl*s1+wi*s2-wl*s3-sl*s4-cl*wl*s5-wl*s6-wi*s7-s8-cl*s4-(-sl)*wl*s9+s10+s11;
g2=wl*s12+wi*s13+wl*s14+sl*s15+cl*wl*s16-wl*s17-wi*s18+wl*s19+cl*s20+(-sl)*wl*s21-wn*s22-
wi*s23;
g3=wl*s24+wn*s25+wl*s26+wn*s27+s28;
h7=w1u*(-wb*cz5*cb*wb)+w1u*(wb*sz5*sz6*(-sb)*wb)-w1v*(wb*cz5*(-sb)*wb)+
w1v*(wb*sz5*sz6*(-cb)*wb);
h8=w1u*(wb*cz5*(-sb)*wb)+w1v*(wb*cz5*(-cb)*wb);
h9=w1u*(wb*sz5*(-cb)*wb)+w1u*(wb*cz5*sz6*(-sb)*wb)+w1v*(wb*sz5*(-sb)*wb)+
w1v*(wb*cz5*sz6*(-cb)*wb);

d=[(ab*(w1u*(-cz5*sb+cb*sz5*sz6)-w1v*(cz5*cb+sz5*sz6*sb))+h7)-
g1+f1+m1+m2+m3;(ab*(w1u*(cz6*cb)-w1v*(cz6*sb))+h8)-g2+f2+m4+m5+m6;(ab*(w1u*(-
sb*sz5+cz5*sz6*cb)+w1v*(sz5*cb-cz5*sz6*sb))+h9)+f3+m7+m8-g3];
r=pinv(A)*d
%RESULTADOS

141