Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add Compound Sensors #32

Draft
wants to merge 2 commits into
base: noetic-devel
Choose a base branch
from
Draft

Add Compound Sensors #32

wants to merge 2 commits into from

Conversation

DavidPL1
Copy link
Contributor

@DavidPL1 DavidPL1 commented Dec 21, 2023

This PR, based on the discussion in #26, overhauls the MuJoCo sensor to ROS serialization and introduces compound sensors.

The currently available compound sensors are Twist, IMU, Wrench, and Pose. New ones can easily be added.

Points to Discuss

  • Is a factory for compound sensors overkill or worth the reduced maintenance cost?
  • Sensors used in compound sensors are not separately published
  • Should the frame ids of sensors used in a compound sensor always be the same?
  • Defining compound sensors requires a list of
    • sensors
    • (frame_id? For now we assume the frame id of the included sensors which should be coherent)
    • frequency
    • type
    • name
    • publishing rate (at some point when the feature is introduced)
      Since the custom tag does not really allow adding all this information conveniently, I moved the whole configuration to the plugin configuration.

TODOs

  • IMU covariance configuration is NYI
  • Tests for compound sensors

@DavidPL1
Copy link
Contributor Author

@balandbal, this is what I came up with based on our discussion. Could you take a look and tell me what you think?
I'd also appreciate your take on the points of discussion mentioned above.

Copy link

codecov bot commented Dec 21, 2023

Codecov Report

Attention: Patch coverage is 40.79602% with 238 lines in your changes missing coverage. Please review.

Project coverage is 61.87%. Comparing base (455e135) to head (86f691c).
Report is 39 commits behind head on noetic-devel.

Files with missing lines Patch % Lines
mujoco_ros_sensors/src/compound_sensors.cpp 0.00% 169 Missing ⚠️
...o_ros_sensors/src/mujoco_sensor_handler_plugin.cpp 48.70% 59 Missing ⚠️
mujoco_ros_sensors/src/serialization.cpp 91.57% 7 Missing ⚠️
...o_ros_sensors/include/mujoco_ros_sensors/sensors.h 91.43% 3 Missing ⚠️
Additional details and impacted files
@@               Coverage Diff                @@
##           noetic-devel      #32      +/-   ##
================================================
- Coverage         71.43%   61.87%   -9.56%     
================================================
  Files                10       12       +2     
  Lines              1414     1602     +188     
================================================
- Hits               1010      991      -19     
- Misses              404      611     +207     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

@balandbal
Copy link

  • Sensors used in compound sensors are not separately published

I'd say this is reasonable behaviour. I don't remember a case when I needed the components of, e.g., a wrench signal separately available in ROS at the same time I used the compound signal.

  • Should the frame ids of sensors used in a compound sensor always be the same?

I'd say yes. These compound signals should correspond to those from some real-world hardware; I'm not aware of a tool that would be "distributed" in the sense that it would require different frames to model.

  • Defining compound sensors requires a list of
    • sensors
    • (frame_id? For now we assume the frame id of the included sensors which should be coherent)
    • frequency
    • type
    • name
    • publishing rate (at some point when the feature is introduced)
      Since the custom tag does not really allow adding all this information conveniently, I moved the whole configuration to the plugin configuration.

I'd be happier if the grouping of the sensors could move with the MJCF and not separately, though this is because I usually use my models with, e.g., the official Python bindings as well and not only mujoco_ros.

The mujoco_ros_sensors/config/example_sensors.yaml file would look something like the XML below with the custom tag.
We could reserve the names for the compound sensor types and use the prm attribute to store the frequencies.

<custom>
  <tuple name="IMUSensor">
    <element objname="test_imu"   objtype="tuple" prm="500" />
    <element objname="test_imu_2" objtype="tuple" />
  </tuple>

  <tuple name="WrenchSensor">
    <element objname="test_wrench" objtype="tuple" prm="500"/>
  </tuple>

  <tuple name="TwistSensor">
    <element objname="test_twist" objtype="tuple" prm="500"/>
  </tuple>

  <tuple name="PoseSensor">
    <element objname="test_pose" objtype="tuple" prm="250" />
  </tuple>

  <tuple name="test_imu">
    <element objname="linacc_EE" objtype="sensor"/>
    <element objname="gyro_EE"   objtype="sensor"/>
    <element objname="quat_EE"   objtype="sensor"/>
  </tuple>

  <tuple name="test_imu_2">
    <element objname="linacc_2" objtype="sensor"/>
    <element objname="gyro_2"   objtype="sensor"/>
    <element objname="quat_2"   objtype="sensor"/>
  </tuple>

  <tuple name="test_wrench">
    <element objname="force_EE"  objtype="sensor"/>
    <element objname="torque_EE" objtype="sensor"/>
  </tuple>

  <tuple name="test_twist">
    <element objname="linvel_EE" objtype="sensor"/>
    <element objname="angvel_EE" objtype="sensor"/>
  </tuple>

  <tuple name="test_pose">
    <element objname="pos_joint1"  objtype="sensor"/>
    <element objname="quat_joint1" objtype="sensor"/>
  </tuple>

</custom>

@DavidPL1
Copy link
Contributor Author

DavidPL1 commented Feb 6, 2024

  • Sensors used in compound sensors are not separately published

I'd say this is reasonable behaviour. I don't remember a case when I needed the components of, e.g., a wrench signal separately available in ROS at the same time I used the compound signal.

Alright then let's keep this as is.

  • Should the frame ids of sensors used in a compound sensor always be the same?

I'd say yes. These compound signals should correspond to those from some real-world hardware; I'm not aware of a tool that would be "distributed" in the sense that it would require different frames to model.

What I was worrying about was correct reference frames of the sensor readings. Torques, for instance, only make sense if their reference frame, i.e., their frame_id in ROS, is set correctly. Since a compound sensor could technically consist of arbitrarily far apart sensors, a misconfigured torque sensor could send torques in reference to its local frame instead of the compound sensor frame. I guess we could pin the responsibility of correct configuration to the user, but I'd be happier with sanity checks.
With this in mind, I think it makes sense to use a common frame_id and add a check that each sensor is attached to the same site (or reftype and refobject in case of frame-related sensors). Agreed?

I'd be happier if the grouping of the sensors could move with the MJCF and not separately, though this is because I usually use my models with, e.g., the official Python bindings as well and not only mujoco_ros.

I was unhappy with the separated definition, too.
I was not aware that one could add more than two children to a tuple element and that was the main reason I did not consider this way of configuration. My mistake, I'll try and adapt the code to use this.

@balandbal
Copy link

What I was worrying about was correct reference frames of the sensor readings. Torques, for instance, only make sense if their reference frame, i.e., their frame_id in ROS, is set correctly. Since a compound sensor could technically consist of arbitrarily far apart sensors, a misconfigured torque sensor could send torques in reference to its local frame instead of the compound sensor frame. I guess we could pin the responsibility of correct configuration to the user, but I'd be happier with sanity checks. With this in mind, I think it makes sense to use a common frame_id and add a check that each sensor is attached to the same site (or reftype and refobject in case of frame-related sensors). Agreed?

Yes, I would be happy with that.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants