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,