Beispiel #1
0
def interpolate_vector(v0, v1, tend, dt):
    """ Give the interpolation between 2 vectors

    inputs:
    v0: the beginning rotation matrix
    v1: the ending rotation matrix
    tend: the end time
    dt: the time step

    this interpolation is done with no velocity or acceleration
    at the beginning and at the end

    Returns 3 lists: ([v], [dv], [ddv])
    """
    assert(len(v0)==len(v1))
    v = array([ppi([0, tend], [[v0[i], 0, 0], [v1[i], 0, 0]], \
               arange(0, tend+dt, dt)) for i in arange(len(v0))]).T
    dv = simple_der(v, dt)
    ddv = simple_der(dv, dt)

    return v, dv, ddv
Beispiel #2
0
def interpolate_vector(v0, v1, tend, dt):
    """ Give the interpolation between 2 vectors

    inputs:
    v0: the beginning rotation matrix
    v1: the ending rotation matrix
    tend: the end time
    dt: the time step

    this interpolation is done with no velocity or acceleration
    at the beginning and at the end

    Returns 3 lists: ([v], [dv], [ddv])
    """
    assert (len(v0) == len(v1))
    v = array([ppi([0, tend], [[v0[i], 0, 0], [v1[i], 0, 0]], \
               arange(0, tend+dt, dt)) for i in arange(len(v0))]).T
    dv = simple_der(v, dt)
    ddv = simple_der(dv, dt)

    return v, dv, ddv
Beispiel #3
0
def interpolate_rotation_matrix(R0, R1, tend, dt):
    """ Give the interpolation between 2 rotation matrices

    inputs:
    R0: the beginning rotation matrix
    R1: the ending rotation matrix
    tend: the end time
    dt: the time step

    this interpolation is done with no velocity or acceleration
    at the beginning and at the end

    Returns 3 lists: ([R], [omega], [domega])
    """
    def der_q(qim1, qip1, dt):
        dqi = qip1 - qim1
        return dqi.to_array()/dt

    def Q(q):
        r = q.real
        i0, i1, i2 = q.img
        return array([[-i0,   r, -i2,  i1],
                      [-i1,  i2,   r, -i0],
                      [-i2, -i1,  i0,   r]])

    q0, q1 = quaternion(R=R0), quaternion(R=R1)
    fraction = ppi([0, tend], [[0, 0, 0], [1, 0, 0]], arange(0, tend+dt, dt))
    q = [slerp(q0, q1, f) for f in fraction]
    R = [qi.to_rot() for qi in q]

    dq = [der_q(q[0], q[1], dt)] + \
          [der_q(q[i-1], q[i+1], 2*dt) for i in arange(1, len(q)-1)] + \
          [der_q(q[-2], q[-1], dt)]

    omega = [2*dot(Q(q[i]), dq[i]) for i in arange(len(q))]
    domega = simple_der(omega, dt)

    return R, omega, domega
Beispiel #4
0
def interpolate_rotation_matrix(R0, R1, tend, dt):
    """ Give the interpolation between 2 rotation matrices

    inputs:
    R0: the beginning rotation matrix
    R1: the ending rotation matrix
    tend: the end time
    dt: the time step

    this interpolation is done with no velocity or acceleration
    at the beginning and at the end

    Returns 3 lists: ([R], [omega], [domega])
    """
    def der_q(qim1, qip1, dt):
        dqi = qip1 - qim1
        return dqi.to_array() / dt

    def Q(q):
        r = q.real
        i0, i1, i2 = q.img
        return array([[-i0, r, -i2, i1], [-i1, i2, r, -i0], [-i2, -i1, i0, r]])

    q0, q1 = quaternion(R=R0), quaternion(R=R1)
    fraction = ppi([0, tend], [[0, 0, 0], [1, 0, 0]], arange(0, tend + dt, dt))
    q = [slerp(q0, q1, f) for f in fraction]
    R = [qi.to_rot() for qi in q]

    dq = [der_q(q[0], q[1], dt)] + \
          [der_q(q[i-1], q[i+1], 2*dt) for i in arange(1, len(q)-1)] + \
          [der_q(q[-2], q[-1], dt)]

    omega = [2 * dot(Q(q[i]), dq[i]) for i in arange(len(q))]
    domega = simple_der(omega, dt)

    return R, omega, domega
