From 25e1248f6c3ed591896afd58b1e40e3a9334cd31 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Fri, 23 Sep 2022 17:04:50 +0200
Subject: [PATCH] tuning
---
launch/demo_uav_identification.launch | 2 +-
rviz/online.rviz | 52 +++---
...force_torque_inertial_dynamics_rosbag.yaml | 164 ++++++++++++++++++
...e_torque_inertial_dynamics_simulation.yaml | 6 +-
yaml/solver.yaml | 2 +-
5 files changed, 194 insertions(+), 32 deletions(-)
create mode 100644 yaml/problem_force_torque_inertial_dynamics_rosbag.yaml
diff --git a/launch/demo_uav_identification.launch b/launch/demo_uav_identification.launch
index a4d3a66..203e6df 100644
--- a/launch/demo_uav_identification.launch
+++ b/launch/demo_uav_identification.launch
@@ -30,7 +30,7 @@
output="screen"
required="true"
launch-prefix="$(arg launch_pref)">
- <param name="~yaml_file_path" value="$(find wolf_demo_uav_identification)/yaml/problem_force_torque_inertial_dynamics_simulation.yaml" />
+ <param name="~yaml_file_path" value="$(find wolf_demo_uav_identification)/yaml/problem_force_torque_inertial_dynamics_rosbag.yaml" />
<remap from="/wrench_accel" to="/fmu/sensor_accel/out"/>
<remap from="/wrench_gyro" to="/fmu/sensor_gyro/out"/>
<remap from="/wrench_actuators" to="/fmu/actuator_outputs/out"/>
diff --git a/rviz/online.rviz b/rviz/online.rviz
index 9979b32..78600ea 100644
--- a/rviz/online.rviz
+++ b/rviz/online.rviz
@@ -5,9 +5,10 @@ Panels:
Property Tree Widget:
Expanded:
- /TF1/Frames1
+ - /Factors1
- /Factors1/Namespaces1
- /Trajectory1/Namespaces1
- Splitter Ratio: 0.5
+ Splitter Ratio: 0.6941176652908325
Tree Height: 741
- Class: rviz/Selection
Name: Selection
@@ -24,7 +25,6 @@ Panels:
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
- Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
@@ -53,50 +53,47 @@ Visualization Manager:
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- - Class: rviz/Axes
+ - Alpha: 1
+ Class: rviz/Axes
Enabled: true
Length: 0.10000000149011612
Name: Axes
Radius: 0.009999999776482582
Reference Frame: base_footprint
+ Show Trail: false
Value: true
- Class: rviz/TF
- Enabled: true
+ Enabled: false
Frame Timeout: 15
Frames:
All Enabled: false
- base_footprint:
- Value: true
- map:
- Value: true
- odom:
- Value: true
+ Marker Alpha: 1
Marker Scale: 0.5
Name: TF
Show Arrows: false
Show Axes: true
Show Names: false
Tree:
- map:
- odom:
- base_footprint:
- {}
+ {}
Update Interval: 0
- Value: true
+ Value: false
- Class: rviz/MarkerArray
- Enabled: true
+ Enabled: false
Marker Topic: /wolf_apriltag_vio/marker_array
Name: MarkerArray
Namespaces:
{}
Queue Size: 1
- Value: true
+ Value: false
- Class: rviz/MarkerArray
Enabled: true
Marker Topic: /wolf_ros_node/graph_factors
Name: Factors
Namespaces:
- {}
+ factors_processor Pose: false
+ factors_text_processor Pose: false
+ factors_text_unnamed_processor: false
+ factors_unnamed_processor: false
Queue Size: 1
Value: true
- Class: rviz/MarkerArray
@@ -104,17 +101,18 @@ Visualization Manager:
Marker Topic: /wolf_ros_node/graph_trajectory
Name: Trajectory
Namespaces:
- {}
+ frames: true
+ frames_text: false
Queue Size: 1
Value: true
- Class: rviz/Marker
- Enabled: true
+ Enabled: false
Marker Topic: visualization_marker
Name: Ground truth
Namespaces:
{}
Queue Size: 100
- Value: true
+ Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
@@ -143,16 +141,17 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 2.5275864601135254
+ Distance: 4.454471111297607
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
+ Field of View: 0.7853981852531433
Focal Point:
- X: 0.22070589661598206
- Y: -0.007580682635307312
- Z: -0.5612056255340576
+ X: 0.013524828478693962
+ Y: 0.3803035020828247
+ Z: -0.060807570815086365
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
@@ -160,7 +159,6 @@ Visualization Manager:
Near Clip Distance: 0.009999999776482582
Pitch: 0.7147972583770752
Target Frame: <Fixed Frame>
- Value: Orbit (rviz)
Yaw: 5.045364856719971
Saved: ~
Window Geometry:
@@ -169,7 +167,7 @@ Window Geometry:
Height: 986
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 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
+ QMainWindow State: 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
Selection:
collapsed: false
Time:
diff --git a/yaml/problem_force_torque_inertial_dynamics_rosbag.yaml b/yaml/problem_force_torque_inertial_dynamics_rosbag.yaml
new file mode 100644
index 0000000..dfe26c9
--- /dev/null
+++ b/yaml/problem_force_torque_inertial_dynamics_rosbag.yaml
@@ -0,0 +1,164 @@
+config:
+
+ debug:
+ profiling: true
+ profiling_file: "~/wolf_demo_uav_identification.txt"
+ print_problem: true
+ print_depth: 3 # only if print_problem
+ print_constr_by: false # only if print_problem
+ print_metric: true # only if print_problem
+ print_state_blocks: true # only if print_problem
+ print_period: 1 # only if print_problem
+
+
+
+
+ problem:
+ node_rate: 100
+ frame_structure: "POVL"
+ dimension: 3
+ prior:
+ mode: "initial_guess"
+ $state:
+ P: [0,0,0]
+ O: [0,0,0,1]
+ V: [0,0,0]
+ L: [0,0,0]
+ $sigma:
+ P: [0.31, 0.31, 0.31]
+ O: [0.31, 0.31, 0.31]
+ V: [100,100,100]
+ L: [100,100,100]
+ time_tolerance: 0.005
+ tree_manager:
+ type: "TreeManagerSlidingWindow"
+ plugin: "core"
+ n_frames: 1000
+ n_fix_first_frames: 0
+ viral_remove_empty_parent: true
+ map:
+ type: "MapBase"
+ plugin: "core"
+
+ solver:
+ follow: "solver.yaml"
+
+ sensors:
+ -
+ name: "sensor FTI"
+ type: "SensorForceTorqueInertial"
+ plugin: "bodydynamics"
+ extrinsic:
+ pose: [0,0,0, 0,0,0,1]
+
+ # IMU
+ acc_noise_std: 0.001 # std dev of acc noise in m/s2
+ gyro_noise_std: 0.001 # std dev of gyro noise in rad/s
+ accb_initial_std: 0.01 # m/s2 - initial bias
+ gyrob_initial_std: 0.01 # rad/sec - initial bias
+ acc_drift_std: 0.0001 # std dev of acc drift m/s2/sqrt(s)
+ gyro_drift_std: 0.0001 # std dev of gyro drift rad/s/sqrt(s)
+ imu_bias_fix: true
+
+ #FT
+ force_noise_std: 0.001 # std dev of force noise in N
+ torque_noise_std: 0.0001 # std dev of torque noise in N/m
+
+ # Dynamics
+ com: [0.00,0.00,0.0] # center of mass [m]
+ inertia: [0.017,0.017,0.029] # moments of inertia i_xx, i_yy, i_zz [kg m2]
+ mass: 1.95 # mass [kg]
+ com_fix: true
+ inertia_fix: true
+ mass_fix: true
+
+ -
+ name: "sensor Pose"
+ type: "SensorPose"
+ plugin: "core"
+ extrinsic:
+ pose: [0.0114,-0.0513,-0.0035, 0,0,0,1]
+ std_p: 0.01
+ std_o: 0.01
+
+
+ processors:
+ -
+ name: "proc FTID"
+ type: "ProcessorForceTorqueInertialDynamics"
+ sensor_name: "sensor FTI"
+ plugin: "bodydynamics"
+ time_tolerance: 0.0015 # 1 / 330 Hz / 2 = 0.0015
+ apply_loss_function: false
+ unmeasured_perturbation_std: 0.00001
+ state_getter: true
+ state_priority: 1
+ # n_propellers: 6
+ keyframe_vote:
+ voting_active: true
+ max_time_span: 0.99 # De moment el que funciona millor és 1 s o 2s
+ max_buff_length: 999 # motion deltas
+ dist_traveled: 999 # meters
+ angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
+
+ -
+ name: "processor Pose"
+ type: "ProcessorPose"
+ sensor_name: "sensor Pose"
+ plugin: "core"
+ time_tolerance: 0.005 # 1 / 100Hz / 2 = 0.005
+ apply_loss_function: false
+ keyframe_vote:
+ voting_active: true
+
+
+
+ ROS subscriber:
+ -
+ package: "wolf_ros_bodydynamics"
+ type: "SubscriberForceTorqueInertialDrone"
+ topic: " "
+ topic_acc: "/fmu/sensor_accel/out"
+ topic_gyro: "/fmu/sensor_gyro/out"
+ topic_actuators: "/fmu/actuator_outputs/out"
+ sensor_name: "sensor FTI"
+ imu_x_axis: 1
+ imu_y_axis: 2
+ imu_z_axis: 3
+ cov_imu_source: "sensor"
+ in_degrees: false
+ synchronization_mode: "imu"
+ duty_min: 1000
+ duty_max: 2000
+ w_min: 0
+ w_max: 2150
+ follow: "./borinot_platform.yaml"
+ -
+ package: "wolf_ros_node"
+ type: "SubscriberPose"
+ topic: "/vrpn_client/borinot_fur_ot/pose"
+ sensor_name: "sensor Pose"
+
+
+ ROS publisher:
+ -
+ type: "PublisherGraph"
+ topic: "graph"
+ package: "wolf_ros_node"
+ period: 0.1
+ viz_scale: 0.3
+ text_scale: 0.3
+ landmark_text_z_offset: 0.3
+ landmark_length: 0.5
+ frame_width: 0.01
+ frame_length: 0.1
+ frame_vel_scale: 1.0
+ -
+ type: "PublisherTf"
+ topic: " "
+ package: "wolf_ros_node"
+ period: 0.1
+ map_frame_id: "map"
+ odom_frame_id: "odom"
+ base_frame_id: "base_footprint"
+ publish_odom_tf: true
diff --git a/yaml/problem_force_torque_inertial_dynamics_simulation.yaml b/yaml/problem_force_torque_inertial_dynamics_simulation.yaml
index 5130081..1add56a 100644
--- a/yaml/problem_force_torque_inertial_dynamics_simulation.yaml
+++ b/yaml/problem_force_torque_inertial_dynamics_simulation.yaml
@@ -4,7 +4,7 @@ config:
profiling: true
profiling_file: "~/wolf_demo_uav_identification.txt"
print_problem: true
- print_depth: 4 # only if print_problem
+ print_depth: 2 # only if print_problem
print_constr_by: false # only if print_problem
print_metric: true # only if print_problem
print_state_blocks: true # only if print_problem
@@ -64,9 +64,9 @@ config:
torque_noise_std: 0.0001 # std dev of torque noise in N/m
# Dynamics
- com: [0.00,0.00,0.0] # center of mass [m]
+ com: [0.00,0.00,0.0] # center of mass [m]
inertia: [0.017,0.017,0.029] # moments of inertia i_xx, i_yy, i_zz [kg m2]
- mass: 1.95 # mass [kg]
+ mass: 1.95 # mass [kg]
-
name: "sensor Pose"
diff --git a/yaml/solver.yaml b/yaml/solver.yaml
index f7d5455..f5c2f30 100644
--- a/yaml/solver.yaml
+++ b/yaml/solver.yaml
@@ -5,7 +5,7 @@ max_num_iterations: 20
function_tolerance: 1e-7
gradient_tolerance: 1e-9
use_nonmonotonic_steps: false
-period: 0.3 # note: this must be smaller (i.e. faster) than the KF period in processor_tracker_landmark_apriltag.yaml
+period: 0.1 # note: this must be smaller (i.e. faster) than the KF period in processor_tracker_landmark_apriltag.yaml
n_threads: 2
compute_cov: false
verbose: 0
\ No newline at end of file
--
GitLab