Draft: Implement ROS wrench publishing

[balandbal]

In the ROS ecosystem, force-torque data is often available from one source as a 6-element geometry_msgs/Wrench or its stamped variant geometry_msgs/WrenchStamped.

Having such a single composite data source eliminates the need for the client or the user side to implement some synchronization between the separate force and torque topics, bringing the simulation closer to how a real-world system would function.

Configuration in the MJCF

The feature will look for a custom tuple named "wrench" in the model.

<mujoco>
    <sensor>
        <force  name="force_sensor_1"  site="wrench_site_1"/>
        <torque name="torque_sensor_1" site="wrench_site_1"/>
        <force  name="force_sensor_2"  site="wrench_site_2"/>
        <torque name="torque_sensor_2" site="wrench_site_2"/>
    </sensor>
    <custom>
        <tuple name="wrench">
            <element objname="wrench_sensor_1" objtype="tuple"/>
            <element objname="wrench_sensor_2" objtype="tuple"/>
        </tuple>
        <tuple name="wrench_sensor_1">
            <element objname="force_sensor1"  objtype="sensor"/>
            <element objname="torque_sensor1" objtype="sensor"/>
        </tuple>
        <tuple name="wrench_sensor_2">
            <element objname="force_sensor2"  objtype="sensor"/>
            <element objname="torque_sensor2" objtype="sensor"/>
        </tuple>
    </custom>
</mujoco>

TODOs

• [ ] Resample sensor values with noise or republish?
• [ ] Disable individual publishing values for force and torque sensors already in a wrench sensor?
• [ ] Adapt or develop tests
• [ ] Apply code formatting

Combined Sensors to Consider

• [ ] Wrench
• [ ] Twist
• [ ] Pose

[DavidPL1]

Thanks for the suggestion! When I implemented the sensors this was bothering me, too.

I see no issues with your implementation, however, maybe it makes sense to go even one step further and provide some kind of CombinedMujocoSensor parent class. This could make combining sensors easier with derived classes just specifying which sensors are merged into which messages.

Currently, only IMUs come to mind, but I'm certain there are more combined sensors this implementation would benefit from. Can you think of more?

---

• [ ] Resample sensor values with noise or republish?
• [ ] Disable individual publishing values for force and torque sensors already in a wrench sensor?

I'm not sure I understand what you intend with the first todo. My suggestion for the second one would be to either register either single sensor instances or combined sensor instances. Single sensor instances would behave just like before, and combined sensors would publish a combined topic and individual topics for the included sensors. By checking the subscriber count we could then prevent unnecessary serialization while still providing flexible access to the data.

[balandbal]

• [ ] Resample sensor values with noise or republish?
• [ ] Disable individual publishing values for force and torque sensors already in a wrench sensor?

I'm not sure I understand what you intend with the first todo. [...]

That one was more of a note for me for now. The noise-burdened sensor values are now resampled when publishing for the combined sensor. This means that if a noise model is registered for the given single sensor, the values appearing on the single sensor topic will not be the same as on the combined one for any given timestep.

[...] My suggestion for the second one would be to either register either single sensor instances or combined sensor instances. Single sensor instances would behave just like before, and combined sensors would publish a combined topic and individual topics for the included sensors. By checking the subscriber count, we could prevent unnecessary serialization while providing flexible access to the data.

In any case, if I understood correctly, you suggest registering only the combined sensor (and not the single sensors of which it consists) and publishing the values for the single or combined topics as needed from there -- that would take care of the resampling discrepancy.

---

Currently, only IMUs come to mind, but I'm certain there are more combined sensors this implementation would benefit from. Can you think of more?

Perhaps Pose, Twist; these too have MuJoCo sensors for their components but not combined.

---

I see no issues with your implementation, however, maybe it makes sense to go even one step further and provide some kind of CombinedMujocoSensor parent class. This could make combining sensors easier with derived classes just specifying which sensors are merged into which messages.

Yes, that makes sense. I will see what I can do.

[DavidPL1]

In any case, if I understood correctly, you suggest registering only the combined sensor (and not the single sensors of which it consists) and publishing the values for the single or combined topics as needed from there -- that would take care of the resampling discrepancy.

yes, exactly. Probably it would be best to use an inheritance pattern like

GeneralSensorConfig
├── CombinedSensorConfig
│   ├── PoseSensorConfig
│   ├── ...
│   ├── TwistSensorConfig
│   └── WrenchSensorConfig
└── SingleSensorConfig

and

class GeneralSensorConfig {
public:
  // Iterate through registered sensors, and register on matching SensorNoiseModel.sensor_name
   virtual void registerNoise(std::vector<mujoco_ros_msgs::SensorNoiseModel> noise);
   // For each registered sensor(-pair) fetch data, convert to message and publish
   virtual void publishSensorsData(mjDataPtr data, bool gt_enabled);

// and some initialization related functions
...
}

class SingleSensorConfig : GeneralSensorConfig {
....
private:
  std::string frame_id;
  int sensor_type;
  int sensor_adr;
  ros::Publisher sensor_data;
  ros::Publisher gt_sensor_data;
// we could also add a configurable sensor publishing rate
}

...

maybe with templated conversion functions for mujoco sensor addresses in the data vectors for the different sensor types. And then we would just register sensors configurations into a vector on init and call the main functions defined by GeneralSensorConfig for processing in the lastStageCallback.

At least that's my rough idea without having thought too hard about it yet. We can discuss this in more detail and modify the draft after the IROS .

Perhaps Pose, Twist; these too have MuJoCo sensors for their components but not combined.

I started an additional todo list above so that we can track them better :)