Skip to content

Commit

Permalink
Revert "ergoCubV1_1: remove ft sensors of the arms"
Browse files Browse the repository at this point in the history
This reverts commit 6be5893.
  • Loading branch information
Nicogene committed Jan 8, 2024
1 parent 19e99a0 commit 8f562f4
Show file tree
Hide file tree
Showing 8 changed files with 249 additions and 81 deletions.
14 changes: 7 additions & 7 deletions tests/ergocub-model-test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -449,7 +449,7 @@ bool checkFTSensorsAreOddAndNotNull(iDynTree::ModelLoader & mdlLoader)
}

/**
* All the ergoCub have a even and not null number of F/T sensors.
* All the iCub have a even and not null number of F/T sensors.
*/
bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader)
{
Expand Down Expand Up @@ -524,9 +524,9 @@ bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp)
0.5, 0.866025, 0,
0, 0, -1);

// bool ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft");
// ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft") && ok;
bool ok = checkFrameIsCorrectlyOriented(comp, rootLink_L_sensorFrameExpectedLeg, "l_leg_ft");
bool ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft");
ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft") && ok;
ok = checkFrameIsCorrectlyOriented(comp, rootLink_L_sensorFrameExpectedLeg, "l_leg_ft") && ok;
ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedLeg, "r_leg_ft") && ok;
ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_rear_ft") && ok;
ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_rear_ft") && ok;
Expand Down Expand Up @@ -687,9 +687,9 @@ bool checkAllFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGi
iDynTree::SensorsList sensors = mdlLoader.sensors();


// bool ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_arm_ft");
// ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_arm_ft") && ok;
bool ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_leg_ft");
bool ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_arm_ft");
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_arm_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_leg_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_leg_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "l_foot_rear_ft") && ok;
ok = checkFTMeasurementFrameGivenBySensorTagsIsCoherentWithMeasurementFrameGivenByFrame(modelPath, comp, sensors, "r_foot_rear_ft") && ok;
Expand Down
48 changes: 24 additions & 24 deletions urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -702,30 +702,30 @@ exportedFrames:

# Sensors options
forceTorqueSensors:
# - jointName: l_arm_ft_sensor
# directionChildToParent: Yes
# exportFrameInURDF: Yes
# frame: sensor
# frameName: SCSYS_L_SHOULDER_2_FT
# linkName: l_shoulder_2
# sensorName: l_arm_ft
# sensorBlobs:
# - |
# <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
# <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
# </plugin>
# - jointName: r_arm_ft_sensor
# directionChildToParent: Yes
# exportFrameInURDF: Yes
# frame: sensor
# frameName: SCSYS_R_SHOULDER_2_FT
# linkName: r_shoulder_2
# sensorName: r_arm_ft
# sensorBlobs:
# - |
# <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
# <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
# </plugin>
- jointName: l_arm_ft_sensor
directionChildToParent: Yes
exportFrameInURDF: Yes
frame: sensor
frameName: SCSYS_L_SHOULDER_2_FT
linkName: l_shoulder_2
sensorName: l_arm_ft
sensorBlobs:
- |
<plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
</plugin>
- jointName: r_arm_ft_sensor
directionChildToParent: Yes
exportFrameInURDF: Yes
frame: sensor
frameName: SCSYS_R_SHOULDER_2_FT
linkName: r_shoulder_2
sensorName: r_arm_ft
sensorBlobs:
- |
<plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
</plugin>
# left leg
- jointName: l_leg_ft_sensor
directionChildToParent: Yes
Expand Down
48 changes: 24 additions & 24 deletions urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_gazebo.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -919,30 +919,30 @@ assignedInertias:

