From 50b3cace3b304dc76b4635854b572fe7b58f8570 Mon Sep 17 00:00:00 2001
From: Arnau Marzabal Gatell <amarzabal@iri.upc.edu>
Date: Fri, 28 Jul 2023 16:00:38 +0200
Subject: [PATCH] mass & com est from sim (correct thrust map)
---
.../mass_and_com_estimation_simulation.launch | 2 +-
yaml/mass_and_com_estimation_simulation.yaml | 38 ++++++++++++-------
2 files changed, 25 insertions(+), 15 deletions(-)
diff --git a/launch/mass_and_com_estimation_simulation.launch b/launch/mass_and_com_estimation_simulation.launch
index 1eefb5f..9cb0d30 100644
--- a/launch/mass_and_com_estimation_simulation.launch
+++ b/launch/mass_and_com_estimation_simulation.launch
@@ -12,7 +12,7 @@
<arg name="launch_pref" value="valgrind --tool=callgrind --callgrind-out-file='callgrind.wolf.%p'" if="$(arg profiling)" />
<arg name="launch_pref" value="gdb -ex run --args" if="$(arg gdb)" />
- <arg name="bag" default="rosbag_borinot_sim_hover_pure"/>
+ <arg name="bag" default="rosbag_borinot_sim_hover_V2"/>
<!--VISUALIZATION-->
diff --git a/yaml/mass_and_com_estimation_simulation.yaml b/yaml/mass_and_com_estimation_simulation.yaml
index c95afd8..c463cb8 100644
--- a/yaml/mass_and_com_estimation_simulation.yaml
+++ b/yaml/mass_and_com_estimation_simulation.yaml
@@ -52,8 +52,8 @@ config:
pose: [0,0,0, 0,0,0,1]
# IMU
- acc_noise_std: 0.015 # std dev of acc noise in m/s2 # 0.6/4
- gyro_noise_std: 0.05 # std dev of gyro noise in rad/s
+ acc_noise_std: 0.1 # std dev of acc noise in m/s2 # 0.6/4
+ gyro_noise_std: 0.01 # 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)
@@ -61,17 +61,27 @@ config:
imu_bias_fix: false
#FT
- force_noise_std: 0.075 # std dev of force noise in N
- torque_noise_std: 0.01 # std dev of torque noise in N/m
+ force_noise_std: 1 # std dev of force noise in N
+ torque_noise_std: 1 # std dev of torque noise in N/m
# Dynamics
com: [0.00,0.00,0.0] # center of mass [m]
inertia: [0.0134943,0.0141622,0.0237319] # moments of inertia i_xx, i_yy, i_zz [kg m2]
- mass: 1.5 # mass [kg]
+ mass: 2 # mass [kg]
com_fix: false
inertia_fix: true
mass_fix: false
+ # regularization
+ set_com_prior: true
+ prior_com_std: [10,10,10]
+ set_inertia_prior: true
+ prior_inertia_std: [1,1,1]
+ set_mass_prior: true
+ prior_mass_std: 10
+ set_bias_prior: true
+ prior_bias_std: [10,10,10,10,10,10]
+
-
name: "sensor Pose"
type: "SensorPose"
@@ -97,7 +107,7 @@ config:
# n_propellers: 6
keyframe_vote:
voting_active: true
- max_time_span: 0.2 # De moment el que funciona millor és 1 s o 2s
+ max_time_span: 0.2
max_buff_length: 999 # motion deltas
dist_traveled: 999 # meters
angle_turned: 999 # radians (1 rad approx 57 deg, approx 60 deg)
@@ -132,15 +142,14 @@ config:
synchronization_mode: "imu"
duty_min: 1000
duty_max: 2000
- w_min: 0
- w_max: 1116
- # w_max: 1116
- quadratic_aprox: true ######################################
- follow: "./borinot_platform.yaml"
+ w_min: 100 # arm rotor velocity
+ w_max: 2216
+ quadratic_aprox: true # true in simulation
+ follow: "./borinot_platform_simulation.yaml"
-
package: "wolf_ros_node"
type: "SubscriberPose"
- topic: "/pose_from_tf" # amg # msg generated from tf to mimic the optitrack msg
+ topic: "/pose_from_tf" # msg generated from tf to mimic the optitrack msg
sensor_name: "sensor Pose"
@@ -154,9 +163,10 @@ config:
text_scale: 0.3
landmark_text_z_offset: 0.3
landmark_length: 0.5
- frame_width: 0.01
- frame_length: 0.1
+ frame_width: 0.05
+ frame_length: 0.5
frame_vel_scale: 1.0
+ factors_absolute_height: 00.1
-
type: "PublisherTf"
topic: " "
--
GitLab