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')