# Sensors options
forceTorqueSensors:
# - jointName: l_arm_ft_sensor
# directionChildToParent: Yes
# exportFrameInURDF: Yes
# frame: sensor
# frameName: SCSYS_L_SHOULDER_2_FT
# linkName: l_shoulder_2
# sensorName: l_arm_ft
# sensorBlobs:
# - |
# <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
# <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
# </plugin>
# - jointName: r_arm_ft_sensor
# directionChildToParent: Yes
# exportFrameInURDF: Yes
# frame: sensor
# frameName: SCSYS_R_SHOULDER_2_FT
# linkName: r_shoulder_2
# sensorName: r_arm_ft
# sensorBlobs:
# - |
# <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
# <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
# </plugin>
- jointName: l_arm_ft_sensor
directionChildToParent: Yes
exportFrameInURDF: Yes
frame: sensor
frameName: SCSYS_L_SHOULDER_2_FT
linkName: l_shoulder_2
sensorName: l_arm_ft
sensorBlobs:
- |
<plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
</plugin>
- jointName: r_arm_ft_sensor
directionChildToParent: Yes
exportFrameInURDF: Yes
frame: sensor
frameName: SCSYS_R_SHOULDER_2_FT
linkName: r_shoulder_2
sensorName: r_arm_ft
sensorBlobs:
- |
<plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
</plugin>
# left leg
- jointName: l_leg_ft_sensor
directionChildToParent: Yes
Expand Down
48 changes: 24 additions & 24 deletions urdf/creo2urdf/data/ergocub1_1/ERGOCUB_all_options_minContacts.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -1032,30 +1032,30 @@ assignedCollisionGeometry:

# Sensors options
forceTorqueSensors:
# - jointName: l_arm_ft_sensor
# directionChildToParent: Yes
# exportFrameInURDF: Yes
# frame: sensor
# frameName: SCSYS_L_SHOULDER_2_FT
# linkName: l_shoulder_2
# sensorName: l_arm_ft
# sensorBlobs:
# - |
# <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
# <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
# </plugin>
# - jointName: r_arm_ft_sensor
# directionChildToParent: Yes
# exportFrameInURDF: Yes
# frame: sensor
# frameName: SCSYS_R_SHOULDER_2_FT
# linkName: r_shoulder_2
# sensorName: r_arm_ft
# sensorBlobs:
# - |
# <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
# <yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
# </plugin>
- jointName: l_arm_ft_sensor
directionChildToParent: Yes
exportFrameInURDF: Yes
frame: sensor
frameName: SCSYS_L_SHOULDER_2_FT
linkName: l_shoulder_2
sensorName: l_arm_ft
sensorBlobs:
- |
<plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
</plugin>
- jointName: r_arm_ft_sensor
directionChildToParent: Yes
exportFrameInURDF: Yes
frame: sensor
frameName: SCSYS_R_SHOULDER_2_FT
linkName: r_shoulder_2
sensorName: r_arm_ft
sensorBlobs:
- |
<plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
</plugin>
# left leg
- jointName: l_leg_ft_sensor
directionChildToParent: Yes
Expand Down
4 changes: 2 additions & 2 deletions urdf/ergoCub/conf/ergocub.xml
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,8 @@
<xi:include href="wrappers/FT/right_leg-FT_remapper.xml" />
<xi:include href="wrappers/FT/left_leg-FT_wrapper.xml" />
<xi:include href="wrappers/FT/right_leg-FT_wrapper.xml" />
<!-- <xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
<xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" /> -->
<xi:include href="wrappers/FT/left_arm-FT_wrapper.xml" />
<xi:include href="wrappers/FT/right_arm-FT_wrapper.xml" />

