[ VOXL ] VIsual Odometry

 

 

 

yocto:/# voxl-vision-px4 -o loading our own config file ================================================================= Parameters as loaded from config file: qgc_ip: 192.168.219.182 en_localhost_mavlink_udp 1 en_secondary_qgc: 0 secondary_qgc_ip: 192.168.1.214 qgc_udp_port_number: 14550 localhost_udp_port_number: 14551 en_vio: 1 en_voa: 1 en_send_vio_to_qgc: 0 en_send_voa_to_qgc: 0 en_adsb: 0 adsb_uart_bus: 7 adsb_uart_baudrate: 57600 px4_uart_bus: 5 px4_uart_baudrate: 921600 offboard_mode: figure_eight follow_tag_id: 0 en_apriltag_fixed_frame: 0 fixed_frame_filter_len: 5 en_transform_mavlink_pos_setpoints_from_fixed_frame:1 ================================================================= loading extrinsics and apriltag external config files #0: tag id: 0 name: default_name location: fixed size_m: 0.400 T_tag_wrt_fixed: 0.0 0.0 0.0 R_tag_to_fixed: 0.0 -1.0 0.0 1.0 0.0 0.0 0.0 0.0 1.0 #1: tag id: 1 name: default_name location: fixed size_m: 0.400 T_tag_wrt_fixed: 0.0 0.0 0.0 R_tag_to_fixed: 0.0 -1.0 0.0 1.0 0.0 0.0 0.0 0.0 1.0 loaded in 2 fixed apriltags #0: parent: imu1 child: imu0 T_child_wrt_parent: -0.048 0.037 0.002 RPY_parent_to_child: 0.0 0.0 0.0 R_child_to_parent: 1.000 -0.000 0.000 0.000 1.000 -0.000 0.000 0.000 1.000 #1: parent: imu0 child: tracking T_child_wrt_parent: 0.065 -0.014 0.013 RPY_parent_to_child: 0.0 45.0 90.0 R_child_to_parent: 0.000 -0.707 0.707 1.000 0.000 -0.000 -0.000 0.707 0.707 #2: parent: imu1 child: tracking T_child_wrt_parent: 0.017 0.015 0.013 RPY_parent_to_child: 0.0 45.0 90.0 R_child_to_parent: 0.000 -0.707 0.707 1.000 0.000 -0.000 -0.000 0.707 0.707 #3: parent: body child: imu0 T_child_wrt_parent: 0.020 0.014 -0.008 RPY_parent_to_child: 0.0 0.0 0.0 R_child_to_parent: 1.000 -0.000 0.000 0.000 1.000 -0.000 0.000 0.000 1.000 #4: parent: body child: imu1 T_child_wrt_parent: 0.068 -0.015 -0.008 RPY_parent_to_child: 0.0 0.0 0.0 R_child_to_parent: 1.000 -0.000 0.000 0.000 1.000 -0.000 0.000 0.000 1.000 #5: parent: body child: stereo_l T_child_wrt_parent: 0.100 -0.040 0.000 RPY_parent_to_child: 0.0 90.0 90.0 R_child_to_parent: 0.000 -0.000 1.000 1.000 0.000 -0.000 -0.000 1.000 0.000 #6: parent: body child: tof T_child_wrt_parent: 0.100 0.000 0.000 RPY_parent_to_child: 0.0 90.0 90.0 R_child_to_parent: 0.000 -0.000 1.000 1.000 0.000 -0.000 -0.000 1.000 0.000 #7: parent: body child: ground T_child_wrt_parent: 0.000 0.000 0.100 RPY_parent_to_child: 0.0 0.0 0.0 R_child_to_parent: 1.000 -0.000 0.000 0.000 1.000 -0.000 0.000 0.000 1.000 starting geometry module starting uart mavlink Successfully opened mavparser starting udp mavlink Adding manual QGC IP address to udp connection list: 192.168.219.182 Added new UDP connection to 192.168.219.182 starting px4 shell starting fixed pose input starting vio manager starting apriltag manager starting voa manager starting offboard figure eight Init complete, entering main loop Detected PX4 Mavlink SYSID 1 Connected to voxl-qvio-server done updating transforms to use imu: imu0 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 -0.00 PX4 Connected over UART with sysid 1 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: 0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: 0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 -0.00 0.00 T_body_wrt_local: -0.00 0.00 -0.10 RPY: -0.02 0.00 -0.00 velocity: -0.00 0.00 -0.00