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