ddq[i,:] = np.array(estimator_kin.ddx.value);
if(i==2):
print ("F/T sensor offsets are: ", estimator_ft.getFTsensorOffsets());
if(i%1000==0):
print 'Estimation time: \t %.3f' % (i*dt);
return (torques, dq, ddq);
if __name__=='__main__':
from load_hrpsys_log import load_hrpsys_log_astate
sensors = load_hrpsys_log_astate('/home/adelpret/devel/yarp_gazebo/src/motorFrictionIdentification/data/20140807-legTorqueId/legTorqueId_pos1-astate.log',
'rad');
# sensors = sensors[:2000];
if(PLOT_SENSOR_DATA):
print "Plot sensor data";
for i in range(sensors['enc'].shape[1]):
plut.plot_x_vs_y(sensors['time'], sensors['joint'][:,i], 'Joint angle '+str(i));
f, ax = plt.subplots(3, 1, sharex=True);
ax[0].set_title('Accelerometer');
ax[0].plot(sensors['time'], sensors['acc'][:,0]);
ax[1].plot(sensors['time'], sensors['acc'][:,1]);
ax[2].plot(sensors['time'], sensors['acc'][:,2]);
f, ax = plt.subplots(3, 1, sharex=True);
ax[0].set_title('Gyroscope');
"""
from compute_estimates_from_sensors import compute_estimates_from_sensors
from load_hrpsys_log import load_hrpsys_log_astate
from load_hrpsys_log import load_hrpsys_log_rstate
import numpy as np
import matplotlib.pyplot as plt
out_data_folder = '../results/20140807-legTorqueId1/'
OUT_DATA_FILE_NAME = 'data.npz'
A_STATE_FILE = '/home/adelpret/devel/yarp_gazebo/src/motorFrictionIdentification/data/20140807-legTorqueId/legTorqueId_pos1-astate.log'
R_STATE_FILE = '/home/adelpret/devel/yarp_gazebo/src/motorFrictionIdentification/data/20140807-legTorqueId/legTorqueId_pos1-rstate.log'
ESTIMATION_DELAY = 0.2
COMPUTE_TORQUES_WITHOUT_GYRO = False
sensors = load_hrpsys_log_astate(A_STATE_FILE, 'rad')
ref = load_hrpsys_log_rstate(R_STATE_FILE, 'rad')
#sensors = sensors[:5000];
#ref = ref[:5000];
(torques, dq, ddq) = compute_estimates_from_sensors(sensors, ESTIMATION_DELAY)
if (COMPUTE_TORQUES_WITHOUT_GYRO):
sensors['gyro'] = np.zeros(sensors['gyro'].shape)
(torques_no_gyro, dq,
ddq) = compute_estimates_from_sensors(sensors, ESTIMATION_DELAY)
for i in range(12): #torques.shape[1]):
print "Plot data joint %d out of %d" % (i, torques.shape[1])
f, ax = plt.subplots(1, 1, sharex=True)
"""
from compute_estimates_from_sensors import compute_estimates_from_sensors
from load_hrpsys_log import load_hrpsys_log_astate
from load_hrpsys_log import load_hrpsys_log_rstate
import numpy as np
import matplotlib.pyplot as plt
out_data_folder = '../results/20140807-legTorqueId1/';
OUT_DATA_FILE_NAME = 'data.npz';
A_STATE_FILE = '/home/adelpret/devel/yarp_gazebo/src/motorFrictionIdentification/data/20140807-legTorqueId/legTorqueId_pos1-astate.log';
R_STATE_FILE = '/home/adelpret/devel/yarp_gazebo/src/motorFrictionIdentification/data/20140807-legTorqueId/legTorqueId_pos1-rstate.log';
ESTIMATION_DELAY = 0.2;
COMPUTE_TORQUES_WITHOUT_GYRO = False;
sensors = load_hrpsys_log_astate(A_STATE_FILE, 'rad');
ref = load_hrpsys_log_rstate(R_STATE_FILE, 'rad');
#sensors = sensors[:5000];
#ref = ref[:5000];
(torques, dq, ddq) = compute_estimates_from_sensors(sensors, ESTIMATION_DELAY);
if(COMPUTE_TORQUES_WITHOUT_GYRO):
sensors['gyro'] = np.zeros(sensors['gyro'].shape);
(torques_no_gyro, dq, ddq) = compute_estimates_from_sensors(sensors, ESTIMATION_DELAY);
for i in range(12): #torques.shape[1]):
print "Plot data joint %d out of %d" % (i, torques.shape[1]);
f, ax = plt.subplots(1, 1, sharex=True);
ax.plot(sensors['time'], sensors['torque'][:,i], 'b');
dq[i,:] = np.array(estimator.jointsVelocities.value);
ddq[i,:] = np.array(estimator.jointsAccelerations.value);
if(i==2):
print ("F/T sensor offsets are: ", estimator.getFTsensorOffsets());
if(i%1000==0):
print 'Estimation time: \t %.3f' % (i*dt);
return (torques, dq, ddq);
if __name__=='__main__':
sensors = load_hrpsys_log_astate('/home/adelpret/devel/yarp_gazebo/src/motorFrictionIdentification/data/20140807-legTorqueId/legTorqueId_pos1-astate.log',
'rad');
# sensors = sensors[:2000];
if(PLOT_SENSOR_DATA):
print "Plot sensor data";
for i in range(sensors['enc'].shape[1]):
plut.plot_x_vs_y(sensors['time'], sensors['joint'][:,i], 'Joint angle '+str(i));
f, ax = plt.subplots(3, 1, sharex=True);
ax[0].set_title('Accelerometer');
ax[0].plot(sensors['time'], sensors['acc'][:,0]);
ax[1].plot(sensors['time'], sensors['acc'][:,1]);
ax[2].plot(sensors['time'], sensors['acc'][:,2]);
f, ax = plt.subplots(3, 1, sharex=True);
ax[0].set_title('Gyroscope');
if i == 2:
print("F/T sensor offsets are: ",
estimator_ft.getFTsensorOffsets())
if i % 1000 == 0:
print('Estimation time: \t %.3f' % (i * dt))
return (torques, dq, ddq)
if __name__ == '__main__':
from load_hrpsys_log import load_hrpsys_log_astate
# /home/adelpret/devel/yarp_gazebo/src/motorFrictionIdentification
# /data/20140807-legTorqueId/legTorqueId_pos1-astate.log
sensors = load_hrpsys_log_astate('/tmp/legTorqueId_pos1-astate.log', 'rad')
# sensors = sensors[:2000];
if PLOT_SENSOR_DATA:
print("Plot sensor data")
for i in range(sensors['enc'].shape[1]):
plut.plot_x_vs_y(sensors['time'], sensors['joint'][:, i],
'Joint angle ' + str(i))
f, ax = plt.subplots(3, 1, sharex=True)
ax[0].set_title('Accelerometer')
ax[0].plot(sensors['time'], sensors['acc'][:, 0])
ax[1].plot(sensors['time'], sensors['acc'][:, 1])
ax[2].plot(sensors['time'], sensors['acc'][:, 2])
f, ax = plt.subplots(3, 1, sharex=True)