def compute_err_dic(df_state, quat_state_lst, df_gtr, quat_gtr_lst, struct):
err_dic = {}
err_dic['px'] = df_state['px'] - df_gtr['px']
err_dic['py'] = df_state['py'] - df_gtr['py']
err_dic['pz'] = df_state['pz'] - df_gtr['pz']
# q_est - q_gtr =def log3(q_gtr.inv * q_est)
quat_err_arr = np.array([
pin.log3((q_gtr.inverse() * q_est).toRotationMatrix())
for q_gtr, q_est in zip(quat_gtr_lst, quat_state_lst)
])
quat_err_mag = np.apply_along_axis(np.linalg.norm, 1, quat_err_arr)
err_dic['ox'] = quat_err_arr[:, 0]
err_dic['oy'] = quat_err_arr[:, 1]
err_dic['oz'] = quat_err_arr[:, 2]
err_dic['omag'] = quat_err_mag
err_dic['vx'] = df_state['vx'] - df_gtr['vx']
err_dic['vy'] = df_state['vy'] - df_gtr['vy']
err_dic['vz'] = df_state['vz'] - df_gtr['vz']
if struct == 'POVCDL':
err_dic['cx'] = df_state['cx'] - df_gtr['cx']
err_dic['cy'] = df_state['cy'] - df_gtr['cy']
err_dic['cz'] = df_state['cz'] - df_gtr['cz']
err_dic['cdx'] = df_state['cdx'] - df_gtr['cdx']
err_dic['cdy'] = df_state['cdy'] - df_gtr['cdy']
err_dic['cdz'] = df_state['cdz'] - df_gtr['cdz']
err_dic['Lcx'] = df_state['Lcx'] - df_gtr['Lcx']
err_dic['Lcy'] = df_state['Lcy'] - df_gtr['Lcy']
err_dic['Lcz'] = df_state['Lcz'] - df_gtr['Lcz']
return err_dic
def compute_for_normalized_time(self, t):
if t < 0:
print "Trajectory called with negative time."
return self.compute_for_normalized_time(0)
elif t > self.t_total:
print "Trajectory called after final time."
return self.compute_for_normalized_time(self.t_total)
self.M = SE3.Identity()
self.v = Motion.Zero()
self.a = Motion.Zero()
self.M.translation = self.curves(t)
self.v.linear = self.curves.d(t)
self.a.linear = self.curves.dd(t)
#rotation :
if self.curves.isInFirstNonZero(t):
self.M.rotation = self.placement_init.rotation.copy()
elif self.curves.isInLastNonZero(t):
self.M.rotation = self.placement_end.rotation.copy()
else:
# make a slerp between self.effector_placement[id][0] and [1] :
quat0 = Quaternion(self.placement_init.rotation)
quat1 = Quaternion(self.placement_end.rotation)
t_rot = t - self.t_mid_begin
"""
print "t : ",t
print "t_mid_begin : ",self.t_mid_begin
print "t_rot : ",t_rot
print "t mid : ",self.t_mid
"""
assert t_rot >= 0, "Error in the time intervals of the polybezier"
assert t_rot <= (self.t_mid + 1e-6
), "Error in the time intervals of the polybezier"
u = t_rot / self.t_mid
# normalized time without the pre defined takeoff/landing phases
self.M.rotation = (quat0.slerp(u, quat1)).matrix()
# angular velocity :
dt = 0.001
u_dt = dt / self.t_mid
r_plus_dt = (quat0.slerp(u + u_dt, quat1)).matrix()
self.v.angular = pin.log3(self.M.rotation.T * r_plus_dt) / dt
r_plus2_dt = (quat0.slerp(u + (2. * u_dt), quat1)).matrix()
next_angular_velocity = pin.log3(r_plus_dt.T * r_plus2_dt) / dt
self.a.angular = (next_angular_velocity - self.v.angular) / dt
#r_plus_dt = (quat0.slerp(u+u_dt,quat1)).matrix()
#next_angular_vel = (pin.log3(self.M.rotation.T * r_plus_dt)/dt)
#self.a.angular = (next_angular_vel - self.v.angular)/dt
return self.M, self.v, self.a
def computeRootYawAngleBetwwenConfigs(q0, q1):
quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5])
quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5])
v_angular = np.array(log3(quat0.matrix().T.dot(quat1.matrix())))
#print ("q_prev : ",q0)
#print ("q : ",q1)
#print ("v_angular = ",v_angular)
return v_angular[2]
def invgeom(h = 0.2, lx = 0.1946, ly=0.16891, leg_dir = [+1,+1,-1,-1]):
#Inputs to be modified by the user
feet_position_ref = [np.array([lx, ly, 0.0191028]),np.array([lx, -ly, 0.0191028]),np.array([-lx, ly, 0.0191028]),np.array([-lx, -ly, 0.0191028])]
base_orientation_ref = pin.utils.rpyToMatrix(0,0,0)
com_ref = np.array([0,0,h])
FL_FOOT_ID = robot.model.getFrameId('FL_FOOT')
FR_FOOT_ID = robot.model.getFrameId('FR_FOOT')
HL_FOOT_ID = robot.model.getFrameId('HL_FOOT')
HR_FOOT_ID = robot.model.getFrameId('HR_FOOT')
BASE_ID = robot.model.getFrameId('base_link')
foot_ids = np.array([FL_FOOT_ID, FR_FOOT_ID, HL_FOOT_ID, HR_FOOT_ID])
q = np.array([ 0., 0.,0.235, 0. , 0. , 0. , 1. ,
0.1 , +0.8 * leg_dir[0] , -1.6 * leg_dir[0] ,
-0.1 , +0.8 * leg_dir[1], -1.6 * leg_dir[1] ,
0.1 , -0.8 * leg_dir[2] , +1.6 * leg_dir[2] ,
-0.1 , -0.8 * leg_dir[3] , +1.6 * leg_dir[3]])
dq = robot.v0.copy()
for i in range(100):
#Compute fwd kin
pin.forwardKinematics(rmodel,rdata,q,np.zeros(rmodel.nv),np.zeros(rmodel.nv))
pin.updateFramePlacements(rmodel,rdata)
### FEET
Jfeet = []
pfeet_err = []
for i_ee in range(4):
idx = int(foot_ids[i_ee])
pos = rdata.oMf[idx].translation
ref = feet_position_ref[i_ee]
Jfeet.append(pin.computeFrameJacobian(robot.model,robot.data,q,idx,pin.LOCAL_WORLD_ALIGNED)[:3])
pfeet_err.append(ref-pos)
### CoM
com = robot.com(q)
Jcom = robot.Jcom(q)
e_com = com_ref-com
### BASE ROTATION
idx = BASE_ID
rot = rdata.oMf[idx].rotation
rotref = base_orientation_ref
Jwbasis = pin.computeFrameJacobian(robot.model,robot.data,q,idx,pin.LOCAL_WORLD_ALIGNED)[3:]
e_basisrot = -rotref @ pin.log3(rotref.T@rot)
#All tasks
J = np.vstack(Jfeet+[Jcom,Jwbasis])
x_err = np.concatenate(pfeet_err+[e_com,e_basisrot])
residual = np.linalg.norm(x_err)
if residual<1e-5:
print (np.linalg.norm(x_err))
print ("iteration: {} residual: {}".format(i,residual))
return q
dq=np.linalg.pinv(J)@(x_err)
q=pin.integrate(robot.model,q,dq)
return q * np.nan
def __init__(self, placement_init, placement_final, time_interval):
RefTrajectory.__init__(self, "TrajectorySE3LinearInterp")
self.placement_init = placement_init
self.placement_final = placement_final
self.t0 = time_interval[0]
self.t1 = time_interval[1]
self.length = self.t1 - self.t0
self.quat0 = Quaternion(self.placement_init.rotation)
self.quat1 = Quaternion(self.placement_final.rotation)
self.M = SE3.Identity()
self.v = Motion.Zero()
self.a = Motion.Zero()
# constant velocity and null acceleration :
self.v.linear = (placement_final.translation -
placement_final.translation) / self.length
self.v.angular = pin.log3(placement_final.rotation.T *
placement_final.rotation) / self.length
def update_des_acc(self, q, dq, com_ref, orien_ref, mom_ref, dmom_ref,
endeff_pos_ref, endeff_vel_ref, endeff_acc_ref,
joint_regularization_ref):
measured_op_space_velocities = self.J @ dq
# get the ref momentum acc
self.desired_acceleration[:6] = dmom_ref + np.diag(
self.p_mom_tracking) @ (mom_ref - self.robot.data.hg)
# com part
self.desired_acceleration[:3] += self.p_com_tracking * (
com_ref - self.robot.com(q))
# orientation part
base_orien = self.robot.data.oMf[self.base_id].rotation
orient_error = pin.log3(
base_orien.T @ orien_ref.matrix()) # rotated in world
self.desired_acceleration[3:6] += (
self.p_orient_tracking * orient_error -
self.d_orient_tracking * dq[3:6])
# desired motion of the feet
for i, idx in enumerate(self.endeff_ids):
self.desired_acceleration[6 + 3 * i:6 + 3 *
(i + 1)] = self.p_endeff_tracking * (
endeff_pos_ref[i] -
self.robot.data.oMf[idx].translation)
self.desired_acceleration[6 + 3 * i:6 + 3 * (
i + 1)] += self.d_endeff_tracking * (
endeff_vel_ref[i] -
measured_op_space_velocities[6 + 3 * i:6 + 3 * (i + 1)])
self.desired_acceleration[6 + 3 * i:6 + 3 *
(i + 1)] += endeff_acc_ref[i]
if joint_regularization_ref is None:
self.desired_acceleration[(self.ne + 2) * 3:] = zero(self.nv - 6)
else:
# we add some damping
self.desired_acceleration[(self.ne + 2) *
3:] = self.p_joint_regularization * (
joint_regularization_ref - q[7:])
# REVIEW(mkhadiv): I am not sure if the negative sign makes sense here!
self.desired_acceleration[
(self.ne + 2) * 3:] += -self.d_joint_regularization * dq[6:]
df_gtr['qx'], df_gtr['qy'], df_gtr['qz'], df_gtr['qw'])
]
quat_est_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_est['qx'], df_est['qy'], df_est['qz'], df_est['qw'])
]
quat_fbk_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_fbk['qx'], df_fbk['qy'], df_fbk['qz'], df_fbk['qw'])
]
quat_kfs_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_kfs['qx'], df_kfs['qy'], df_kfs['qz'], df_kfs['qw'])
]
o_gtr_arr = np.array([pin.log3(q.toRotationMatrix()) for q in quat_gtr_lst])
o_est_arr = np.array([pin.log3(q.toRotationMatrix()) for q in quat_est_lst])
o_fbk_arr = np.array([pin.log3(q.toRotationMatrix()) for q in quat_fbk_lst])
o_kfs_arr = np.array([pin.log3(q.toRotationMatrix()) for q in quat_kfs_lst])
for i in range(3):
xyz = 'xyz'[i]
df_gtr['o' + xyz] = o_gtr_arr[:, i]
df_est['o' + xyz] = o_est_arr[:, i]
df_fbk['o' + xyz] = o_fbk_arr[:, i]
df_kfs['o' + xyz] = o_kfs_arr[:, i]
def compute_err_dic(df_state, quat_state_lst, df_gtr, quat_gtr_lst, struct):
err_dic = {}
err_dic['px'] = df_state['px'] - df_gtr['px']
err_dic['py'] = df_state['py'] - df_gtr['py']
def se3Interp(s1, s2, alpha):
t = (s1.translation * alpha + s2.translation * (1 - alpha))
r = se3.exp3(
se3.log3(s1.rotation) * alpha + se3.log3(s2.rotation) * (1 - alpha))
return se3.SE3(r, t)
def test_log3(self):
m = eye(3)
v = pin.log3(m)
self.assertApprox(v, zero(3))
S = 2000
E = 10000
# print(f_sum_arr[:,S:E])
# print()
# print(mg_arr[:,S:E])
M = f_sum_arr[:, S:E] @ mg_arr[:, S:E].T
u, s, vh = np.linalg.svd(M, full_matrices=True)
R_est = u @ vh
print(R_est)
q_est = pin.Quaternion(R_est)
print(q_est.coeffs())
rpy_est = pin.rpy.matrixToRpy(R_est)
print('Angular error (in deg): ', np.rad2deg(pin.log3(R_est)))
f_sum_arr_rot = R_est.T @ f_sum_arr
# print(f_sum_arr_rot[:,S:E])
t_arr = np.arange(N)
plt.figure('Total forces world frame')
plt.plot(t_arr, f_sum_arr[0, :], 'r', label='f sum x')
plt.plot(t_arr, f_sum_arr[1, :], 'g', label='f sum y')
plt.plot(t_arr, f_sum_arr[2, :], 'b', label='f sum z')
plt.hlines(0, -1000, N + 1000, 'k')
plt.hlines(m_solo * g, -1000, N + 1000, 'k')
plt.legend()
plt.figure('Total rotated forces world frame')
Lc_est_arr = np.array(Lc_est_lst)
p_gtr_arr = np.array(p_gtr_lst)
v_gtr_arr = np.array(v_gtr_lst)
c_gtr_arr = np.array(c_gtr_lst)
dc_gtr_arr = np.array(dc_gtr_lst)
Lc_gtr_arr = np.array(Lc_gtr_lst)
# compute errors
p_err_arr = p_est_arr - p_gtr_arr
v_err_arr = v_est_arr - v_gtr_arr
c_err_arr = c_est_arr - c_gtr_arr
dc_err_arr = dc_est_arr - dc_gtr_arr
Lc_err_arr = Lc_est_arr - Lc_gtr_arr
oRb_err_arr = np.array([
pin.log3(oRb_gtr.T @ oRb_est)
for oRb_est, oRb_gtr in zip(oRb_est_lst, oRb_gtr_lst)
])
#######
# PLOTS
#######
err_arr_lst = [
p_err_arr, oRb_err_arr, v_err_arr, c_err_arr, dc_err_arr, Lc_err_arr
]
fig_titles = [
'P error', 'O error', 'V error', 'C error', 'D error', 'Lc error'
]
for err_arr, fig_title in zip(err_arr_lst, fig_titles):
plt.figure(fig_title)
def test_log3(self):
m = eye(3)
v = pin.log3(m)
self.assertApprox(v, zero(3))
def compute(self, q, dq):
### FEET
Jfeet = []
afeet = []
pfeet_err = []
vfeet_ref = []
pin.forwardKinematics(self.rmodel, self.rdata, q, dq,
np.zeros(self.rmodel.nv))
pin.updateFramePlacements(self.rmodel, self.rdata)
for i_ee in range(4):
idx = int(self.foot_ids[i_ee])
pos = self.rdata.oMf[idx].translation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
ref = self.feet_position_ref[i_ee]
vref = self.feet_velocity_ref[i_ee]
aref = self.feet_acceleration_ref[i_ee]
J1 = pin.computeFrameJacobian(self.robot.model, self.robot.data, q,
idx, pin.LOCAL_WORLD_ALIGNED)[:3]
e1 = ref - pos
acc1 = -self.Kp_flyingfeet * (pos - ref) - self.Kd_flyingfeet * (
nu.linear - vref) + aref
if self.flag_in_contact[i_ee]:
acc1 *= 0 # In contact = no feedback
drift1 = np.zeros(3)
drift1 += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).linear
drift1 += self.cross3(nu.angular, nu.linear)
acc1 -= drift1
Jfeet.append(J1)
afeet.append(acc1)
pfeet_err.append(e1)
vfeet_ref.append(vref)
### BASE POSITION
idx = self.BASE_ID
pos = self.rdata.oMf[idx].translation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
ref = self.base_position_ref
Jbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data, q,
idx, pin.LOCAL_WORLD_ALIGNED)[:3]
e_basispos = ref - pos
accbasis = -self.Kp_base_position * (
pos - ref) - self.Kd_base_position * nu.linear
drift = np.zeros(3)
drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).linear
drift += self.cross3(nu.angular, nu.linear)
accbasis -= drift
### BASE ROTATION
idx = self.BASE_ID
rot = self.rdata.oMf[idx].rotation
nu = pin.getFrameVelocity(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED)
rotref = self.base_orientation_ref
Jwbasis = pin.computeFrameJacobian(self.robot.model, self.robot.data,
q, idx, pin.LOCAL_WORLD_ALIGNED)[3:]
e_basisrot = -rotref @ pin.log3(rotref.T @ rot)
accwbasis = -self.Kp_base_orientation * rotref @ pin.log3(
rotref.T @ rot) - self.Kd_base_orientation * nu.angular
drift = np.zeros(3)
drift += pin.getFrameAcceleration(self.rmodel, self.rdata, idx,
pin.LOCAL_WORLD_ALIGNED).angular
accwbasis -= drift
J = np.vstack(Jfeet + [Jbasis, Jwbasis])
acc = np.concatenate(afeet + [accbasis, accwbasis])
x_err = np.concatenate(pfeet_err + [e_basispos, e_basisrot])
dx_ref = np.concatenate(
vfeet_ref +
[self.base_linearvelocity_ref, self.base_angularvelocity_ref])
invJ = self.dinv(J) #or np.linalg.inv(J) since full rank
ddq = invJ @ acc
self.q_cmd = pin.integrate(self.robot.model, q, invJ @ x_err)
self.dq_cmd = invJ @ dx_ref
return ddq
def test_sensor_noise_bias(self):
"""
@brief Test sensor noise and bias for an IMU sensor on a simple pendulum in static pose.
"""
# Add IMU.
imu_sensor = jiminy.ImuSensor("PendulumLink")
self.robot.attach_sensor(imu_sensor)
imu_sensor.initialize("PendulumLink")
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
engine.initialize(self.robot)
x0 = np.array([0.0, 0.0])
tf = 200.0
# Configure the engine: No gravity
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6)
engine.set_options(engine_options)
# Configure the IMU
imu_options = imu_sensor.get_options()
imu_options['noiseStd'] = np.linspace(0.0, 0.2, 9)
imu_options['bias'] = np.linspace(0.0, 1.0, 9)
imu_sensor.set_options(imu_options)
# Run simulation
engine.simulate(tf, x0)
log_data, _ = engine.get_log()
quat_jiminy = np.stack(
[log_data['PendulumLink.Quat' + s] for s in ['x', 'y', 'z', 'w']],
axis=-1)
gyro_jiminy = np.stack(
[log_data['PendulumLink.Gyro' + s] for s in ['x', 'y', 'z']],
axis=-1)
accel_jiminy = np.stack(
[log_data['PendulumLink.Accel' + s] for s in ['x', 'y', 'z']],
axis=-1)
# Convert quaternion to a rotation vector.
quat_axis = np.stack(
[log3(Quaternion(q[:, np.newaxis]).matrix()) for q in quat_jiminy],
axis=0)
# Estimate the quaternion noise and bias
# Because the IMU rotation is identity, the resulting rotation will
# simply be R_b R_noise. Since R_noise is a small rotation, we can
# consider that the resulting rotation is simply the rotation resulting
# from the sum of the rotation vector (this is only true at the first
# order) and thus directly recover the unbiased sensor data.
quat_axis_bias = np.mean(quat_axis, axis=0)
quat_axis_std = np.std(quat_axis, axis=0)
# Remove sensor rotation bias from gyro / accel data
quat_rot_bias = exp3(quat_axis_bias)
gyro_jiminy = np.vstack([quat_rot_bias @ v for v in gyro_jiminy])
accel_jiminy = np.vstack([quat_rot_bias @ v for v in accel_jiminy])
# Estimate the gyroscope and accelerometer noise and bias
gyro_std = np.std(gyro_jiminy, axis=0)
gyro_bias = np.mean(gyro_jiminy, axis=0)
accel_std = np.std(accel_jiminy, axis=0)
accel_bias = np.mean(accel_jiminy, axis=0)
# Compare estimated sensor noise and bias with the configuration
self.assertTrue(
np.allclose(imu_options['noiseStd'][:3],
quat_axis_std,
atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][:3], quat_axis_bias, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['noiseStd'][3:-3], gyro_std, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][3:-3], gyro_bias, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['noiseStd'][-3:], accel_std, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][-3:], accel_bias, atol=1.0e-2))
def test_sensor_noise_bias(self):
"""
@brief Test sensor noise and bias for an IMU sensor on a simple
pendulum in static pose.
"""
# Create an engine: no controller and no internal dynamics
engine = jiminy.Engine()
setup_controller_and_engine(engine, self.robot)
# Configure the engine: No gravity
engine_options = engine.get_options()
engine_options["world"]["gravity"] = np.zeros(6)
engine.set_options(engine_options)
# Configure the IMU
imu_options = self.imu_sensor.get_options()
imu_options['noiseStd'] = np.linspace(0.0, 0.2, 9)
imu_options['bias'] = np.linspace(0.0, 1.0, 9)
self.imu_sensor.set_options(imu_options)
# Run simulation and extract log data
x0 = np.array([0.0, 0.0])
tf = 200.0
_, quat_jiminy, gyro_jiminy, accel_jiminy = \
SimulateSimplePendulum._simulate_and_get_imu_data_evolution(engine, tf, x0, split=True)
# Convert quaternion to a rotation vector.
quat_axis = np.stack(
[log3(Quaternion(q[:, np.newaxis]).matrix()) for q in quat_jiminy],
axis=0)
# Estimate the quaternion noise and bias
# Because the IMU rotation is identity, the resulting rotation will
# simply be R_b R_noise. Since R_noise is a small rotation, we can
# consider that the resulting rotation is simply the rotation resulting
# from the sum of the rotation vector (this is only true at the first
# order) and thus directly recover the unbiased sensor data.
quat_axis_bias = np.mean(quat_axis, axis=0)
quat_axis_std = np.std(quat_axis, axis=0)
# Remove sensor rotation bias from gyro / accel data
quat_rot_bias = exp3(quat_axis_bias)
gyro_jiminy = np.vstack([quat_rot_bias @ v for v in gyro_jiminy])
accel_jiminy = np.vstack([quat_rot_bias @ v for v in accel_jiminy])
# Estimate the gyroscope and accelerometer noise and bias
gyro_std = np.std(gyro_jiminy, axis=0)
gyro_bias = np.mean(gyro_jiminy, axis=0)
accel_std = np.std(accel_jiminy, axis=0)
accel_bias = np.mean(accel_jiminy, axis=0)
# Compare estimated sensor noise and bias with the configuration
self.assertTrue(
np.allclose(imu_options['noiseStd'][:3],
quat_axis_std,
atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][:3], quat_axis_bias, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['noiseStd'][3:-3], gyro_std, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][3:-3], gyro_bias, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['noiseStd'][-3:], accel_std, atol=1.0e-2))
self.assertTrue(
np.allclose(imu_options['bias'][-3:], accel_bias, atol=1.0e-2))
def calc(self, data, x, u):
data.rRf = self.Rref.oRf.transpose() * data.shared.pinocchio.oMf[
self.Rref.frame].rotation
data.r = pinocchio.log3(data.rRf)
self.activation.calc(data.activation, data.r)
data.cost = data.activation.a
df_gtr_kfs = df_gtr_kfs.append(df_gtr.loc[np.isclose(df_gtr['t'], t)],
ignore_index=True)
# COMPUTE ERROR ARRAYS
# Extract quaternions to compute orientation difference (xyzw reversed in constructor -> normal)
quat_gtr_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_gtr['qx'], df_gtr['qy'], df_gtr['qz'], df_gtr['qw'])
]
quat_est_lst = [
pin.Quaternion(qw, qx, qy, qz) for qx, qy, qz, qw in zip(
df_est['qx'], df_est['qy'], df_est['qz'], df_est['qw'])
]
# q_est - q_gtr =def log3(q_gtr.inv * q_est)
quat_err_arr = np.array([
pin.log3((q_gtr.inverse() * q_est).toRotationMatrix())
for q_gtr, q_est in zip(quat_gtr_lst, quat_est_lst)
])
errpx = df_est['px'] - df_gtr['px']
errpy = df_est['py'] - df_gtr['py']
errpz = df_est['pz'] - df_gtr['pz']
errvx = df_est['vx'] - df_gtr['vx']
errvy = df_est['vy'] - df_gtr['vy']
errvz = df_est['vz'] - df_gtr['vz']
errcx = df_est['cx'] - df_gtr['cx']
errcy = df_est['cy'] - df_gtr['cy']
errcz = df_est['cz'] - df_gtr['cz']
errcdx = df_est['cdx'] - df_gtr['cdx']
errcdy = df_est['cdy'] - df_gtr['cdy']
errcdz = df_est['cdz'] - df_gtr['cdz']
errLx = df_est['Lx'] - df_gtr['Lx']
def calc(self, data, x, u):
data.rRf = self.ref.transpose() * data.pinocchio.oMf[
self.frame].rotation
data.residuals[:] = m2a(pinocchio.log3(data.rRf))
data.cost = sum(self.activation.calc(data.activation, data.residuals))
return data.cost
o_rpy_i_arr = np.zeros((N, 3))
w_rpy_m_arr = np.zeros((N, 3))
for i in range(N):
o_R_i = pin.Quaternion(arr_dic['o_q_i'][i, :].reshape(
(4, 1))).toRotationMatrix()
w_R_m = pin.Quaternion(arr_dic['w_q_m'][i, :].reshape(
(4, 1))).toRotationMatrix()
# o_R_i = arr_dic['o_R_i'][i,:,:]
# w_R_m = arr_dic['w_R_m'][i,:,:]
o_rpy_i = pin.rpy.matrixToRpy(o_R_i)
w_rpy_m = pin.rpy.matrixToRpy(w_R_m)
o_rpy_i_arr[i, :] = np.rad2deg(o_rpy_i)
w_rpy_m_arr[i, :] = np.rad2deg(w_rpy_m)
m_R_i = w_R_m.T @ o_R_i
w_R_o = w_R_m @ o_R_i.T
err_m_R_i = np.rad2deg(pin.log3(m_R_i))
err_w_R_o = np.rad2deg(pin.log3(w_R_o))
if np.linalg.norm(err_m_R_i) > np.linalg.norm(err_max_m_R_i):
err_max_m_R_i = err_m_R_i.copy()
if np.linalg.norm(err_w_R_o) > np.linalg.norm(err_max_w_R_o):
err_max_w_R_o = err_w_R_o.copy()
print()
print(data_file, 'err_o_deg')
print('err_max_m_R_i: ', err_max_m_R_i)
print('err_max_w_R_o: ', err_max_w_R_o)
plt.figure(data_file + ' DEG error')
for j in range(3):
plt.subplot(3, 1, 1 + j)
plt.plot(arr_dic['t'], o_rpy_i_arr[:, j], 'r', label='IMU')
plt.plot(arr_dic['t'], w_rpy_m_arr[:, j], 'b', label='MOCAP')