<!-- INERTIAL SENSOR-->
<xi:include href="wrappers/inertials/head-inertials_wrapper.xml" />
Expand Down
56 changes: 56 additions & 0 deletions urdf/ergoCub/robots/ergoCubGazeboV1_1/model.urdf
Original file line number Diff line number Diff line change
Expand Up @@ -2146,6 +2146,12 @@
<parent link="realsense"/>
<child link="realsense_imu_0"/>
</joint>
<link name="l_arm_ft"/>
<joint name="l_arm_ft_fixed_joint" type="fixed">
<origin xyz="-0.004349999999999996 -2.7755575615628914e-17 -0.054300000000000015" rpy="3.141592653589793 2.498001805406602e-16 -1.5707963267948952"/>
<parent link="l_shoulder_2"/>
<child link="l_arm_ft"/>
</joint>
<link name="l_foot_front_ft"/>
<joint name="l_foot_front_ft_fixed_joint" type="fixed">
<origin xyz="0.13525 -4.163336342344337e-17 -0.047899999999999054" rpy="-4.259189482954547e-17 -7.513330770412076e-17 -2.0943951023931966"/>
Expand All @@ -2164,6 +2170,12 @@
<parent link="l_hip_2"/>
<child link="l_leg_ft"/>
</joint>
<link name="r_arm_ft"/>
<joint name="r_arm_ft_fixed_joint" type="fixed">
<origin xyz="-0.006849999999999986 -2.7755575615628914e-17 -0.05429999999999999" rpy="3.141592653589793 1.6653345369377348e-16 1.5707963267948981"/>
<parent link="r_shoulder_2"/>
<child link="r_arm_ft"/>
</joint>
<link name="r_foot_front_ft"/>
<joint name="r_foot_front_ft_fixed_joint" type="fixed">
<origin xyz="0.13525 1.3877787807814457e-17 -0.04789999999999983" rpy="-4.2591894829555776e-17 -7.513330770412949e-17 -2.094395102393197"/>
Expand Down Expand Up @@ -3247,6 +3259,28 @@
<gazebo>
<pose>0.000000 0.000000 0.800000 0.000000 0.000000 0.000000</pose>
</gazebo>
<gazebo reference="l_arm_ft_sensor">
<sensor name="l_arm_ft" type="force_torque">
<always_on>1</always_on>
<update_rate>100</update_rate>
<force_torque>
<frame>sensor</frame>
<measure_direction>child_to_parent</measure_direction>
</force_torque>
<pose>-3.46945e-18 0 0 3.14159 1.94289e-16 -1.5708 </pose>
<plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
</plugin>
</sensor>
</gazebo>
<sensor name="l_arm_ft" type="force_torque">
<parent joint="l_arm_ft_sensor"/>
<force_torque>
<frame>sensor</frame>
<measure_direction>child_to_parent</measure_direction>
</force_torque>
<origin rpy="3.14159 1.94289e-16 -1.5708 " xyz="-3.46945e-18 0 0"/>
</sensor>
<gazebo reference="l_foot_front_ft_sensor">
<sensor name="l_foot_front_ft" type="force_torque">
<always_on>1</always_on>
Expand Down Expand Up @@ -3313,6 +3347,28 @@
</force_torque>
<origin rpy="3.14159 -2.02132e-15 2.61799 " xyz="5.20417e-18 0 0"/>
</sensor>
<gazebo reference="r_arm_ft_sensor">
<sensor name="r_arm_ft" type="force_torque">
<always_on>1</always_on>
<update_rate>100</update_rate>
<force_torque>
<frame>sensor</frame>
<measure_direction>child_to_parent</measure_direction>
</force_torque>
<pose>0 0 0 -3.14159 -2.77556e-17 1.5708 </pose>
<plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
</plugin>
</sensor>
</gazebo>
<sensor name="r_arm_ft" type="force_torque">
<parent joint="r_arm_ft_sensor"/>
<force_torque>
<frame>sensor</frame>
<measure_direction>child_to_parent</measure_direction>
</force_torque>
<origin rpy="-3.14159 -2.77556e-17 1.5708 " xyz="0 0 0"/>
</sensor>
<gazebo reference="r_foot_front_ft_sensor">
<sensor name="r_foot_front_ft" type="force_torque">
<always_on>1</always_on>
Expand Down
56 changes: 56 additions & 0 deletions urdf/ergoCub/robots/ergoCubGazeboV1_1_minContacts/model.urdf
Original file line number Diff line number Diff line change
Expand Up @@ -1960,6 +1960,12 @@
<parent link="realsense"/>
<child link="realsense_imu_0"/>
</joint>
<link name="l_arm_ft"/>
<joint name="l_arm_ft_fixed_joint" type="fixed">
<origin xyz="-0.004349999999999996 -2.7755575615628914e-17 -0.054300000000000015" rpy="3.141592653589793 2.498001805406602e-16 -1.5707963267948952"/>
<parent link="l_shoulder_2"/>
<child link="l_arm_ft"/>
</joint>
<link name="l_foot_front_ft"/>
<joint name="l_foot_front_ft_fixed_joint" type="fixed">
<origin xyz="0.13525 -4.163336342344337e-17 -0.047899999999999054" rpy="-4.259189482954547e-17 -7.513330770412076e-17 -2.0943951023931966"/>
Expand All @@ -1978,6 +1984,12 @@
<parent link="l_hip_2"/>
<child link="l_leg_ft"/>
</joint>
<link name="r_arm_ft"/>
<joint name="r_arm_ft_fixed_joint" type="fixed">
<origin xyz="-0.006849999999999986 -2.7755575615628914e-17 -0.05429999999999999" rpy="3.141592653589793 1.6653345369377348e-16 1.5707963267948981"/>
<parent link="r_shoulder_2"/>
<child link="r_arm_ft"/>
</joint>
<link name="r_foot_front_ft"/>
<joint name="r_foot_front_ft_fixed_joint" type="fixed">
<origin xyz="0.13525 1.3877787807814457e-17 -0.04789999999999983" rpy="-4.2591894829555776e-17 -7.513330770412949e-17 -2.094395102393197"/>
Expand Down Expand Up @@ -3061,6 +3073,28 @@
<gazebo>
<pose>0.000000 0.000000 0.800000 0.000000 0.000000 0.000000</pose>
</gazebo>
<gazebo reference="l_arm_ft_sensor">
<sensor name="l_arm_ft" type="force_torque">
<always_on>1</always_on>
<update_rate>100</update_rate>
<force_torque>
<frame>sensor</frame>
<measure_direction>child_to_parent</measure_direction>
</force_torque>
<pose>-3.46945e-18 0 0 3.14159 1.94289e-16 -1.5708 </pose>
<plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_left_arm_ft.ini</yarpConfigurationFile>
</plugin>
</sensor>
</gazebo>
<sensor name="l_arm_ft" type="force_torque">
<parent joint="l_arm_ft_sensor"/>
<force_torque>
<frame>sensor</frame>
<measure_direction>child_to_parent</measure_direction>
</force_torque>
<origin rpy="3.14159 1.94289e-16 -1.5708 " xyz="-3.46945e-18 0 0"/>
</sensor>
<gazebo reference="l_foot_front_ft_sensor">
<sensor name="l_foot_front_ft" type="force_torque">
<always_on>1</always_on>
Expand Down Expand Up @@ -3127,6 +3161,28 @@
</force_torque>
<origin rpy="3.14159 -2.02132e-15 2.61799 " xyz="5.20417e-18 0 0"/>
</sensor>
<gazebo reference="r_arm_ft_sensor">
<sensor name="r_arm_ft" type="force_torque">
<always_on>1</always_on>
<update_rate>100</update_rate>
<force_torque>
<frame>sensor</frame>
<measure_direction>child_to_parent</measure_direction>
</force_torque>
<pose>0 0 0 -3.14159 -2.77556e-17 1.5708 </pose>
<plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
<yarpConfigurationFile>model://ergoCub/conf/FT/gazebo_ergocub_right_arm_ft.ini</yarpConfigurationFile>
</plugin>
</sensor>
</gazebo>
<sensor name="r_arm_ft" type="force_torque">
<parent joint="r_arm_ft_sensor"/>
<force_torque>
<frame>sensor</frame>
<measure_direction>child_to_parent</measure_direction>
</force_torque>
<origin rpy="-3.14159 -2.77556e-17 1.5708 " xyz="0 0 0"/>
</sensor>
<gazebo reference="r_foot_front_ft_sensor">
<sensor name="r_foot_front_ft" type="force_torque">
<always_on>1</always_on>
Expand Down
Loading

0 comments on commit 8f562f4

Please sign in to comment.