Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
W
wolf_demo_uav_identification
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
mobile_robotics
wolf_projects
wolf_ros
demos
wolf_demo_uav_identification
Commits
794f380f
Commit
794f380f
authored
1 year ago
by
Arnau Marzabal Gatell
Browse files
Options
Downloads
Patches
Plain Diff
com estimation
parent
6d3904f4
No related branches found
No related tags found
1 merge request
!2
Draft: Resolve "real data"
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
launch/com_estimation.launch
+53
-0
53 additions, 0 deletions
launch/com_estimation.launch
yaml/com_estimation.yaml
+179
-0
179 additions, 0 deletions
yaml/com_estimation.yaml
with
232 additions
and
0 deletions
launch/com_estimation.launch
0 → 100644
+
53
−
0
View file @
794f380f
<!-- -->
<launch>
<!--USER ARGS-->
<arg name="rviz" default="true" />
<arg name="speed" default="1" />
<arg name="sec" default="0" />
<arg name="sim_time" default="true" />
<arg name="profiling" default="false" />
<arg name="gdb" default="false" />
<arg name="launch_pref" default="" unless="$(eval profiling or gdb)"/>
<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_real_hover_V0_with_pose_nc"/>
<!--VISUALIZATION-->
<group if="$(arg rviz)">
<node name="rviz"
pkg="rviz"
type="rviz"
args="-d $(find wolf_demo_uav_identification)/rviz/online.rviz" />
</group>
<!--WOLF-->
<node type="wolf_ros_node"
name="wolf_ros_node"
pkg="wolf_ros_node"
output="screen"
required="true"
launch-prefix="$(arg launch_pref)">
<param name="~yaml_file_path" value="$(find wolf_demo_uav_identification)/yaml/com_estimation.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"/>
</node>
<!-- ROSBAG -->
<node pkg="rosbag"
type="play"
name="player"
required="true"
args="-r $(arg speed)
-s $(arg sec)
--clock
$(find wolf_demo_uav_identification)/bag/$(arg bag).bag"/>
</launch>
This diff is collapsed.
Click to expand it.
yaml/com_estimation.yaml
0 → 100644
+
179
−
0
View file @
794f380f
config
:
debug
:
profiling
:
false
profiling_file
:
"
~/wolf_demo_uav_identification.txt"
print_problem
:
true
print_depth
:
1
# 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
:
3
# only if print_problem
problem
:
node_rate
:
100
frame_structure
:
"
POVL"
dimension
:
3
prior
:
mode
:
"
initial_guess"
##
$state
:
P
:
[
0
,
0
,
10
]
# amg # initial position
O
:
[
0
,
0
,
0
,
1
]
# amg # initial orientation
V
:
[
0
,
0
,
0
]
L
:
[
0
,
0
,
0
]
$sigma
:
P
:
[
1
,
1
,
1
]
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
:
2
# std dev of acc noise in m/s2 # 0.6/4
gyro_noise_std
:
0.1
# 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
:
false
#FT
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
:
2
# mass [kg]
com_fix
:
false
inertia_fix
:
true
mass_fix
:
true
# 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"
plugin
:
"
core"
extrinsic
:
pose
:
[
0.0
,
0.0
,
0.0
,
0
,
0
,
0
,
1
]
#amg
std_p
:
0.0001
std_o
:
0.0001
processors
:
-
name
:
"
proc
FTID"
type
:
"
ProcessorForceTorqueInertialDynamics"
sensor_name
:
"
sensor
FTI"
plugin
:
"
bodydynamics"
# time_tolerance: 0.00125 # 1/471/2
time_tolerance
:
0.01
# amg 1/50/2
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.1
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.01
# 1 / 100Hz / 2 = 0.005
apply_loss_function
:
false
keyframe_vote
:
voting_active
:
false
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"
topic_batteryStatus
:
"
/fmu/battery_status/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
:
100
# arm rotor velocity
w_max
:
2216
quadratic_aprox
:
false
# true in simulation
follow
:
"
./borinot_platform.yaml"
-
package
:
"
wolf_ros_node"
type
:
"
SubscriberPose"
topic
:
"
/pose_from_tf"
# msg generated from tf to mimic the optitrack msg
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.05
frame_length
:
0.5
frame_vel_scale
:
1.0
factors_absolute_height
:
00.1
-
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
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment