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.

Sign up for GitHub

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

Jump to bottom

Revert "ergoCubV1_1: remove ft sensors of the arms" #208

Merged
merged 1 commit into from
Jan 8, 2024
Merged

Revert "ergoCubV1_1: remove ft sensors of the arms" #208

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 7 additions & 7 deletions tests/ergocub-model-test.cpp
View file
Open in desktop
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.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this change intentional?

Copy link
Member Author

@Nicogene Nicogene Jan 8, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No I fixed it in the previous commit and reverting it put back iCub

*/
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
View file
Open in desktop
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
Loading