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
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
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
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
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
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]