-
Notifications
You must be signed in to change notification settings - Fork 10
/
Copy pathanalyse.py
111 lines (85 loc) · 3.42 KB
/
analyse.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
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
import numpy as np
from IPython import embed
from matplotlib import pyplot as plt
from solo_estimator import BaseEstimator
from example_robot_data.robots_loader import load_full
data = np.load("/tmp/data_main_solo12_estimation.npz")
# data = np.load("data_main_solo12_estimation_with_control.npz")
# data = np.load("data_main_solo12_estimation.npz")
q_mes = data['q_mes']
v_mes = data['v_mes']
baseOrientation = data['baseOrientation']
baseAngularVelocity = data['baseAngularVelocity']
baseLinearAcceleration = data['baseLinearAcceleration']
mocapPosition = data['mocapPosition']
mocapVelocity = data['mocapVelocity']
mocapAngularVelocity = data['mocapAngularVelocity']
mocapOrientationMat9 = data['mocapOrientationMat9']
mocapOrientationQuat = data['mocapOrientationQuat']
dt=0.001
N = len(q_mes)
mocapVelocityRotated = np.zeros([N,3])
for i in range(N):
mocapVelocityRotated[i] = (np.matrix(mocapOrientationMat9[i])*np.matrix(mocapVelocity[i]).T).A1
mocapAngularVelocityRotated = np.zeros([N,3])
for i in range(N):
mocapAngularVelocityRotated[i] = (np.matrix(mocapOrientationMat9[i]).T * np.matrix(mocapAngularVelocityRotated[i]).T).A1
velocityImuIntegration = np.zeros([N,3])
for i in range(1,N):
velocityImuIntegration[i]=velocityImuIntegration[i-1]+baseLinearAcceleration[i]*dt
#This will come from estimator
velocityKinematic = np.zeros([4,N,3])
robot, q0, urdf, srdf = load_full('solo12')
be = BaseEstimator(urdf=urdf,modelPath='/'.join(urdf.split('/')[:-3]),contactFrameNames = ['HR_FOOT', 'HL_FOOT', 'FR_FOOT', 'FL_FOOT'])
#example of use:
for i in range(N):
be.update_sensors(q_mes[i],v_mes[i],baseAngularVelocity[i])
velocityKinematic[0][i] = be.BaseVelocitiesFromKin[0]
velocityKinematic[1][i] = be.BaseVelocitiesFromKin[1]
velocityKinematic[2][i] = be.BaseVelocitiesFromKin[2]
velocityKinematic[3][i] = be.BaseVelocitiesFromKin[3]
#complementary_filter
def HP(x,alpha):
N=len(x)
y = np.zeros(x.shape)
y[0]=0
for i in range(1,N):
y[i]=alpha*(x[i]-x[i-1] + y[i-1])
return y
def LP(x,alpha):
N=len(x)
y = np.zeros(x.shape)
y[0]=x[0]
for i in range(1,N):
y[i]=alpha*y[i-1] + (1-alpha)*x[i]
return y
plt.figure("linear velocity: Imu Integration/Mocap Rotated")
for axis in range(3):
plt.subplot(3,1,axis+1)
plt.plot(velocityImuIntegration[:,axis])
plt.plot(mocapVelocityRotated[:,axis])
#plt.plot(velocityKinematic[0][:,axis])
#plt.plot(velocityKinematic[1][:,axis])
#plt.plot(velocityKinematic[2][:,axis])
#plt.plot(velocityKinematic[3][:,axis])
plt.figure("linear velocity: Estimated/Mocap Rotated")
alpha=0.99
#mocapLP = LP(mocapVelocityRotated,alpha)
imuHP = HP(velocityImuIntegration,alpha)
velocityKinematic0LP = LP(velocityKinematic[0],alpha)
velocityKinematic1LP = LP(velocityKinematic[1],alpha)
velocityKinematic2LP = LP(velocityKinematic[2],alpha)
velocityKinematic3LP = LP(velocityKinematic[3],alpha)
for axis in range(3):
plt.subplot(3,1,axis+1)
plt.plot(velocityKinematic0LP[:,axis]+imuHP[:,axis],'g')
plt.plot(velocityKinematic1LP[:,axis]+imuHP[:,axis],'b')
plt.plot(velocityKinematic2LP[:,axis]+imuHP[:,axis])
plt.plot(velocityKinematic3LP[:,axis]+imuHP[:,axis])
plt.plot(mocapVelocityRotated[:,axis],'r')
plt.figure("Angular velocity: Estimated/Mocap Rotated")
for axis in range(3):
plt.subplot(3,1,axis+1)
plt.plot(baseAngularVelocity[:,axis],'g')
plt.plot(mocapVelocityRotated[:,axis],'r')
plt.show()