def __init__(self, x, y, z, height, duree): self.duree = duree self.height = height self.spline = interpolation.LinearSpline3D() self.spline.add_entry(0, 0, 0, -height) self.spline.add_entry(duree / 4, -x, -y, -height) self.spline.add_entry(2 * duree / 4, x, y, z - height) self.spline.add_entry(3 * duree / 4, x, y, -height) self.spline.add_entry(duree, 0, 0, -height)
def walk(freq, params, targets, teta, length, height, body): global triPos1 global triPos2 global position t = time.time() if freq == 0: tri1 = [0, 0, 0] tri2 = [0, 0, 0] else: T = 1 / freq triangle = interpolation.LinearSpline3D() triangle.add_entry(0, length / 2, 0, 0) triangle.add_entry(T / 2, -length / 2, 0, 0) triangle.add_entry(3 * T / 4, 0, 0, height) triangle.add_entry(T, length / 2, 0, 0) tri1 = triangle.interpolate(t % T) tri2 = triangle.interpolate((t + T / 2) % T) #Update Position if tri1[2] == 0: triPos2 = [length / 2, 0, 0] previousTri = triPos1 triPos1 = tri1 position[0] -= (triPos1[0] - previousTri[0]) * np.cos(teta) position[1] -= (triPos1[0] - previousTri[0]) * np.sin(teta) else: triPos1 = [length / 2, 0, 0] previousTri = triPos2 triPos2 = tri2 position[0] -= (triPos2[0] - previousTri[0]) * np.cos(teta) position[1] -= (triPos2[0] - previousTri[0]) * np.sin(teta) for leg_id in [1, 3, 5]: alphas = computeIKOriented(tri1[0], tri1[1], tri1[2] - body, leg_id, params, teta, verbose=True) set_leg_angles_2(alphas, leg_id, targets, params) for leg_id in [2, 4, 6]: alphas = computeIKOriented(tri2[0], tri2[1], tri2[2] - body, leg_id, params, teta, verbose=True) set_leg_angles_2(alphas, leg_id, targets, params)
def toupie(t, freq, d_x, d_y, height, z): if freq == 0: tri1 = [0, 0, 0] tri2 = [0, 0, 0] else: T = 1 / freq triangle = interpolation.LinearSpline3D() triangle.add_entry(0, d_x, 0, z / 2) triangle.add_entry(T / 2, d_x, 0, z) triangle.add_entry((3 * T) / 4, d_x, -d_y, z) triangle.add_entry(T, d_x, 0, z + height) tri1 = triangle.interpolate(t % T) tri2 = triangle.interpolate((t + T / 2) % T) return tri1, tri2
def draw(t): interpolator = interpolation.LinearSpline3D() interpolator.add_entry(0, 0.05, 0, 0) interpolator.add_entry(1, 0.05, 0, 0.1) interpolator.add_entry(2, 0.05, 0.1, 0.1) interpolator.add_entry(3, 0.05, 0, 0) x, y, z = interpolator.interpolate(t % 3) """ python simulator.py -m draw Le robot est figé en l'air, on ne contrôle qu'une patte Le but est, à partir du temps donné, de suivre une trajectoire de triangle. Pour ce faire, on utilisera une interpolation linéaire entre trois points, et la fonction inverse précédente. - Entrée: t, le temps (secondes écoulées depuis le début) - Sortie: un tableau contenant les 3 positions angulaires cibles (en radians) """ return inverse(x, y, z)
def step(t, patte_num, speed_x, speed_y): d = 0.120 z0 = -0.05 initiale = [(-d, d, z0), (-d, -d, z0), (d, -d, z0), (d, d, z0)] start = (initiale[patte_num][0] + speed_x / 2, initiale[patte_num][1] + speed_y / 2, initiale[patte_num][2]) high = (initiale[patte_num][0] + speed_x / 2, initiale[patte_num][1] + speed_y / 2, initiale[patte_num][2] + 0.05) end = (initiale[patte_num][0] - speed_x / 2, initiale[patte_num][1] - speed_y / 2, initiale[patte_num][2]) interpolator = interpolation.LinearSpline3D() interpolator.add_entry(0.0, *start) interpolator.add_entry(0.5, *end) interpolator.add_entry(0.75, *high) interpolator.add_entry(1.0, *start) x, y, z = interpolator.interpolate(t % 1.0) print(t) return x, y, z
theta, i + 1, params, verbose=False) for j in range(0, len(inv)): targets.append(inv[j]) return targets init_legs = [[0.0, 0.0, -0.06]] * 6 init_legs2 = [[0.0, 0.0, 0.0]] * 6 is_init = False delta = 0 oldt = 0 splines = [ interpolation.LinearSpline3D(), interpolation.LinearSpline3D(), interpolation.LinearSpline3D(), interpolation.LinearSpline3D(), interpolation.LinearSpline3D(), interpolation.LinearSpline3D() ] def walk(t, speed_x, speed_y, theta, param, l1=constL1, l2=constL2,