def jacobian_i(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
betai = qi[3:]
betaj = qj[3:]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(betai)
Aj = ep2dcm(betaj)
v1 = Ai.dot(self.vii)
v2 = Ai.dot(self.vij)
v3 = Aj.dot(self.vjk)
v4 = Aj.dot(self.vjj)
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j) + 10 * v3
Hiv1 = B(betai, self.vii)
Hiv2 = B(betai, self.vij)
Hiup = B(betai, self.u_i)
Z = sparse.csr_matrix([[0, 0, 0]])
jac = sparse.bmat(
[[Z, v3.T.dot(Hiv1)], [Z, v3.T.dot(Hiv2)],
[v1.T, rij.T.dot(Hiv1) + v1.T.dot(Hiup)],
[v2.T, rij.T.dot(Hiv2) + v2.T.dot(Hiup)], [Z, v4.T.dot(Hiv1)]],
format='csr')
return jac
def __init__(self, p1, p2, outer, inner):
self.name = ''
# defining and calculating required properties
axis = p2 - p1
length = axis.mag
cm_local = vector(0.5 * length * axis.unit, p1.frame)
# Refering the cm to the global reference frame
# cm_global=p1+cm_local
cm_global=recursive_transformation(axis.frame,grf).dot(p1)+\
recursive_transformation(axis.frame,grf).dot(cm_local)
volume = (np.pi / 4) * (outer - inner)**2 * length * 10**-3 # cm cube
mass = 7.9 * volume
# defining the self.attributes
self.cm_local = vector(cm_local, frame=axis.frame)
self.cm_global = point(self.name + '_cm', cm_global)
self.length = length
self.volume = volume
self.mass = mass
self.p1 = p1
self.p2 = p2
self.r = outer / 2
def __init__(self, location, i_body, j_body, axis):
super().__init__(location, i_body, j_body, axis)
self.type = 'translational joint'
self.name = self.name + '_trans'
self.nc = 5
self.p2 = vector(location) + 10 * vector(axis).unit
self.u_j = j_body.dcm.T.dot(self.p2 - j_body.R)
def equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
v1 = self.vii
v2 = self.vij
v3 = self.vjk
v4 = self.vjj
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j) + 10 * v3
eq1 = np.linalg.multi_dot([v1.T, Ai.T, Aj, v3])
eq2 = np.linalg.multi_dot([v2.T, Ai.T, Aj, v3])
eq3 = np.linalg.multi_dot([v1.T, Ai.T, rij])
eq4 = np.linalg.multi_dot([v2.T, Ai.T, rij])
eq5 = np.linalg.multi_dot([v1.T, Ai.T, Aj, v4])
c = [eq1, eq2, eq3, eq4, eq5]
return np.array([c]).reshape((5, 1))
def __init__(self, location, i_body, j_body, axis):
super().__init__(location, i_body, j_body, axis)
self.type = 'cylindrical joint'
self.name = self.name + '_cyl'
self.nc = 4
self.p2 = vector(location) + 10 * vector(axis).unit
self.u_j = j_body.dcm.T.dot(self.p2 - j_body.R)
def move():
"Move pacman and all ghosts."
writer.undo()
writer.write(state['score'])
# writer.write(score)
clear()
if valid(pacman + aim):
pacman.move(aim)
index = offset(pacman)
if tiles[index] == 1:
tiles[index] = 2
state['score'] += 1
x = (index % 20) * 20 - 200
y = 180 - (index // 20) * 20
square(x, y)
up()
goto(pacman.x + 10, pacman.y + 10)
dot(20, 'yellow')
for point, course in ghosts:
if valid(point + course):
point.move(course)
else:
options = [
vector(5, 0),
vector(-5, 0),
vector(0, 5),
vector(0, -5),
]
plan = choice(options)
course.x = plan.x
course.y = plan.y
change = 0
up()
goto(point.x + 10, point.y + 10)
dot(20, 'red')
update()
for point, course in ghosts:
if abs(pacman - point) < 20:
return
if state['score'] == 20:
writer.undo()
writer.up()
writer.goto(-140, 0)
writer.down()
writer.color('red')
writer.write('You Win!', font=("Arial", 64, 'bold'))
return
ontimer(move, 100)
def tap(x, y):
"Store starting point or draw shape."
start = state['start']
if start is None:
state['start'] = vector(x, y)
else:
shape = state['shape']
end = vector(x, y)
shape(start, end)
state['start'] = None
def move():
"Move ball and targets."
if randrange(40) == 0:
y = randrange(-150, 150)
target = vector(200, y)
targets.append(target)
boss = vector(200, y)
bosses.append(boss)
for target in targets:
target.x -= 1.5
for boss in bosses:
boss.x -= 10
if inside(ball):
speed.y -= 0.35
ball.move(speed)
# dupe = targets.copy()
# targets.clear()
# for target in dupe:
# if abs(target - ball) > 13:
# targets.append(target)
for target in targets:
if abs(target - ball) < 13:
targets.remove(target)
ball.x = -199
ball.y = -199
speed.x = -10
speed.y = -10
for boss in bosses:
if abs(boss - ball) < 23:
bosses.remove(boss)
clear
speed.x = 5
draw()
for target in targets:
if not inside(target):
writer.goto(30, 30)
writer.color('black')
writer.write('Game Over', font=("Verdana", 15, "normal"))
return
ontimer(move, 50)
def equation(self, q, qdot):
qi = q[self.bodyi.dic.index]
qj = q[self.bodyj.dic.index]
qi_dot = qdot[self.bodyi.dic.index]
qj_dot = qdot[self.bodyj.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
betai = qi[3:]
betaj = qj[3:]
Ai = ep2dcm(betai)
Aj = ep2dcm(betaj)
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j)
l = np.linalg.norm(rij)
defflection = max([0, self.lf - l])
nij = rij / l
self.defflection = defflection
Ri_dot = qi_dot[0:3].values.reshape((3, 1))
Rj_dot = qj_dot[0:3].values.reshape((3, 1))
betai_dot = qi_dot[3:]
betaj_dot = qj_dot[3:]
bid = betai_dot.values.reshape((4, 1))
bjd = betaj_dot.values.reshape((4, 1))
Bip = B(betai, self.u_i)
Bjp = B(betaj, self.u_j)
rij_dot = Ri_dot + Bip.dot(bid) - Rj_dot - Bjp.dot(bjd)
velocity = nij.T.dot(rij_dot)
self.velocity = velocity
self.springforce = self.k * defflection
self.damperforce = self.c * velocity
force = self.k * (defflection) + self.c * velocity + self.h
fi = float(force) * nij
ni = 2 * G(betai).T.dot(vec2skew(self.u_i).dot(Ai.T.dot(fi)))
Qi = np.bmat([[fi], [ni]])
fj = -float(force) * nij
nj = 2 * G(betaj).T.dot(vec2skew(self.u_j).dot(Aj.T.dot(fj)))
Qj = np.bmat([[fj], [nj]])
return Qi, Qj
def move():
# Movement for Pacman
writer.undo()
writer.write(state['score'])
clear()
if valid(pacman + aim):
pacman.move(aim)
index = offset(pacman)
if tiles[index] == 1:
tiles[index] = 2
state['score'] += 1
x = (index % 20) * 20 - 200
y = 180 - (index // 20) * 20
square(x, y)
up()
goto(pacman.x + 10, pacman.y + 10)
dot(20, 'yellow')
# Movement for ghosts
for point, course in ghosts:
if valid(point + course):
point.move(course)
else:
options = [
vector(5, 0),
vector(-5, 0),
vector(0, 5),
vector(0, -5)
]
plan = choice(options)
course.x = plan.x
course.y = plan.y
up()
goto(point.x + 10, point.y + 10)
dot(20, 'red')
update()
# checking for collisions.
for point, course in ghosts:
if abs(pacman - point) < 20:
return
ontimer(move, 100)
def move():
"Move ball and targets."
if randrange(40) == 0:
y = randrange(-150, 150)
target = vector(200, y)
targets.append(target)
for target in targets:
target.x -= 0.5
if inside(ball):
speed.y -= 0.35
ball.move(speed)
dupe = targets.copy()
targets.clear()
for target in dupe:
if abs(target - ball) > 13:
targets.append(target)
draw()
for target in targets:
if not inside(target):
return
ontimer(move, 50)
def step_impl_generic_ray_full_with_sqrt(context, item, px, py, pz, vx, vynum,
vydenom, vznum, vzdenom):
pt = base.point(float(px), float(py), float(pz))
vc = base.vector(float(vx), -math.sqrt(float(vynum)) / float(vydenom),
math.sqrt(float(vznum)) / float(vzdenom))
ensure_context_has_dict(context)
context.dict[item] = base.ray(pt, vc)
def step_impl_generic_ray_full_with_sqrt2(context, item, pznum, pzdenom, vx,
vy, vz):
pt = base.point(np.float32(0), np.float32(0),
np.float32(math.sqrt(float(pznum)) / float(pzdenom)))
vc = base.vector(np.float32(vx), np.float32(vy), np.float32(vz))
ensure_context_has_dict(context)
context.dict[item] = base.ray(pt, vc)
def equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
ui = self.u_i
uj = self.u_j
r_p = Ri + Ai.dot(ui) - Rj - Aj.dot(uj)
return r_p
def move():
# movement of ball and target
if randrange(40) == 0:
y = randrange(-150,150)
target = vector(200,y)
targets.append(target)
for target in targets:
target.x -= 0.5
# print(target.x)
if inside(ball):
speed.y -= 0.35
ball.move(speed)
print(f"{speed.y} --> {ball}")
dupe = targets.copy()
targets.clear()
for target in dupe:
if abs(target - ball) > 13:
print(abs(target - ball))
targets.append(target)
draw()
for taraget in targets:
if not inside(target):
return
# ontimer(move, 50)
ontimer(move,50)
def move():
# Movement of ball and target
# 0-40 - define stop limit
if randrange(40) == 0:
# Y range between -150 -> 150
y = randrange(-150, 150)
# X positon is constant because only y moving
target = vector(200, y)
# Append to the target array
targets.append(target)
for target in targets:
target.x -= 0.5
if inside(ball):
speed.y -= 0.35
ball.move(speed)
dupe = targets.copy()
targets.clear()
for target in dupe:
if abs(target - ball) > 5:
targets.append(target)
draw()
for target in targets:
if not inside(target):
return
ontimer(move, 50)
def move():
"Update object positions."
bird.y -= 5
for ball in balls:
ball.x -= 3
if randrange(10) == 0:
y = randrange(-199, 199)
ball = vector(199, y)
balls.append(ball)
while len(balls) > 0 and not inside(balls[0]):
balls.pop(0)
if not inside(bird):
draw(False)
return
for ball in balls:
if abs(ball - bird) < 15:
draw(False)
return
draw(True)
ontimer(move, 50)
def step_impl_ray_element_vector(context, item, element, x, y, z):
assert (item in context.dict.keys())
assert (element in valid_ray_elements)
ray = context.dict[str(item)]
thing = eval("ray." + str(element))
vec4_value = base.vector(float(x), float(y), float(z))
assert (base.equal(thing, vec4_value))
def equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
eq1, eq2, eq3 = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j)
eq4 = np.linalg.multi_dot([self.h_i.T, Ai.T, Aj, self.h_j])
c = [float(i) for i in (eq1, eq2, eq3, eq4)]
return np.array([c]).reshape((4, 1))
def joint_pos(self, q):
qi = q[self.i_body.dic.index]
Ri = vector(qi[0:3]).a
Ai = ep2dcm(qi[3:])
ui = self.u_i
r_p = Ri + Ai.dot(ui)
return r_p
def R(self, value):
loc_type = isinstance(
value, (list, tuple, np.ndarray, point, vector, pd.Series))
if not loc_type: raise TypeError('should be a list, tuple or ndarray')
loc_length = len(value) == 3
if not loc_length:
raise ValueError('location should be a vector of 3 components')
self.loc = vector(value)
def step_impl_ray_element_vector(context, item, element, xnum, xdenom, znum,
zdenom):
assert (item in context.dict.keys())
assert (element in valid_ray_elements)
ray = context.dict[str(item)]
thing = eval("ray." + str(element))
vec4_value = base.vector(
np.sqrt(float(xnum)) / float(xdenom), 0,
-np.sqrt(float(znum)) / float(zdenom))
assert (base.equal(thing, vec4_value))
def move():
writer.undo()
writer.write(state['score'])
t.clear()
if valid(pacman + aim):
pacman.move(aim)
index = offset(pacman)
if tiles[index] == 1:
tiles[index] = 2
state['score'] += 1
x = (index % 20) * 20 - 200
y = 180 - (index // 20) * 20
square(x, y)
t.up()
t.goto(pacman.x + 10, pacman.y + 10)
t.dot(20, 'yellow')
for point, course in ghosts:
if valid(point + course):
point.move(course)
else:
options = [
b.vector(5, 0),
b.vector(-5, 0),
b.vector(0, 5),
b.vector(0, -5)
]
plan = r.choice(options)
course.x = plan.x
course.y = plan.y
t.up()
t.goto(point.x + 10, point.y + 10)
t.dot(20, 'red')
t.update()
for point, course in ghosts:
if abs(pacman - point) < 20:
return
t.ontimer(move, 100)
def move():
#move pacman and ghosts
clear()
if valid(pacman + aim):
pacman.move(aim)
index = offset(pacman)
if tiles[index] == 1:
tiles[index] = 2
x = (index % 20) * 20 - 200
y = 180 - (index // 20) * 20
square(x, y)
up()
goto(pacman.x + 10, pacman.y + 10)
dot(20, 'yellow')
#enemy
for point, course in ghost:
if valid(point + course):
point.move(course)
else:
options = [
vector(5, 0),
vector(-5, 0),
vector(0, 5),
vector(0, -5),
]
plan = choice(options)
course.x = plan.x
course.y = plan.y
up()
goto(point.x + 10, point.y + 10)
dot(20, 'red')
update()
for point, course in ghost:
if abs(pacman - point) < 20:
return
ontimer(move, 100)
def __init__(self, geometries):
# composite body total mass as the sum of it's subcomponents
self.mass = sum([i.mass for i in geometries])
# center of mass vector relative to the origin
self.cm = vector(
(1 / self.mass) * sum([i.mass * i.cm for i in geometries]))
self.J = sum([
i.C.dot(i.J).dot(i.C.T) + i.mass *
((i.cm - self.cm).mag**2 * np.eye(3) - (i.cm - self.cm).a.dot(
(i.cm - self.cm).a.T)) for i in geometries
])
def __init__(self, location, i_body, j_body, axis=[0, 0, 1]):
# defining the basic attributes in the joint class
self.name = location.name
self.i_body = i_body
self.j_body = j_body
self.type = "None" # for representing the joint type in subclasses
self.typ = location.typ
self._loc = location
# defining the joint axis which is needed for the different types of joints
self.axis = axis
self.frame = orient_along_axis(axis)
self.u_irf = i_body.dcm.T.dot(self.frame)
self.u_jrf = j_body.dcm.T.dot(self.frame)
# joint locations wrt to each body origin cm referenced to the body reference
self.u_i = i_body.dcm.T.dot(self._loc - i_body.R)
self.u_j = j_body.dcm.T.dot(self._loc - j_body.R)
# axes to be used in the cylindrical and revolute joints classes
self.vii = vector(self.u_irf[:, 0])
self.vij = vector(self.u_irf[:, 1])
self.vik = vector(self.u_irf[:, 2])
self.vji = vector(self.u_jrf[:, 0])
self.vjj = vector(self.u_jrf[:, 1])
self.vjk = vector(self.u_jrf[:, 2])
def tap(x, y):
x = floor(x, 100)
y = floor(y, 100)
mark = vector(x, y)
for neighbor in neighbors:
spot = mark + neighbor
if spot in tiles and tiles[spot] is None:
num = tiles[mark]
tiles[spot] = num
square(spot, num)
tiles[mark] = None
square(mark, None)
def equations(self, q):
qi = q[self.i_body.dic.index]
qj = q[self.j_body.dic.index]
Ri = vector(qi[0:3]).a
Rj = vector(qj[0:3]).a
Ai = ep2dcm(qi[3:])
Aj = ep2dcm(qj[3:])
v1 = Ai.dot(self.vii)
v2 = Ai.dot(self.vij)
v3 = Aj.dot(self.vjk)
rij = Ri + Ai.dot(self.u_i) - Rj - Aj.dot(self.u_j)
eq1, eq2, eq3 = rij
eq4 = v1.T.dot(v3)
eq5 = v2.T.dot(v3)
c = [float(i) for i in (eq1, eq2, eq3, eq4, eq5)]
return np.array([c]).reshape((5, 1))
def tap(x, y):
"Swap tile and empty square."
x = floor(x, 100)
y = floor(y, 100)
mark = vector(x, y)
for neighbor in neighbors:
spot = mark + neighbor
if spot in tiles and tiles[spot] is None:
number = tiles[mark]
tiles[spot] = number
square(spot, number)
tiles[mark] = None
square(mark, None)
def load():
count = 1
for y in range(-200, 200, 100):
for x in range(-200, 200, 100):
mark = vector(x, y)
tiles[mark] = count
count += 1
tiles[mark] = None
for count in range(1000):
neighbor = choice(neighbors)
spot = mark + neighbor
if spot in tiles:
num = tiles[spot]
tiles[spot] = None
tiles[mark] = num
mark = spot
