-
Notifications
You must be signed in to change notification settings - Fork 0
/
simulate.py
64 lines (52 loc) · 1.87 KB
/
simulate.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
from nextstate import getnextState
from feedForward import feedForwardControl
from math import pi
from trajgen import generateTrajectory
from youBot import ControlVector, endEffectorinSpace, flatTrajtoTransform, youBotState
import numpy as np
import matplotlib.pyplot as plt
import modern_robotics as mr
initialState = youBotState()
initialState.jointState = np.array([0,0,0.3,-1.6,0])
initialState.chasisState = np.array([0,-0.3,-0.5])
currentState = initialState
controls = ControlVector()
Kp = 2 * np.eye(6)
Ki = 1 * np.eye(6)
initCube = [1,0,0]
finalCube = [-0,-1,-pi/2]
stateMatrix = []
Error = []
flatList = []
######### Generate Trajectory ########
Trajectory = generateTrajectory(initialState,initCube,finalCube)
np.savetxt('traj.csv',Trajectory,fmt="%f",delimiter=",")
# trajectory defined
### Simulating the stuff ####
for i in range( len(Trajectory) -1 ):
update = np.concatenate((currentState.chasisState,currentState.jointState,
currentState.wheelState,currentState.gripperState),axis=None)
stateMatrix.append(update)
X = endEffectorinSpace(currentState)
Xd,gripper = flatTrajtoTransform(Trajectory[i])
XdNext,_gripper = flatTrajtoTransform(Trajectory[i+1])
currentState.gripperState = np.array(gripper)
Xrr = feedForwardControl(X,Xd,XdNext,currentState,controls,Kp,Ki)
Error.append(Xrr)
#flatList.append(flat)
getnextState(currentState,controls)
Error = np.array(Error)
flatList = np.array(flatList)
stateMatrix = np.array(stateMatrix)
np.savetxt('stateMat.csv',stateMatrix,fmt='%f',delimiter=',')
np.savetxt('flat.csv',flatList,fmt='%f',delimiter=',')
### plot the results
iterations = range(len(Trajectory)-1)
plt.plot(iterations,Error[:,0])
plt.plot(iterations,Error[:,1])
plt.plot(iterations,Error[:,2])
plt.plot(iterations,Error[:,3])
plt.plot(iterations,Error[:,4])
plt.plot(iterations,Error[:,5])
plt.show()
plt.close('all')