Example #1
0
        return x*x
    
    R[0,0] = -1.0 + 2.0*sqr(q[0]) + 2.0*sqr(q[1]);
    R[1,1] = -1.0 + 2.0*sqr(q[0]) + 2.0*sqr(q[2]);
    R[2,2] = -1.0 + 2.0*sqr(q[0]) + 2.0*sqr(q[3]);
    R[1,0] = 2.0 * (q[1]*q[2] + q[0]*q[3]);
    R[2,0] = 2.0 * (q[1]*q[3] - q[0]*q[2]);
    R[0,1] = 2.0 * (q[1]*q[2] - q[0]*q[3]);
    R[2,1] = 2.0 * (q[2]*q[3] + q[0]*q[1]);
    R[0,2] = 2.0 * (q[1]*q[3] + q[0]*q[2]);
    R[1,2] = 2.0 * (q[2]*q[3] - q[0]*q[1]);
    
    return R

 
data = ClmcFile('d00486')
time = array(data.get_variables('time'))
cog = array(data.get_variables(['cog_x','cog_y','cog_z']))
des_cog = array(data.get_variables(['des_com_x','des_com_y','des_com_z']))
left_foot = array(data.get_variables(['LEFT_FOOT_x','LEFT_FOOT_y','LEFT_FOOT_z']))    
right_foot = array(data.get_variables(['RIGHT_FOOT_x','RIGHT_FOOT_y','RIGHT_FOOT_z']))
left_foot_q = array(data.get_variables(['LEFT_FOOT_q0','LEFT_FOOT_q1','LEFT_FOOT_q2','LEFT_FOOT_q3']))
right_foot_q = array(data.get_variables(['RIGHT_FOOT_q0','RIGHT_FOOT_q1','RIGHT_FOOT_q2','RIGHT_FOOT_q3']))
left_foot_d = array(data.get_variables(['LEFT_FOOT_xd','LEFT_FOOT_yd','LEFT_FOOT_zd']))
dcog = array(data.get_variables(['com_xd','com_yd','com_zd'])) 
des_dcog = array(data.get_variables(['des_com_xd','des_com_yd','des_com_zd']))
  
time = array(data.get_variables('time'))
ati = array(data.get_variables(['ati_Fx','ati_Fy','ati_Fz']))
F_tot = sqrt(ati[0,:]*ati[0,:] + ati[1,:]*ati[1,:] + ati[2,:]*ati[2,:])
  
import os, sys
from clmcplot.clmcplot_utils import ClmcFile
import numpy
from pylab import *
from scipy import signal


data = ClmcFile('d00390')
[time, th, thd, val, F] = data.get_variables(['time', 'L_KFE_th', 'L_KFE_thd', 'L_KFE_gdc_val',
                                              'L_KFE_load'])

dF = diff(F)/0.001

beg = 4500
en = 6000
dF = dF[beg:en]
th = th[beg:en]
thd = thd[beg:en]
val = val[beg:en]

(b, a) = signal.butter(2, 0.01)
dF = signal.lfilter(b,a, dF)
thd = signal.lfilter(b,a, thd)
val = signal.lfilter(b,a, val)
th = signal.lfilter(b,a, th)

a = 0.0826
b = 0.1143
c = 0.0818
d = 0.0825
f = 0.0812
Example #3
0
def plot_cog(filename):
    data = ClmcFile(filename)
    time = array(data.get_variables('time'))
    cog = array(data.get_variables(['cog_x','cog_y','cog_z']))
    des_cog = array(data.get_variables(['des_cog_x','des_cog_y','des_cog_z']))
    left_foot = array(data.get_variables(['LEFT_FOOT_x','LEFT_FOOT_y','LEFT_FOOT_z']))    
    right_foot = array(data.get_variables(['RIGHT_FOOT_x','RIGHT_FOOT_y','RIGHT_FOOT_z']))
    left_foot_q = array(data.get_variables(['LEFT_FOOT_q0','LEFT_FOOT_q1','LEFT_FOOT_q2','LEFT_FOOT_q3']))
    right_foot_q = array(data.get_variables(['RIGHT_FOOT_q0','RIGHT_FOOT_q1','RIGHT_FOOT_q2','RIGHT_FOOT_q3']))
    left_foot_d = array(data.get_variables(['LEFT_FOOT_xd','LEFT_FOOT_yd','LEFT_FOOT_zd']))
    dcog = array(data.get_variables(['dcog_x','dcog_y','dcog_z'])) 
    des_dcog = array(data.get_variables(['des_dcog_x','des_dcog_y','des_dcog_z']))
    
    for i in range(size(time)):
        mid_pose = 0.5*(left_foot[:,i] + right_foot[:,i])
        Rl = quaternionToRot(left_foot_q[:,i])
        Rr = quaternionToRot(right_foot_q[:,i])
        R = zeros([3,3])
        R[:,1] = -Rl[:,2] + Rr[:,2]
        R[:,1] = R[:,1]
        R[2,1] = 0.0
        R[:,1] = R[:,1] / linalg.norm(R[:,1])
        R[:,2] = array([0,0,1])
        R[:,0] = cross(R[:,1], R[:,2])
        cog[:,i] = dot(transpose(R),cog[:,i] - mid_pose)
        des_cog[:,i] = dot(transpose(R),des_cog[:,i] - mid_pose)
        dcog[:,i] = dot(transpose(R), dcog[:,i]-left_foot_d[:,i])
        des_dcog[:,i] = dot(transpose(R), des_dcog[:,i])
    
    fig = figure()
    fig.canvas.set_window_title(filename + str(" cog position")) 
    for i in range(3):
        subplot(3,1,i+1)
        plot(time, cog[i], time, des_cog[i],linewidth=3)
        ylim([-.08,.08]+mean(des_cog[i]))
        print 'RMSe = ' + str(sqrt(sum(square((cog[i]-des_cog[i])))/size(cog[i])))
    fig = figure()
    fig.canvas.set_window_title(filename + str(" cog vel"))
    for i in range(3):
        subplot(3,1,i+1)
        plot(time, dcog[i], time, des_dcog[i],linewidth=3)
        ylim([-.15,.15]+mean(des_dcog[i]))
        
    fig = figure()
    subplot(2,1,1)
    plot(time, cog[2], time, des_cog[2], linewidth=4)
    ylim([-0.08,0.08]+mean(des_cog[2]))
    
    subplot(2,1,2)
    plot(time, dcog[i], time, des_dcog[i],linewidth=4)
    ylim([-.15,.15]+mean(des_dcog[2]))
