Xp_gyro = np.column_stack((
imu_gyro.x.as_matrix(),
imu_gyro.y.as_matrix(),
imu_gyro.z.as_matrix()
))
Xp = np.column_stack((Xp_orientation, Xp_joints_position, Xp_joints_speed,
Xp_acc, Xp_gyro))
##########################################################################
# READ THE GP_MODEL
##########################################################################
m = data.open_object(path_gpy_gaussian_process_model_file)
[pred_mean, pred_var] = m.predict(Xp)
#################
## RE-SAMPLE ##
#################
reference = reference.resample(resampling_time).mean()
odometry = odometry.resample(resampling_time).mean()
orb_slam2 = orb_slam2.resample(resampling_time).pad()
##########################################################################
#rotate and translate the reference trajectory wrt the world frame
##########################################################################
reference_position = np.column_stack((reference.x.values, reference.y.values, reference.z.values ))
reference_position[:] = [navigation_orient.data[0].rot(x) + navigation_position.data[0] for x in reference_position]
imu_acc.readData(pose_imu_acceleration_file, cov=False)
imu_acc.eigenValues()
# IMU Angular Velocity
imu_gyro = data.ThreeData()
imu_gyro.readData(pose_imu_angular_velocity_file, cov=False)
imu_gyro.eigenValues()
# Robot Joints Position and Speed
names = "left_passive", "fl_mimic", "fl_walking", "fl_steer", "fl_drive", "fl_contact", "fl_translation", "fl_slipx", "fl_slipy", "fl_slipz", "ml_mimic", "ml_walking", "ml_drive", "ml_contact", "ml_translation", "ml_slipx", "ml_slipy", "ml_slipz", "rear_passive", "rl_mimic", "rl_walking", "rl_steer", "rl_drive", "rl_contact", "rl_translation", "rl_slipx", "rl_slipy", "rl_slipz", "rr_mimic", "rr_walking", "rr_steer", "rr_drive", "rr_contact", "rr_translation", "rr_slipx", "rr_slipy", "rr_slipz", "right_passive", "fr_mimic", "fr_walking", "fr_steer", "fr_drive", "fr_contact", "fr_translation", "fr_slipx", "fr_slipy", "fr_slipz", "mr_mimic", "mr_walking", "mr_drive", "mr_contact", "mr_translation", "mr_slipx", "mr_slipy", "mr_slipz"
robot_joints = js.Joints(names)
robot_joints.readData(joints_position_file, joints_speed_file)
# Read Gaussian process model
gp_m = data.open_object('./data/gaussian_processes/gpy_uncertainty_model_1000_sparse_regression.out')
#######################################
# Form the New Inputs to predict
# Joints Inputs #
joints = np.column_stack((
robot_joints.getSpeed("fl_translation"),
robot_joints.getSpeed("fr_translation"),
robot_joints.getSpeed("ml_translation"),
robot_joints.getSpeed("mr_translation"),
robot_joints.getSpeed("rl_translation"),
robot_joints.getSpeed("rr_translation"),
robot_joints.getPosition("fl_steer"),
robot_joints.getPosition("fr_steer"),
robot_joints.getPosition("rl_steer"),
robot_joints.getPosition("rr_steer"),