CREO2URDF – Wrong transform when exporting a sensors that uses a csys not present in exportedFrames list

[Nicogene]

Given a configuration file as follow:

forceTorqueSensors: 
   - jointName: l_foot_front_ft_sensor
      directionChildToParent: No
      exportFrameInURDF: Yes
      frame: sensor
      linkName: l_ankle_2
      frameName: SCSYS_L_ANKLE_2_FT_FRONT
      sensorName: l_foot_front_ft
      sensorBlobs:
      - |
          <plugin name="left_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
            <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_front_ft.ini</yarpConfigurationFile>
          </plugin>

sensors:
  - frameName: SCSYS_L_ANKLE_2_FT_FRONT
    linkName: l_ankle_2
    exportFrameInURDF: No # This is not since the frame is already exported in the forceTorqueSensors
    sensorName: l_foot_front_ft_imu
    sensorType: "accelerometer"
    updateRate: "100"
    sensorBlobs:
    - |
        <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
            <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
        </plugin>

The ft sensor is correctly exported, on the other hand, the sensor (imu) is exported coincident w/ linkName frame

  <gazebo reference="l_ankle_2">
    <sensor name="l_foot_front_ft_imu" type="imu">
      <always_on>1</always_on>
      <update_rate>100.000000</update_rate>
      <pose>0 0 0 0 -0 0 </pose>
      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_front_ft_sensor_inertial.ini</yarpConfigurationFile>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="l_foot_front_ft_imu" type="accelerometer">
    <parent link="l_ankle_2"/>
    <origin rpy="0 -0 0 " xyz="0 0 0"/>
  </sensor>
  <gazebo reference="l_ankle_2">
    <sensor name="l_foot_rear_ft_imu" type="imu">
      <always_on>1</always_on>
      <update_rate>100.000000</update_rate>
      <pose>0 0 0 0 -0 0 </pose>
      <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
      </plugin>
    </sensor>
  </gazebo>

But instead there is a transform between the frameName and linkName cys:

cc @GiulioRomualdi @mfussi66

[Nicogene]

Actually seems that it is not due to the fact that the frame is already used, because I tried to export an imu that uses the FT frame and I am getting the same (wrong) result indipendently both when I export also the ft and when I do not export it.

In the message window I am getting this error that is quite strange, since this frame is used for exporting the ft sensor, I will investigate further

���l_foot_rear_imu: SCSYS_L_ANKLE_2_FT_REAR was not found

cc @mfussi66

[Nicogene]

This problem seems to be related to:

• CREO2URDF – Make possible to export a sensor with a frameName not present in exportedFrames #60

With the solution proposed this issue should not happen if ExportAllUserAdded is set to yes but in our case we do not want to export all the frames, but still we want to be able to compute the correct transform for defining the sensor.

In particular, for the ft sensor, we do not use the exportedFrames:

creo2urdf/src/creo2urdf/src/Sensorizer.cpp

Lines 105 to 125 in 3d72a45

	void Sensorizer::assignTransformToFTSensor(const std::map<std::string, LinkInfo>& link_info_map, const std::map<std::string, JointInfo>& joint_info_map, const std::array<double, 3> scale)
	{
	// Iterate over all sensors
	for (auto& f : ft_sensors)
	{
	JointInfo j_info = joint_info_map.at(f.second.frameName);
	LinkInfo parent_l_info = link_info_map.at(j_info.parent_link_name);
	LinkInfo child_l_info = link_info_map.at(j_info.child_link_name);
	auto parent_csys_H_sensor = (getTransformFromPart(parent_l_info.modelhdl, f.second.frameName, scale)).second;
	auto parent_csys_H_parent_link = (getTransformFromPart(parent_l_info.modelhdl, parent_l_info.link_frame_name, scale)).second;
	// This transform is used for exporting the ft frame
	f.second.parent_link_H_sensor = parent_csys_H_parent_link.inverse() * parent_csys_H_sensor;
	auto child_csys_H_sensor = (getTransformFromPart(child_l_info.modelhdl, f.second.frameName, scale)).second;
	auto child_csys_H_child_link = (getTransformFromPart(child_l_info.modelhdl, child_l_info.link_frame_name, scale)).second;
	// This transform is used for defining the pose of the ft sensor
	f.second.child_link_H_sensor = child_csys_H_child_link.inverse() * child_csys_H_sensor;
	}
	}

On the other hand for the sensors we use the exportedFrames:

creo2urdf/src/creo2urdf/src/Sensorizer.cpp

Lines 221 to 235 in 3d72a45

	void Sensorizer::assignTransformToSensors(const std::map<std::string, ExportedFrameInfo>& exported_frame_info_map)
	{
	for (auto& s : sensors)
	{
	if (exported_frame_info_map.find(s.frameName) != exported_frame_info_map.end())
	{
	s.transform = exported_frame_info_map.at(s.frameName).linkFrame_H_additionalFrame;
	}
	else
	{
	printToMessageWindow(s.sensorName + ": " + s.frameName + " was not found", c2uLogLevel::WARN);
	}
	}
	}

It should not to be too difficult to fix it, but I need to align w/ @mfussi66 for understanding why there is this difference

[Nicogene]

This PR:

• Fix the way we retrieve the transform for the sensors #79

Should fix the issue, here is the resulting sensor:

  <gazebo reference="l_ankle_2">
    <sensor name="l_foot_rear_imu" type="imu">
      <always_on>1</always_on>
      <update_rate>100.000000</update_rate>
      <pose>0.016 -2.77556e-17 -0.0479 -4.25919e-17 -7.51333e-17 -2.0944 </pose>
      <plugin name="ergocub_yarp_gazebo_plugin_l_foot_rear_IMU" filename="libgazebo_yarp_imu.so">
        <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_inertial.ini</yarpConfigurationFile>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="l_foot_rear_imu" type="accelerometer">
    <parent link="l_ankle_2"/>
    <origin rpy="-4.25919e-17 -7.51333e-17 -2.0944 " xyz="0.016 -2.77556e-17 -0.0479"/>
  </sensor>

This is similar to the one added as xmlBlob by @GiulioRomualdi

    - |
            <gazebo reference="l_ankle_2">
              <sensor name="l_foot_rear_ft_imu" type="imu">
                <always_on>1</always_on>
                <update_rate>100</update_rate>
                <pose>0.016000000000000004 0 -0.07019999999999993 0.0 -0.0 -2.0943952105869315</pose>
                <plugin name="ergocub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
                  <yarpConfigurationFile>model://ergoCub/conf/gazebo_ergocub_l_foot_rear_ft_sensor_inertial.ini</yarpConfigurationFile>
                </plugin>
              </sensor>
            </gazebo>
    - |
            <sensor name="l_foot_rear_ft_imu" type="accelerometer">
              <parent link="l_ankle_2" />
              <origin rpy="0.0 -0.0 -2.0943952105869315" xyz="0.016000000000000004 0 -0.07019999999999993" />
            </sensor>

the only difference is on the Z 🤔

[GiulioRomualdi]

It should be placed in the FT. Nothing prevents an error in the proposed xml blob. Can you generate the robot model so I can do a similar test done in icub-tech-iit/ergocub-software#217 (comment) ?

[Nicogene]

I think that the correct one seems to be the one I obtained after the fix, the ft sensor is not associated to a link but a joint, and the reference frame of a joint is the child link frame (in our case l_foot_rear), looking these snippets from ergocub-software

  <joint name="l_foot_rear_ft_sensor" type="fixed">
    <origin xyz="0.015999999999999986 -4.163336342344337e-17 -0.04789999999999994" rpy="-1.0970096963229695e-30 1.942890293093975e-16 2.0306083615826833e-17"/>
    <parent link="l_ankle_2"/>
    <child link="l_foot_rear"/>
  </joint>

  <gazebo reference="l_foot_rear_ft_sensor">
    <sensor name="l_foot_rear_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0 0 0 1.25667e-16 2.20112e-17 -2.0944 </pose>
      <plugin name="left_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
        <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_foot_rear_ft.ini</yarpConfigurationFile>
      </plugin>
    </sensor>
  </gazebo>

If we refer to l_ankle_2 we should have the Z translation -0.0479. (Maybe linkName should be l_foot_rear)?

I don't know if it is sufficient but I have an urdf with only that imu added:
model.zip

[GiulioRomualdi]

Hi @Nicogene you are right!

I visualized your model and what I proposed in ergocub-software

Your model

#77 (comment)

Here I asked to plot the following frames

testFrames = {'l_foot_rear_imu', 'l_foot_rear_ft', 'l_sole'}; %% The frames to display

As you can notice l_foot_rear_imu and l_foot_rear_ft are coincident!

model I proposed in ergocub-software

I asked to plot the same frame.

Here you can notice that the frame are not coincident!

[Nicogene]

Hi @GiulioRomualdi, if you or Guglielmo can double check these changes:

• ergoCubSN001/GazeboV1_1*: add ft imu via sensor ergocub-software#222

We can merge both the PR in ergocub-software and creo2urdf

[GiulioRomualdi]

I will test it today :)