Example #4
0
def plot_cog(filename):
    data = ClmcFile(filename)
    time = array(data.get_variables('time'))
    cog = array(data.get_variables(['cog_x','cog_y','cog_z']))
    des_cog = array(data.get_variables(['des_com_x','des_com_y','des_com_z']))
    left_foot = array(data.get_variables(['LEFT_FOOT_x','LEFT_FOOT_y','LEFT_FOOT_z']))    
    right_foot = array(data.get_variables(['RIGHT_FOOT_x','RIGHT_FOOT_y','RIGHT_FOOT_z']))
    left_foot_q = array(data.get_variables(['LEFT_FOOT_q0','LEFT_FOOT_q1','LEFT_FOOT_q2','LEFT_FOOT_q3']))
    right_foot_q = array(data.get_variables(['RIGHT_FOOT_q0','RIGHT_FOOT_q1','RIGHT_FOOT_q2','RIGHT_FOOT_q3']))
    left_foot_d = array(data.get_variables(['LEFT_FOOT_xd','LEFT_FOOT_yd','LEFT_FOOT_zd']))
    dcog = array(data.get_variables(['com_xd','com_yd','com_zd'])) 
    des_dcog = array(data.get_variables(['des_com_xd','des_com_yd','des_com_zd']))
    
    des_right_foot = array(data.get_variables(['RF_ref_pos_X','RF_ref_pos_Y','RF_ref_pos_Z']))
    
    zmp = array(data.get_variables(['des_zmp_X','des_zmp_Y','des_zmp_Z']))
    des_zmp = array(data.get_variables(['zmp_X','zmp_Y','zmp_Z']))
    
    for i in range(size(time)):
        Rl = quaternionToRot(left_foot_q[:,i])
        mid_pose = left_foot[:,i] + dot(Rl, array([0,0.09,0]))
        R = zeros([3,3])
        R[:,1] = -Rl[:,2]
        R[2,1] = 0.0
        R[:,1] = R[:,1] / linalg.norm(R[:,1])
        R[:,2] = array([0,0,1])
        R[:,0] = cross(R[:,1], R[:,2])
        cog[:,i] = dot(transpose(R),cog[:,i] - mid_pose)
        des_cog[:,i] = dot(transpose(R),des_cog[:,i] - mid_pose)
        dcog[:,i] = dot(transpose(R), dcog[:,i]-left_foot_d[:,i])
        des_dcog[:,i] = dot(transpose(R), des_dcog[:,i])
        left_foot[:,i] = dot(transpose(R),left_foot[:,i] - mid_pose)
        right_foot[:,i] = dot(transpose(R),right_foot[:,i] - mid_pose)
        des_right_foot[:,i] = dot(transpose(R),des_right_foot[:,i] - mid_pose)
        
        zmp[2,i] = mid_pose[2]
        des_zmp[2,i] = mid_pose[2]
        zmp[:,i] = dot(transpose(R),zmp[:,i] - mid_pose)
        des_zmp[:,i] = dot(transpose(R),des_zmp[:,i] - mid_pose)
    
    fig = figure()
    fig.canvas.set_window_title(filename + str(" cog position")) 
    for i in range(3):
        subplot(3,1,i+1)
        plot(time, cog[i], time, des_cog[i],linewidth=3)
        plot(time, left_foot[i], linewidth=3)
        plot(time, right_foot[i], linestyle='--', linewidth=3)
#         ylim([-.10,.10]+mean(des_cog[i]))
#         print 'RMSe = ' + str(sqrt(sum(square((cog[i]-des_cog[i])))/size(cog[i])))
#     fig = figure()
#     fig.canvas.set_window_title(filename + str(" cog vel"))
#     for i in range(3):
#         subplot(3,1,i+1)
#         plot(time, dcog[i], time, des_dcog[i],linewidth=3)
# #         ylim([-.15,.15]+mean(des_dcog[i]))
# 
#     fig = figure()
#     for i in range(3):
#         subplot(3,1,i+1)
#         plot(time, zmp[i], time, des_zmp[i],linewidth=3)
    fig = figure()
    for i in range(3):
        subplot(3,1,i+1)
        plot(time, right_foot[i], time, des_right_foot[i], linewidth=3)