-
Notifications
You must be signed in to change notification settings - Fork 0
/
JacobTest.py
91 lines (65 loc) · 2.43 KB
/
JacobTest.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
# -*- coding: utf-8 -*-
"""
Created on Tue Apr 07 14:57:41 2015
@author: Dan
"""
import numpy as np
from CinematicaInversa import*
from array import*
from numpy import arange
from math import*
from matplotlib import pyplot as plt
# Calculo de Trayectoria
L1=7; L2=13; L3=10; L4=10;
dt=0.025; #paso de tiempo
Tper=2; #tiempo en completar recorrido
Nt=Tper/dt;
[q1,q2,q3,q4,q5,status]=cinInv(0,0,15,0,24,L1,L2,L3,L4);
Q=np.zeros(shape=(5,int(Nt)))
Q[:,0]=np.matrix([q1,q2,q3,q4+pi/2,q5]);
xini=np.matrix([15,0,24,0,0]).T;
x=np.zeros(shape=(5,int(Nt)))
x[:,0]=xini.T;
amp=5;
Kp=5;
#x=np.zeros(shape=(5,int(Nt)))
xd=np.zeros(shape=(5,int(Nt)-1))
vel=np.zeros(shape=(5,int(Nt)))
t=np.zeros((int(Nt)-1))
error=np.zeros(shape=(5,int(Nt)-1))
for n in arange (0,int(Nt)-1):
t[n]=n*dt+dt;
#####
xd[1,n]=xini[1]+amp*cos((2*math.pi*t[n])/Tper);
xd[2,n]=xini[2]+amp*sin((2*math.pi*t[n])/Tper);
xd[0,:]=xini[0];
xd[3,:]=0;
xd[4,:]=0;
#####
error[:,n]=xd[:,n]-x[:,n];
# Actualizacion del Jacobiano
q1=Q[0,n];q2=Q[1,n];q3=Q[2,n];q4=Q[3,n];q5=Q[4,n];
J0_s=np.matrix(([-sin(q1)*(L3*cos(q2 + q3) + L2*cos(q2) + L4*sin(q2 + q3 + q4)), -cos(q1)*(L3*sin(q2 + q3) + L2*sin(q2) - L4*cos(q2 + q3 + q4)),-cos(q1)*(L3*sin(q2 + q3) - L4*cos(q2 + q3 + q4)),L4*cos(q2 + q3 + q4)*cos(q1), 0],
[cos(q1)*(L3*cos(q2 + q3) + L2*cos(q2) + L4*sin(q2 + q3 + q4)), -sin(q1)*(L3*sin(q2 + q3) + L2*sin(q2) - L4*cos(q2 + q3 + q4)), -sin(q1)*(L3*sin(q2 + q3) - L4*cos(q2 + q3 + q4)), L4*cos(q2 + q3 + q4)*sin(q1), 0],
[0,L3*cos(q2 + q3) + L2*cos(q2) + L4*sin(q2 + q3 + q4),L3*cos(q2 + q3) + L4*sin(q2 + q3 + q4),L4*sin(q2 + q3 + q4), 0],
[0,-1,-1,-1,0], [0,0,0,0,1]));
#####
invJ0=J0_s.I;
vel[:,n]=Kp*error[:,n];
#Q[:,n+1]=Q[:,n]+invJ0*(vel[:,n].T*dt);
Q[:,n+1]=Q[:,n]+np.dot(invJ0,vel[:,n]*dt)
q1=Q[0,n+1];
q2=Q[1,n+1];
q3=Q[2,n+1];
q4=Q[3,n+1];
q5=Q[4,n+1];
x[0,n+1] =cos(q1)*(L3*cos(q2 + q3) + L2*cos(q2) + L4*sin(q2 + q3 + q4));
x[1,n+1] =math.sin(q1)*(L3*math.cos(q2 + q3) + L2*math.cos(q2) + L4*math.sin(q2 + q3 + q4));
x[2,n+1] =L1 + L3*sin(q2 + q3) + L2*sin(q2) - L4*cos(q2 + q3 + q4);
x[3,n+1]=0;
x[4,n+1]=0;
plt.plot(Q[0,:]/pi*180)
plt.plot(Q[1,:]/pi*180)
plt.plot(Q[2,:]/pi*180)
plt.plot((Q[3,:]-pi/2)/pi*180)
plt.plot(Q[4,:]/pi*180)