Beispiel #5
0
def zmppoints2foottraj(point, step_time, ratio, step_height, dt, cdof, R0):
    """Compute the trajectory of the feet.

    :param point: the list of the feet location
    :param step_time: the time between 2 steps
    :param ratio: ratio between sigle support phase time and step_time
    :param step_height: the max distance between the foot and the floor
    :param dt: dt of simulation
    :param Hfloor: the transformation matrix of the floor

    :return: a list with all step trajectories [(pos_i, vel_i, acc_i)]

    """
    def get_bounded_angles(p0, p1):
        #WARNING: do this trick to get the shortest path:
        a0, a1 = (p0[2])%(2*pi), (p1[2])%(2*pi)
        diff = abs(a1 - a0)
        if   abs(a1+2*pi - a0) <diff:
            a1 += 2*pi
        elif abs(a1-2*pi - a0) <diff:
            a1 -= 2*pi
        return a0, a1

    foot_traj = []
    if   0 not in cdof:
        up = 0
        operation = rotx
    elif 1 not in cdof:
        up = 1
        operation = roty
    elif 2 not in cdof:
        up = 2
        operation = rotz

    xout      = arange(0, step_time*ratio+dt, dt)
    xin, xin2 = [0, step_time*ratio], [0, step_time*ratio/2, step_time*ratio]
    yup       = [[0, 0, 0], [step_height, 0], [0, 0, 0]]

    for i in arange(len(point)-2):
        yin0   = [[point[i][0], 0, 0], [point[i+2][0], 0, 0]]
        yin1   = [[point[i][1], 0, 0], [point[i+2][1], 0, 0]]
        a_start, a_end = get_bounded_angles(point[i], point[i+2])
        yangle = [[a_start, 0, 0], [a_end, 0, 0]]
        data = [(cdof[0], xin, yin0), \
                (cdof[1], xin, yin1), \
                (up,xin2, yup), \
                ('angle', xin, yangle)]
        res = {}
        for c, xx, yy in data:
            res[c] = (ppi(xx, yy, xout), \
                      ppi(xx, yy, xout, der=1),
                      ppi(xx, yy, xout, der=2))

        ## save traj
#        pos = zeros((len(xout), 4, 4))
        pos = tile(eye(4), (len(xout), 1, 1))
        vel = zeros((len(xout), 6))
        acc = zeros((len(xout), 6))

        for j in arange(len(xout)):
            pos[j, 0:3, 0:3] = dot(operation(res['angle'][0][j])[0:3, 0:3], \
                                  R0[0:3, 0:3] )
        vel[:, up] = res['angle'][1]
        acc[:, up] = res['angle'][2]

        for j in arange(3):
            pos[:, j, 3] = res[j][0]
            vel[:, 3+j]  = res[j][1]
            acc[:, 3+j]  = res[j][2]

        foot_traj.append( [pos, vel, acc] )
    return foot_traj
Beispiel #6
0
def interpolate_log(start, end, tend, dt):
    logs, loge = log(start), log(end)
    logtrans = ppi([0, tend], [[logs, 0], [loge, 0]], \
                   arange(0, round(tend/dt + 1))*dt)
    return [exp(i) for i in logtrans]
Beispiel #7
0
def interpolate_log(start, end, tend, dt):
    logs, loge = log(start), log(end)
    logtrans = ppi([0, tend], [[logs, 0], [loge, 0]], \
                   arange(0, round(tend/dt + 1))*dt)
    return [exp(i) for i in logtrans]