forked from ZhaomingXie/CDDP
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathconstraints.py
65 lines (60 loc) · 2.72 KB
/
constraints.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
import numpy as np
class CircleConstraintForCar:
def __init__(self, center, r, system):
self.center = center
self.r = r
self.system = system
def evaluate_constraint(self, x):
#evolve the system for one to evaluate constraint
x_next = self.system.transition(x, np.zeros(self.system.control_size))
length = (x_next[0] - self.center[0])**2 + (x_next[1] - self.center[1])**2
#print(x_next, self.r**2 - length)
return self.r**2 - length
def evaluate_constraint_J(self, x):
#evolve the system for one to evaluate constraint
x_next = self.system.transition(x, np.zeros(self.system.control_size))
result = np.zeros(x.shape)
result[0] = -2*(x_next[0] - self.center[0])
result[1] = -2*(x_next[1] - self.center[1])
result[2] = -2*(x_next[0] - self.center[0]) * self.system.dt * x[3] * np.cos(x[2]) + 2*(x_next[1] - self.center[1]) * self.system.dt * x[3] * np.sin(x[2])
result[3] = -2*(x_next[1] - self.center[1]) * self.system.dt * np.cos(x[2]) -2*(x_next[0] - self.center[0]) * self.system.dt * np.sin(x[2])
return result
class CircleConstraintForDoubleIntegrator:
def __init__(self, center, r, system):
self.center = center
self.r = r
self.system = system
def evaluate_constraint(self, x):
#evolve the system for one to evaluate constraint
x_next = self.system.transition(x, np.zeros(self.system.control_size))
length = (x_next[0] - self.center[0])**2 + (x_next[1] - self.center[1])**2
#print(x_next, self.r**2 - length)
return self.r**2 - length
def evaluate_constraint_J(self, x):
#evolve the system for one to evaluate constraint
x_next = self.system.transition(x, np.zeros(self.system.control_size))
result = np.zeros(x.shape)
result[0] = -2*(x_next[0] - self.center[0])
result[1] = -2*(x_next[1] - self.center[1])
result[2] = -2*(x_next[0] - self.center[0]) * self.system.dt
result[3] = -2*(x_next[1] - self.center[1]) * self.system.dt
return result
class SphereConstraintForQuadrotor:
def __init__(self, center, r, system):
self.center = center
self.r = r
self.system = system
def evaluate_constraint(self, x):
x_next = self.system.transition(x, np.zeros(self.system.control_size))
length = (x_next[0] - self.center[0])**2 + (x_next[1] - self.center[1])**2 + (x_next[2] - self.center[2])**2
return self.r**2 - length
def evaluate_constraint_J(self, x):
x_next = self.system.transition(x, np.zeros(self.system.control_size))
result = np.zeros(x.shape)
result[0] = -2*(x_next[0] - self.center[0])
result[1] = -2*(x_next[1] - self.center[1])
result[2] = -2*(x_next[2] - self.center[2])
result[3] = -2*(x_next[0] - self.center[0]) * self.system.dt
result[4] = -2*(x_next[1] - self.center[1]) * self.system.dt
result[5] = -2*(x_next[2] - self.center[2]) * self.system.dt
return result