Gaits for Minipupper

We moved the calculation of the foot trajectory to a Python class

import numpy as np
import matplotlib.pyplot as plt
%matplotlib widget

from gaits import GaitController

# set values for minipupper
step_length = 0.05
step_height = 0.03
velocity_adapt = 4
number_of_points = 20
theta = -3*np.pi/4  # upper leg angle for origin of bezier curve
gamma = np.pi/2

gc = GaitController(step_length, step_height, velocity_adapt, number_of_points, theta, gamma)
trajectory = gc.get_trajectory()

gc.v_adj

array([0.        , 0.00950315, 0.02364571, 0.04444672, 0.07451808,
       0.11692519, 0.17468076, 0.24971679, 0.34144914, 0.44560524,
       0.55439476, 0.65855086, 0.75028321, 0.82531924, 0.88307481,
       0.92548192, 0.95555328, 0.97635429, 0.99049685, 1.        ])

fig, ax = plt.subplots()
plt.xlim(-0.15, 0.15)
plt.ylim(-0.15, 0)
plt.axis('equal')
plt.grid(visible=False)
plt.scatter(trajectory[0], trajectory[1])
bezier_points = gc.get_bezier_points()
plt.scatter(bezier_points[0], bezier_points[1])

<matplotlib.collections.PathCollection at 0x11745d1f0>

leg_servo_positions = gc.get_leg_servo_positions()

p = gc.mpForwardKin(leg_servo_positions[11])
fig, ax = plt.subplots()
plt.xlim(-0.15, 0.15)
plt.ylim(-0.15, 0)
plt.axis('equal')
plt.grid(visible=False)
plt.plot(p[0], p[1])

[<matplotlib.lines.Line2D at 0x117551d30>]

import matplotlib.animation as animation
from collections import deque

fig, ax = plt.subplots()
plt.xlim(-0.15, 0.15)
plt.ylim(-0.15, 0)
plt.axis('equal')
plt.grid(visible=False)

line, = ax.plot([], [], 'o-', lw=2)
trace, = ax.plot([], [], '.-', lw=1, ms=2)
step_template = 'step = %.1f'
step_text = ax.text(0.05, 0.9, '', transform=ax.transAxes)
history_len = 50
history_x, history_y = deque(maxlen=history_len), deque(maxlen=history_len)


def animate(i):
    p = gc.mpForwardKin(leg_servo_positions[i])

    if i == 0:
        history_x.clear()
        history_y.clear()

    history_x.appendleft(p[0][2])
    history_y.appendleft(p[1][2])

    line.set_data(p[0], p[1])
    trace.set_data(history_x, history_y)
    step_text.set_text(step_template % (i))
    #step_text.set_text("%s" % (p[0]))
    return line, trace, step_text


ani = animation.FuncAnimation(fig, animate, len(leg_servo_positions), interval=200, blit=True)

plt.show()

joints = ['hip', 'upper_leg', 'lower_leg']
legs = ['lf', 'rf', 'lh', 'rh']

for joint in joints:
    for leg in legs:
        fn = "%s_%s" % (leg, joint)
        if 'lf' in fn or 'rh' in fn:
            lp = leg_servo_positions
        else:
            lp = np.roll(leg_servo_positions, number_of_points, axis=0)
        with open("../controller/servos/walk/%s" % fn, "w") as f:
            if 'hip' in fn:
                f.write("%.10f\n" % 0.0)
            elif 'upper' in fn:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][0])
            else:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][1])

Leg position for testing

# neutral position
lp = [[-3*np.pi/4, np.pi/2]]

for joint in joints:
    for leg in legs:
        fn = "%s_%s" % (leg, joint)
        with open("../controller/servos/neutral/%s" % fn, "w") as f:
            if 'hip' in fn:
                f.write("%.10f\n" % 0.0)
            elif 'upper' in fn:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][0])
            else:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][1])

# streched position (max allowed on minipupper)
lp = [[-np.pi/2, np.pi/4]]

for joint in joints:
    for leg in legs:
        fn = "%s_%s" % (leg, joint)
        with open("../controller/servos/streched_legs/%s" % fn, "w") as f:
            if 'hip' in fn:
                f.write("%.10f\n" % 0.0)
            elif 'upper' in fn:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][0])
            else:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][1])

# sitting position (max allowed on minipupper)
lp = [[-np.pi/2, np.pi/2]]

for joint in joints:
    for leg in legs:
        fn = "%s_%s" % (leg, joint)
        with open("../controller/servos/sitting/%s" % fn, "w") as f:
            if 'hip' in fn:
                f.write("%.10f\n" % 0.0)
            elif 'upper' in fn:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][0])
            else:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][1])

# retracted position (max allowed on minipupper)
lp = [[-np.pi, 3*np.pi/4]]

for joint in joints:
    for leg in legs:
        fn = "%s_%s" % (leg, joint)
        with open("../controller/servos/retracted_legs/%s" % fn, "w") as f:
            if 'hip' in fn:
                f.write("%.10f\n" % 0.0)
            elif 'upper' in fn:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][0])
            else:
                for j in range(len(lp)):
                    f.write("%.10f\n" % lp[j][1])