├── README.md
├── files
├── quadrotor.urdf
└── quadrotor_base.obj
└── notebooks
├── costs.py
├── data
├── frankaemika_new
│ ├── meshes
│ │ ├── collision
│ │ │ ├── finger.obj
│ │ │ ├── finger.stl
│ │ │ ├── hand.obj
│ │ │ ├── hand.stl
│ │ │ ├── link0.obj
│ │ │ ├── link0.stl
│ │ │ ├── link1.obj
│ │ │ ├── link1.stl
│ │ │ ├── link2.obj
│ │ │ ├── link2.stl
│ │ │ ├── link3.obj
│ │ │ ├── link3.stl
│ │ │ ├── link4.obj
│ │ │ ├── link4.stl
│ │ │ ├── link5.obj
│ │ │ ├── link5.stl
│ │ │ ├── link6.mtl
│ │ │ ├── link6.obj
│ │ │ ├── link6.stl
│ │ │ ├── link7.obj
│ │ │ └── link7.stl
│ │ └── visual
│ │ │ ├── colors.png
│ │ │ ├── finger.dae
│ │ │ ├── finger.mtl
│ │ │ ├── finger.obj
│ │ │ ├── finger.stl
│ │ │ ├── hand.dae
│ │ │ ├── hand.mtl
│ │ │ ├── hand.obj
│ │ │ ├── hand.stl
│ │ │ ├── link0.dae
│ │ │ ├── link0.stl
│ │ │ ├── link1.dae
│ │ │ ├── link1.mtl
│ │ │ ├── link1.obj
│ │ │ ├── link1.stl
│ │ │ ├── link2.dae
│ │ │ ├── link2.mtl
│ │ │ ├── link2.obj
│ │ │ ├── link2.stl
│ │ │ ├── link3.dae
│ │ │ ├── link3.mtl
│ │ │ ├── link3.obj
│ │ │ ├── link3.stl
│ │ │ ├── link4.dae
│ │ │ ├── link4.mtl
│ │ │ ├── link4.obj
│ │ │ ├── link4.stl
│ │ │ ├── link5.dae
│ │ │ ├── link5.mtl
│ │ │ ├── link5.obj
│ │ │ ├── link5.stl
│ │ │ ├── link6.dae
│ │ │ ├── link6.mtl
│ │ │ ├── link6.obj
│ │ │ ├── link6.stl
│ │ │ ├── link7.dae
│ │ │ └── link7.stl
│ ├── panda_arm.urdf
│ ├── panda_arm_franka.urdf
│ ├── panda_arm_kdl.urdf
│ └── panda_arm_rivet.urdf
├── meshes
│ ├── collision
│ │ ├── finger.stl
│ │ ├── hand.stl
│ │ ├── link0.stl
│ │ ├── link1.stl
│ │ ├── link2.stl
│ │ ├── link3.stl
│ │ ├── link4.stl
│ │ ├── link5.stl
│ │ ├── link6.stl
│ │ └── link7.stl
│ └── visual
│ │ ├── finger.dae
│ │ ├── hand.dae
│ │ ├── link0.dae
│ │ ├── link1.dae
│ │ ├── link2.dae
│ │ ├── link3.dae
│ │ ├── link4.dae
│ │ ├── link5.dae
│ │ ├── link6.dae
│ │ └── link7.dae
├── panda_arm.urdf
└── panda_arm_gripper.urdf
├── ilqr.ipynb
├── mixture_model.py
├── ocp.py
├── ocp_sys.py
├── panda.ipynb
├── process_data.ipynb
├── quadcopter.ipynb
└── utils.py
/README.md:
--------------------------------------------------------------------------------
1 | # Probabilistic Iterative LQR for Short Time Horizon MPC
2 | Repository for computing the probability distribution of an optimal control problem and use a short horizon MPC tracking controller to track the trajectory distribution. It contains the codes to run the experiments in the paper (https://arxiv.org/abs/2012.06349).
3 |
4 | To run the experiments, use the following notebooks:
5 | - panda.ipynb
6 | - quadcopter.ipynb
7 |
--------------------------------------------------------------------------------
/files/quadrotor.urdf:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
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 |
--------------------------------------------------------------------------------
/notebooks/costs.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | import matplotlib.pyplot as plt
3 | from numpy import dot
4 | from numpy.linalg import inv
5 | from utils import computePose, computeJacobian
6 | class CostModelQuadratic():
7 | def __init__(self, sys, Q = None, R = None, x_ref = None, u_ref = None):
8 | self.sys = sys
9 | self.Dx, self.Du = sys.Dx, sys.Du
10 | self.Q, self.R = Q, R
11 | if Q is None: self.Q = np.zeros((self.Dx,self.Dx))
12 | if R is None: self.R = np.zeros((self.Du,self.Du))
13 | self.x_ref, self.u_ref = x_ref, u_ref
14 | if x_ref is None: self.x_ref = np.zeros(self.Dx)
15 | if u_ref is None: self.u_ref = np.zeros(self.Du)
16 |
17 | def set_ref(self, x_ref=None, u_ref=None):
18 | if x_ref is not None:
19 | self.x_ref = x_ref
20 | if u_ref is not None:
21 | self.u_ref = u_ref
22 |
23 | def calc(self, x, u):
24 | self.L = 0.5*(x-self.x_ref).T.dot(self.Q).dot(x-self.x_ref) + 0.5*(u-self.u_ref).T.dot(self.R).dot(u-self.u_ref)
25 | return self.L
26 |
27 | def calcDiff(self, x, u):
28 | self.Lx = self.Q.dot(x-self.x_ref)
29 | self.Lu = self.R.dot(u-self.u_ref)
30 | self.Lxx = self.Q.copy()
31 | self.Luu = self.R.copy()
32 | self.Lxu = np.zeros((self.Dx, self.Du))
33 |
34 | class CostModelSum():
35 | def __init__(self, sys, costs):
36 | self.sys = sys
37 | self.costs = costs
38 | self.Dx, self.Du = sys.Dx, sys.Du
39 |
40 | def calc(self, x, u):
41 | self.L = 0
42 | for i,cost in enumerate(self.costs):
43 | cost.calc(x, u)
44 | self.L += cost.L
45 | return self.L
46 |
47 | def calcDiff(self, x, u):
48 | self.Lx = np.zeros(self.Dx)
49 | self.Lu = np.zeros(self.Du)
50 | self.Lxx = np.zeros((self.Dx,self.Dx))
51 | self.Luu = np.zeros((self.Du,self.Du))
52 | self.Lxu = np.zeros((self.Dx,self.Du))
53 | for i,cost in enumerate(self.costs):
54 | cost.calcDiff(x, u)
55 | self.Lx += cost.Lx
56 | self.Lu += cost.Lu
57 | self.Lxx += cost.Lxx
58 | self.Luu += cost.Luu
59 | self.Lxu += cost.Lxu
60 |
61 | class CostModelQuadraticTranslation():
62 | '''
63 | The quadratic cost model for the end effector, p = f(x)
64 | '''
65 | def __init__(self, sys, W, p_ref = None):
66 | self.sys = sys
67 | self.Dx, self.Du = sys.Dx, sys.Du
68 | self.W = W
69 | self.p_ref = p_ref
70 | if p_ref is None: self.p_ref = np.zeros(3)
71 |
72 | def set_ref(self, p_ref):
73 | self.p_ref = p_ref
74 |
75 | def calc(self, x, u):
76 | p,_ = self.sys.compute_ee(x)
77 | self.L = 0.5*(p-self.p_ref).T.dot(self.W).dot(p-self.p_ref)
78 | return self.L
79 |
80 | def calcDiff(self, x, u):
81 | self.J = self.sys.compute_Jacobian(x)
82 | p,_ = self.sys.compute_ee(x)
83 | self.Lx = self.J.T.dot(self.W).dot(p-self.p_ref)
84 | self.Lx = np.concatenate([self.Lx, np.zeros(self.Dx/2)])
85 | self.Lu = np.zeros(self.Du)
86 | self.Lxx = np.zeros((self.Dx, self.Dx))
87 | self.Lxx[:self.Dx/2, :self.Dx/2] = self.J.T.dot(self.W).dot(self.J)
88 | self.Luu = np.zeros((self.Du, self.Du))
89 | self.Lxu = np.zeros((self.Dx, self.Du))
90 |
91 | class ActivationCollision():
92 | def __init__(self, nr, threshold=0.3):
93 | self.threshold = threshold
94 | self.nr = nr
95 | def calc(self, r):
96 | self.d = np.linalg.norm(r)
97 |
98 | if self.d < self.threshold:
99 | self.a = 0.5*(self.d-self.threshold)**2
100 | else:
101 | self.a = 0
102 | return self.a
103 |
104 | def calcDiff(self, r, recalc=True):
105 | if recalc:
106 | self.calc(r)
107 |
108 | if self.d < self.threshold:
109 | self.Ar = (self.d-self.threshold)*r[:,None]/self.d
110 | self.Arr = np.eye(self.nr)*(self.d-self.threshold)/self.d + self.threshold*np.outer(r,r.T)/(self.d**3)
111 | else:
112 | self.Ar = np.zeros((self.nr,1))
113 | self.Arr = np.zeros((self.nr,self.nr))
114 | return self.Ar, self.Arr
115 |
116 |
117 | class SphereSphereCollisionCost():
118 | def __init__(self, activation=None, nu=None, r_body = 0., r_obs = 0., pos_obs = np.array([0,0,0]), w= 1.):
119 | self.activation = activation
120 | self.r_body = r_body
121 | self.r_obs = r_obs
122 | self.pos_obs = pos_obs
123 | self.w = w
124 |
125 | def calc(self, x, u):
126 | #calculate residual
127 | pos_body = x[:3]
128 | self.r = pos_body-self.pos_obs
129 | self.activation.calc(self.r)
130 | self.L = self.activation.a*self.w
131 | return self.L
132 |
133 | def calcDiff(self, x, u, recalc=True):
134 | if recalc:
135 | self.calc( x, u)
136 |
137 | #Calculate the Jacobian at p1
138 | self.J = np.hstack([np.eye(3), np.zeros((3,3))])
139 | ###Compute the cost derivatives###
140 | self.activation.calcDiff(self.r, recalc)
141 | self.Rx = np.hstack([self.J, np.zeros((3, 6))])
142 | self.Lx = np.vstack([self.J.T.dot(self.activation.Ar), np.zeros((6, 1))])
143 | self.Lxx = np.vstack([
144 | np.hstack([self.J.T.dot(self.activation.Arr).dot(self.J),
145 | np.zeros((6, 6))]),
146 | np.zeros((6, 12))
147 | ])*self.w
148 | self.Lx = self.Lx[:,0]*self.w
149 | self.Lu = np.zeros(4)
150 | self.Luu = np.zeros((4,4))
151 | self.Lxu = np.zeros((12, 4))
152 | return self.Lx, self.Lxx
153 |
154 | class SphereCapsuleCollisionCost():
155 | def __init__(self, activation=None, nu=None, p1 = np.zeros(3), p2 = np.zeros(3), w= 1.):
156 | self.activation = activation
157 | self.p1 = p1
158 | self.p2 = p2
159 | self.length = np.linalg.norm(self.p2-self.p1)
160 | self.v = (p2-p1)/self.length
161 | self.w = w
162 |
163 | def calc(self, x, u):
164 | #calculate residual
165 | p = x[:3]
166 | pt = p - self.p1
167 | t = pt.dot(self.v)
168 | if t < 0:
169 | r = p - self.p1
170 | elif t > self.length:
171 | r = p - self.p2
172 | else:
173 | r = pt - t*self.v
174 |
175 | self.pt = pt
176 | self.t = t
177 | self.r = r
178 | self.activation.calc(self.r)
179 | self.L = self.activation.a*self.w
180 | return self.L
181 |
182 | def calcDiff(self, x, u, recalc=True):
183 | if recalc:
184 | self.calc( x, u)
185 |
186 | #Calculate the Jacobian at p1
187 | self.J = np.hstack([np.eye(3), np.zeros((3,3))])
188 | ###Compute the cost derivatives###
189 | self.activation.calcDiff(self.r, recalc)
190 | self.Rx = np.hstack([self.J, np.zeros((3, 6))])
191 | self.Lx = np.vstack([self.J.T.dot(self.activation.Ar), np.zeros((6, 1))])
192 | self.Lxx = np.vstack([
193 | np.hstack([self.J.T.dot(self.activation.Arr).dot(self.J),
194 | np.zeros((6, 6))]),
195 | np.zeros((6, 12))
196 | ])*self.w
197 | self.Lx = self.Lx[:,0]*self.w
198 | self.Lu = np.zeros(4)
199 | self.Luu = np.zeros((4,4))
200 | self.Lxu = np.zeros((12, 4))
201 | return self.Lx, self.Lxx
202 |
203 | def num_diff(f, x, d_out = 1, inc = 0.001):
204 | J = np.zeros((d_out, len(x)))
205 | u = np.zeros(4)
206 | for i in range(len(x)):
207 | x_ip = x.copy()
208 | x_ip[i] += inc
209 | val_p = f(x_ip, u)
210 | x_im = x.copy()
211 | x_im[i] -= inc
212 | val_m = f(x_im, u)
213 |
214 |
215 | diff = (val_p - val_m)/(2*inc)
216 | J[:,i] = diff
217 | return J.T
218 |
219 | def num_calc_diff(x, d_out = 1, inc = 0.001):
220 | J = np.zeros((d_out, len(x)))
221 | u = np.zeros(4)
222 | f = cost_collision.calcDiff
223 | for i in range(len(x)):
224 | x_ip = x.copy()
225 | x_ip[i] += inc
226 | val_p = f(x_ip, u)[0].flatten()
227 | x_im = x.copy()
228 | x_im[i] -= inc
229 | val_m = f(x_im, u)[0].flatten()
230 |
231 | diff = (val_p - val_m)/(2*inc)
232 | J[:,i] = diff
233 | return J
234 |
235 |
236 | class RobotSphereCollisionCost():
237 | def __init__(self, activation=None, nu=None, rmodel = None, rdata = None, ee_id = 0, r_body = 0., r_obs = 0., pos_obs = np.array([0,0,0]), w= 1.):
238 | self.activation = activation
239 | self.r_body = r_body
240 | self.r_obs = r_obs
241 | self.pos_obs = pos_obs
242 | self.w = w
243 | self.rmodel = rmodel
244 | self.rdata = rdata
245 | self.ee_id = ee_id
246 |
247 | def calc(self, x, u):
248 | #calculate the position of the end-effector
249 | pos_body, ori = computePose(self.rmodel, self.rdata, self.ee_id, x[:7])
250 | self.r = pos_body-self.pos_obs
251 | self.activation.calc(self.r)
252 | self.L = self.activation.a*self.w
253 | return self.L
254 |
255 | def calcDiff(self, x, u, recalc=True):
256 | if recalc:
257 | self.calc( x, u)
258 |
259 | #Calculate the Jacobian at p1
260 | #self.J = np.hstack([np.eye(3), np.zeros((3,3))])
261 | self.J = computeJacobian(self.rmodel, self.rdata, self.ee_id, x[:7])[:3]
262 | ###Compute the cost derivatives###
263 | self.activation.calcDiff(self.r, recalc)
264 | self.Rx = np.hstack([self.J, np.zeros((3, 7))])
265 | self.Lx = np.vstack([self.J.T.dot(self.activation.Ar), np.zeros((7, 1))])
266 | self.Lxx = np.vstack([
267 | np.hstack([self.J.T.dot(self.activation.Arr).dot(self.J),
268 | np.zeros((7, 7))]),
269 | np.zeros((7, 14))
270 | ])*self.w
271 | self.Lx = self.Lx[:,0]*self.w
272 | self.Lu = np.zeros(7)
273 | self.Luu = np.zeros((7,7))
274 | self.Lxu = np.zeros((14, 7))
275 | return self.Lx, self.Lxx
276 |
277 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/finger.obj:
--------------------------------------------------------------------------------
1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net)
2 | # (assimp v3.0.1262)
3 |
4 | mtllib finger.stl.obj.mtl
5 |
6 | # 96 vertex positions
7 | v 0.01036 0.0264034 0.000154629
8 | v 0.0104486 0.0025833 0.000146801
9 | v -0.0103872 0.00253418 0.000131696
10 | v -0.0103872 0.00253418 0.000131696
11 | v -0.0104795 0.0161016 0.0189882
12 | v -0.0104013 0.0263094 0.00016651
13 | v -0.0104013 0.0263094 0.00016651
14 | v -0.0104795 0.0161016 0.0189882
15 | v -0.0103889 0.0252203 0.0191876
16 | v 0.0104486 0.0025833 0.000146801
17 | v -0.0087304 -2.35252e-05 0.0361648
18 | v -0.0103872 0.00253418 0.000131696
19 | v 0.0104005 0.0252534 0.0190366
20 | v -0.0103889 0.0252203 0.0191876
21 | v 0.00583983 0.0142743 0.0538035
22 | v -0.0103872 0.00253418 0.000131696
23 | v -0.0104013 0.0263094 0.00016651
24 | v 0.01036 0.0264034 0.000154629
25 | v 0.00861608 0.0139887 0.0513279
26 | v 0.0104948 0.0151026 0.0184356
27 | v 0.0104005 0.0252534 0.0190366
28 | v 0.0104948 0.0151026 0.0184356
29 | v 0.00869277 -0.000132643 0.0501662
30 | v 0.0104486 0.0025833 0.000146801
31 | v 0.00861608 0.0139887 0.0513279
32 | v 0.00869277 -0.000132643 0.0501662
33 | v 0.0104948 0.0151026 0.0184356
34 | v -0.00862294 -5.68019e-05 0.0509528
35 | v -0.0103872 0.00253418 0.000131696
36 | v -0.0087304 -2.35252e-05 0.0361648
37 | v 0.0104486 0.0025833 0.000146801
38 | v 0.00869277 -0.000132643 0.0501662
39 | v -0.0087304 -2.35252e-05 0.0361648
40 | v 0.00869277 -0.000132643 0.0501662
41 | v -0.00548142 -9.11208e-05 0.0537247
42 | v -0.0087304 -2.35252e-05 0.0361648
43 | v 0.0104005 0.0252534 0.0190366
44 | v 0.0104948 0.0151026 0.0184356
45 | v 0.0104486 0.0025833 0.000146801
46 | v 0.0104005 0.0252534 0.0190366
47 | v 0.0104486 0.0025833 0.000146801
48 | v 0.01036 0.0264034 0.000154629
49 | v 0.0104005 0.0252534 0.0190366
50 | v 0.01036 0.0264034 0.000154629
51 | v -0.0103889 0.0252203 0.0191876
52 | v -0.0103889 0.0252203 0.0191876
53 | v -0.00527792 0.0142931 0.053849
54 | v 0.00583983 0.0142743 0.0538035
55 | v -0.00777838 0.0142182 0.0523659
56 | v -0.00527792 0.0142931 0.053849
57 | v -0.0103889 0.0252203 0.0191876
58 | v -0.00862294 -5.68019e-05 0.0509528
59 | v -0.00548142 -9.11208e-05 0.0537247
60 | v -0.00777838 0.0142182 0.0523659
61 | v -0.0103872 0.00253418 0.000131696
62 | v -0.00862294 -5.68019e-05 0.0509528
63 | v -0.0104795 0.0161016 0.0189882
64 | v 0.0104005 0.0252534 0.0190366
65 | v 0.00583983 0.0142743 0.0538035
66 | v 0.00861608 0.0139887 0.0513279
67 | v 0.01036 0.0264034 0.000154629
68 | v -0.0104013 0.0263094 0.00016651
69 | v -0.0103889 0.0252203 0.0191876
70 | v -0.0104795 0.0161016 0.0189882
71 | v -0.00884117 0.0139176 0.0505894
72 | v -0.0103889 0.0252203 0.0191876
73 | v -0.00884117 0.0139176 0.0505894
74 | v -0.00777838 0.0142182 0.0523659
75 | v -0.0103889 0.0252203 0.0191876
76 | v 0.00583983 0.0142743 0.0538035
77 | v -0.00548142 -9.11208e-05 0.0537247
78 | v 0.00613802 -2.06026e-05 0.0535776
79 | v -0.00527792 0.0142931 0.053849
80 | v -0.00548142 -9.11208e-05 0.0537247
81 | v 0.00583983 0.0142743 0.0538035
82 | v 0.00869277 -0.000132643 0.0501662
83 | v 0.00613802 -2.06026e-05 0.0535776
84 | v -0.00548142 -9.11208e-05 0.0537247
85 | v 0.00613802 -2.06026e-05 0.0535776
86 | v 0.00869277 -0.000132643 0.0501662
87 | v 0.00861608 0.0139887 0.0513279
88 | v -0.00548142 -9.11208e-05 0.0537247
89 | v -0.00862294 -5.68019e-05 0.0509528
90 | v -0.0087304 -2.35252e-05 0.0361648
91 | v -0.00777838 0.0142182 0.0523659
92 | v -0.00548142 -9.11208e-05 0.0537247
93 | v -0.00527792 0.0142931 0.053849
94 | v -0.00862294 -5.68019e-05 0.0509528
95 | v -0.00884117 0.0139176 0.0505894
96 | v -0.0104795 0.0161016 0.0189882
97 | v 0.00613802 -2.06026e-05 0.0535776
98 | v 0.00861608 0.0139887 0.0513279
99 | v 0.00583983 0.0142743 0.0538035
100 | v -0.00777838 0.0142182 0.0523659
101 | v -0.00884117 0.0139176 0.0505894
102 | v -0.00862294 -5.68019e-05 0.0509528
103 |
104 | # 0 UV coordinates
105 |
106 | # 96 vertex normals
107 | vn 0.000724164 0.000331308 -1
108 | vn 0.000724164 0.000331308 -1
109 | vn 0.000724164 0.000331308 -1
110 | vn -0.99999 -0.000585782 -0.00447174
111 | vn -0.99999 -0.000585782 -0.00447174
112 | vn -0.99999 -0.000585782 -0.00447174
113 | vn -0.99995 0.00990875 0.00122006
114 | vn -0.99995 0.00990875 0.00122006
115 | vn -0.99995 0.00990875 0.00122006
116 | vn 0.00240304 -0.997479 -0.0709137
117 | vn 0.00240304 -0.997479 -0.0709137
118 | vn 0.00240304 -0.997479 -0.0709137
119 | vn 0.000668274 0.953556 0.301214
120 | vn 0.000668274 0.953556 0.301214
121 | vn 0.000668274 0.953556 0.301214
122 | vn -0.0005789 0.00146393 -0.999999
123 | vn -0.0005789 0.00146393 -0.999999
124 | vn -0.0005789 0.00146393 -0.999999
125 | vn 0.998344 0.00589157 0.0572229
126 | vn 0.998344 0.00589157 0.0572229
127 | vn 0.998344 0.00589157 0.0572229
128 | vn 0.998185 -0.050838 0.0322792
129 | vn 0.998185 -0.050838 0.0322792
130 | vn 0.998185 -0.050838 0.0322792
131 | vn 0.998371 0.000729252 0.0570496
132 | vn 0.998371 0.000729252 0.0570496
133 | vn 0.998371 0.000729252 0.0570496
134 | vn -0.871287 -0.490747 0.00522707
135 | vn -0.871287 -0.490747 0.00522707
136 | vn -0.871287 -0.490747 0.00522707
137 | vn 0.0362712 -0.99794 -0.0529128
138 | vn 0.0362712 -0.99794 -0.0529128
139 | vn 0.0362712 -0.99794 -0.0529128
140 | vn -0.00372285 -0.999988 -0.00316058
141 | vn -0.00372285 -0.999988 -0.00316058
142 | vn -0.00372285 -0.999988 -0.00316058
143 | vn 0.999909 0.00984192 -0.00926313
144 | vn 0.999909 0.00984192 -0.00926313
145 | vn 0.999909 0.00984192 -0.00926313
146 | vn 0.999991 0.00372285 -0.00191858
147 | vn 0.999991 0.00372285 -0.00191858
148 | vn 0.999991 0.00372285 -0.00191858
149 | vn -0.0011495 0.99815 0.0607926
150 | vn -0.0011495 0.99815 0.0607926
151 | vn -0.0011495 0.99815 0.0607926
152 | vn 0.00284562 0.953846 0.300284
153 | vn 0.00284562 0.953846 0.300284
154 | vn 0.00284562 0.953846 0.300284
155 | vn -0.218924 0.920873 0.32259
156 | vn -0.218924 0.920873 0.32259
157 | vn -0.218924 0.920873 0.32259
158 | vn -0.661425 -0.0350314 0.749193
159 | vn -0.661425 -0.0350314 0.749193
160 | vn -0.661425 -0.0350314 0.749193
161 | vn -0.998169 -0.0513123 0.0320353
162 | vn -0.998169 -0.0513123 0.0320353
163 | vn -0.998169 -0.0513123 0.0320353
164 | vn 0.377717 0.867563 0.323518
165 | vn 0.377717 0.867563 0.323518
166 | vn 0.377717 0.867563 0.323518
167 | vn -0.00448618 0.998355 0.0571685
168 | vn -0.00448618 0.998355 0.0571685
169 | vn -0.00448618 0.998355 0.0571685
170 | vn -0.998589 0.0087761 0.0523757
171 | vn -0.998589 0.0087761 0.0523757
172 | vn -0.998589 0.0087761 0.0523757
173 | vn -0.665951 0.690851 0.281485
174 | vn -0.665951 0.690851 0.281485
175 | vn -0.665951 0.690851 0.281485
176 | vn 0.0127505 -0.0155288 0.999798
177 | vn 0.0127505 -0.0155288 0.999798
178 | vn 0.0127505 -0.0155288 0.999798
179 | vn 0.00408502 -0.00870048 0.999954
180 | vn 0.00408502 -0.00870048 0.999954
181 | vn 0.00408502 -0.00870048 0.999954
182 | vn 0.006542 -0.999267 0.0377181
183 | vn 0.006542 -0.999267 0.0377181
184 | vn 0.006542 -0.999267 0.0377181
185 | vn 0.798906 -0.0450007 0.59977
186 | vn 0.798906 -0.0450007 0.59977
187 | vn 0.798906 -0.0450007 0.59977
188 | vn -0.00899609 -0.999957 -0.00218479
189 | vn -0.00899609 -0.999957 -0.00218479
190 | vn -0.00899609 -0.999957 -0.00218479
191 | vn -0.510144 -0.000216662 0.860089
192 | vn -0.510144 -0.000216662 0.860089
193 | vn -0.510144 -0.000216662 0.860089
194 | vn -0.998608 -0.0142745 0.0507836
195 | vn -0.998608 -0.0142745 0.0507836
196 | vn -0.998608 -0.0142745 0.0507836
197 | vn 0.665648 0.00209596 0.746263
198 | vn 0.665648 0.00209596 0.746263
199 | vn 0.665648 0.00209596 0.746263
200 | vn -0.858159 -4.90786e-05 0.513384
201 | vn -0.858159 -4.90786e-05 0.513384
202 | vn -0.858159 -4.90786e-05 0.513384
203 |
204 | # Mesh '' with 32 faces
205 | g
206 | usemtl DefaultMaterial
207 | f 1//1 2//2 3//3
208 | f 4//4 5//5 6//6
209 | f 7//7 8//8 9//9
210 | f 10//10 11//11 12//12
211 | f 13//13 14//14 15//15
212 | f 16//16 17//17 18//18
213 | f 19//19 20//20 21//21
214 | f 22//22 23//23 24//24
215 | f 25//25 26//26 27//27
216 | f 28//28 29//29 30//30
217 | f 31//31 32//32 33//33
218 | f 34//34 35//35 36//36
219 | f 37//37 38//38 39//39
220 | f 40//40 41//41 42//42
221 | f 43//43 44//44 45//45
222 | f 46//46 47//47 48//48
223 | f 49//49 50//50 51//51
224 | f 52//52 53//53 54//54
225 | f 55//55 56//56 57//57
226 | f 58//58 59//59 60//60
227 | f 61//61 62//62 63//63
228 | f 64//64 65//65 66//66
229 | f 67//67 68//68 69//69
230 | f 70//70 71//71 72//72
231 | f 73//73 74//74 75//75
232 | f 76//76 77//77 78//78
233 | f 79//79 80//80 81//81
234 | f 82//82 83//83 84//84
235 | f 85//85 86//86 87//87
236 | f 88//88 89//89 90//90
237 | f 91//91 92//92 93//93
238 | f 94//94 95//95 96//96
239 |
240 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/finger.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/finger.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/hand.obj:
--------------------------------------------------------------------------------
1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net)
2 | # (assimp v3.1.187496374)
3 |
4 | mtllib hand.obj.mtl
5 |
6 | # 102 vertex positions
7 | v 0.0167349 0.0901289 0.0644319
8 | v 0.0171343 -0.0892712 0.0636144
9 | v 0.0315898 -0.000558518 0.00621423
10 | v -0.0170537 -0.0955248 0.0221212
11 | v -0.0175104 -0.0979076 0.0516662
12 | v -0.0315975 -0.0019865 0.00115293
13 | v -0.025981 0.0757697 -0.00508223
14 | v -0.0316359 -0.000578733 0.00597307
15 | v 0.0170332 -0.0968023 0.030322
16 | v 0.0316158 -0.00186086 0.00117971
17 | v 0.0175851 -0.0974071 0.0519846
18 | v 0.0185559 0.0946019 0.0566173
19 | v -0.0168042 -0.0919061 0.00629548
20 | v -0.0168122 -0.0875329 -0.00120645
21 | v 0.0162301 -0.0995375 0.0515841
22 | v -4.10844e-05 -0.10399 0.00569088
23 | v -1.72733e-05 -0.103743 -0.0091993
24 | v -0.00361677 -0.101158 -0.0126103
25 | v -0.00342956 -0.102598 -0.0089643
26 | v -0.0252704 0.0820651 0.00519704
27 | v -0.0280109 0.0417843 -0.0063964
28 | v -0.000112569 0.0928085 0.0659622
29 | v -0.0125667 0.0937125 0.0651685
30 | v -0.014422 0.0912765 0.0656897
31 | v -0.0172547 -0.0907003 0.063082
32 | v -0.0154717 -0.0907063 0.0651901
33 | v -0.0167672 0.0902522 0.0643322
34 | v 0.0169555 -0.0916029 0.00556283
35 | v 0.0176072 -0.0938476 0.0584485
36 | v 0.0125057 -0.100188 0.0568941
37 | v 0.0155509 0.1002 0.0546726
38 | v 0.0192688 0.0984936 0.0449417
39 | v 0.0162184 0.100285 0.0458698
40 | v 0.00191744 -0.0455199 -0.0256594
41 | v 0.0280151 0.0420423 -0.0063496
42 | v 0.00381034 -0.0829662 -0.0244493
43 | v 0.0206709 0.0936918 0.0433083
44 | v 0.0254479 0.0811805 0.0035006
45 | v -0.00363682 -0.0835859 -0.0246063
46 | v -0.0165327 -0.0822363 -0.00511811
47 | v -0.0140171 -0.100091 0.0560269
48 | v -0.0160131 -0.0996286 0.0493014
49 | v -0.00321509 -0.087766 -0.0238844
50 | v -0.0129879 -0.0924958 0.0657119
51 | v -0.000505639 -0.0929453 0.0659531
52 | v 0.0135396 0.0926721 0.0656879
53 | v 0.0134248 -0.091927 0.0657313
54 | v -0.0192292 0.0972606 0.0505884
55 | v 0.0165582 -0.0851322 -0.00387274
56 | v 0.00349477 -0.102716 -0.00802934
57 | v 0.0238692 0.0904014 0.00436824
58 | v 0.0227078 0.0937356 0.01434
59 | v 0.0260343 0.0752976 -0.00503085
60 | v 0.0248167 0.0743775 -0.0071557
61 | v -0.00150015 -0.0458698 -0.0259248
62 | v -0.000833771 0.08026 -0.00732172
63 | v 0.0229591 0.0779014 -0.00739904
64 | v -0.0227611 0.0801744 -0.00690954
65 | v 3.18097e-05 -0.0994514 -0.0171612
66 | v -0.0166321 -0.098554 0.0547339
67 | v 0.00360213 -0.101195 -0.0126276
68 | v -0.00161673 0.0974711 0.0623073
69 | v 0.0157184 -0.0917796 0.0649125
70 | v -0.0184487 0.0920155 0.0583932
71 | v -0.000626994 -0.0879747 -0.0249616
72 | v 0.003076 -0.0879729 -0.0239787
73 | v 0.019802 0.0970111 0.020914
74 | v 0.00994155 0.100035 0.0583408
75 | v 0.0171295 0.0990672 0.0543052
76 | v -0.000314131 0.0946153 0.00757466
77 | v -1.59471e-05 0.0984961 0.0270818
78 | v -0.000337323 0.0865616 -0.00416328
79 | v 0.0223143 0.08825 -0.0019332
80 | v 0.022377 0.0833552 -0.00552611
81 | v -0.021325 0.0909269 0.000979802
82 | v -0.0227328 0.0854345 -0.00412033
83 | v -0.0244953 0.0858923 -0.00183273
84 | v 0.00184279 -0.0840023 -0.0255249
85 | v -0.0246502 0.076668 -0.00693213
86 | v 0.0131387 -0.0940629 0.0646832
87 | v -0.00932387 -0.0999826 0.0584892
88 | v -0.0190668 0.0976971 0.0246275
89 | v -0.00449439 0.100426 0.0581191
90 | v -0.0146231 0.100168 0.0557349
91 | v 0.0158049 -0.0994163 0.0549703
92 | v 0.0213426 0.0943331 0.00894201
93 | v -0.000173406 0.090995 0.000576784
94 | v 0.0214829 0.0917947 0.00268413
95 | v 0.0245152 0.0817026 -0.00505142
96 | v 0.0247294 0.0857401 -0.00123169
97 | v -0.0206576 0.0946679 0.00953798
98 | v 0.00119059 -0.100301 0.0586231
99 | v 0.000469602 -0.0966559 0.0630784
100 | v 0.0153969 0.0929056 0.0645563
101 | v -0.000867829 -0.0825485 -0.0258702
102 | v -0.0235558 0.0911663 0.00460792
103 | v -0.014617 -0.0934417 0.0645
104 | v -0.0230261 0.0928969 0.00987686
105 | v -0.0218221 0.0954417 0.0217564
106 | v -0.0181926 0.0995179 0.0438142
107 | v -0.0160455 0.0932093 0.0636169
108 | v -0.0169948 0.0993629 0.0541148
109 |
110 | # 0 UV coordinates
111 |
112 | # 200 vertex normals
113 | vn 0.96934 0.00103808 0.245721
114 | vn -0.986759 -0.159734 -0.0281361
115 | vn -0.997162 0.0697563 -0.0283202
116 | vn 0.986571 -0.160626 -0.0296212
117 | vn 0.99137 -0.125585 0.0376046
118 | vn 0.972565 0.0101093 0.232413
119 | vn -0.981257 -0.166939 -0.096261
120 | vn -0.991092 -0.129756 0.0299978
121 | vn -0.984842 -0.16507 -0.0532708
122 | vn 0.303856 -0.952595 -0.0153173
123 | vn -0.745916 -0.631674 -0.211181
124 | vn -0.996795 0.0769815 0.0217643
125 | vn -0.992516 0.0633187 -0.104418
126 | vn -0.044276 0.241169 0.969473
127 | vn -0.763533 -0.00240486 0.645764
128 | vn 0.98594 -0.162885 -0.0372967
129 | vn 0.986849 -0.0650596 0.147976
130 | vn 0.991549 -0.115042 0.0599665
131 | vn 0.203398 -0.97883 0.0228453
132 | vn 0.516573 0.854926 0.0474658
133 | vn 0.587131 0.00352045 -0.809484
134 | vn 0.99247 0.0777169 0.094674
135 | vn -0.832137 -0.113798 -0.542768
136 | vn -0.990867 -0.10289 0.0871544
137 | vn -0.24667 -0.969077 0.00656585
138 | vn -0.796752 -0.375025 -0.473859
139 | vn -0.0190316 -2.76701e-05 0.999819
140 | vn -0.0193168 -7.80837e-06 0.999813
141 | vn 0.0200855 -9.11851e-05 0.999798
142 | vn 0.0159035 0.000225346 0.999874
143 | vn -0.991348 0.0773534 0.106043
144 | vn 0.846324 -0.531423 -0.0364002
145 | vn 0.838336 -0.119317 -0.531936
146 | vn 0.713391 -0.685355 -0.146159
147 | vn 0.752915 -0.0507794 -0.656156
148 | vn 0.962439 0.234445 0.136915
149 | vn 0.98673 0.0540529 0.153111
150 | vn 0.984464 0.162241 0.0671482
151 | vn 0.963684 0.258201 0.0681518
152 | vn 0.988283 0.13579 0.0696969
153 | vn 0.992401 0.0632947 -0.105521
154 | vn 0.997132 0.0735636 0.0178061
155 | vn 0.997125 0.0744552 -0.0141125
156 | vn 0.525671 0.0308018 -0.85013
157 | vn 0.0112429 0.145846 -0.989244
158 | vn -0.0191625 0.145985 -0.989101
159 | vn 0.295801 0.090268 -0.950975
160 | vn -0.753521 -0.0512984 -0.655419
161 | vn -0.557064 -0.525003 -0.643468
162 | vn -0.826525 -0.175737 -0.534765
163 | vn -0.815256 -0.315571 -0.485564
164 | vn -0.947514 -0.156066 -0.279037
165 | vn -0.93384 -0.292644 0.205677
166 | vn 0.168928 -0.868066 -0.466825
167 | vn -0.713161 -0.698683 0.0569532
168 | vn -0.304509 -0.95237 -0.0162868
169 | vn 0.0219418 0.621147 0.783387
170 | vn -0.00551132 0.61581 0.787876
171 | vn 0.36612 0.000413018 0.930568
172 | vn 0.336199 1.25396e-05 0.941791
173 | vn -0.969753 -7.34049e-05 0.244089
174 | vn -0.962126 0.000708185 0.272604
175 | vn -0.988229 0.0614231 0.14011
176 | vn -0.360462 -0.538401 -0.761703
177 | vn 0.829279 -0.171621 -0.53183
178 | vn 0.677571 -0.00184298 0.735455
179 | vn 0.496348 0.867083 -0.0424839
180 | vn 0.0755333 0.722595 0.687133
181 | vn 0.904977 0.383975 0.183246
182 | vn 0.0127058 0.980664 -0.195286
183 | vn 0.851811 0.0713165 -0.518972
184 | vn 0.0351954 0.591653 -0.805424
185 | vn -0.538898 0.643215 -0.543933
186 | vn 0.0777842 -0.00363624 -0.996964
187 | vn -0.19873 0.11334 -0.973479
188 | vn -0.793118 -0.605993 -0.0611343
189 | vn 0.283571 -0.958905 -0.00945907
190 | vn -0.178475 -0.865689 -0.467685
191 | vn -0.987868 0.153186 -0.0255232
192 | vn -0.206645 -0.00149425 0.978415
193 | vn -0.501132 0.000514844 0.865371
194 | vn 0.0467749 -0.445101 0.894258
195 | vn -0.0152497 -0.997215 0.0730019
196 | vn -0.0160242 0.980702 -0.194854
197 | vn -0.10472 0.777309 0.620342
198 | vn -0.0162484 0.999109 -0.0389505
199 | vn 0.812943 -0.46373 -0.352249
200 | vn 0.734943 -0.648754 -0.197427
201 | vn 0.355505 -0.889833 -0.28603
202 | vn 0.77975 -0.493229 0.385637
203 | vn 0.264343 -0.962054 0.0676396
204 | vn 0.542077 -0.664141 0.514848
205 | vn 0.470493 -0.711825 0.52148
206 | vn 0.584351 -0.807368 -0.0817933
207 | vn 0.826668 -0.548978 0.123462
208 | vn 0.873315 -0.427986 0.232701
209 | vn 0.0134981 0.821385 0.570215
210 | vn 0.481279 -0.00399798 -0.876558
211 | vn 0.0262961 0.976262 -0.21499
212 | vn 0.035393 0.991525 -0.125004
213 | vn -0.0252692 0.468427 -0.883141
214 | vn 0.029533 0.795427 -0.605329
215 | vn 0.0095104 0.447459 -0.894254
216 | vn 0.0293922 0.327461 -0.944407
217 | vn 0.0628955 0.135567 -0.98877
218 | vn -0.512379 0.0361849 -0.857997
219 | vn -0.82575 0.0709576 -0.559555
220 | vn 0.671124 0.490386 -0.555981
221 | vn -0.583104 0.00463288 -0.812385
222 | vn -0.0162994 0.980639 -0.195146
223 | vn -0.157039 0.79873 0.580834
224 | vn 0.973711 -0.16665 -0.155286
225 | vn 0.0757908 -0.76565 0.638776
226 | vn -0.0242742 0.999674 0.00785851
227 | vn 0.0265568 0.999088 0.0334344
228 | vn 0.763663 0.633795 -0.122975
229 | vn 0.59401 0.799769 0.086723
230 | vn 0.277533 0.743174 0.608825
231 | vn 0.344643 0.752738 0.560898
232 | vn 0.784743 0.463667 0.411327
233 | vn -0.0358679 0.684935 -0.72772
234 | vn 0.932801 0.220325 -0.285201
235 | vn 0.624229 0.728724 -0.281602
236 | vn -0.0116186 0.888023 -0.459652
237 | vn 0.0118944 0.888215 -0.459274
238 | vn 0.12674 0.000708284 -0.991936
239 | vn -0.412606 -0.00846907 -0.91087
240 | vn -0.798626 -0.520306 -0.302452
241 | vn -0.541514 -0.834316 0.103337
242 | vn -0.319295 -0.94743 -0.0206758
243 | vn -0.723656 -0.67023 -0.164661
244 | vn 0.024012 -0.608318 0.79333
245 | vn -0.0315557 -0.776033 0.629902
246 | vn 0.0205919 -0.997403 0.0690183
247 | vn -0.0311286 -0.997047 0.0701972
248 | vn -0.982775 0.184638 -0.00783846
249 | vn 0.216124 -0.540324 -0.813229
250 | vn 0.246085 -0.284389 -0.926588
251 | vn 0.556314 -0.528433 -0.641306
252 | vn 0.792539 -0.37877 -0.477928
253 | vn 0.880372 -0.312129 0.3571
254 | vn 0.325605 -0.451322 0.830837
255 | vn 0.101105 -0.786037 0.609856
256 | vn 0.53596 -0.156588 -0.829594
257 | vn 0.580736 0.650676 0.489251
258 | vn 0.491562 0.198884 0.847828
259 | vn 0.828341 0.380744 0.410957
260 | vn 0.035746 0.926351 -0.374962
261 | vn 0.656777 0.749488 -0.083134
262 | vn 0.720531 0.692906 -0.0267625
263 | vn 0.572813 0.248018 -0.781263
264 | vn 0.990951 0.132224 -0.0230681
265 | vn 0.985773 0.167665 0.011866
266 | vn -0.0153618 0.730516 -0.682723
267 | vn 0.0128537 0.730055 -0.683268
268 | vn -0.0185446 0.449169 -0.893255
269 | vn -0.619205 -0.779582 -0.0940023
270 | vn -0.320427 -0.88655 -0.333701
271 | vn -0.0989435 -0.715546 0.691523
272 | vn -0.0841099 -0.712852 0.696252
273 | vn -0.60803 -0.644894 0.463045
274 | vn -0.423761 -0.343804 0.837989
275 | vn -0.605208 0.788757 -0.107633
276 | vn -0.0174541 0.996656 -0.0798282
277 | vn -0.0193589 0.998627 -0.0486783
278 | vn -0.634294 0.640925 -0.432304
279 | vn 0.0379495 -0.163387 -0.985832
280 | vn 0.538782 0.502971 -0.675821
281 | vn 0.813644 0.191216 -0.549017
282 | vn 0.652479 0.653287 -0.384041
283 | vn 0.717815 0.58535 -0.37697
284 | vn -0.286085 -0.763532 0.578942
285 | vn -0.0374327 -0.618147 0.785171
286 | vn -0.709079 -0.37258 0.598657
287 | vn -0.878489 0.351219 0.323886
288 | vn -0.890816 0.299973 0.341268
289 | vn -0.868133 0.372523 0.327981
290 | vn -0.605163 0.772391 -0.192849
291 | vn -0.984555 0.156402 0.0786764
292 | vn -0.982298 0.176886 0.06166
293 | vn -0.981595 0.187328 0.0371542
294 | vn -0.44589 0.832394 -0.329094
295 | vn -0.363723 0.729882 0.578773
296 | vn -0.575721 0.322294 0.751446
297 | vn 0.442358 0.333164 -0.832659
298 | vn -0.70625 0.351573 -0.614497
299 | vn -0.654528 0.356931 -0.666478
300 | vn -0.838844 0.274101 -0.47033
301 | vn -0.354702 0.933343 0.0552966
302 | vn -0.561743 0.820557 -0.105505
303 | vn -0.851741 0.52214 0.0436597
304 | vn -0.759618 0.642942 0.0980105
305 | vn -0.0360333 0.916716 -0.39791
306 | vn -0.560808 0.66826 0.488798
307 | vn -0.414234 0.480837 0.772791
308 | vn -0.390321 -0.168871 -0.905059
309 | vn -0.752648 -0.506645 0.420514
310 | vn -0.029743 0.995213 -0.0930902
311 | vn -0.346496 -0.193469 -0.917884
312 | vn -0.602128 0.796943 -0.0481963
313 |
314 | # Mesh '' with 200 faces
315 | g
316 | usemtl DefaultMaterial
317 | f 1//1 2//1 3//1
318 | f 4//2 5//2 6//2
319 | f 7//3 6//3 8//3
320 | f 9//4 10//4 11//4
321 | f 10//5 3//5 11//5
322 | f 12//6 1//6 3//6
323 | f 13//7 6//7 14//7
324 | f 8//8 6//8 5//8
325 | f 4//9 6//9 13//9
326 | f 15//10 16//10 17//10
327 | f 18//11 19//11 13//11
328 | f 20//12 7//12 8//12
329 | f 21//13 6//13 7//13
330 | f 22//14 23//14 24//14
331 | f 25//15 26//15 27//15
332 | f 28//16 10//16 9//16
333 | f 2//17 29//17 3//17
334 | f 29//18 11//18 3//18
335 | f 30//19 16//19 15//19
336 | f 31//20 32//20 33//20
337 | f 34//21 35//21 36//21
338 | f 37//22 3//22 38//22
339 | f 6//23 39//23 40//23
340 | f 25//24 8//24 5//24
341 | f 41//25 42//25 16//25
342 | f 18//26 14//26 43//26
343 | f 22//27 24//27 44//27
344 | f 44//28 45//28 22//28
345 | f 46//29 22//29 45//29
346 | f 45//30 47//30 46//30
347 | f 20//31 8//31 48//31
348 | f 9//32 11//32 15//32
349 | f 36//33 10//33 49//33
350 | f 50//34 28//34 9//34
351 | f 10//35 36//35 35//35
352 | f 12//36 37//36 32//36
353 | f 37//37 12//37 3//37
354 | f 37//38 38//38 51//38
355 | f 37//39 52//39 32//39
356 | f 52//40 37//40 51//40
357 | f 10//41 35//41 53//41
358 | f 10//42 53//42 38//42
359 | f 10//43 38//43 3//43
360 | f 54//44 35//44 34//44
361 | f 55//45 56//45 57//45
362 | f 56//46 55//46 58//46
363 | f 57//47 54//47 34//47
364 | f 39//48 6//48 21//48
365 | f 59//49 18//49 43//49
366 | f 40//50 39//50 43//50
367 | f 14//51 40//51 43//51
368 | f 14//52 6//52 40//52
369 | f 60//53 25//53 5//53
370 | f 59//54 61//54 17//54
371 | f 42//55 60//55 5//55
372 | f 16//56 42//56 17//56
373 | f 62//57 22//57 46//57
374 | f 22//58 62//58 23//58
375 | f 1//59 46//59 63//59
376 | f 47//60 63//60 46//60
377 | f 25//61 64//61 8//61
378 | f 64//62 25//62 27//62
379 | f 64//63 48//63 8//63
380 | f 65//64 59//64 43//64
381 | f 66//65 36//65 49//65
382 | f 63//66 2//66 1//66
383 | f 33//67 32//67 67//67
384 | f 68//68 62//68 46//68
385 | f 32//69 69//69 12//69
386 | f 70//70 71//70 67//70
387 | f 53//71 35//71 54//71
388 | f 72//72 73//72 74//72
389 | f 75//73 76//73 77//73
390 | f 78//74 55//74 34//74
391 | f 55//75 79//75 58//75
392 | f 4//76 42//76 5//76
393 | f 50//77 15//77 17//77
394 | f 59//78 17//78 18//78
395 | f 7//79 20//79 77//79
396 | f 24//80 26//80 44//80
397 | f 27//81 26//81 24//81
398 | f 45//82 80//82 47//82
399 | f 41//83 16//83 81//83
400 | f 82//84 71//84 70//84
401 | f 62//85 83//85 23//85
402 | f 33//86 84//86 83//86
403 | f 49//87 28//87 61//87
404 | f 61//88 28//88 50//88
405 | f 61//89 50//89 17//89
406 | f 63//90 85//90 29//90
407 | f 85//91 30//91 15//91
408 | f 80//92 85//92 63//92
409 | f 85//93 80//93 30//93
410 | f 9//94 15//94 50//94
411 | f 85//95 15//95 11//95
412 | f 11//96 29//96 85//96
413 | f 83//97 62//97 68//97
414 | f 34//98 36//98 78//98
415 | f 70//99 67//99 86//99
416 | f 71//100 33//100 67//100
417 | f 58//101 76//101 72//101
418 | f 73//102 87//102 88//102
419 | f 72//103 74//103 56//103
420 | f 56//104 74//104 57//104
421 | f 34//105 55//105 57//105
422 | f 21//106 79//106 55//106
423 | f 21//107 7//107 79//107
424 | f 89//108 73//108 90//108
425 | f 39//109 21//109 55//109
426 | f 91//110 82//110 70//110
427 | f 84//111 23//111 83//111
428 | f 28//112 49//112 10//112
429 | f 92//113 80//113 93//113
430 | f 31//114 33//114 68//114
431 | f 68//115 33//115 83//115
432 | f 51//116 86//116 52//116
433 | f 69//117 32//117 31//117
434 | f 46//118 94//118 68//118
435 | f 94//119 31//119 68//119
436 | f 94//120 12//120 69//120
437 | f 76//121 75//121 72//121
438 | f 90//122 53//122 89//122
439 | f 86//123 51//123 88//123
440 | f 75//124 70//124 87//124
441 | f 87//125 70//125 88//125
442 | f 95//126 55//126 78//126
443 | f 39//127 55//127 95//127
444 | f 14//128 18//128 13//128
445 | f 42//129 41//129 60//129
446 | f 19//130 17//130 42//130
447 | f 19//131 4//131 13//131
448 | f 45//132 93//132 80//132
449 | f 92//133 93//133 81//133
450 | f 16//134 30//134 92//134
451 | f 92//135 81//135 16//135
452 | f 96//136 77//136 20//136
453 | f 66//137 59//137 65//137
454 | f 65//138 78//138 66//138
455 | f 59//139 66//139 61//139
456 | f 49//140 61//140 66//140
457 | f 2//141 63//141 29//141
458 | f 80//142 63//142 47//142
459 | f 80//143 92//143 30//143
460 | f 78//144 36//144 66//144
461 | f 94//145 69//145 31//145
462 | f 94//146 46//146 1//146
463 | f 12//147 94//147 1//147
464 | f 70//148 86//148 88//148
465 | f 67//149 52//149 86//149
466 | f 52//150 67//150 32//150
467 | f 57//151 89//151 54//151
468 | f 53//152 90//152 38//152
469 | f 90//153 51//153 38//153
470 | f 75//154 87//154 72//154
471 | f 72//155 87//155 73//155
472 | f 56//156 58//156 72//156
473 | f 42//157 4//157 19//157
474 | f 19//158 18//158 17//158
475 | f 97//159 81//159 44//159
476 | f 81//160 93//160 44//160
477 | f 41//161 97//161 60//161
478 | f 26//162 97//162 44//162
479 | f 98//163 99//163 91//163
480 | f 100//164 33//164 71//164
481 | f 84//165 33//165 100//165
482 | f 96//166 75//166 77//166
483 | f 95//167 78//167 65//167
484 | f 73//168 89//168 74//168
485 | f 53//169 54//169 89//169
486 | f 73//170 88//170 51//170
487 | f 90//171 73//171 51//171
488 | f 41//172 81//172 97//172
489 | f 44//173 93//173 45//173
490 | f 25//174 97//174 26//174
491 | f 101//175 48//175 64//175
492 | f 64//176 27//176 101//176
493 | f 102//177 48//177 101//177
494 | f 96//178 98//178 91//178
495 | f 48//179 99//179 20//179
496 | f 99//180 98//180 20//180
497 | f 98//181 96//181 20//181
498 | f 96//182 91//182 75//182
499 | f 84//183 101//183 23//183
500 | f 27//184 24//184 101//184
501 | f 89//185 57//185 74//185
502 | f 77//186 76//186 79//186
503 | f 79//187 76//187 58//187
504 | f 79//188 7//188 77//188
505 | f 102//189 84//189 100//189
506 | f 91//190 99//190 82//190
507 | f 48//191 100//191 99//191
508 | f 100//192 48//192 102//192
509 | f 75//193 91//193 70//193
510 | f 84//194 102//194 101//194
511 | f 24//195 23//195 101//195
512 | f 43//196 95//196 65//196
513 | f 25//197 60//197 97//197
514 | f 82//198 100//198 71//198
515 | f 43//199 39//199 95//199
516 | f 100//200 82//200 99//200
517 |
518 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/hand.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/hand.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link0.obj:
--------------------------------------------------------------------------------
1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net)
2 | # (assimp v3.1.187496374)
3 |
4 | mtllib link0.obj.mtl
5 |
6 | # 102 vertex positions
7 | v -0.00122674 -0.0946137 -3.24928e-05
8 | v 0.0710809 0.00844602 0.00133993
9 | v 0.071567 -0.00127396 0.0013494
10 | v -0.00051723 0.0946704 -6.42576e-06
11 | v 0.0706646 -0.0115626 0.00133336
12 | v -0.135131 -0.0658707 0.00751985
13 | v -0.136305 -0.0659457 3.26395e-07
14 | v -0.131625 -0.0687992 -5.27309e-07
15 | v 0.0543353 -0.0469489 0.00104792
16 | v 0.0619449 -0.0359785 0.00118116
17 | v 0.0673593 -0.0245271 0.00127621
18 | v 0.0526517 -0.0163561 0.14
19 | v 0.0548547 0.00447165 0.14
20 | v -0.0549666 0.00345713 0.14
21 | v 0.0391015 0.0387489 0.14
22 | v 0.00777804 -0.0910717 0.00846191
23 | v 0.0395665 -0.0384696 0.140003
24 | v -0.029337 -0.0466451 0.140001
25 | v 0.000349224 -0.0550819 0.14
26 | v 0.0686186 0.0208178 0.00129774
27 | v 0.0635871 0.0329931 0.00120988
28 | v 0.0525783 0.048771 0.00101755
29 | v 0.05859 0.0412413 0.00112268
30 | v 0.0509475 0.0207886 0.14
31 | v -0.0218528 0.0506849 0.14
32 | v -0.0317059 0.0450099 0.14
33 | v 0.0317161 0.0450507 0.14
34 | v -0.0400853 0.0378337 0.14
35 | v -0.0477037 0.0275963 0.14
36 | v 0.00485769 0.0549582 0.14
37 | v 0.0474762 -0.0279259 0.14
38 | v -0.00886536 0.0543586 0.140002
39 | v 0.0276805 -0.0476799 0.14
40 | v -0.054766 -0.0054659 0.14
41 | v -0.0528838 -0.015585 0.14
42 | v -0.122615 0.00563594 0.0611487
43 | v -0.122711 -0.00614431 0.060957
44 | v 0.0143701 -0.0532489 0.139999
45 | v -0.071675 -0.0756205 0.0575204
46 | v -0.111235 -0.0474773 0.0590551
47 | v -0.137824 -0.0624363 0.00755269
48 | v -0.133699 0.0669321 0.00741502
49 | v -0.13583 0.0663151 8.33834e-07
50 | v -0.136293 0.0646171 0.00756638
51 | v -0.131612 0.0688472 7.4968e-07
52 | v 0.0031716 0.0933497 0.00863144
53 | v 0.00408648 0.0939818 0.000170711
54 | v 0.0462474 0.0300326 0.14
55 | v 0.0548768 -0.00456585 0.14
56 | v -0.129159 0.0686836 0.00744486
57 | v -0.0528609 0.0157105 0.14
58 | v 0.0200202 0.0514462 0.139999
59 | v 0.00648893 0.0735192 0.08108
60 | v 0.00826788 -0.0918247 0.00024289
61 | v -0.0464371 -0.0299037 0.14
62 | v -0.140043 0.0607227 -1.6486e-07
63 | v 0.00769447 0.0922715 0.000234291
64 | v -0.00159938 0.0936322 0.00849181
65 | v 0.00734398 0.0914466 0.00831775
66 | v -0.148808 -0.0420547 -7.21858e-08
67 | v -0.149244 -0.0371861 0.00781712
68 | v -0.152578 -0.0248176 1.68373e-07
69 | v 0.00907648 0.0737151 0.0784549
70 | v -0.121673 -0.0147367 0.0614178
71 | v 0.00678171 -0.0735316 0.0809757
72 | v -0.130951 -0.0681401 0.00766805
73 | v -0.0189172 -0.051755 0.14
74 | v -0.00908423 -0.0542971 0.14
75 | v -0.0658985 -0.0765477 0.0589793
76 | v -0.0376956 -0.0401377 0.14
77 | v -0.143472 -0.0548617 -4.38612e-08
78 | v -0.145452 -0.0485961 0.00768247
79 | v -0.1203 -0.0235449 0.0615099
80 | v -0.118609 -0.03105 0.06134
81 | v -0.114761 -0.0419663 0.0601784
82 | v -0.154079 -0.00554168 -3.75503e-07
83 | v -0.152725 -0.0137992 0.00819143
84 | v -0.153166 -0.00203576 0.00835078
85 | v -0.142117 0.0555469 0.00762839
86 | v 0.0535544 0.0128185 0.14
87 | v -0.109983 0.0486112 0.0588796
88 | v 0.00366946 -0.0932492 0.00847566
89 | v 0.00349566 -0.0942157 0.000177738
90 | v 0.00906588 -0.073708 0.0785962
91 | v -0.117004 -0.0366378 0.0608364
92 | v -0.151587 -0.0248333 0.00804774
93 | v -0.15241 0.0258275 -1.09161e-08
94 | v -0.153841 0.0105757 0
95 | v -0.15202 0.0217804 0.00810954
96 | v -0.121149 0.0181378 0.0615703
97 | v -0.113238 0.0447149 0.0596848
98 | v -0.000866581 -0.0937369 0.00817823
99 | v -0.141842 -0.0559592 0.00761703
100 | v -0.149634 0.0355816 0.00786878
101 | v -0.14929 0.0402499 3.01802e-07
102 | v -0.152949 0.0100689 0.00821985
103 | v -0.145377 0.0508924 -1.27878e-07
104 | v -0.11571 0.0395945 0.0607111
105 | v -0.0231112 -0.0759725 0.0689683
106 | v -0.117233 0.035881 0.060957
107 | v -0.146172 0.0467529 0.00770437
108 | v -0.119414 0.0287903 0.0611397
109 |
110 | # 0 UV coordinates
111 |
112 | # 200 vertex normals
113 | vn 0.0190098 -2.30831e-05 -0.999819
114 | vn 0.0188816 6.69136e-05 -0.999822
115 | vn 0.0191344 -0.000120273 -0.999817
116 | vn -0.518479 -0.850405 0.0893954
117 | vn 0.0222843 -0.00331509 -0.999746
118 | vn 0.0205047 -0.00139704 -0.999789
119 | vn 7.25974e-07 -5.81537e-06 1
120 | vn 9.85433e-07 -5.68784e-06 1
121 | vn 0.991829 0.0497189 0.11749
122 | vn 0.692904 -0.711464 0.11706
123 | vn 4.45103e-05 3.61862e-05 1
124 | vn 0.0193277 0.000437566 -0.999813
125 | vn 0.0199677 0.0010368 -0.9998
126 | vn 0.0232843 0.0046319 -0.999718
127 | vn 0.0210964 0.0022106 -0.999775
128 | vn 0.0197075 -0.000616531 -0.999806
129 | vn 6.42989e-05 2.18738e-05 1
130 | vn 0.91755 0.380023 0.116978
131 | vn 3.35708e-05 -1.87847e-05 1
132 | vn -7.63878e-06 1.53928e-05 1
133 | vn -8.50133e-06 1.24836e-05 1
134 | vn 8.62742e-06 2.71648e-06 1
135 | vn 4.31916e-06 1.10982e-05 1
136 | vn 0.000248513 -0.00151077 0.999999
137 | vn -0.000100683 -0.000160507 1
138 | vn 0.000441559 2.26903e-05 1
139 | vn -0.759017 -0.00436697 0.651056
140 | vn -0.758153 -0.0170388 0.651854
141 | vn -4.08147e-05 -6.51499e-05 1
142 | vn 3.63574e-05 2.96612e-05 1
143 | vn -0.488489 -0.714003 0.501575
144 | vn -0.779928 -0.612675 0.127831
145 | vn -0.6563 0.74377 0.126791
146 | vn -0.51319 0.854867 0.0764032
147 | vn 0.143998 0.98555 0.0891981
148 | vn 6.6605e-05 0.00021454 1
149 | vn 0.794096 -0.595678 0.120749
150 | vn 0.815277 -0.566949 0.117862
151 | vn 0.897326 -0.425249 0.118194
152 | vn 0.906239 -0.40539 0.119958
153 | vn 0.848855 0.515505 0.117047
154 | vn 0.992823 0.00242477 0.11957
155 | vn 0.000137166 -1.78568e-05 1
156 | vn -0.256878 0.90791 0.331229
157 | vn -4.88056e-05 2.05051e-05 1
158 | vn 1.50367e-05 -7.30032e-06 1
159 | vn -3.22116e-05 6.85885e-05 1
160 | vn -0.0415945 0.953296 0.29916
161 | vn 0.0339572 -0.0169327 -0.99928
162 | vn -9.01903e-05 -3.23136e-05 1
163 | vn -0.795774 0.599474 0.0858742
164 | vn 0.423964 0.898605 0.112979
165 | vn 0.047941 0.0640793 -0.996793
166 | vn 0.0548988 0.990257 0.12797
167 | vn 0.0494285 0.963916 0.261577
168 | vn 0.0344071 0.0175091 -0.999255
169 | vn 0.691328 0.715168 0.102958
170 | vn 0.691451 0.712649 0.118438
171 | vn -0.973898 -0.213048 0.0783147
172 | vn 7.42399e-05 3.32698e-05 1
173 | vn 0.884937 0.449942 0.120157
174 | vn 0.989315 -0.0869511 0.117026
175 | vn 0.962281 -0.245852 0.116503
176 | vn -0.192564 0.977601 0.0849446
177 | vn -0.191831 0.976828 0.0949088
178 | vn -0.123193 0.952732 0.277713
179 | vn -0.441485 0.766549 0.466362
180 | vn 0.467327 0.85467 0.22615
181 | vn 0.692847 -0.713157 0.106629
182 | vn -0.756883 -0.0565346 0.651101
183 | vn 0.123333 -0.943265 0.308285
184 | vn -0.493923 -0.720807 0.48629
185 | vn -0.477017 -0.8711 0.116787
186 | vn -0.44611 -0.794944 0.411157
187 | vn -6.11448e-05 -0.000331755 1
188 | vn -1.43156e-05 -8.03482e-05 1
189 | vn -0.228986 -0.885719 0.403816
190 | vn -0.608389 -0.519659 0.599848
191 | vn -0.920452 -0.383492 0.0755124
192 | vn -0.736949 -0.180808 0.651318
193 | vn -0.611742 -0.514007 0.601306
194 | vn 0.419302 0.901141 0.110141
195 | vn -0.991437 -0.0387918 0.124694
196 | vn -0.993276 -0.0773118 0.0861707
197 | vn -0.833082 0.535892 0.137092
198 | vn 8.17296e-05 2.2162e-05 1
199 | vn 0.943552 0.308615 0.120276
200 | vn 0.981039 0.152834 0.119183
201 | vn 0.973953 0.194242 0.116982
202 | vn 0.767761 0.629431 0.119836
203 | vn 0.775121 0.620511 0.118969
204 | vn 0.975511 -0.184098 0.120356
205 | vn -0.334118 0.859344 0.38716
206 | vn -0.186878 0.944088 0.271613
207 | vn -0.461627 0.746886 0.478605
208 | vn 0.418676 0.882839 0.21285
209 | vn 0.516842 0.832916 0.197804
210 | vn 0.465813 -0.878241 0.10822
211 | vn 0.444917 -0.890581 0.0944139
212 | vn 0.0505572 -0.0737673 -0.995993
213 | vn 0.458814 -0.864421 0.205587
214 | vn 0.516064 -0.83361 0.196908
215 | vn -0.387155 -0.789577 0.476109
216 | vn -0.244693 -0.878338 0.410666
217 | vn -0.208468 -0.972704 0.101918
218 | vn -0.193464 -0.977355 0.0857284
219 | vn -0.182203 -0.978184 0.0997879
220 | vn -0.519874 -0.667682 0.532852
221 | vn -0.000155222 -6.31553e-05 1
222 | vn -4.55781e-05 -2.14887e-05 1
223 | vn -0.773654 -0.387569 0.501247
224 | vn -0.836842 -0.541097 0.0831258
225 | vn -0.938711 -0.313668 0.142944
226 | vn 0.642112 0.752523 0.146293
227 | vn -0.975003 -0.187197 0.119691
228 | vn -0.98721 -0.103388 0.121358
229 | vn -0.991212 0.0930191 0.0940533
230 | vn -0.745994 0.128207 0.653495
231 | vn -0.357321 0.923722 0.138052
232 | vn -2.44058e-05 1.82076e-05 1
233 | vn -0.562279 0.66248 0.494937
234 | vn -0.609088 0.612822 0.50345
235 | vn 0.183429 0.950262 0.251704
236 | vn 0.21534 0.929691 0.298837
237 | vn 0.229004 0.927667 0.294942
238 | vn 0.604019 -0.779541 0.165763
239 | vn 0.230026 -0.929068 0.289689
240 | vn 0.372231 -0.889715 0.264293
241 | vn -0.171199 -0.978844 0.112047
242 | vn 0.0790207 -0.991596 0.102437
243 | vn -0.774998 -0.384377 0.501629
244 | vn -0.890148 -0.437507 0.127373
245 | vn -0.67661 -0.535887 0.505
246 | vn -0.844574 -0.525041 0.105012
247 | vn -0.695542 -0.313148 0.646653
248 | vn -0.714122 -0.263551 0.648514
249 | vn -0.974069 0.210719 0.082385
250 | vn -0.866667 0.0208868 0.498449
251 | vn -0.993956 0.0146296 0.108802
252 | vn -0.994723 0.0188777 0.100843
253 | vn -0.990466 0.0795982 0.112429
254 | vn -0.875827 0.475205 0.0843008
255 | vn -0.648594 0.437886 0.622561
256 | vn -0.631999 0.470323 0.615934
257 | vn -0.542549 0.633495 0.551656
258 | vn -0.569254 0.593159 0.569309
259 | vn -0.854631 -0.127937 0.503228
260 | vn 0.187376 -0.949866 0.250288
261 | vn -0.0299751 -0.962324 0.270247
262 | vn -0.733474 -0.46004 0.500378
263 | vn -0.819413 -0.278244 0.501142
264 | vn -0.818885 -0.280329 0.500842
265 | vn -0.840131 -0.200724 0.503876
266 | vn -0.865477 -0.0010483 0.500948
267 | vn -0.861767 -0.077237 0.501391
268 | vn -0.734892 0.455474 0.502471
269 | vn -0.789187 0.35685 0.499842
270 | vn -0.899886 0.416094 0.130652
271 | vn -0.934904 0.343772 0.0881805
272 | vn -0.750321 -0.110129 0.651836
273 | vn -0.744296 -0.138475 0.653336
274 | vn -0.0486186 -0.96337 0.263731
275 | vn -0.0569915 -0.952558 0.298973
276 | vn -0.0786935 -0.945844 0.314939
277 | vn 0.0865387 -0.962264 0.257989
278 | vn 0.0988769 -0.988653 0.113084
279 | vn -0.040165 -0.954927 0.294112
280 | vn -0.679455 -0.364641 0.636692
281 | vn -0.676925 -0.535527 0.504959
282 | vn -0.860556 -0.0952615 0.500368
283 | vn -0.866101 -0.0391639 0.498333
284 | vn -0.848661 -0.170347 0.500757
285 | vn -0.976331 0.171097 0.132301
286 | vn -0.784297 0.365988 0.500931
287 | vn -0.743675 0.147478 0.652072
288 | vn -0.75636 0.0667582 0.650741
289 | vn -0.726694 0.470061 0.500957
290 | vn -0.946042 0.295169 0.133715
291 | vn -0.699993 0.303725 0.646344
292 | vn -0.690574 0.326044 0.645603
293 | vn -0.722223 0.238827 0.64912
294 | vn -0.862635 0.0731211 0.500513
295 | vn -0.860436 0.0839585 0.502594
296 | vn -0.823144 0.266065 0.501641
297 | vn -0.850508 0.155797 0.502359
298 | vn -0.850275 0.158766 0.501823
299 | vn -0.824425 0.262885 0.501214
300 | vn -0.000104827 0.000138107 -1
301 | vn -6.4345e-05 4.8788e-05 -1
302 | vn -0.000255153 -5.05889e-05 -1
303 | vn 8.56579e-06 1.35717e-06 -1
304 | vn 3.45225e-06 -5.63502e-06 -1
305 | vn 0.000304041 -4.54974e-06 -1
306 | vn 0.0004587 -0.000166978 -1
307 | vn -4.30629e-05 3.84989e-05 -1
308 | vn -0.000206776 -3.99625e-05 -1
309 | vn -7.87823e-05 -3.27836e-06 -1
310 | vn -0.000409956 0.000110369 -1
311 | vn 6.31049e-05 7.3988e-06 -1
312 | vn -0.0003035 2.77655e-05 -1
313 |
314 | # Mesh '' with 200 faces
315 | g
316 | usemtl DefaultMaterial
317 | f 1//1 2//1 3//1
318 | f 4//2 2//2 1//2
319 | f 3//3 5//3 1//3
320 | f 6//4 7//4 8//4
321 | f 9//5 1//5 10//5
322 | f 1//6 11//6 10//6
323 | f 12//7 13//7 14//7
324 | f 13//7 15//8 14//7
325 | f 2//9 13//9 3//9
326 | f 16//10 9//10 17//10
327 | f 14//11 18//11 19//11
328 | f 20//12 2//12 4//12
329 | f 21//13 20//13 4//13
330 | f 22//14 23//14 4//14
331 | f 23//15 21//15 4//15
332 | f 1//16 5//16 11//16
333 | f 13//17 24//17 15//17
334 | f 20//18 21//18 24//18
335 | f 25//19 26//19 14//19
336 | f 15//20 27//20 14//20
337 | f 28//21 29//21 14//20
338 | f 14//22 30//22 25//22
339 | f 14//22 31//23 12//23
340 | f 32//24 25//24 30//24
341 | f 33//25 17//25 14//25
342 | f 34//26 35//26 14//26
343 | f 36//27 37//27 14//27
344 | f 14//28 37//28 34//28
345 | f 38//29 33//29 14//29
346 | f 19//30 38//30 14//30
347 | f 39//31 18//31 40//31
348 | f 7//32 6//32 41//32
349 | f 42//33 43//33 44//33
350 | f 45//34 43//34 42//34
351 | f 4//35 46//35 47//35
352 | f 17//36 31//36 14//36
353 | f 9//37 31//37 17//37
354 | f 10//38 31//38 9//38
355 | f 11//39 31//39 10//39
356 | f 12//40 31//40 11//40
357 | f 21//41 23//41 48//41
358 | f 13//42 49//42 3//42
359 | f 12//43 49//43 13//43
360 | f 25//44 32//44 50//44
361 | f 14//45 29//45 51//45
362 | f 52//46 30//46 14//46
363 | f 27//47 52//47 14//47
364 | f 53//48 32//48 30//48
365 | f 9//49 54//49 1//49
366 | f 35//50 55//50 14//50
367 | f 56//51 44//51 43//51
368 | f 57//52 47//52 46//52
369 | f 4//53 47//53 57//53
370 | f 58//54 46//54 4//54
371 | f 53//55 46//55 58//55
372 | f 57//56 22//56 4//56
373 | f 57//57 59//57 22//57
374 | f 59//58 15//58 22//58
375 | f 60//59 61//59 62//59
376 | f 24//60 48//60 15//60
377 | f 24//61 21//61 48//61
378 | f 49//62 5//62 3//62
379 | f 5//63 12//63 11//63
380 | f 50//64 4//64 45//64
381 | f 4//65 50//65 58//65
382 | f 32//66 53//66 58//66
383 | f 42//67 26//67 25//67
384 | f 52//68 27//68 63//68
385 | f 9//69 16//69 54//69
386 | f 37//70 64//70 34//70
387 | f 38//71 19//71 65//71
388 | f 40//72 6//72 39//72
389 | f 8//73 66//73 6//73
390 | f 6//74 66//74 39//74
391 | f 18//75 67//75 19//75
392 | f 68//76 19//76 67//76
393 | f 67//77 69//77 68//77
394 | f 40//78 70//78 55//78
395 | f 71//79 72//79 60//79
396 | f 73//80 74//80 35//80
397 | f 55//81 75//81 40//81
398 | f 57//82 46//82 59//82
399 | f 76//83 77//83 78//83
400 | f 62//84 77//84 76//84
401 | f 79//85 44//85 56//85
402 | f 80//86 24//86 13//86
403 | f 24//87 80//87 20//87
404 | f 2//88 80//88 13//88
405 | f 20//89 80//89 2//89
406 | f 15//90 48//90 22//90
407 | f 22//91 48//91 23//91
408 | f 49//92 12//92 5//92
409 | f 25//93 50//93 42//93
410 | f 58//94 50//94 32//94
411 | f 42//95 81//95 26//95
412 | f 59//96 46//96 63//96
413 | f 59//97 63//97 27//97
414 | f 82//98 54//98 16//98
415 | f 82//99 83//99 54//99
416 | f 54//100 83//100 1//100
417 | f 16//101 84//101 82//101
418 | f 33//102 84//102 16//102
419 | f 39//103 67//103 18//103
420 | f 69//104 67//104 39//104
421 | f 8//105 39//105 66//105
422 | f 1//106 39//106 8//106
423 | f 69//107 39//107 1//107
424 | f 40//108 18//108 70//108
425 | f 70//109 18//109 14//109
426 | f 55//110 70//110 14//110
427 | f 72//111 75//111 85//111
428 | f 7//112 41//112 71//112
429 | f 60//113 72//113 61//113
430 | f 59//114 27//114 15//114
431 | f 62//115 61//115 86//115
432 | f 86//116 77//116 62//116
433 | f 87//117 88//117 89//117
434 | f 51//118 90//118 14//118
435 | f 42//119 50//119 45//119
436 | f 14//120 26//120 28//120
437 | f 42//121 44//121 81//121
438 | f 44//122 91//122 81//122
439 | f 53//123 63//123 46//123
440 | f 52//124 53//124 30//124
441 | f 63//125 53//125 52//125
442 | f 33//126 16//126 17//126
443 | f 65//127 84//127 38//127
444 | f 33//128 38//128 84//128
445 | f 69//129 1//129 92//129
446 | f 83//130 92//130 1//130
447 | f 75//131 72//131 93//131
448 | f 93//132 72//132 71//132
449 | f 6//133 40//133 75//133
450 | f 41//134 93//134 71//134
451 | f 85//135 55//135 35//135
452 | f 74//136 85//136 35//136
453 | f 87//137 94//137 95//137
454 | f 36//138 96//138 78//138
455 | f 76//139 96//139 88//139
456 | f 78//140 96//140 76//140
457 | f 88//141 96//141 89//141
458 | f 97//142 79//142 56//142
459 | f 29//143 91//143 98//143
460 | f 28//144 91//144 29//144
461 | f 26//145 81//145 28//145
462 | f 91//146 28//146 81//146
463 | f 73//147 64//147 86//147
464 | f 65//148 82//148 84//148
465 | f 92//149 65//149 99//149
466 | f 93//150 41//150 75//150
467 | f 72//151 85//151 61//151
468 | f 61//152 85//152 74//152
469 | f 61//153 74//153 73//153
470 | f 36//154 78//154 37//154
471 | f 37//155 77//155 64//155
472 | f 98//156 91//156 79//156
473 | f 79//157 100//157 98//157
474 | f 97//158 101//158 79//158
475 | f 95//159 101//159 97//159
476 | f 64//160 73//160 34//160
477 | f 73//161 35//161 34//161
478 | f 69//162 92//162 99//162
479 | f 19//163 69//163 99//163
480 | f 68//164 69//164 19//164
481 | f 92//165 82//165 65//165
482 | f 92//166 83//166 82//166
483 | f 99//167 65//167 19//167
484 | f 85//168 75//168 55//168
485 | f 41//169 6//169 75//169
486 | f 64//170 77//170 86//170
487 | f 77//171 37//171 78//171
488 | f 86//172 61//172 73//172
489 | f 87//173 89//173 94//173
490 | f 79//174 101//174 100//174
491 | f 102//175 90//175 51//175
492 | f 36//176 14//176 90//176
493 | f 91//177 44//177 79//177
494 | f 95//178 94//178 101//178
495 | f 29//179 98//179 51//179
496 | f 100//180 51//180 98//180
497 | f 102//181 51//181 100//181
498 | f 89//182 96//182 36//182
499 | f 36//183 90//183 89//183
500 | f 101//184 102//184 100//184
501 | f 94//185 89//185 102//185
502 | f 90//186 102//186 89//186
503 | f 101//187 94//187 102//187
504 | f 4//188 1//188 87//188
505 | f 4//189 87//189 45//189
506 | f 1//190 8//190 87//190
507 | f 60//191 87//191 71//191
508 | f 56//192 87//191 97//192
509 | f 76//193 87//193 62//193
510 | f 56//194 43//194 87//194
511 | f 45//195 87//195 43//195
512 | f 8//196 7//196 87//196
513 | f 62//197 87//197 60//197
514 | f 97//198 87//198 95//198
515 | f 71//199 87//199 7//199
516 | f 88//200 87//200 76//200
517 |
518 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link0.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/link0.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link1.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/link1.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link2.obj:
--------------------------------------------------------------------------------
1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net)
2 | # (assimp v3.1.187496374)
3 |
4 | mtllib link2.obj.mtl
5 |
6 | # 152 vertex positions
7 | v 0.0547799 0.00875545 0.000695036
8 | v 0.0549849 0.00179704 0.0600771
9 | v 0.0549358 -0.194003 -0.0050002
10 | v 0.0541351 0.00944441 0.0628215
11 | v 0.031051 -0.126082 -0.0455013
12 | v 0.047601 0.0280333 0.000489744
13 | v 0.0502436 0.0226741 0.0632384
14 | v 0.0212053 -0.126588 -0.0508097
15 | v 0.0399461 -0.124494 -0.0379263
16 | v 0.0484134 -0.123261 -0.026532
17 | v -0.0518832 -0.194 -0.0183946
18 | v -0.0500141 -0.121697 -0.0232611
19 | v -0.0466703 -0.194 -0.0294255
20 | v -0.0439629 0.0332954 0.000489695
21 | v -0.036151 0.0416476 0.0644805
22 | v -0.0320994 0.0449999 0.000508252
23 | v -0.0152371 -0.0171926 0.125084
24 | v -0.0237602 -0.194 0.0497122
25 | v -0.010578 -0.194 0.054149
26 | v -0.0254664 -0.0161975 0.121439
27 | v 0.0307019 -0.0160908 0.118495
28 | v 0.0218367 -0.194 0.0506868
29 | v 0.0369534 -0.194 0.041087
30 | v 0.016094 0.0527631 0.0641438
31 | v 0.0285992 0.047274 0.000499651
32 | v 0.013564 0.053444 0.000497281
33 | v 0.0287201 0.0470227 0.0647734
34 | v 0.00351193 -0.12558 -0.054882
35 | v 0.0114522 -0.126129 -0.0537865
36 | v -0.0138711 -0.126413 -0.0533398
37 | v 0.0390762 0.0387882 0.000497352
38 | v -0.0439361 -0.0117967 0.108632
39 | v -0.0462278 -0.00365254 0.107627
40 | v -0.0487421 -0.193999 0.0259655
41 | v -0.0548289 0.00642324 0.000591963
42 | v -0.0534466 -0.118112 -0.0131661
43 | v -0.0551534 -0.19401 0.00510135
44 | v -0.0545204 -0.194 -0.00818656
45 | v -0.0358292 -0.125777 -0.0418274
46 | v -0.0511798 0.0205574 0.000498796
47 | v -0.00343009 -0.125759 -0.0548761
48 | v -0.0179118 0.0521237 0.000504139
49 | v -0.0214171 0.0509056 0.0645642
50 | v -0.00776943 0.0545048 0.0645032
51 | v -0.00246019 0.05522 0.000492765
52 | v -0.0512272 0.0203715 0.0627504
53 | v 0.053404 -0.118713 -0.0135225
54 | v 0.000751583 -0.00741707 0.129075
55 | v -0.00348431 -0.0167312 0.127099
56 | v 0.00626452 -0.0182407 0.126383
57 | v 0.00559829 -0.194 0.0549029
58 | v 0.0379164 -0.0147356 0.113617
59 | v 0.0462819 -0.00300058 0.107483
60 | v 0.0477805 -0.194 0.0274755
61 | v 0.0534314 -0.194 0.013614
62 | v 0.0446073 -0.0117334 0.107859
63 | v -0.0100452 -0.00110755 0.128719
64 | v -0.00688605 0.00774291 0.128603
65 | v -0.0150385 0.0113995 0.126577
66 | v -0.0366572 -0.01521 0.114621
67 | v 0.0445974 0.0322464 0.0639897
68 | v 0.0517857 0.00163012 0.0815724
69 | v 0.0494481 0.017366 0.0790175
70 | v 0.0282936 -0.194 -0.0472499
71 | v 0.0458178 -0.194 -0.0305876
72 | v 0.0373247 -0.194 -0.0405746
73 | v -0.0517812 0.00221134 0.0817068
74 | v -0.054916 0.00463229 0.0610593
75 | v -0.0374301 -0.194 0.0406659
76 | v -0.0276795 -0.126212 -0.0476675
77 | v -0.0452128 0.0314492 0.0634568
78 | v -0.0395945 0.0333466 0.0831425
79 | v -0.00855073 0.0509026 0.0845959
80 | v -0.0197877 0.0474677 0.0851107
81 | v -0.0130229 0.0448056 0.107063
82 | v -0.025218 0.0448655 0.0847447
83 | v -0.0313187 0.0409526 0.0839825
84 | v 0.00337921 0.0550079 0.0644398
85 | v 0.0517594 -0.194 -0.0190919
86 | v 0.0190505 -0.0172288 0.123856
87 | v -0.014125 -0.0095805 0.127168
88 | v 0.0298218 -0.00866372 0.120929
89 | v 0.0240752 -0.00408641 0.1245
90 | v 0.00397345 0.000518457 0.129489
91 | v -0.0208855 -0.000462331 0.12582
92 | v -0.0129757 -0.194 -0.0535885
93 | v -4.27424e-05 -0.194 -0.0550966
94 | v 0.0364876 0.0411348 0.0655198
95 | v -0.0430603 -0.123826 -0.0343229
96 | v 0.00901678 0.0126839 0.127561
97 | v 0.00261172 0.00856949 0.128877
98 | v 0.0118591 -0.00831063 0.127792
99 | v 0.0137665 0.0204527 0.124378
100 | v 0.0176171 0.00425055 0.126807
101 | v 0.0260309 0.00970153 0.122739
102 | v 0.0459354 0.00673847 0.107678
103 | v 0.0392982 -0.00592915 0.114605
104 | v 0.0363838 0.0032781 0.117268
105 | v -0.0255566 0.00795001 0.123264
106 | v -0.0236907 0.0215258 0.120405
107 | v -0.0323034 0.015809 0.1176
108 | v -0.00434828 0.0171063 0.126883
109 | v -0.0144221 0.0199306 0.124302
110 | v 0.0486098 0.0101519 0.0933213
111 | v -0.0462521 0.00699219 0.106752
112 | v -0.0427638 0.00539408 0.111331
113 | v -0.0267486 -0.194 -0.0482902
114 | v 0.016019 -0.194 -0.0528808
115 | v -0.0514251 0.0132731 0.0747568
116 | v -0.0465312 0.023048 0.0822049
117 | v -0.0374757 0.0275452 0.107331
118 | v -0.0425322 0.018274 0.107673
119 | v -0.0266734 0.0324841 0.112352
120 | v -0.0311617 0.0343971 0.107352
121 | v 0.00196963 0.0512017 0.0866911
122 | v -0.0440648 0.0154429 0.10713
123 | v 0.000309921 0.0235193 0.124821
124 | v 0.0331091 0.0137207 0.117691
125 | v -0.0302799 0.00225558 0.121284
126 | v -0.0362039 0.00731356 0.116785
127 | v -0.0313528 -0.00896166 0.119999
128 | v -0.0381205 -0.194 -0.0397839
129 | v 0.0438507 0.0239595 0.0918484
130 | v 0.0399564 0.0325702 0.084039
131 | v -0.017877 0.0426078 0.107975
132 | v -0.00582199 0.0430587 0.110978
133 | v -0.00952703 0.0376232 0.115319
134 | v -0.0171561 0.0323609 0.117111
135 | v -0.0244552 0.0394989 0.107471
136 | v -0.0387607 0.0166246 0.112248
137 | v -0.032079 0.0257153 0.113308
138 | v 0.0169924 0.0287418 0.119516
139 | v 0.00312588 0.0325689 0.119968
140 | v -0.00276387 0.046319 0.107409
141 | v 0.0114487 0.0500631 0.0856468
142 | v 0.0424678 0.0187545 0.107658
143 | v 0.0365812 0.0285008 0.107512
144 | v 0.0341516 0.0224739 0.113524
145 | v -0.0374517 -0.00313331 0.116357
146 | v 0.00294327 0.0404966 0.113812
147 | v -0.0117918 0.0286979 0.120959
148 | v 0.01203 0.0394531 0.11315
149 | v 0.0204365 0.0372159 0.111937
150 | v 0.00874197 0.0456755 0.107309
151 | v 0.0277535 0.0252894 0.116309
152 | v 0.0242468 0.019117 0.120984
153 | v 0.0393956 0.0156438 0.11198
154 | v 0.0200004 0.0467444 0.0881052
155 | v 0.026041 0.038696 0.106945
156 | v 0.0196621 0.0420999 0.107295
157 | v 0.0249062 0.0313688 0.114376
158 | v 0.0317282 0.0340296 0.107127
159 |
160 | # 0 UV coordinates
161 |
162 | # 298 vertex normals
163 | vn 0.999994 0.000863099 -0.00335135
164 | vn 0.994196 0.1072 0.00912929
165 | vn 0.531767 0.189234 -0.825478
166 | vn 0.937121 0.348871 -0.00966981
167 | vn 0.455706 0.209252 -0.865185
168 | vn 0.795259 0.0854375 -0.600219
169 | vn -0.904114 -0.00538631 -0.427258
170 | vn -0.702299 0.711846 -0.00717604
171 | vn -0.297328 -0.362249 0.883386
172 | vn -0.344298 -0.354079 0.869533
173 | vn 0.50623 -0.329053 0.797155
174 | vn 0.379639 0.92512 -0.00519485
175 | vn 0.413766 0.910379 0.00278169
176 | vn 0.150207 0.281818 -0.947637
177 | vn 0.118088 0.287398 -0.950504
178 | vn -0.25926 0.264253 -0.928953
179 | vn 0.314677 0.250075 -0.915664
180 | vn 0.29327 0.254232 -0.921607
181 | vn 0.219778 0.271602 -0.936979
182 | vn 0.959393 0.281989 0.00682976
183 | vn 0.624884 0.152369 -0.765704
184 | vn -0.866797 -0.186891 0.462315
185 | vn -0.943599 0.0259913 -0.33007
186 | vn -0.998867 0.00054697 -0.0475853
187 | vn -0.623931 0.156035 -0.765744
188 | vn -0.156925 0.280761 -0.946862
189 | vn -0.142678 0.283609 -0.948266
190 | vn -0.448711 0.893645 -0.00756096
191 | vn -0.25498 0.966936 0.00443328
192 | vn -0.196484 0.980492 -0.00534121
193 | vn -0.968252 0.249977 8.6395e-06
194 | vn 0.990526 0.00461689 -0.137249
195 | vn 0.0369453 -0.223424 0.974021
196 | vn 0.009697 -0.376742 0.926267
197 | vn -0.0430879 -0.378729 0.924504
198 | vn 0.571795 -0.310337 0.759435
199 | vn 0.915645 -0.14918 0.373283
200 | vn 0.90661 -0.156962 0.391691
201 | vn 0.694927 -0.272904 0.665282
202 | vn -0.204033 0.0855425 0.975219
203 | vn -0.512723 -0.325422 0.79449
204 | vn 0.0328199 0.293682 -0.95534
205 | vn 0.861547 0.507629 0.00707201
206 | vn 0.987442 0.0568267 0.147407
207 | vn 0.951023 0.275313 0.140563
208 | vn 0.474434 0.00340186 -0.880284
209 | vn 0.76178 -0.00404598 -0.647823
210 | vn 0.802386 0.0047729 -0.596787
211 | vn 0.647875 0.00459 -0.761732
212 | vn -0.978746 -0.0597654 0.196172
213 | vn -0.988569 -0.0397853 0.145426
214 | vn -0.968207 -0.00271314 -0.250134
215 | vn -0.684587 -0.276094 0.67462
216 | vn -0.946467 0.00274025 -0.322788
217 | vn -0.989865 0.00469307 -0.141931
218 | vn -0.999998 0.00158798 -0.00139374
219 | vn -0.567546 0.173276 -0.804902
220 | vn -0.746469 0.648161 0.15057
221 | vn -0.059283 0.292332 -0.954478
222 | vn -0.00833902 0.292587 -0.956203
223 | vn -0.279998 0.942977 0.179987
224 | vn -0.278063 0.93959 0.199629
225 | vn -0.526534 0.836596 0.151228
226 | vn -0.547007 0.820748 0.164792
227 | vn -0.53205 0.846645 0.0106701
228 | vn 0.110161 0.993891 -0.00676278
229 | vn 0.173941 0.984749 0.00361981
230 | vn 0.975511 -0.00504341 -0.219893
231 | vn 0.929545 0.0309468 -0.367409
232 | vn 0.233598 -0.368724 0.899708
233 | vn 0.208231 -0.369148 0.905743
234 | vn -0.102318 -0.161682 0.981524
235 | vn 0.399687 -0.19669 0.895301
236 | vn 0.420176 -0.341391 0.840776
237 | vn 0.757259 -0.252465 0.602346
238 | vn 0.996386 -0.0270171 0.080536
239 | vn 0.988252 -0.0426667 0.146755
240 | vn -0.0512336 -0.0313597 0.998194
241 | vn -0.107676 -0.128634 0.985829
242 | vn -0.949783 -0.11269 0.291913
243 | vn -0.254295 0.0637289 0.965025
244 | vn -0.522082 -0.324077 0.788926
245 | vn -0.115824 -0.00253905 -0.993267
246 | vn 0.978533 -0.060234 0.197091
247 | vn 0.783664 0.621171 -0.00414522
248 | vn 0.739147 0.673525 0.00512004
249 | vn 0.629394 0.77708 -0.00298668
250 | vn -0.767806 -0.247805 0.590819
251 | vn -0.6978 0.125969 -0.705129
252 | vn -0.747757 0.663956 0.00462416
253 | vn -0.0450397 0.998958 0.00742623
254 | vn 0.932906 0.0062564 -0.360066
255 | vn 0.139193 0.0985125 0.985353
256 | vn -0.142669 -0.371777 0.917294
257 | vn 0.0971861 -0.19331 0.976313
258 | vn -0.154903 -0.239709 0.958407
259 | vn 0.31021 0.212658 0.926578
260 | vn 0.425097 -0.235974 0.873847
261 | vn 0.711656 0.0112243 0.702439
262 | vn 0.705103 0.018177 0.708871
263 | vn -0.442238 0.242649 0.863451
264 | vn -0.260852 -0.0508872 0.964037
265 | vn -0.168845 0.265372 0.949247
266 | vn 0.286162 0.208247 0.935278
267 | vn -0.329188 0.102325 0.938704
268 | vn -0.000929673 0.00318577 -0.999995
269 | vn 0.976601 0.0304743 0.212892
270 | vn 0.975681 0.0851268 0.201991
271 | vn 0.594393 -0.00342816 -0.804167
272 | vn 0.888339 -0.00627229 -0.459145
273 | vn -0.786216 0.0488445 0.616018
274 | vn -0.976984 0.0152622 0.212768
275 | vn -0.366464 0.234435 -0.900413
276 | vn -0.401575 0.226884 -0.887278
277 | vn 2.34947e-05 -1 -5.69195e-05
278 | vn 3.61495e-05 -1 -1.824e-05
279 | vn -5.85523e-07 -1 1.49966e-06
280 | vn 3.03192e-06 -1 2.42667e-06
281 | vn -4.71086e-07 -1 2.77471e-06
282 | vn -1.48549e-06 -1 8.9384e-06
283 | vn 3.51975e-06 -1 -4.53679e-06
284 | vn 4.04839e-05 -1 -4.58034e-05
285 | vn -9.82561e-05 -1 -1.11942e-06
286 | vn 2.84184e-05 -1 9.31264e-06
287 | vn -2.01463e-05 -1 2.9116e-05
288 | vn -7.02999e-05 -1 4.29144e-05
289 | vn 4.18487e-06 -1 -1.60743e-05
290 | vn -0.00105549 -0.999999 4.71011e-05
291 | vn -0.927549 0.328182 0.178744
292 | vn -0.878846 0.477105 0.000755021
293 | vn -0.870057 0.492942 -0.00281789
294 | vn -0.823025 0.537695 0.183067
295 | vn -0.85243 0.473119 0.222535
296 | vn -0.873196 0.464722 0.146839
297 | vn -0.379932 0.00327956 -0.925008
298 | vn -0.434877 0.881895 0.182049
299 | vn -0.542201 0.49762 0.677047
300 | vn -0.250985 0.954448 0.161354
301 | vn -0.062547 0.982799 0.173763
302 | vn -0.950623 0.237106 0.200242
303 | vn -0.970154 0.215406 0.111366
304 | vn -0.973732 0.227636 0.0053393
305 | vn 0.0549176 0.269324 0.961482
306 | vn 0.106643 -0.0950194 0.989747
307 | vn 0.169683 0.0853083 0.981799
308 | vn 0.403024 0.0594556 0.913256
309 | vn 0.525421 0.130692 0.840745
310 | vn 0.309201 -0.164124 0.936727
311 | vn 0.68757 -0.186612 0.701729
312 | vn -0.058787 0.0339928 0.997692
313 | vn -0.0347986 0.0699661 0.996942
314 | vn -0.528972 0.148128 0.835612
315 | vn -0.511188 0.15351 0.845649
316 | vn -0.14308 0.216216 0.965804
317 | vn 0.104308 0.32179 0.941048
318 | vn 0.0273576 0.265676 0.963674
319 | vn -0.126123 0.385563 0.914021
320 | vn -0.384405 -0.150357 0.910838
321 | vn 0.136665 -0.00399225 -0.990609
322 | vn 0.13639 -0.0040438 -0.990647
323 | vn -0.145676 0.00171098 -0.989331
324 | vn -0.838539 0.069106 -0.540441
325 | vn -0.719662 0.00337278 -0.694316
326 | vn 0.895452 0.406326 0.181834
327 | vn 0.742708 0.649867 0.161426
328 | vn 0.960091 0.223039 0.168755
329 | vn 0.976886 0.124038 0.174094
330 | vn 0.416955 -0.00415406 -0.908917
331 | vn 0.292217 0.00689914 -0.956327
332 | vn -0.755259 0.167188 0.633745
333 | vn -0.971282 0.14821 0.186131
334 | vn -1.53238e-05 -1 2.11839e-06
335 | vn -9.43751e-05 -1 -4.17948e-05
336 | vn -6.76337e-05 -1 -3.86085e-05
337 | vn 0.00343091 -0.999994 -0.00056314
338 | vn -0.000102877 -1 3.39915e-05
339 | vn -0.598978 -0.00386878 -0.800756
340 | vn -0.928173 0.320251 0.189565
341 | vn -0.367104 0.903141 0.222646
342 | vn -0.194938 0.710847 0.675793
343 | vn -0.198998 0.690831 0.695092
344 | vn -0.34802 0.610139 0.711767
345 | vn -0.341961 0.607215 0.717184
346 | vn -0.643954 0.375783 0.666416
347 | vn -0.625549 0.380285 0.681228
348 | vn -0.38876 0.47454 0.789732
349 | vn -0.431779 0.454863 0.778889
350 | vn -0.544307 0.526906 0.652763
351 | vn -0.820962 0.534748 0.200163
352 | vn -0.0435197 0.985204 0.16577
353 | vn -0.977211 0.134589 0.164147
354 | vn -0.264308 0.631408 0.729016
355 | vn 0.1541 0.454601 0.877264
356 | vn -0.136379 0.78679 0.601965
357 | vn 0.136436 0.974993 0.175423
358 | vn 0.175233 0.971466 0.159838
359 | vn -0.0702394 0.974396 0.213587
360 | vn 0.640947 0.396988 0.656954
361 | vn 0.487217 0.042278 0.872257
362 | vn 0.206759 -0.250703 0.945727
363 | vn 0.573957 -0.17969 0.798927
364 | vn 0.582319 -0.190013 0.790442
365 | vn -0.575564 0.336043 0.745521
366 | vn -0.334494 0.265346 0.904271
367 | vn -0.704479 -0.0206901 0.709423
368 | vn -0.608945 0.206044 0.765984
369 | vn -0.0449612 0.192288 0.980308
370 | vn -0.0570372 0.646519 0.760763
371 | vn -0.0456084 0.612006 0.789537
372 | vn -0.0688257 0.487991 0.870131
373 | vn -0.126278 0.386235 0.913716
374 | vn -0.518988 -0.260184 0.814221
375 | vn -0.771213 -0.00475059 -0.63656
376 | vn -0.846044 0.00632099 -0.533076
377 | vn 0.603962 0.79701 0.00198059
378 | vn -0.609696 -0.151946 0.777935
379 | vn -0.582506 -0.000532993 -0.812826
380 | vn -0.546305 0.810879 0.209823
381 | vn -0.433541 0.873649 0.220861
382 | vn -0.430493 0.874883 0.221938
383 | vn -0.592933 0.774279 0.221183
384 | vn -0.674545 0.712683 0.192542
385 | vn -0.196288 0.558895 0.805672
386 | vn 0.276421 -0.0514573 0.959658
387 | vn 0.196341 -0.0131192 0.980448
388 | vn 0.265625 0.580477 0.769733
389 | vn 0.181753 0.563216 0.806073
390 | vn 0.13212 0.714636 0.686906
391 | vn 0.139644 0.966955 0.2133
392 | vn 0.497395 0.326283 0.803827
393 | vn 0.494051 0.32504 0.806389
394 | vn 0.465971 0.246662 0.849722
395 | vn 0.326294 0.232587 0.916207
396 | vn 0.638437 0.333373 0.69373
397 | vn 0.732082 -0.111401 0.672046
398 | vn 0.565801 -0.0587605 0.822445
399 | vn 0.515701 -0.0207842 0.856517
400 | vn -0.87513 0.431818 0.218357
401 | vn -0.646555 0.0460771 0.761474
402 | vn -0.362237 0.239517 0.900786
403 | vn -0.304864 0.440552 0.844377
404 | vn -0.0868798 0.547816 0.832075
405 | vn -0.384864 -0.131303 0.913586
406 | vn -0.346098 -0.200322 0.916563
407 | vn 0.503112 0.841219 0.198061
408 | vn 0.823456 0.53742 0.181934
409 | vn 0.854693 0.490881 0.168927
410 | vn -0.66898 -0.148703 0.728253
411 | vn -0.697792 -0.109036 0.707953
412 | vn -0.359042 -0.00132172 -0.933321
413 | vn -0.467735 0.599727 0.64927
414 | vn -0.673614 0.710425 0.203813
415 | vn -0.717482 0.660503 0.22126
416 | vn -0.00182421 0.739045 0.673654
417 | vn -0.149619 0.961356 0.231102
418 | vn 0.239458 0.728761 0.641534
419 | vn 0.279376 0.684365 0.673494
420 | vn 0.468902 0.856539 0.215576
421 | vn 0.829645 0.504676 0.238727
422 | vn 0.312865 0.559373 0.767605
423 | vn 0.803861 0.555776 0.211944
424 | vn 0.528926 0.482947 0.697853
425 | vn -0.656801 0.216914 0.722191
426 | vn -0.290729 0.415762 0.861753
427 | vn -0.584387 0.0366406 0.810648
428 | vn -0.542911 -0.044139 0.838629
429 | vn -0.447889 -0.0593559 0.892116
430 | vn -0.425426 0.0383972 0.904178
431 | vn 0.128926 0.437161 0.890095
432 | vn 0.926929 0.310385 0.210866
433 | vn 0.90493 0.374345 0.202403
434 | vn 0.0481549 0.759987 0.648152
435 | vn 0.0562881 0.968845 0.241186
436 | vn 0.933093 0.269674 0.237935
437 | vn 0.51793 0.469893 0.714807
438 | vn 0.328419 0.379525 0.864929
439 | vn 0.726008 0.652385 0.217498
440 | vn 0.719844 0.208843 0.661974
441 | vn 0.594619 0.286757 0.751132
442 | vn -0.718715 0.201884 0.665351
443 | vn -0.659221 0.175302 0.73123
444 | vn -0.494093 0.355034 0.793614
445 | vn 0.127052 0.610153 0.78203
446 | vn 0.617571 0.760526 0.200515
447 | vn 0.580874 0.790772 0.193041
448 | vn 0.401827 0.901464 0.160927
449 | vn 0.525761 0.508862 0.681641
450 | vn 0.390316 0.666012 0.635674
451 | vn 0.309209 0.933033 0.183955
452 | vn 0.303165 0.92496 0.229214
453 | vn 0.382883 0.437987 0.813368
454 | vn 0.379105 0.412169 0.82849
455 | vn 0.681299 0.704644 0.198262
456 | vn 0.674255 0.145379 0.724048
457 | vn 0.634115 0.168265 0.754709
458 | vn 0.432342 0.6082 0.665713
459 | vn 0.465852 0.593485 0.656321
460 | vn 0.295628 0.928431 0.224987
461 |
462 | # Mesh '' with 300 faces
463 | g
464 | usemtl DefaultMaterial
465 | f 1//1 2//1 3//1
466 | f 1//2 4//2 2//2
467 | f 5//3 6//3 1//3
468 | f 6//4 7//4 1//4
469 | f 5//5 8//5 6//5
470 | f 9//6 1//6 10//6
471 | f 11//7 12//7 13//7
472 | f 14//8 15//8 16//8
473 | f 17//9 18//9 19//9
474 | f 20//10 18//10 17//10
475 | f 21//11 22//11 23//11
476 | f 24//12 25//12 26//12
477 | f 25//13 24//13 27//13
478 | f 28//14 25//14 29//14
479 | f 28//15 26//15 25//15
480 | f 16//16 30//16 14//16
481 | f 8//17 31//17 6//17
482 | f 29//18 31//18 8//18
483 | f 29//19 25//19 31//19
484 | f 7//20 4//20 1//20
485 | f 5//21 1//21 9//21
486 | f 32//22 33//22 34//22
487 | f 35//23 12//23 36//23
488 | f 37//24 35//24 38//24
489 | f 39//25 35//25 40//25
490 | f 16//26 41//26 30//26
491 | f 42//27 41//27 16//27
492 | f 16//28 43//28 42//28
493 | f 44//29 42//29 43//29
494 | f 44//30 45//30 42//30
495 | f 46//31 40//31 35//31
496 | f 3//32 47//32 1//32
497 | f 48//33 49//33 50//33
498 | f 49//34 51//34 50//34
499 | f 49//35 19//35 51//35
500 | f 52//36 21//36 23//36
501 | f 53//37 54//37 55//37
502 | f 53//38 56//38 54//38
503 | f 52//39 23//39 56//39
504 | f 57//40 58//40 59//40
505 | f 18//41 20//41 60//41
506 | f 28//42 45//42 26//42
507 | f 7//43 6//43 61//43
508 | f 2//44 4//44 62//44
509 | f 4//45 7//45 63//45
510 | f 8//46 5//46 64//46
511 | f 9//47 65//47 66//47
512 | f 10//48 65//48 9//48
513 | f 66//49 5//49 9//49
514 | f 33//50 67//50 37//50
515 | f 67//51 68//51 37//51
516 | f 36//52 11//52 38//52
517 | f 32//53 69//53 60//53
518 | f 36//54 12//54 11//54
519 | f 38//55 35//55 36//55
520 | f 68//56 35//56 37//56
521 | f 70//57 39//57 40//57
522 | f 15//58 71//58 72//58
523 | f 41//59 42//59 45//59
524 | f 41//60 45//60 28//60
525 | f 73//61 43//61 74//61
526 | f 73//62 74//62 75//62
527 | f 76//63 43//63 15//63
528 | f 15//64 77//64 76//64
529 | f 43//65 16//65 15//65
530 | f 78//66 26//66 45//66
531 | f 78//67 24//67 26//67
532 | f 79//68 47//68 3//68
533 | f 1//69 47//69 10//69
534 | f 80//70 51//70 22//70
535 | f 80//71 50//71 51//71
536 | f 81//72 49//72 48//72
537 | f 82//73 83//73 80//73
538 | f 21//74 80//74 22//74
539 | f 56//75 23//75 54//75
540 | f 55//76 3//76 2//76
541 | f 2//77 62//77 55//77
542 | f 84//78 57//78 48//78
543 | f 57//79 81//79 48//79
544 | f 33//80 37//80 34//80
545 | f 85//81 57//81 59//81
546 | f 69//82 18//82 60//82
547 | f 86//83 41//83 87//83
548 | f 53//84 55//84 62//84
549 | f 31//85 61//85 6//85
550 | f 61//86 31//86 88//86
551 | f 31//87 25//87 88//87
552 | f 32//88 34//88 69//88
553 | f 35//89 39//89 89//89
554 | f 14//90 71//90 15//90
555 | f 44//91 78//91 45//91
556 | f 79//92 10//92 47//92
557 | f 90//93 91//93 84//93
558 | f 49//94 17//94 19//94
559 | f 92//95 48//95 50//95
560 | f 49//96 81//96 17//96
561 | f 93//97 94//97 95//97
562 | f 80//98 21//98 82//98
563 | f 96//99 97//99 53//99
564 | f 96//100 98//100 97//100
565 | f 99//101 100//101 101//101
566 | f 81//102 57//102 85//102
567 | f 102//103 103//103 59//103
568 | f 90//104 94//104 93//104
569 | f 85//105 59//105 99//105
570 | f 41//106 28//106 87//106
571 | f 62//107 96//107 53//107
572 | f 62//108 104//108 96//108
573 | f 5//109 66//109 64//109
574 | f 65//110 10//110 79//110
575 | f 105//111 33//111 106//111
576 | f 67//112 33//112 105//112
577 | f 30//113 70//113 14//113
578 | f 70//114 40//114 14//114
579 | f 79//115 107//115 86//115
580 | f 66//116 65//116 79//116
581 | f 79//117 19//117 18//117
582 | f 23//118 22//118 79//117
583 | f 79//117 22//118 51//119
584 | f 79//117 51//119 19//117
585 | f 79//117 38//120 107//120
586 | f 38//121 79//117 18//117
587 | f 18//117 69//121 38//121
588 | f 79//122 108//122 64//122
589 | f 79//123 55//123 54//123
590 | f 79//124 54//124 23//124
591 | f 79//125 86//125 87//125
592 | f 79//126 64//126 66//126
593 | f 87//127 108//127 79//127
594 | f 79//128 3//128 55//128
595 | f 109//129 110//129 46//129
596 | f 71//130 40//130 46//130
597 | f 40//131 71//131 14//131
598 | f 110//132 72//132 71//132
599 | f 111//133 110//133 112//133
600 | f 110//134 71//134 46//134
601 | f 107//135 70//135 30//135
602 | f 76//136 74//136 43//136
603 | f 111//137 113//137 114//137
604 | f 43//138 73//138 44//138
605 | f 115//139 44//139 73//139
606 | f 116//140 109//140 105//140
607 | f 68//141 109//141 46//141
608 | f 68//142 46//142 35//142
609 | f 102//143 91//143 117//143
610 | f 48//144 92//144 84//144
611 | f 94//145 90//145 84//145
612 | f 94//146 83//146 95//146
613 | f 95//147 98//147 118//147
614 | f 80//148 83//148 92//148
615 | f 97//149 52//149 56//149
616 | f 84//150 58//150 57//150
617 | f 84//151 91//151 58//151
618 | f 119//152 99//152 101//152
619 | f 120//153 119//153 101//153
620 | f 58//154 102//154 59//154
621 | f 117//155 90//155 93//155
622 | f 91//156 90//156 117//156
623 | f 117//157 103//157 102//157
624 | f 85//158 121//158 81//158
625 | f 28//159 108//159 87//159
626 | f 108//160 28//160 29//160
627 | f 41//161 86//161 30//161
628 | f 89//162 12//162 35//162
629 | f 39//163 122//163 89//163
630 | f 7//164 123//164 63//164
631 | f 88//165 124//165 61//165
632 | f 4//166 63//166 104//166
633 | f 104//167 62//167 4//167
634 | f 64//168 108//168 8//168
635 | f 108//169 29//169 8//169
636 | f 116//170 105//170 106//170
637 | f 105//171 109//171 67//171
638 | f 38//172 122//172 107//172
639 | f 38//173 13//173 122//173
640 | f 38//174 11//174 13//174
641 | f 38//175 34//175 37//175
642 | f 38//176 69//176 34//176
643 | f 122//177 39//177 107//177
644 | f 110//178 109//178 116//178
645 | f 75//179 74//179 125//179
646 | f 126//180 75//180 125//180
647 | f 127//181 126//181 125//181
648 | f 113//182 128//182 125//182
649 | f 113//183 125//183 129//183
650 | f 111//184 112//184 130//184
651 | f 131//185 111//185 130//185
652 | f 100//186 128//186 113//186
653 | f 113//187 131//187 100//187
654 | f 113//188 111//188 131//188
655 | f 111//189 72//189 110//189
656 | f 44//190 115//190 78//190
657 | f 68//191 67//191 109//191
658 | f 125//192 128//192 127//192
659 | f 132//193 133//193 93//193
660 | f 126//194 134//194 75//194
661 | f 135//195 78//195 115//195
662 | f 135//196 24//196 78//196
663 | f 134//197 115//197 73//197
664 | f 136//198 137//198 138//198
665 | f 95//199 83//199 98//199
666 | f 92//200 50//200 80//200
667 | f 21//201 52//201 97//201
668 | f 97//202 82//202 21//202
669 | f 101//203 131//203 130//203
670 | f 103//204 100//204 59//204
671 | f 106//205 33//205 139//205
672 | f 120//206 101//206 130//206
673 | f 58//207 91//207 102//207
674 | f 126//208 127//208 140//208
675 | f 140//209 127//209 133//209
676 | f 141//210 117//210 133//210
677 | f 117//211 141//211 103//211
678 | f 121//212 60//212 20//212
679 | f 89//213 122//213 13//213
680 | f 89//214 13//214 12//214
681 | f 27//215 88//215 25//215
682 | f 121//216 139//216 60//216
683 | f 39//217 70//217 107//217
684 | f 77//218 129//218 76//218
685 | f 74//219 76//219 129//219
686 | f 74//220 129//220 125//220
687 | f 114//221 129//221 77//221
688 | f 77//222 15//222 72//222
689 | f 127//223 128//223 141//223
690 | f 94//224 92//224 83//224
691 | f 94//225 84//225 92//225
692 | f 142//226 132//226 143//226
693 | f 142//227 133//227 132//227
694 | f 144//228 140//228 142//228
695 | f 135//229 115//229 144//229
696 | f 145//230 146//230 118//230
697 | f 118//231 138//231 145//231
698 | f 95//232 118//232 146//232
699 | f 93//233 95//233 146//233
700 | f 136//234 138//234 147//234
701 | f 53//235 97//235 56//235
702 | f 97//236 98//236 82//236
703 | f 82//237 98//237 83//237
704 | f 112//238 110//238 116//238
705 | f 106//239 139//239 120//239
706 | f 99//240 59//240 100//240
707 | f 100//241 141//241 128//241
708 | f 141//242 133//242 127//242
709 | f 20//243 81//243 121//243
710 | f 20//244 17//244 81//244
711 | f 148//245 149//245 27//245
712 | f 124//246 123//246 61//246
713 | f 7//247 61//247 123//247
714 | f 60//248 139//248 32//248
715 | f 139//249 33//249 32//249
716 | f 107//250 30//250 86//250
717 | f 129//251 114//251 113//251
718 | f 72//252 114//252 77//252
719 | f 72//253 111//253 114//253
720 | f 140//254 134//254 126//254
721 | f 134//255 73//255 75//255
722 | f 150//256 144//256 142//256
723 | f 150//257 142//257 143//257
724 | f 150//258 149//258 148//258
725 | f 136//259 123//259 137//259
726 | f 151//260 143//260 132//260
727 | f 124//261 137//261 123//261
728 | f 151//262 138//262 137//262
729 | f 106//263 112//263 116//263
730 | f 141//264 100//264 103//264
731 | f 120//265 139//265 119//265
732 | f 121//266 119//266 139//266
733 | f 119//267 121//267 85//267
734 | f 99//268 119//268 85//268
735 | f 117//269 93//269 133//269
736 | f 63//270 136//270 104//270
737 | f 123//271 136//271 63//271
738 | f 134//272 140//272 144//272
739 | f 115//273 134//273 144//273
740 | f 136//274 96//274 104//274
741 | f 145//275 138//275 151//275
742 | f 132//276 93//276 146//276
743 | f 152//277 137//277 124//277
744 | f 96//278 136//278 147//278
745 | f 147//279 138//279 118//279
746 | f 106//280 130//280 112//280
747 | f 130//281 106//281 120//281
748 | f 101//282 100//282 131//282
749 | f 142//283 140//283 133//283
750 | f 88//284 149//284 152//284
751 | f 88//285 27//285 149//285
752 | f 24//286 148//286 27//286
753 | f 152//287 151//287 137//287
754 | f 150//288 143//288 149//288
755 | f 148//289 24//289 135//289
756 | f 144//290 150//290 148//290
757 | f 132//291 145//291 151//291
758 | f 132//292 146//292 145//292
759 | f 152//293 124//293 88//293
760 | f 98//294 96//294 147//294
761 | f 98//295 147//295 118//295
762 | f 143//296 151//296 149//296
763 | f 151//297 152//297 149//297
764 | f 148//298 135//298 144//298
765 |
766 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link2.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/link2.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link3.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/link3.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link4.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/link4.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link5.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/link5.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link6.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'link2.blend'
2 | # Material Count: 17
3 |
4 | newmtl Face064_002_001_002_001.001
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 1.000000 0.000000 0.000000
8 | Ks 0.003906 0.003906 0.003906
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 |
14 | newmtl Face065_002_001_002_001.001
15 | Ns -1.960784
16 | Ka 1.000000 1.000000 1.000000
17 | Kd 0.000000 1.000000 0.000000
18 | Ks 0.003906 0.003906 0.003906
19 | Ke 0.000000 0.000000 0.000000
20 | Ni 1.000000
21 | d 1.000000
22 | illum 2
23 |
24 | newmtl Face374_002_001_002_001.001
25 | Ns -1.960784
26 | Ka 1.000000 1.000000 1.000000
27 | Kd 1.000000 1.000000 1.000000
28 | Ks 0.003906 0.003906 0.003906
29 | Ke 0.000000 0.000000 0.000000
30 | Ni 1.000000
31 | d 1.000000
32 | illum 2
33 |
34 | newmtl Face539_002_001_002_001.001
35 | Ns -1.960784
36 | Ka 1.000000 1.000000 1.000000
37 | Kd 0.250980 0.250980 0.250980
38 | Ks 0.003906 0.003906 0.003906
39 | Ke 0.000000 0.000000 0.000000
40 | Ni 1.000000
41 | d 1.000000
42 | illum 2
43 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png
44 |
45 | newmtl Part__Feature001_009_001_002_001.001
46 | Ns -1.960784
47 | Ka 1.000000 1.000000 1.000000
48 | Kd 0.250980 0.250980 0.250980
49 | Ks 0.003906 0.003906 0.003906
50 | Ke 0.000000 0.000000 0.000000
51 | Ni 1.000000
52 | d 1.000000
53 | illum 2
54 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png
55 |
56 | newmtl Part__Feature002_006_001_002_001.001
57 | Ns -1.960784
58 | Ka 1.000000 1.000000 1.000000
59 | Kd 0.250980 0.250980 0.250980
60 | Ks 0.003906 0.003906 0.003906
61 | Ke 0.000000 0.000000 0.000000
62 | Ni 1.000000
63 | d 1.000000
64 | illum 2
65 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png
66 |
67 | newmtl Shell002_002_001_002_001.001
68 | Ns -1.960784
69 | Ka 1.000000 1.000000 1.000000
70 | Kd 1.000000 1.000000 1.000000
71 | Ks 0.003906 0.003906 0.003906
72 | Ke 0.000000 0.000000 0.000000
73 | Ni 1.000000
74 | d 1.000000
75 | illum 2
76 |
77 | newmtl Shell003_002_001_002_001.001
78 | Ns -1.960784
79 | Ka 1.000000 1.000000 1.000000
80 | Kd 1.000000 1.000000 1.000000
81 | Ks 0.003906 0.003906 0.003906
82 | Ke 0.000000 0.000000 0.000000
83 | Ni 1.000000
84 | d 1.000000
85 | illum 2
86 |
87 | newmtl Shell004_001_001_002_001.001
88 | Ns -1.960784
89 | Ka 1.000000 1.000000 1.000000
90 | Kd 1.000000 1.000000 1.000000
91 | Ks 0.003906 0.003906 0.003906
92 | Ke 0.000000 0.000000 0.000000
93 | Ni 1.000000
94 | d 1.000000
95 | illum 2
96 |
97 | newmtl Shell005_001_001_002_001.001
98 | Ns -1.960784
99 | Ka 1.000000 1.000000 1.000000
100 | Kd 1.000000 1.000000 1.000000
101 | Ks 0.003906 0.003906 0.003906
102 | Ke 0.000000 0.000000 0.000000
103 | Ni 1.000000
104 | d 1.000000
105 | illum 2
106 |
107 | newmtl Shell006_003_002_001.001
108 | Ns -1.960784
109 | Ka 1.000000 1.000000 1.000000
110 | Kd 0.901961 0.921569 0.929412
111 | Ks 0.015625 0.015625 0.015625
112 | Ke 0.000000 0.000000 0.000000
113 | Ni 1.000000
114 | d 1.000000
115 | illum 2
116 |
117 | newmtl Shell007_002_002_001.001
118 | Ns -1.960784
119 | Ka 1.000000 1.000000 1.000000
120 | Kd 0.250000 0.250000 0.250000
121 | Ks 0.003906 0.003906 0.003906
122 | Ke 0.000000 0.000000 0.000000
123 | Ni 1.000000
124 | d 1.000000
125 | illum 2
126 |
127 | newmtl Shell011_002_002_001.001
128 | Ns -1.960784
129 | Ka 1.000000 1.000000 1.000000
130 | Kd 1.000000 1.000000 1.000000
131 | Ks 0.003906 0.003906 0.003906
132 | Ke 0.000000 0.000000 0.000000
133 | Ni 1.000000
134 | d 1.000000
135 | illum 2
136 |
137 | newmtl Shell012_002_002_001.001
138 | Ns -1.960784
139 | Ka 1.000000 1.000000 1.000000
140 | Kd 1.000000 1.000000 1.000000
141 | Ks 0.003906 0.003906 0.003906
142 | Ke 0.000000 0.000000 0.000000
143 | Ni 1.000000
144 | d 1.000000
145 | illum 2
146 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png
147 |
148 | newmtl Shell_003_001_002_001.001
149 | Ns -1.960784
150 | Ka 1.000000 1.000000 1.000000
151 | Kd 0.250980 0.250980 0.250980
152 | Ks 0.003906 0.003906 0.003906
153 | Ke 0.000000 0.000000 0.000000
154 | Ni 1.000000
155 | d 1.000000
156 | illum 2
157 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png
158 |
159 | newmtl Union001_001_001_002_001.001
160 | Ns -1.960784
161 | Ka 1.000000 1.000000 1.000000
162 | Kd 0.039216 0.541176 0.780392
163 | Ks 0.003906 0.003906 0.003906
164 | Ke 0.000000 0.000000 0.000000
165 | Ni 1.000000
166 | d 1.000000
167 | illum 2
168 |
169 | newmtl Union_001_001_002_001.001
170 | Ns -1.960784
171 | Ka 1.000000 1.000000 1.000000
172 | Kd 0.039216 0.541176 0.780392
173 | Ks 0.003906 0.003906 0.003906
174 | Ke 0.000000 0.000000 0.000000
175 | Ni 1.000000
176 | d 1.000000
177 | illum 2
178 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link6.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/link6.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/collision/link7.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/collision/link7.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/colors.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/colors.png
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/finger.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'link2.blend'
2 | # Material Count: 2
3 |
4 | newmtl Part__Feature001_006.001
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 0.901961 0.921569 0.929412
8 | Ks 0.250000 0.250000 0.250000
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 | map_Kd colors.png
14 |
15 | newmtl Part__Feature_007.001
16 | Ns -1.960784
17 | Ka 1.000000 1.000000 1.000000
18 | Kd 0.250980 0.250980 0.250980
19 | Ks 0.250000 0.250000 0.250000
20 | Ke 0.000000 0.000000 0.000000
21 | Ni 1.000000
22 | d 1.000000
23 | illum 2
24 | map_Kd colors.png
25 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/finger.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/finger.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/hand.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'link2.blend'
2 | # Material Count: 5
3 |
4 | newmtl Part__Feature001_008_005.001
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 0.250980 0.250980 0.250980
8 | Ks 0.007812 0.007812 0.007812
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 | map_Kd colors.png
14 |
15 | newmtl Part__Feature002_005_005.001
16 | Ns -1.960784
17 | Ka 1.000000 1.000000 1.000000
18 | Kd 0.901961 0.921569 0.929412
19 | Ks 0.015625 0.015625 0.015625
20 | Ke 0.000000 0.000000 0.000000
21 | Ni 1.000000
22 | d 1.000000
23 | illum 2
24 |
25 | newmtl Part__Feature005_001_005.001
26 | Ns -1.960784
27 | Ka 1.000000 1.000000 1.000000
28 | Kd 1.000000 1.000000 1.000000
29 | Ks 0.015625 0.015625 0.015625
30 | Ke 0.000000 0.000000 0.000000
31 | Ni 1.000000
32 | d 1.000000
33 | illum 2
34 | map_Kd colors.png
35 |
36 | newmtl Part__Feature005_001_005_001.001
37 | Ns -1.960784
38 | Ka 1.000000 1.000000 1.000000
39 | Kd 0.901961 0.921569 0.929412
40 | Ks 0.015625 0.015625 0.015625
41 | Ke 0.000000 0.000000 0.000000
42 | Ni 1.000000
43 | d 1.000000
44 | illum 2
45 | map_Kd colors.png
46 |
47 | newmtl Part__Feature_009_005.001
48 | Ns -1.960784
49 | Ka 1.000000 1.000000 1.000000
50 | Kd 0.250980 0.250980 0.250980
51 | Ks 0.015625 0.015625 0.015625
52 | Ke 0.000000 0.000000 0.000000
53 | Ni 1.000000
54 | d 1.000000
55 | illum 2
56 | map_Kd colors.png
57 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/hand.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/hand.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link0.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/link0.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link1.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'link2.blend'
2 | # Material Count: 1
3 |
4 | newmtl Part__Feature_001
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 1.000000 1.000000 1.000000
8 | Ks 0.062500 0.062500 0.062500
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 | map_Kd colors.png
14 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link1.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/link1.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link2.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'link3.blend'
2 | # Material Count: 1
3 |
4 | newmtl Part__Feature024
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 1.000000 1.000000 1.000000
8 | Ks 0.125000 0.125000 0.125000
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 | map_Kd colors.png
14 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link2.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/link2.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link3.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'link4.blend'
2 | # Material Count: 4
3 |
4 | newmtl Part__Feature001_010_001_002
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 1.000000 1.000000 1.000000
8 | Ks 0.007812 0.007812 0.007812
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 | map_Kd colors.png
14 |
15 | newmtl Part__Feature002_007_001_002
16 | Ns -1.960784
17 | Ka 1.000000 1.000000 1.000000
18 | Kd 1.000000 1.000000 1.000000
19 | Ks 0.007812 0.007812 0.007812
20 | Ke 0.000000 0.000000 0.000000
21 | Ni 1.000000
22 | d 1.000000
23 | illum 2
24 | map_Kd colors.png
25 |
26 | newmtl Part__Feature003_004_001_002
27 | Ns -1.960784
28 | Ka 1.000000 1.000000 1.000000
29 | Kd 1.000000 1.000000 1.000000
30 | Ks 0.007812 0.007812 0.007812
31 | Ke 0.000000 0.000000 0.000000
32 | Ni 1.000000
33 | d 1.000000
34 | illum 2
35 | map_Kd colors.png
36 |
37 | newmtl Part__Feature_001_001_001_002
38 | Ns -1.960784
39 | Ka 1.000000 1.000000 1.000000
40 | Kd 0.250980 0.250980 0.250980
41 | Ks 0.007812 0.007812 0.007812
42 | Ke 0.000000 0.000000 0.000000
43 | Ni 1.000000
44 | d 1.000000
45 | illum 2
46 | map_Kd colors.png
47 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link3.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/link3.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link4.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'link4.blend'
2 | # Material Count: 4
3 |
4 | newmtl Part__Feature001_001_003_001.001
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 1.000000 1.000000 1.000000
8 | Ks 0.007812 0.007812 0.007812
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 |
14 | newmtl Part__Feature002_001_003_001.001
15 | Ns -1.960784
16 | Ka 1.000000 1.000000 1.000000
17 | Kd 0.250980 0.250980 0.250980
18 | Ks 0.007812 0.007812 0.007812
19 | Ke 0.000000 0.000000 0.000000
20 | Ni 1.000000
21 | d 1.000000
22 | illum 2
23 | map_Kd colors.png
24 |
25 | newmtl Part__Feature003_001_003_001.001
26 | Ns -1.960784
27 | Ka 1.000000 1.000000 1.000000
28 | Kd 1.000000 1.000000 1.000000
29 | Ks 0.007812 0.007812 0.007812
30 | Ke 0.000000 0.000000 0.000000
31 | Ni 1.000000
32 | d 1.000000
33 | illum 2
34 | map_Kd colors.png
35 |
36 | newmtl Part__Feature_002_003_001.001
37 | Ns -1.960784
38 | Ka 1.000000 1.000000 1.000000
39 | Kd 1.000000 1.000000 1.000000
40 | Ks 0.007812 0.007812 0.007812
41 | Ke 0.000000 0.000000 0.000000
42 | Ni 1.000000
43 | d 1.000000
44 | illum 2
45 | map_Kd colors.png
46 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link4.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/link4.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link5.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'None'
2 | # Material Count: 3
3 |
4 | newmtl Part__Feature_002_004_003.002
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 1.000000 1.000000 1.000000
8 | Ks 0.015625 0.015625 0.015625
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 | map_Kd colors.png
14 |
15 | newmtl Shell001_001_001_003.002
16 | Ns -1.960784
17 | Ka 1.000000 1.000000 1.000000
18 | Kd 0.250000 0.250000 0.250000
19 | Ks 0.015625 0.015625 0.015625
20 | Ke 0.000000 0.000000 0.000000
21 | Ni 1.000000
22 | d 1.000000
23 | illum 2
24 | map_Kd colors.png
25 |
26 | newmtl Shell_001_001_003.002
27 | Ns -1.960784
28 | Ka 1.000000 1.000000 1.000000
29 | Kd 1.000000 1.000000 1.000000
30 | Ks 0.015625 0.015625 0.015625
31 | Ke 0.000000 0.000000 0.000000
32 | Ni 1.000000
33 | d 1.000000
34 | illum 2
35 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link5.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/link5.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link6.mtl:
--------------------------------------------------------------------------------
1 | # Blender MTL File: 'link2.blend'
2 | # Material Count: 17
3 |
4 | newmtl Face064_002_001_002_001.001
5 | Ns -1.960784
6 | Ka 1.000000 1.000000 1.000000
7 | Kd 1.000000 0.000000 0.000000
8 | Ks 0.003906 0.003906 0.003906
9 | Ke 0.000000 0.000000 0.000000
10 | Ni 1.000000
11 | d 1.000000
12 | illum 2
13 |
14 | newmtl Face065_002_001_002_001.001
15 | Ns -1.960784
16 | Ka 1.000000 1.000000 1.000000
17 | Kd 0.000000 1.000000 0.000000
18 | Ks 0.003906 0.003906 0.003906
19 | Ke 0.000000 0.000000 0.000000
20 | Ni 1.000000
21 | d 1.000000
22 | illum 2
23 |
24 | newmtl Face374_002_001_002_001.001
25 | Ns -1.960784
26 | Ka 1.000000 1.000000 1.000000
27 | Kd 1.000000 1.000000 1.000000
28 | Ks 0.003906 0.003906 0.003906
29 | Ke 0.000000 0.000000 0.000000
30 | Ni 1.000000
31 | d 1.000000
32 | illum 2
33 |
34 | newmtl Face539_002_001_002_001.001
35 | Ns -1.960784
36 | Ka 1.000000 1.000000 1.000000
37 | Kd 0.250980 0.250980 0.250980
38 | Ks 0.003906 0.003906 0.003906
39 | Ke 0.000000 0.000000 0.000000
40 | Ni 1.000000
41 | d 1.000000
42 | illum 2
43 | map_Kd colors.png
44 |
45 | newmtl Part__Feature001_009_001_002_001.001
46 | Ns -1.960784
47 | Ka 1.000000 1.000000 1.000000
48 | Kd 0.250980 0.250980 0.250980
49 | Ks 0.003906 0.003906 0.003906
50 | Ke 0.000000 0.000000 0.000000
51 | Ni 1.000000
52 | d 1.000000
53 | illum 2
54 | map_Kd colors.png
55 |
56 | newmtl Part__Feature002_006_001_002_001.001
57 | Ns -1.960784
58 | Ka 1.000000 1.000000 1.000000
59 | Kd 0.250980 0.250980 0.250980
60 | Ks 0.003906 0.003906 0.003906
61 | Ke 0.000000 0.000000 0.000000
62 | Ni 1.000000
63 | d 1.000000
64 | illum 2
65 | map_Kd colors.png
66 |
67 | newmtl Shell002_002_001_002_001.001
68 | Ns -1.960784
69 | Ka 1.000000 1.000000 1.000000
70 | Kd 1.000000 1.000000 1.000000
71 | Ks 0.003906 0.003906 0.003906
72 | Ke 0.000000 0.000000 0.000000
73 | Ni 1.000000
74 | d 1.000000
75 | illum 2
76 |
77 | newmtl Shell003_002_001_002_001.001
78 | Ns -1.960784
79 | Ka 1.000000 1.000000 1.000000
80 | Kd 1.000000 1.000000 1.000000
81 | Ks 0.003906 0.003906 0.003906
82 | Ke 0.000000 0.000000 0.000000
83 | Ni 1.000000
84 | d 1.000000
85 | illum 2
86 |
87 | newmtl Shell004_001_001_002_001.001
88 | Ns -1.960784
89 | Ka 1.000000 1.000000 1.000000
90 | Kd 1.000000 1.000000 1.000000
91 | Ks 0.003906 0.003906 0.003906
92 | Ke 0.000000 0.000000 0.000000
93 | Ni 1.000000
94 | d 1.000000
95 | illum 2
96 |
97 | newmtl Shell005_001_001_002_001.001
98 | Ns -1.960784
99 | Ka 1.000000 1.000000 1.000000
100 | Kd 1.000000 1.000000 1.000000
101 | Ks 0.003906 0.003906 0.003906
102 | Ke 0.000000 0.000000 0.000000
103 | Ni 1.000000
104 | d 1.000000
105 | illum 2
106 |
107 | newmtl Shell006_003_002_001.001
108 | Ns -1.960784
109 | Ka 1.000000 1.000000 1.000000
110 | Kd 0.901961 0.921569 0.929412
111 | Ks 0.015625 0.015625 0.015625
112 | Ke 0.000000 0.000000 0.000000
113 | Ni 1.000000
114 | d 1.000000
115 | illum 2
116 |
117 | newmtl Shell007_002_002_001.001
118 | Ns -1.960784
119 | Ka 1.000000 1.000000 1.000000
120 | Kd 0.250000 0.250000 0.250000
121 | Ks 0.003906 0.003906 0.003906
122 | Ke 0.000000 0.000000 0.000000
123 | Ni 1.000000
124 | d 1.000000
125 | illum 2
126 |
127 | newmtl Shell011_002_002_001.001
128 | Ns -1.960784
129 | Ka 1.000000 1.000000 1.000000
130 | Kd 1.000000 1.000000 1.000000
131 | Ks 0.003906 0.003906 0.003906
132 | Ke 0.000000 0.000000 0.000000
133 | Ni 1.000000
134 | d 1.000000
135 | illum 2
136 |
137 | newmtl Shell012_002_002_001.001
138 | Ns -1.960784
139 | Ka 1.000000 1.000000 1.000000
140 | Kd 1.000000 1.000000 1.000000
141 | Ks 0.003906 0.003906 0.003906
142 | Ke 0.000000 0.000000 0.000000
143 | Ni 1.000000
144 | d 1.000000
145 | illum 2
146 | map_Kd colors.png
147 |
148 | newmtl Shell_003_001_002_001.001
149 | Ns -1.960784
150 | Ka 1.000000 1.000000 1.000000
151 | Kd 0.250980 0.250980 0.250980
152 | Ks 0.003906 0.003906 0.003906
153 | Ke 0.000000 0.000000 0.000000
154 | Ni 1.000000
155 | d 1.000000
156 | illum 2
157 | map_Kd colors.png
158 |
159 | newmtl Union001_001_001_002_001.001
160 | Ns -1.960784
161 | Ka 1.000000 1.000000 1.000000
162 | Kd 0.039216 0.541176 0.780392
163 | Ks 0.003906 0.003906 0.003906
164 | Ke 0.000000 0.000000 0.000000
165 | Ni 1.000000
166 | d 1.000000
167 | illum 2
168 |
169 | newmtl Union_001_001_002_001.001
170 | Ns -1.960784
171 | Ka 1.000000 1.000000 1.000000
172 | Kd 0.039216 0.541176 0.780392
173 | Ks 0.003906 0.003906 0.003906
174 | Ke 0.000000 0.000000 0.000000
175 | Ni 1.000000
176 | d 1.000000
177 | illum 2
178 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link6.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/link6.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/meshes/visual/link7.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/frankaemika_new/meshes/visual/link7.stl
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/panda_arm.urdf:
--------------------------------------------------------------------------------
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 |
107 |
108 |
109 |
110 |
111 |
112 |
113 |
114 |
115 |
116 |
117 |
118 |
119 |
120 |
121 |
122 |
123 |
124 |
125 |
126 |
127 |
128 |
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 |
138 |
139 |
140 |
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 |
156 |
157 |
158 |
159 |
160 |
161 |
162 |
163 |
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
201 |
202 |
203 |
204 |
205 |
206 |
207 |
208 |
209 |
210 |
211 |
212 |
213 |
214 |
215 |
216 |
217 |
218 |
219 |
220 |
221 |
222 |
223 |
224 |
225 |
226 |
227 |
228 |
229 |
230 |
231 |
232 |
233 |
234 |
235 |
236 |
237 |
238 |
239 |
240 |
241 |
242 |
243 |
244 |
245 |
246 |
247 |
248 |
249 |
250 |
251 |
252 |
253 |
254 |
255 |
256 |
257 |
258 |
259 |
260 |
261 |
262 |
263 |
264 |
265 |
266 |
267 |
268 |
269 |
270 |
271 |
272 |
273 |
274 |
275 |
276 |
277 |
278 |
279 |
280 |
281 |
282 |
283 |
284 |
285 |
286 |
287 |
288 |
289 |
290 |
291 |
292 |
293 |
294 |
295 |
296 |
297 |
298 |
299 |
300 |
301 |
302 |
303 |
304 |
305 |
306 |
307 |
308 |
309 |
310 |
311 |
312 |
313 |
314 |
315 |
316 |
317 |
318 |
319 |
320 |
321 |
322 |
323 |
324 |
325 |
326 |
327 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/panda_arm_franka.urdf:
--------------------------------------------------------------------------------
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 |
107 |
108 |
109 |
110 |
111 |
112 |
113 |
114 |
115 |
116 |
117 |
118 |
119 |
120 |
121 |
122 |
123 |
124 |
125 |
126 |
127 |
128 |
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 |
138 |
139 |
140 |
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 |
156 |
157 |
158 |
159 |
160 |
161 |
162 |
163 |
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
201 |
202 |
203 |
204 |
205 |
206 |
207 |
208 |
209 |
210 |
211 |
212 |
213 |
214 |
215 |
216 |
217 |
218 |
219 |
220 |
221 |
222 |
223 |
224 |
225 |
226 |
227 |
228 |
229 |
230 |
231 |
232 |
233 |
234 |
235 |
236 |
237 |
238 |
239 |
240 |
241 |
242 |
243 |
244 |
245 |
246 |
247 |
248 |
249 |
250 |
251 |
252 |
253 |
254 |
255 |
256 |
257 |
258 |
259 |
260 |
261 |
262 |
263 |
264 |
265 |
266 |
267 |
268 |
269 |
270 |
271 |
272 |
273 |
274 |
275 |
276 |
277 |
278 |
279 |
280 |
281 |
282 |
283 |
284 |
285 |
286 |
287 |
288 |
289 |
290 |
291 |
292 |
293 |
294 |
295 |
296 |
297 |
298 |
299 |
300 |
301 |
302 |
303 |
304 |
305 |
306 |
307 |
308 |
309 |
310 |
311 |
312 |
313 |
314 |
315 |
316 |
317 |
318 |
319 |
320 |
321 |
322 |
323 |
324 |
325 |
326 |
327 |
328 |
329 |
330 |
331 |
332 |
333 |
334 |
335 |
336 |
337 |
338 |
339 |
340 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/panda_arm_kdl.urdf:
--------------------------------------------------------------------------------
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 |
107 |
108 |
109 |
110 |
111 |
112 |
113 |
114 |
115 |
116 |
117 |
118 |
119 |
120 |
121 |
122 |
123 |
124 |
125 |
126 |
127 |
128 |
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 |
138 |
139 |
140 |
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 |
156 |
157 |
158 |
159 |
160 |
161 |
162 |
163 |
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
201 |
202 |
203 |
204 |
205 |
206 |
207 |
208 |
209 |
210 |
211 |
212 |
213 |
214 |
215 |
216 |
217 |
218 |
219 |
220 |
221 |
222 |
223 |
224 |
225 |
226 |
227 |
228 |
229 |
230 |
231 |
232 |
233 |
234 |
235 |
236 |
237 |
238 |
239 |
240 |
241 |
242 |
243 |
244 |
245 |
246 |
247 |
248 |
249 |
250 |
251 |
252 |
253 |
254 |
255 |
256 |
257 |
--------------------------------------------------------------------------------
/notebooks/data/frankaemika_new/panda_arm_rivet.urdf:
--------------------------------------------------------------------------------
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 |
107 |
108 |
109 |
110 |
111 |
112 |
113 |
114 |
115 |
116 |
117 |
118 |
119 |
120 |
121 |
122 |
123 |
124 |
125 |
126 |
127 |
128 |
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 |
138 |
139 |
140 |
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 |
156 |
157 |
158 |
159 |
160 |
161 |
162 |
163 |
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
201 |
202 |
203 |
204 |
205 |
206 |
207 |
208 |
209 |
210 |
211 |
212 |
213 |
214 |
215 |
216 |
217 |
218 |
219 |
220 |
221 |
222 |
223 |
224 |
225 |
226 |
227 |
228 |
229 |
230 |
231 |
232 |
233 |
234 |
235 |
236 |
237 |
238 |
239 |
240 |
241 |
242 |
243 |
244 |
245 |
246 |
247 |
248 |
249 |
250 |
251 |
252 |
253 |
254 |
255 |
256 |
257 |
258 |
259 |
260 |
261 |
262 |
263 |
264 |
265 |
266 |
267 |
268 |
269 |
270 |
271 |
272 |
273 |
274 |
275 |
276 |
277 |
278 |
279 |
280 |
281 |
282 |
283 |
284 |
285 |
286 |
287 |
288 |
289 |
290 |
291 |
292 |
293 |
294 |
295 |
296 |
297 |
298 |
299 |
300 |
301 |
302 |
303 |
304 |
305 |
306 |
307 |
308 |
309 |
310 |
311 |
312 |
313 |
314 |
315 |
316 |
317 |
318 |
319 |
320 |
321 |
322 |
323 |
324 |
325 |
326 |
327 |
328 |
329 |
330 |
331 |
332 |
333 |
334 |
335 |
336 |
337 |
338 |
339 |
340 |
341 |
342 |
343 |
344 |
345 |
346 |
347 |
348 |
349 |
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/finger.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/finger.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/hand.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/hand.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/link0.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/link0.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/link1.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/link1.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/link2.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/link2.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/link3.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/link3.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/link4.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/link4.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/link5.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/link5.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/link6.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/link6.stl
--------------------------------------------------------------------------------
/notebooks/data/meshes/collision/link7.stl:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/teguhSL/optimal_control_distribution/dc8cbcb49953dfc0fff586fd6a494e9fadbb7dc6/notebooks/data/meshes/collision/link7.stl
--------------------------------------------------------------------------------
/notebooks/data/panda_arm.urdf:
--------------------------------------------------------------------------------
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 |
107 |
108 |
109 |
110 |
111 |
112 |
113 |
114 |
115 |
116 |
117 |
118 |
119 |
120 |
121 |
122 |
123 |
124 |
125 |
126 |
127 |
128 |
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 |
138 |
139 |
140 |
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 |
156 |
157 |
158 |
159 |
160 |
161 |
162 |
163 |
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
201 |
202 |
203 |
204 |
205 |
206 |
207 |
208 |
209 |
210 |
211 |
212 |
213 |
214 |
215 |
216 |
217 |
218 |
219 |
220 |
221 |
222 |
223 |
224 |
225 |
226 |
242 |
243 |
244 |
245 |
246 |
247 |
248 |
249 |
250 |
251 |
252 |
253 |
254 |
255 |
256 |
257 |
258 |
259 |
260 |
261 |
262 |
263 |
264 |
265 | -->
266 |
267 |
268 |
269 |
270 |
271 |
272 |
273 |
274 |
275 |
276 |
277 |
278 |
279 |
280 |
281 |
282 |
283 |
284 |
285 |
286 |
287 |
288 |
289 |
290 |
291 |
292 |
293 |
294 |
295 |
296 |
297 |
298 |
299 |
300 |
301 |
302 |
303 |
304 |
305 |
306 |
307 |
308 |
309 |
310 |
311 |
312 |
313 |
314 |
315 |
316 |
317 |
318 |
319 |
320 |
321 |
322 |
323 |
324 |
325 |
326 |
327 |
328 |
329 |
330 |
331 |
--------------------------------------------------------------------------------
/notebooks/data/panda_arm_gripper.urdf:
--------------------------------------------------------------------------------
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 |
107 |
108 |
109 |
110 |
111 |
112 |
113 |
114 |
115 |
116 |
117 |
118 |
119 |
120 |
121 |
122 |
123 |
124 |
125 |
126 |
127 |
128 |
129 |
130 |
131 |
132 |
133 |
134 |
135 |
136 |
137 |
138 |
139 |
140 |
141 |
142 |
143 |
144 |
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 |
154 |
155 |
156 |
157 |
158 |
159 |
160 |
161 |
162 |
163 |
164 |
165 |
166 |
167 |
168 |
169 |
170 |
171 |
172 |
173 |
174 |
175 |
176 |
177 |
178 |
179 |
180 |
181 |
182 |
183 |
184 |
185 |
186 |
187 |
188 |
189 |
190 |
191 |
192 |
193 |
194 |
195 |
196 |
197 |
198 |
199 |
200 |
201 |
202 |
203 |
204 |
205 |
206 |
207 |
208 |
209 |
210 |
211 |
212 |
213 |
214 |
215 |
216 |
217 |
218 |
219 |
220 |
221 |
222 |
223 |
224 |
225 |
226 |
227 |
228 |
229 |
230 |
231 |
232 |
233 |
234 |
235 |
236 |
237 |
238 |
239 |
240 |
241 |
242 |
243 |
244 |
245 |
246 |
247 |
248 |
249 |
250 |
251 |
252 |
253 |
254 |
255 |
256 |
257 |
258 |
259 |
260 |
261 |
262 |
263 |
264 |
265 |
266 |
267 |
268 |
269 |
270 |
271 |
272 |
273 |
274 |
275 |
276 |
277 |
278 |
279 |
280 |
281 |
282 |
283 |
284 |
285 |
286 |
287 |
288 |
289 |
290 |
291 |
292 |
--------------------------------------------------------------------------------
/notebooks/ocp_sys.py:
--------------------------------------------------------------------------------
1 | import numpy as np
2 | import matplotlib.pyplot as plt
3 | from IPython.display import clear_output
4 | import time
5 | from casadi import mtimes, MX, sin, cos, vertcat, horzcat, sum1, cross, Function, jacobian
6 | import casadi
7 | from utils import *
8 | import pinocchio
9 | import crocoddyl
10 |
11 | class LinearSystem():
12 | def __init__(self,A,B):
13 | self.A = A
14 | self.B = B
15 | self.Dx = A.shape[0]
16 | self.Du = B.shape[1]
17 |
18 | def reset_AB(self, A,B):
19 | self.A = A
20 | self.B = B
21 |
22 | def set_init_state(self,x0):
23 | self.x0 = x0
24 |
25 | def step(self, x, u):
26 | return self.A.dot(x) + self.B.dot(u)
27 |
28 | def rollout(self,us):
29 | x_cur = self.x0
30 | xs = [x_cur]
31 | T = len(us)
32 | for i in range(T):
33 | x_cur = self.step(x_cur, us[i])
34 | xs += [x_cur]
35 | return np.array(xs)
36 |
37 | class Unicycle():
38 | def __init__(self, dt = 0.01):
39 | self.dt = dt
40 | self.Dx = 3
41 | self.Du = 2
42 |
43 | def set_init_state(self,x0):
44 | self.x0 = x0
45 |
46 | def compute_matrices(self,x,u):
47 | A = np.eye(3)
48 | A[0,2] = -u[0]*np.sin(x[2])*self.dt
49 | A[1,2] = u[0]*np.cos(x[2])*self.dt
50 |
51 | B = np.zeros((3,2))
52 | B[0,0] = np.cos(x[2])*self.dt
53 | B[1,0] = np.sin(x[2])*self.dt
54 | B[2,1] = 1*self.dt
55 | self.A, self.B = A,B
56 | return A,B
57 |
58 | def step(self, x, u):
59 | #A,B = self.compute_matrices(x,u)
60 | x_next = np.zeros(3)
61 |
62 | x_next[0] = x[0] + u[0]*np.cos(x[2])*self.dt
63 | x_next[1] = x[1] + u[0]*np.sin(x[2])*self.dt
64 | x_next[2] = x[2] + u[1]*self.dt
65 | #pdb.set_trace()
66 | return x_next
67 | #return A.dot(x) + B.dot(u)
68 |
69 | def rollout(self,us):
70 | x_cur = self.x0.copy()
71 | xs = [x_cur]
72 | T = len(us)
73 | for i in range(T):
74 | x_cur = self.step(x_cur, us[i])
75 | xs += [x_cur]
76 | return np.array(xs)
77 |
78 | class SecondUnicycle():
79 | def __init__(self, dt = 0.01):
80 | self.dt = dt
81 | self.Dx = 5
82 | self.Du = 2
83 |
84 | def set_init_state(self,x0):
85 | self.x0 = x0
86 |
87 | def compute_matrices(self,x,u):
88 | A = np.eye(self.Dx)
89 | A[0,2] = -x[3]*np.sin(x[2])*self.dt
90 | A[1,2] = x[3]*np.cos(x[2])*self.dt
91 |
92 | A[0,3] = np.cos(x[2])*self.dt
93 | A[1,3] = np.sin(x[2])*self.dt
94 | A[2,4] = 1*self.dt
95 |
96 | B = np.zeros((self.Dx,self.Du))
97 | B[3,0] = self.dt
98 | B[4,1] = self.dt
99 |
100 | self.A, self.B = A,B
101 | return A,B
102 |
103 | def step(self, x, u):
104 | x_next = np.zeros(self.Dx)
105 |
106 | x_next[0] = x[0] + x[3]*np.cos(x[2])*self.dt
107 | x_next[1] = x[1] + x[3]*np.sin(x[2])*self.dt
108 | x_next[2] = x[2] + x[4]*self.dt
109 | x_next[3] = x[3] + u[0]*self.dt
110 | x_next[4] = x[4] + u[1]*self.dt
111 | return x_next
112 |
113 | def rollout(self,us):
114 | x_cur = self.x0.copy()
115 | xs = [x_cur]
116 | T = len(us)
117 | for i in range(T):
118 | x_cur = self.step(x_cur, us[i])
119 | xs += [x_cur]
120 | return np.array(xs)
121 |
122 | class Pendulum():
123 | def __init__(self, dt = 0.01):
124 | self.dt = dt
125 | self.Dx = 2
126 | self.Du = 1
127 | self.b = 1
128 | self.m = 1
129 | self.l = 1
130 |
131 | def set_init_state(self,x0):
132 | self.x0 = x0
133 |
134 | def compute_matrices(self,x,u):
135 | A = np.eye(self.Dx)
136 | B = np.zeros((self.Dx,self.Du))
137 |
138 | A[0,1] = self.dt
139 | A[1,0] = 0.5*9.8*self.dt*np.cos(x[0])/self.l
140 | A[1,1] = 1 - self.dt*self.b/(self.m*self.l**2)
141 |
142 | B[1,0] = self.dt/(self.m*self.l**2)
143 |
144 | self.A, self.B = A,B
145 | return A,B
146 |
147 | def step(self, x, u):
148 | x_next = np.zeros(self.Dx)
149 | x_next[0] = x[0] + x[1]*self.dt
150 | x_next[1] = (1-self.dt*self.b/(self.m*self.l**2))*x[1] + 0.5*9.8*self.dt*np.sin(x[0])/self.l + self.dt*u/(self.m*self.l**2)
151 | return x_next
152 |
153 | def rollout(self,us):
154 | x_cur = self.x0.copy()
155 | xs = [x_cur]
156 | T = len(us)
157 | for i in range(T):
158 | x_cur = self.step(x_cur, us[i])
159 | xs += [x_cur]
160 | return np.array(xs)
161 |
162 | def plot(self, x, color='k'):
163 | px = np.array([0, -self.l*np.sin(x[0])])
164 | py = np.array([0, self.l*np.cos(x[0])])
165 | line = plt.plot(px, py, marker='o', color=color, lw=10, mfc='w', solid_capstyle='round')
166 | xlim = [-2*self.l, 2*self.l]
167 | plt.axes().set_aspect('equal')
168 | plt.axis(xlim+xlim)
169 | return line
170 |
171 | def plot_traj(self, xs, dt = 0.1, filename = None):
172 | for i,x in enumerate(xs):
173 | clear_output(wait=True)
174 | self.plot(x)
175 | if filename is not None:
176 | plt.savefig('temp/fig'+str(i)+'.png')
177 | plt.show()
178 | time.sleep(dt)
179 |
180 |
181 | class Bicopter():
182 | def __init__(self, dt = 0.01):
183 | self.dt = dt
184 | self.Dx = 6
185 | self.Du = 2
186 |
187 | self.m = 2.5
188 | self.l = 1
189 | self.I = 1.2
190 |
191 | def set_init_state(self,x0):
192 | self.x0 = x0
193 |
194 | def compute_matrices(self,x,u):
195 | A = np.eye(self.Dx)
196 | A[:3, 3:] = np.eye(3)*self.dt
197 | A[3,2] = -self.dt*(u[0]+u[1])*np.cos(x[2])/self.m
198 | A[4,2] = -self.dt*(u[0]+u[1])*np.sin(x[2])/self.m
199 |
200 | B = np.zeros((self.Dx,self.Du))
201 | B[3,0] = -self.dt*np.sin(x[2])/self.m
202 | B[3,1] = B[3,0]
203 |
204 | B[4,0] = self.dt*np.cos(x[2])/self.m
205 | B[4,1] = B[4,0]
206 |
207 | B[5,0] = self.dt*self.l*0.5/self.I
208 | B[5,1] = -B[5,0]
209 |
210 | self.A, self.B = A,B
211 | return A,B
212 |
213 | def step(self, x, u):
214 | x_next = np.zeros(self.Dx)
215 |
216 | x_next[0] = x[0] + x[3]*self.dt
217 | x_next[1] = x[1] + x[4]*self.dt
218 | x_next[2] = x[2] + x[5]*self.dt
219 |
220 | x_next[3] = x[3] - (u[0]+u[1])*np.sin(x[2])*self.dt/self.m
221 | x_next[4] = x[4] + (u[0]+u[1])*np.cos(x[2])*self.dt/self.m - 9.8*self.dt
222 | x_next[5] = x[5] + (u[0]-u[1])*self.dt*self.l*0.5/self.I
223 |
224 | return x_next
225 |
226 | def rollout(self,us):
227 | x_cur = self.x0.copy()
228 | xs = [x_cur]
229 | T = len(us)
230 | for i in range(T):
231 | x_cur = self.step(x_cur, us[i])
232 | xs += [x_cur]
233 | return np.array(xs)
234 |
235 | def plot(self, x, color = 'k'):
236 | pxs = np.array([x[0] + 0.5*self.l*np.cos(x[2]), x[0] - 0.5*self.l*np.cos(x[2])])
237 | pys = np.array([x[1] + 0.5*self.l*np.sin(x[2]), x[1] - 0.5*self.l*np.sin(x[2])])
238 | line = plt.plot(pxs, pys, marker='o', color=color, lw=10, mfc='w', solid_capstyle='round')
239 | return line
240 |
241 | def vis_traj(self, xs, dt = 0.1, axes_lim = [-5,5,-5,5]):
242 | T = len(xs)
243 | for x in xs:
244 | clear_output(wait=True)
245 | self.plot(x)
246 | plt.axes().set_aspect('equal')
247 | plt.axis(axes_lim)
248 | plt.show()
249 | time.sleep(dt)
250 |
251 |
252 | class Quadcopter():
253 | def __init__(self, dt = 0.01, I = np.diag(np.array([2,2,4])), kd = 1,
254 | k = 1, L = 0.3, b = 1, m=1, g=9.81, Dx=12, Du=4):
255 | self.I = I #inertia
256 | self.kd = kd #friction
257 | self.k = k #motor constant
258 | self.L = L# distance between center and motor
259 | self.b = b # drag coefficient
260 | self.m = m # mass
261 | self.g = g
262 | self.Dx = Dx
263 | self.Du = Du
264 | self.dt = dt
265 |
266 | def thrust(self, inputs):
267 | T = np.array([0,0, self.k*np.sum(inputs)])
268 | return T
269 |
270 | def torques(self, inputs):
271 | tau = np.array([self.L*self.k*(inputs[0]-inputs[2]), self.L*self.k*(inputs[1]-inputs[3]), \
272 | self.b*(inputs[0]-inputs[1] + inputs[2] - inputs[3])])
273 | return tau
274 |
275 | def acceleration(self, inputs, angles, xdot):
276 | gravity = np.array([0,0,-self.g])
277 | R = self.Rotation(angles)
278 | T = R.dot(self.thrust(inputs))
279 | Fd = -self.kd*xdot
280 | a = gravity + T/self.m + Fd
281 | return a
282 |
283 | def angular_acceleration(self, inputs, omega):
284 | tau = self.torques(inputs)
285 | omegadot = np.linalg.inv(self.I).dot(tau - np.cross(omega, self.I.dot(omega)))
286 | return omegadot
287 |
288 | def thetadot2omega(self, thetadot, theta):
289 | R = np.array([[1, 0, -np.sin(theta[1])], \
290 | [0, np.cos(theta[0]), np.cos(theta[1])*np.sin(theta[0])], \
291 | [0, -np.sin(theta[0]), np.cos(theta[1])*np.cos(theta[0])]])
292 | return R.dot(thetadot)
293 |
294 | def omega2thetadot(self, omega, theta):
295 | R = np.array([[1, 0, -np.sin(theta[1])], \
296 | [0, np.cos(theta[0]), np.cos(theta[1])*np.sin(theta[0])], \
297 | [0, -np.sin(theta[0]), np.cos(theta[1])*np.cos(theta[0])]])
298 | return np.linalg.inv(R).dot(omega)
299 |
300 | def Rotation(self, theta):
301 | c0,s0 = np.cos(theta[0]), np.sin(theta[0])
302 | c1,s1 = np.cos(theta[1]), np.sin(theta[1])
303 | c2,s2 = np.cos(theta[2]), np.sin(theta[2])
304 |
305 | R = np.array([[c0*c2 - c1*s0*s2, -c2*s0 - c0*c1*s2, s1 * s2],
306 | [c1*c2*s0 + c0*s2, c0*c1*c2-s0*s2, -c2*s1],
307 | [s0*s1, c0*s1, c1]])
308 | return R
309 |
310 | def set_init_state(self,x0):
311 | self.x0 = x0
312 |
313 | def compute_matrices(self,x,u, inc = 0.001):
314 | Dx, Du = len(x), len(u)
315 | A = np.zeros((Dx, Dx))
316 | B = np.zeros((Dx, Du))
317 |
318 | xnext = self.step(x, u)
319 | for i in range(Dx):
320 | xp, xm = x.copy(), x.copy()
321 | xp[i] += inc
322 | xnextp = self.step(xp, u)
323 | xm[i] -= inc
324 | xnextm = self.step(xm, u)
325 | diff = (xnextp - xnextm)/(2*inc)
326 | A[:,i] = diff
327 |
328 | for i in range(Du):
329 | up, um = u.copy(), u.copy()
330 | up[i] += inc
331 | xnextp = self.step(x, up)
332 | um[i] -= inc
333 | xnextm = self.step(x, um)
334 | diff = (xnextp - xnextm)/(2*inc)
335 | B[:,i] = diff
336 |
337 | return A,B
338 |
339 | def step(self, x, u, u_offset = None):
340 | if u_offset is None:
341 | u_mag = np.sqrt(9.81/4)
342 | u_offset = np.array([u_mag]*self.Du)**2
343 | u_act = u_offset + u**2
344 | p, pdot, theta, thetadot = x[:3], x[3:6], x[6:9], x[9:]
345 |
346 | #step
347 | omega = self.thetadot2omega(thetadot, theta)
348 |
349 | a = self.acceleration(u_act, theta, pdot)
350 | omegadot = self.angular_acceleration(u_act, omega)
351 | omega = omega + self.dt*omegadot
352 | thetadot= self.omega2thetadot(omega, theta)
353 | theta = theta + self.dt*thetadot
354 | pdot = pdot + self.dt*a
355 | p = p + self.dt*pdot
356 |
357 | x_next = np.concatenate([p, pdot, theta, thetadot])
358 | return x_next
359 |
360 | def rollout(self,us):
361 | x_cur = self.x0.copy()
362 | xs = [x_cur]
363 | T = len(us)
364 | for i in range(T):
365 | x_cur = self.step(x_cur, us[i])
366 | xs += [x_cur]
367 | return np.array(xs)
368 |
369 | def vis_quad(self, quadId, xs, dt = 0.05):
370 | for i,x in enumerate(xs):
371 | ori = euler2quat(xs[i,6:9], 'rzyz')
372 | p.resetBasePositionAndOrientation(quadId, xs[i,:3], ori)
373 | time.sleep(dt)
374 | p.resetDebugVisualizerCamera(cameraDistance=2.5, cameraYaw=90,
375 | cameraPitch=-20, cameraTargetPosition=xs[i,:3])
376 |
377 |
378 | class QuadcopterCasadi():
379 | def __init__(self, dt = 0.01, I = np.diag(np.array([2,2,4])), kd = 1,
380 | k = 1, L = 0.3, b = 1, m=1, g=9.81, Dx=12, Du=4):
381 | self.I = I #inertia
382 | self.I_inv = np.linalg.inv(self.I)
383 | self.kd = kd #friction
384 | self.k = k #motor constant
385 | self.L = L# distance between center and motor
386 | self.b = b # drag coefficient
387 | self.m = m # mass
388 | self.g = g
389 | self.Dx = Dx
390 | self.Du = Du
391 | self.dt = dt
392 | self.gravity = np.array([0,0,-self.g])
393 | self.x = MX.sym('x', Dx)
394 | self.u = MX.sym('u', Du)
395 |
396 | #initialise
397 | self.def_step_func(self.x, self.u)
398 | self.def_jacobian()
399 |
400 | def thrust(self, inputs):
401 | T = vertcat(0,0, self.k*sum1(inputs))
402 | return T
403 |
404 | def torques(self, inputs):
405 | tau = vertcat(self.L*self.k*(inputs[0]-inputs[2]), self.L*self.k*(inputs[1]-inputs[3]), \
406 | self.b*(inputs[0]-inputs[1] + inputs[2] - inputs[3]))
407 | return tau
408 |
409 | def acceleration(self, inputs, thetas, xdot):
410 | R = self.Rotation(thetas)
411 | T = mtimes(R,self.thrust(inputs))
412 | Fd = -self.kd*xdot
413 | a = self.gravity + T/self.m + Fd
414 | return a
415 |
416 | def angular_acceleration(self, inputs, omega):
417 | tau = self.torques(inputs)
418 | omegadot = mtimes(self.I_inv, tau - cross(omega, mtimes(self.I,omega)))
419 | return omegadot
420 |
421 | def thetadot2omega(self, thetadot, theta):
422 | R1 = vertcat(1,0,0)
423 | R2 = vertcat(0, cos(theta[0]), -sin(theta[0]))
424 | R3 = vertcat(-sin(theta[1]), cos(theta[1])*sin(theta[0]), cos(theta[1])*cos(theta[0]))
425 | R = horzcat(R1, R2, R3)
426 | return mtimes(R,thetadot)
427 |
428 | def omega2thetadot(self, omega, theta):
429 | R1 = vertcat(1,0,0)
430 | R2 = vertcat(0, cos(theta[0]), -sin(theta[0]))
431 | R3 = vertcat(-sin(theta[1]), cos(theta[1])*sin(theta[0]), cos(theta[1])*cos(theta[0]))
432 | R = horzcat(R1, R2, R3)
433 | return mtimes(casadi.inv(R), omega)
434 |
435 | def Rotation(self, theta):
436 | c0,s0 = cos(theta[0]), sin(theta[0])
437 | c1,s1 = cos(theta[1]), sin(theta[1])
438 | c2,s2 = cos(theta[2]), sin(theta[2])
439 |
440 | R1 = vertcat(c0*c2 - c1*s0*s2, c1*c2*s0 + c0*s2, s0*s1)
441 | R2 = vertcat(-c2*s0 - c0*c1*s2, c0*c1*c2-s0*s2, c0*s1 )
442 | R3 = vertcat( s1 * s2, -c2*s1, c1)
443 | R = horzcat(R1,R2,R3)
444 | return R
445 |
446 | def set_init_state(self,x0):
447 | self.x0 = x0
448 |
449 | def def_step_func(self, x, u, u_offset = None):
450 | if u_offset is None:
451 | u_mag = np.sqrt(9.81/4)
452 | u_offset = np.array([u_mag]*self.Du)**2
453 | u_act = u_offset + u
454 | p, pdot, theta, thetadot = x[:3], x[3:6], x[6:9], x[9:]
455 |
456 | #step
457 | omega = self.thetadot2omega(thetadot, theta)
458 |
459 | a = self.acceleration(u_act, theta, pdot)
460 | omegadot = self.angular_acceleration(u_act, omega)
461 | omega = omega + self.dt*omegadot
462 | thetadot= self.omega2thetadot(omega, theta)
463 | theta = theta + self.dt*thetadot
464 | pdot = pdot + self.dt*a
465 | p = p + self.dt*pdot
466 |
467 | self.x_next = vertcat(p, pdot, theta, thetadot)
468 | self.step_fun = Function('step', [x, u], [self.x_next])
469 |
470 | def step(self, x, u):
471 | return np.array(self.step_fun(x,u)).flatten()
472 |
473 | def def_jacobian(self):
474 | self.A = jacobian(self.x_next, self.x)
475 | self.B = jacobian(self.x_next, self.u)
476 |
477 | self.A_val = Function('A', [self.x,self.u], [self.A])
478 | self.B_val = Function('B', [self.x,self.u], [self.B])
479 |
480 | def compute_matrices(self,x,u):
481 | A = np.array(self.A_val(x,u))
482 | B = np.array(self.B_val(x,u))
483 | return A, B
484 |
485 | def rollout(self,us):
486 | x_cur = self.x0.copy()
487 | xs = [x_cur]
488 | T = len(us)
489 | for i in range(T):
490 | x_cur = self.step(x_cur, us[i])
491 | xs += [x_cur]
492 | return np.array(xs)
493 |
494 | def vis_traj(self, quadId, xs, dt = 0.05, camDist = 2.5, camYaw = 90, camPitch = -20, changeCamera = True):
495 | for i,x in enumerate(xs):
496 | ori = euler2quat(xs[i,6:9], 'rzyz')
497 | p.resetBasePositionAndOrientation(quadId, xs[i,:3], ori)
498 | time.sleep(dt)
499 | if changeCamera: p.resetDebugVisualizerCamera(cameraDistance=camDist, cameraYaw=camYaw,
500 | cameraPitch= camPitch, cameraTargetPosition=xs[i,:3])
501 |
502 | class ActionModelRobot(crocoddyl.ActionModelAbstract):
503 | def __init__(self, state, nu):
504 | crocoddyl.ActionModelAbstract.__init__(self, state, nu)
505 |
506 | def init_robot_sys(self,robot_sys, nr = 1):
507 | self.robot_sys = robot_sys
508 | self.Du = robot_sys.Du
509 | self.Dx = robot_sys.Dx
510 | self.Dr = nr
511 |
512 | def set_cost(self, cost_model):
513 | self.cost_model = cost_model
514 |
515 | def calc(self, data, x, u):
516 | #calculate the cost
517 | data.cost = self.cost_model.calc(x,u)
518 |
519 | #calculate the next state
520 | data.xnext = self.robot_sys.step(x,u)
521 |
522 | def calcDiff(self, data, x, u, recalc = False):
523 | if recalc:
524 | self.calc(data, x, u)
525 |
526 | #compute cost derivatives
527 | self.cost_model.calcDiff(x, u)
528 | data.Lx = self.cost_model.Lx.copy()
529 | data.Lxx = self.cost_model.Lxx.copy()
530 | data.Lu = self.cost_model.Lu.copy()
531 | data.Luu = self.cost_model.Luu.copy()
532 |
533 | #compute dynamic derivatives
534 | A, B = self.robot_sys.compute_matrices(x,u)
535 | data.Fx = A.copy()
536 | data.Fu = B.copy()
537 |
538 | def createData(self):
539 | data = ActionDataRobot(self)
540 | return data
541 |
542 | class ActionDataRobot(crocoddyl.ActionDataAbstract):
543 | def __init__(self, model):
544 | crocoddyl.ActionDataAbstract.__init__(self,model)
545 |
546 | class TwoLinkRobot():
547 | def __init__(self, dt = 0.01):
548 | self.dt = dt
549 | self.Dx = 4
550 | self.Du = 2
551 | self.l1 = 1.5
552 | self.l2 = 1
553 |
554 | def set_init_state(self,x0):
555 | self.x0 = x0
556 |
557 | def set_pref(self, p_ref):
558 | self.p_ref = p_ref
559 |
560 | def compute_matrices(self,x,u):
561 | A = np.eye(self.Dx)
562 | B = np.zeros((self.Dx,self.Du))
563 |
564 | A[0,2] = self.dt
565 | A[1,3] = self.dt
566 |
567 | B[2:,:] = np.eye(self.Du)
568 |
569 | self.A, self.B = A,B
570 | return A,B
571 |
572 | def compute_Jacobian(self, x):
573 | J = np.zeros((2, self.Dx))
574 | s1 = np.sin(x[0])
575 | c1 = np.cos(x[0])
576 | s12 = np.sin(x[0] + x[1])
577 | c12 = np.cos(x[0] + x[1])
578 |
579 | J[0,0] = -self.l1*s1 - self.l2*s12
580 | J[0,1] = - self.l2*s12
581 | J[1,0] = self.l1*c1 + self.l2*c12
582 | J[1,1] = self.l2*c12
583 |
584 | self.J = J
585 | return self.J
586 |
587 | def step(self, x, u):
588 | x_next = self.A.dot(x) + self.B.dot(u)
589 | return x_next
590 |
591 | def rollout(self,us):
592 | x_cur = self.x0.copy()
593 | xs = [x_cur]
594 | T = len(us)
595 | for i in range(T):
596 | x_cur = self.step(x_cur, us[i])
597 | xs += [x_cur]
598 | return np.array(xs)
599 |
600 | def compute_ee(self,x):
601 | self.p1 = np.array([self.l1*np.cos(x[0]), self.l1*np.sin(x[0])])
602 | self.p2 = np.array([self.p1[0] + self.l2*np.cos(x[0] + x[1]), self.p1[1] + self.l2*np.sin(x[0] + x[1])])
603 | return self.p1, self.p2
604 |
605 | def plot(self, x, color='k'):
606 | self.compute_ee(x)
607 |
608 | line1 = plt.plot(np.array([0, self.p1[0]]),np.array([0, self.p1[1]]) , marker='o', color=color, lw=10, mfc='w', solid_capstyle='round')
609 | line2 = plt.plot(np.array([self.p1[0], self.p2[0]]),np.array([self.p1[1], self.p2[1]]) , marker='o', color=color, lw=10, mfc='w', solid_capstyle='round')
610 | xlim = [-1.5*(self.l1+self.l2), 1.5*(self.l1+self.l2)]
611 | plt.axes().set_aspect('equal')
612 | plt.axis(xlim+xlim)
613 | return line1,line2
614 |
615 | def plot_traj(self, xs, dt = 0.1):
616 | for x in xs:
617 | clear_output(wait=True)
618 | self.plot(x)
619 | plt.plot(self.p_ref[0], self.p_ref[1], '*')
620 | plt.show()
621 | time.sleep(self.dt)
622 |
623 | class Manipulator():
624 | def __init__(self, rmodel, ee_id, dt = 0.01):
625 | self.dt = dt
626 | self.rmodel = rmodel
627 | self.rdata = rmodel.createData()
628 | self.Dx = rmodel.nq + rmodel.nv
629 | self.Du = rmodel.nv
630 | self.ee_id = ee_id
631 |
632 | def set_init_state(self,x0):
633 | self.x0 = x0
634 |
635 | def compute_matrices(self,x,u):
636 | A = np.eye(self.Dx)
637 | B = np.zeros((self.Dx,self.Du))
638 |
639 | A[:self.Dx/2,self.Dx/2:] = np.eye(self.Dx/2)*self.dt
640 |
641 | B[self.Dx/2:,:] = np.eye(self.Du)
642 |
643 | self.A, self.B = A,B
644 | return A,B
645 |
646 | def compute_Jacobian(self, x):
647 | q = x[:self.Dx/2]
648 | pin.forwardKinematics(self.rmodel,self.rdata,q)
649 | pin.updateFramePlacements(self.rmodel,self.rdata)
650 | pin.computeJointJacobians(self.rmodel, self.rdata, q)
651 | J = pin.getFrameJacobian(self.rmodel, self.rdata,self.ee_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
652 | self.J = J[:3,:]
653 | return self.J
654 |
655 | def step(self, x, u):
656 | x_next = self.A.dot(x) + self.B.dot(u)
657 | return x_next
658 |
659 | def rollout(self,us):
660 | x_cur = self.x0.copy()
661 | xs = [x_cur]
662 | T = len(us)
663 | for i in range(T):
664 | x_cur = self.step(x_cur, us[i])
665 | xs += [x_cur]
666 | return np.array(xs)
667 |
668 | def compute_ee(self,x):
669 | q = x[:self.Dx/2]
670 | pin.forwardKinematics(self.rmodel, self.rdata, q)
671 | pin.updateFramePlacements(self.rmodel, self.rdata)
672 | pos, ori = self.rdata.oMf[self.ee_id].translation, self.rdata.oMf[self.ee_id].rotation
673 | return pos, ori
674 |
675 | # def plot_traj(self, xs, dt = 0.1):
676 | # for x in xs:
677 | # q = x[:self.Dx/2]
678 |
679 | # time.sleep(self.dt)
--------------------------------------------------------------------------------
/notebooks/utils.py:
--------------------------------------------------------------------------------
1 | from scipy.spatial.transform import Rotation as R
2 | from numpy.linalg import inv
3 | import pybullet as p
4 | import numpy as np
5 | import pinocchio as pin
6 | import transforms3d
7 | import time
8 | import pyscreenshot as ImageGrab
9 |
10 | def set_ref(ddp, xs):
11 | #given the reference traj. xs, set the ddp to follow this xs
12 | T = len(xs)
13 | for i in range(T-1):
14 | ddp.problem.runningModels[i].cost_model.costs[0].set_ref(xs[i])
15 | ddp.problem.terminalModel.cost_model.costs[0].set_ref(xs[-1])
16 |
17 | def set_Qref(ddp, Qref):
18 | #given the precision matrices Qref, set the ddp to use Qref in the cost function
19 | T = len(Qref)
20 | for i in range(T-1):
21 | ddp.problem.runningModels[i].cost_model.costs[0].Q = Qref[i]
22 | ddp.problem.terminalModel.cost_model.costs[0].Q = Qref[T-1]
23 |
24 | def extract_ref(mu, sigma, Dx, T_hor, x, reg = 1e-6):
25 | #given the distribution N(mu,sigma), obtain the reference traj. distribution
26 | # as the marginal distribution at time t
27 | mu_ = mu.reshape(-1, Dx)
28 | T = mu.shape[0]
29 |
30 | #obtain the reference trajectory
31 | ref_x = subsample(np.vstack([x[None,:], mu_[:T_hor]]), T_hor+1)
32 |
33 | #obtain the reference precision
34 | Qs = np.zeros((T_hor+1, Dx, Dx))
35 | if T_hor < T:
36 | #if the horizon is within the remaining time steps, extract the marginal distribution
37 | for i in range(T_hor):
38 | Qs[i+1] = inv(sigma[Dx*i:Dx*(i+1), Dx*i:Dx*(i+1)]+ reg*np.eye(Dx))
39 | else:
40 | #if the horizon exceeds the remaining time steps
41 | for i in range(T):
42 | Qs[i+1] = inv(sigma[Dx*i:Dx*(i+1), Dx*i:Dx*(i+1)]+ reg*np.eye(Dx))
43 | for i in range(T, T_hor):
44 | Qs[i+1] = Qs[T]
45 |
46 | #the first Q does not affect the OCP and can be set as anything
47 | Qs[0] = Qs[1].copy()
48 |
49 | return ref_x, Qs
50 |
51 | def calc_detail_cost(xs, us, ddp):
52 | rmodels = ddp.problem.runningModels
53 | cost_control = 0.
54 | cost_goal = 0.
55 | cost_state = 0.
56 | cost_col = []
57 | for i in range(len(xs)-1):
58 | costs = rmodels[i].cost_model.costs
59 | cost_state += costs[0].calc(xs[i], us[i])
60 | cost_control += costs[1].calc(xs[i], us[i])
61 | cost_col += [costs[2].calc(xs[i], us[i])]
62 | cost_goal = ddp.problem.terminalModel.cost_model.calc(xs[-1], us[-1])
63 | return cost_state, cost_control, cost_col, cost_goal
64 |
65 | def lin_interpolate(state1, state2, n=1.):
66 | state_list = []
67 | for i in range(n+1):
68 | state_list.append(state1 + 1.*i*(state2-state1)/n)
69 | return state_list
70 |
71 | def subsample(X,N):
72 | '''Subsample in N iterations the trajectory X. The output is a
73 | trajectory similar to X with N points. '''
74 | nx = X.shape[0]
75 | idx = np.arange(float(N))/(N-1)*(nx-1)
76 | hx = []
77 | for i in idx:
78 | i0 = int(np.floor(i))
79 | i1 = int(np.ceil(i))
80 | di = i%1
81 | x = X[i0,:]*(1-di) + X[i1,:]*di
82 | hx.append(x)
83 | return np.vstack(hx)
84 |
85 |
86 | def rotvec2mat(x):
87 | return R.as_matrix(R.from_rotvec(x))
88 |
89 |
90 | def save_screenshot(x,y,w,h,file_name, to_show='False'):
91 | # part of the screen
92 | im=ImageGrab.grab(bbox=(x,y,w,h))
93 | if to_show:
94 | im.show()
95 | # save to file
96 | im.save(file_name)
97 | return im
98 |
99 | def get_pb_config(q):
100 | """
101 | Convert tf's format of 'joint angles + base position' to
102 | 'base_pos + base_ori + joint_angles' according to pybullet order
103 | """
104 | joint_angles = q[:28]
105 | #qnew = np.concatenate([q[28:31], euler2quat(q[-3:]),
106 | #qnew = np.concatenate([np.array([0,0,q[28]]), euler2quat(q[-3:]),
107 | qnew = np.concatenate([q[28:31], euler2quat(np.array([0,0,0])),
108 | joint_angles[-6:], joint_angles[-12:-6],
109 | joint_angles[:2], joint_angles[9:16],
110 | joint_angles[2:9]])
111 | return qnew
112 |
113 | def get_tf_config(q):
114 | """
115 | Convert 'base_pos + base_ori + joint_angles' according to pybullet order
116 | to tf's format of 'joint angles + base position'
117 |
118 | """
119 | joint_angles = q[7:]
120 | qnew = np.concatenate([joint_angles[12:14], joint_angles[-7:],
121 | joint_angles[-14:-7], joint_angles[6:12],
122 | joint_angles[:6], q[:3]])
123 | return qnew
124 |
125 |
126 | def normalize(x):
127 | return x/np.linalg.norm(x)
128 |
129 | def set_q(q, robot_id, joint_indices, set_base = False):
130 | if set_base:
131 | localInertiaPos = np.array(p.getDynamicsInfo(robot_id,-1)[3])
132 | q_root = q[0:7]
133 | ori = q_root[3:]
134 | Rbase = np.array(p.getMatrixFromQuaternion(ori)).reshape(3,3)
135 | shift_base = Rbase.dot(localInertiaPos)
136 | pos = q_root[:3]+shift_base
137 | p.resetBasePositionAndOrientation(robot_id,pos,ori)
138 | q_joint = q[7:]
139 | else:
140 | q_joint = q
141 |
142 | #set joint angles
143 | for i in range(len(q_joint)):
144 | p.resetJointState(robot_id, joint_indices[i], q_joint[i])
145 |
146 |
147 | def vis_traj(qs, vis_func, dt=0.1):
148 | for q in qs:
149 | vis_func(q)
150 | time.sleep(dt)
151 |
152 |
153 | def get_joint_limits(robot_id, indices):
154 | lower_limits = []
155 | upper_limits = []
156 | for i in indices:
157 | info = p.getJointInfo(robot_id, i)
158 | lower_limits += [info[8]]
159 | upper_limits += [info[9]]
160 | limits = np.vstack([lower_limits, upper_limits])
161 | return limits
162 |
163 | def computeJacobian(rmodel,rdata,ee_frame_id,q):
164 | pin.forwardKinematics(rmodel,rdata,q)
165 | pin.updateFramePlacements(rmodel,rdata)
166 | pin.computeJointJacobians(rmodel, rdata, q)
167 | J = pin.getFrameJacobian(rmodel, rdata,ee_frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
168 | return J[:,:7]
169 |
170 | def computePose(rmodel, rdata, ee_frame_id, q):
171 | pin.forwardKinematics(rmodel, rdata, q)
172 | pin.updateFramePlacements(rmodel, rdata)
173 | pos, ori = rdata.oMf[ee_frame_id].translation, rdata.oMf[ee_frame_id].rotation
174 | return pos,ori
175 |
176 | def check_joint_limits(q, joint_limits):
177 | """
178 | Return True if within the limit
179 | """
180 | upper_check = False in ((q-joint_limits[0]) > 0)
181 | lower_check = False in ((joint_limits[1]-q) > 0)
182 | if upper_check or lower_check:
183 | return False
184 | else:
185 | return True
186 |
187 | def calc_dist_limit(q, joint_limits):
188 | lower_error = joint_limits[0]-q
189 | lower_check = (lower_error > 0)
190 | lower_error = lower_error*lower_check
191 | upper_error = q-joint_limits[1]
192 | upper_check = (upper_error > 0)
193 | upper_error = upper_error*upper_check
194 | error = lower_error-upper_error
195 | return error
196 |
197 | def mat2euler(rot, axes = 'rzyx'):
198 | return np.array(transforms3d.euler.mat2euler(rot, axes = axes))
199 |
200 | def euler2quat(rpy, axes='sxyz'):
201 | #euler sxyz: used by Manu's codes
202 | return rectify_quat(transforms3d.euler.euler2quat(*rpy, axes=axes))
203 |
204 | def rectify_quat(quat):
205 | #transform from transforms3d format (w,xyz) to pybullet and pinocchio (xyz, w)
206 | quat_new = np.concatenate([quat[1:], quat[0:1]])
207 | return quat_new
208 |
209 | def mat2w(rot):
210 | rot_aa = pin.AngleAxis(rot)
211 | return rot_aa.angle*rot_aa.axis
212 |
213 | def w2quat(q):
214 | angle = np.linalg.norm(q)
215 | if abs(angle) < 1e-7:
216 | ax = np.array([1,0,0])
217 | else:
218 | ax, angle = normalize(q), np.linalg.norm(q)
219 | w = p.getQuaternionFromAxisAngle(ax, angle)
220 | return np.array(w)
221 |
222 | def quat2w(q):
223 | ax, angle = p.getAxisAngleFromQuaternion(q)
224 | return np.array(ax)*angle
225 |
226 | def w2mat(w):
227 | angle = np.linalg.norm(w)
228 | if abs(angle) < 1e-7:
229 | ax = np.array([1,0,0])
230 | else:
231 | ax, angle = w/angle, angle
232 | R = pin.AngleAxis.toRotationMatrix(pin.AngleAxis(angle, ax))
233 | return R
234 |
235 | def get_link_base(robot_id, frame_id):
236 | '''
237 | Obtain the coordinate of the link frame, according to the convention of pinocchio (at the link origin,
238 | instead of at the COM as in pybullet)
239 | '''
240 | p1 = np.array(p.getLinkState(robot_id,frame_id)[0])
241 | ori1 = np.array(p.getLinkState(robot_id,frame_id)[1])
242 | R1 = np.array(p.getMatrixFromQuaternion(ori1)).reshape(3,3)
243 | p2 = np.array(p.getLinkState(robot_id,frame_id)[2])
244 | return p1 - R1.dot(p2), ori1
245 |
246 |
247 | def create_primitives(shapeType=2, rgbaColor=[1, 1, 0, 1], pos = [0, 0, 0], radius = 1, length = 2, halfExtents = [0.5, 0.5, 0.5], baseMass=1, basePosition = [0,0,0]):
248 | visualShapeId = p.createVisualShape(shapeType=shapeType, rgbaColor=rgbaColor, visualFramePosition=pos, radius=radius, length=length, halfExtents = halfExtents)
249 | collisionShapeId = p.createCollisionShape(shapeType=shapeType, collisionFramePosition=pos, radius=radius, height=length, halfExtents = halfExtents)
250 | bodyId = p.createMultiBody(baseMass=baseMass,
251 | baseInertialFramePosition=[0, 0, 0],
252 | baseVisualShapeIndex=visualShapeId,
253 | baseCollisionShapeIndex=collisionShapeId,
254 | basePosition=basePosition,
255 | useMaximalCoordinates=True)
256 | return visualShapeId, collisionShapeId, bodyId
257 |
258 |
259 | #### Code to modify concave objects in pybullet
260 | #name_in = rl.datapath + '/urdf/bookcase_old.obj'
261 | #name_out = rl.datapath + '/urdf/bookcase.obj'
262 | #name_log = "log.txt"
263 | #p.vhacd(name_in, name_out, name_log, alpha=0.04,resolution=10000000 )
264 |
265 | def Rot_z(angle):
266 | w = np.array([0,0,angle])
267 | Rz = w2mat(w)
268 | return Rz
269 |
--------------------------------------------------------------------------------