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

ergoCubV1*: move down the l/r_sole frames #187

Merged
merged 5 commits into from
Nov 6, 2023
Merged

ergoCubV1*: move down the l/r_sole frames #187

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
ad435d5
ergoCubV1_1: move down the l/r_sole frames
Nicogene Nov 6, 2023
c6e9cb2
ergoCubV1: move down l/r_sole frames
Nicogene Nov 6, 2023
518dd79
ergocub-model-test: add unit test checking the soles
Nicogene Nov 6, 2023
8317c40
Update CHANGELOG
Nicogene Nov 6, 2023
866b8be
Update CHANGELOG
Nicogene Nov 6, 2023
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
6 changes: 6 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,14 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0

## [Unreleased]

### urdf
- Fix location of `l/r_sole` frames(https://github.com/icub-tech-iit/ergocub-software/issues/186)

## [0.5.0] - 2023-10-26

### urdf
- Fixed wobbling of the models after moving feet link frames

## [0.4.0] - 2023-09-04

### urdf
Expand Down
57 changes: 42 additions & 15 deletions tests/ergocub-model-test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,7 @@ bool checkBaseLink(iDynTree::KinDynComputations & comp)
return true;
}

bool checkSolesAreParallel(iDynTree::KinDynComputations & comp)
bool checkSolesAreParallelAndCorrectlyPlaced(iDynTree::KinDynComputations & comp)
{
iDynTree::LinkIndex rootLinkIdx = comp.getFrameIndex("root_link");

Expand Down Expand Up @@ -192,6 +192,32 @@ bool checkSolesAreParallel(iDynTree::KinDynComputations & comp)
iDynTree::Transform root_H_l_sole = comp.getRelativeTransform(rootLinkIdx,l_sole);
iDynTree::Transform root_H_r_sole = comp.getRelativeTransform(rootLinkIdx,r_sole);

iDynTree::Transform root_H_l_sole_expected(iDynTree::Rotation(1, 0, 0,
0, 1, 0,
0, 0, 1),
iDynTree::Position(0.04403,0.0744,-0.7793));
iDynTree::Transform root_H_r_sole_expected(iDynTree::Rotation(1, 0, 0,
0, 1, 0,
0, 0, 1),
iDynTree::Position(0.04403,-0.0744,-0.7793));


if (!checkTransformAreEqual(root_H_l_sole, root_H_l_sole_expected, 1e-5))
{
std::cerr << "ergocub-model-test : transform between root_H_l_sole is not the expected one, test failed." << std::endl;
std::cerr << "ergocub-model-test : root_H_l_sole :" << root_H_l_sole.toString() << std::endl;
std::cerr << "ergocub-model-test : root_H_l_sole_expected :" << root_H_l_sole_expected.toString() << std::endl;
return false;
}

if (!checkTransformAreEqual(root_H_r_sole, root_H_r_sole_expected, 1e-5))
{
std::cerr << "ergocub-model-test : transform between root_H_r_sole is not the expected one, test failed." << std::endl;
std::cerr << "ergocub-model-test : root_H_r_sole :" << root_H_r_sole.toString() << std::endl;
std::cerr << "ergocub-model-test : root_H_r_sole_expected :" << root_H_r_sole_expected.toString() << std::endl;
return false;
}

// height of the sole should be equal
double l_sole_height = root_H_l_sole.getPosition().getVal(2);
double r_sole_height = root_H_r_sole.getPosition().getVal(2);
Expand Down Expand Up @@ -446,21 +472,21 @@ bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader)
}


bool checkFTSensorIsCorrectlyOriented(iDynTree::KinDynComputations & comp,
bool checkFrameIsCorrectlyOriented(iDynTree::KinDynComputations & comp,
const iDynTree::Rotation& expected,
const std::string& sensorName)
const std::string& frameName)
{
// Depending on the ergocub model, the sensor could be absent
if (!comp.model().isFrameNameUsed(sensorName))
if (!comp.model().isFrameNameUsed(frameName))
{
return true;
}

iDynTree::Rotation actual = comp.getRelativeTransform("root_link", sensorName).getRotation();
iDynTree::Rotation actual = comp.getRelativeTransform("root_link", frameName).getRotation();

if (!checkMatrixAreEqual(expected, actual, 1e-3))
{
std::cerr << "ergocub-model-test : transform between root_link and " << sensorName << " is not the expected one, test failed." << std::endl;
std::cerr << "ergocub-model-test : transform between root_link and " << frameName << " is not the expected one, test failed." << std::endl;
std::cerr << "ergocub-model-test : Expected transform : " << expected.toString() << std::endl;
std::cerr << "ergocub-model-test : Actual transform : " << actual.toString() << std::endl;
return false;
Expand All @@ -470,6 +496,7 @@ bool checkFTSensorIsCorrectlyOriented(iDynTree::KinDynComputations & comp,
}



bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp)
{

Expand Down Expand Up @@ -497,14 +524,14 @@ bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp)
0.5, 0.866025, 0,
0, 0, -1);

bool ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft");
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_L_sensorFrameExpectedLeg, "l_leg_ft") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedLeg, "r_leg_ft") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_rear_ft") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_rear_ft") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_front_ft") && ok;
ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_front_ft") && ok;
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;
ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_front_ft") && ok;
ok = checkFrameIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_front_ft") && ok;
return ok;
}

Expand Down Expand Up @@ -726,7 +753,7 @@ int main(int argc, char ** argv)


// Check if l_sole/r_sole have the same distance from the root_link
if( !checkSolesAreParallel(comp) )
if( !checkSolesAreParallelAndCorrectlyPlaced(comp) )
{
return EXIT_FAILURE;
}
Expand Down
4 changes: 2 additions & 2 deletions urdf/ergoCub/robots/ergoCubGazeboV1/model.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -280,7 +280,7 @@
</joint>
<link name="r_sole"/>
<joint name="r_sole_fixed_joint" type="fixed">
<origin xyz="0.05963 0 -0.022299999999999986" rpy="0 0 0"/>
<origin xyz="0.05963 0 -0.024299999999999877" rpy="0 0 0"/>
<parent link="r_foot_rear"/>
<child link="r_sole"/>
<dynamics damping="0.1"/>
Expand Down Expand Up @@ -600,7 +600,7 @@
</joint>
<link name="l_sole"/>
<joint name="l_sole_fixed_joint" type="fixed">
<origin xyz="0.05963 0 -0.022299999999999986" rpy="0 0 0"/>
<origin xyz="0.05963 0 -0.024299999999999877" rpy="0 0 0"/>
<parent link="l_foot_rear"/>
<child link="l_sole"/>
<dynamics damping="0.1"/>
Expand Down
20 changes: 10 additions & 10 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 @@ -1241,8 +1241,8 @@
</collision>
</link>
<joint name="l_index_add" type="revolute">
<origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
<axis xyz="1.3005559878087045e-15 1.0000000000000002 -1.953448495705706e-15"/>
<origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
<axis xyz="1.1895336853461886e-15 0.9999999999999999 -1.9812040713213337e-15"/>
<parent link="l_hand_palm"/>
<child link="l_hand_index_1"/>
<limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
Expand All @@ -1251,7 +1251,7 @@
<link name="l_hand_index_1">
<inertial>
<mass value="0.01923264"/>
<origin xyz="0.0046807100979840005 0.0038510231744862677 -0.018526526203731464" rpy="0 -0 0"/>
<origin xyz="0.0046807100979840005 0.0038510231744863237 -0.018526526203731575" rpy="0 -0 0"/>
<inertia ixx="0.01" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="0.01" iyz="-6.425403568227921e-7" izz="0.01"/>
</inertial>
<visual>
Expand Down Expand Up @@ -1571,8 +1571,8 @@
</collision>
</link>
<joint name="l_index_prox" type="revolute">
<origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
<axis xyz="-1.0000000000000002 2.7755575615627613e-17 7.023654366444809e-10"/>
<origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
<axis xyz="-1 8.326672684688522e-17 7.023654435833748e-10"/>
<parent link="l_hand_index_1"/>
<child link="l_hand_index_2"/>
<limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
Expand Down Expand Up @@ -1604,8 +1604,8 @@
</collision>
</link>
<joint name="l_index_dist" type="revolute">
<origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
<axis xyz="-1 -1.804112664227406e-16 9.020562801946248e-17"/>
<origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
<axis xyz="-1 -2.359224176539984e-16 4.163337069211178e-17"/>
<parent link="l_hand_index_2"/>
<child link="l_hand_index_3"/>
<limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
Expand All @@ -1614,7 +1614,7 @@
<link name="l_hand_index_3">
<inertial>
<mass value="0.007251413"/>
<origin xyz="-0.018787329483201373 0.002754671170301093 -0.018853294932697606" rpy="0 -0 0"/>
<origin xyz="-0.018787329483201345 0.0027546711703011484 -0.018853294932697662" rpy="0 -0 0"/>
<inertia ixx="0.01" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="0.01" iyz="-2.891156821380605e-7" izz="0.01"/>
</inertial>
<visual>
Expand Down Expand Up @@ -2508,7 +2508,7 @@
</joint>
<link name="l_sole"/>
<joint name="l_sole_fixed_joint" type="fixed">
<origin xyz="0.059629999999999996 0 1.1102230246251565e-16" rpy="0 1.5094274020956558e-31 0"/>
<origin xyz="0.059629999999999996 -1.3877787807814457e-17 -0.024299999999999877" rpy="0 1.5094274020956558e-31 0"/>
<parent link="l_foot_rear"/>
<child link="l_sole"/>
</joint>
Expand Down Expand Up @@ -2874,7 +2874,7 @@
</joint>
<link name="r_sole"/>
<joint name="r_sole_fixed_joint" type="fixed">
<origin xyz="0.05963 -1.3877787807814457e-17 7.771561172376096e-16" rpy="0 3.1371750050201436e-30 0"/>
<origin xyz="0.05963 -1.3877787807814457e-17 -0.02429999999999999" rpy="0 3.1371750050201436e-30 0"/>
<parent link="r_foot_rear"/>
<child link="r_sole"/>
</joint>
Expand Down
20 changes: 10 additions & 10 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 @@ -1133,8 +1133,8 @@
</collision>
</link>
<joint name="l_index_add" type="revolute">
<origin xyz="0.022500000000000034 -0.0015000000000006397 -0.06859999999999966" rpy="-8.126657451512497e-16 -0.2967059722446555 1.7414265967526777e-16"/>
<axis xyz="1.3005559878087045e-15 1.0000000000000002 -1.953448495705706e-15"/>
<origin xyz="0.022500000000000023 -0.0015000000000006952 -0.0685999999999996" rpy="-8.416895217637947e-16 -0.29670597224465534 5.804755322508926e-17"/>
<axis xyz="1.1895336853461886e-15 0.9999999999999999 -1.9812040713213337e-15"/>
<parent link="l_hand_palm"/>
<child link="l_hand_index_1"/>
<limit lower="0" upper="0.2617993877991494" effort="1e+9" velocity="1e+9"/>
Expand All @@ -1143,7 +1143,7 @@
<link name="l_hand_index_1">
<inertial>
<mass value="0.01923264"/>
<origin xyz="0.0046807100979840005 0.0038510231744862677 -0.018526526203731464" rpy="0 -0 0"/>
<origin xyz="0.0046807100979840005 0.0038510231744863237 -0.018526526203731575" rpy="0 -0 0"/>
<inertia ixx="0.01" ixy="1.4548370851701976e-7" ixz="1.4373172964405972e-7" iyy="0.01" iyz="-6.425403568227921e-7" izz="0.01"/>
</inertial>
<visual>
Expand Down Expand Up @@ -1433,8 +1433,8 @@
</collision>
</link>
<joint name="l_index_prox" type="revolute">
<origin xyz="0.009192533346793416 -2.7755575615628914e-17 -0.030713451309624162" rpy="-2.7755575615628914e-16 -7.023611553469422e-10 -7.771561172376094e-16"/>
<axis xyz="-1.0000000000000002 2.7755575615627613e-17 7.023654366444809e-10"/>
<origin xyz="0.00919253334679343 0 -0.030713451309624218" rpy="-3.0531133177191815e-16 -7.023611484080483e-10 -7.771561172376096e-16"/>
<axis xyz="-1 8.326672684688522e-17 7.023654435833748e-10"/>
<parent link="l_hand_index_1"/>
<child link="l_hand_index_2"/>
<limit lower="0" upper="1.5707963267948966" effort="1e+9" velocity="1e+9"/>
Expand Down Expand Up @@ -1463,8 +1463,8 @@
</collision>
</link>
<joint name="l_index_dist" type="revolute">
<origin xyz="-2.7755575615628914e-17 -0.0014999999999999736 -0.039999999999999925" rpy="-7.482326169760975e-8 1.734723475976807e-16 -6.106226635438361e-16"/>
<axis xyz="-1 -1.804112664227406e-16 9.020562801946248e-17"/>
<origin xyz="-1.3877787807814457e-17 -0.001500000000000029 -0.03999999999999987" rpy="-7.482326172536535e-8 1.8041124150158794e-16 -6.106226635438361e-16"/>
<axis xyz="-1 -2.359224176539984e-16 4.163337069211178e-17"/>
<parent link="l_hand_index_2"/>
<child link="l_hand_index_3"/>
<limit lower="0" upper="1.7994344588061537" effort="1e+9" velocity="1e+9"/>
Expand All @@ -1473,7 +1473,7 @@
<link name="l_hand_index_3">
<inertial>
<mass value="0.007251413"/>
<origin xyz="-0.018787329483201373 0.002754671170301093 -0.018853294932697606" rpy="0 -0 0"/>
<origin xyz="-0.018787329483201345 0.0027546711703011484 -0.018853294932697662" rpy="0 -0 0"/>
<inertia ixx="0.01" ixy="4.65463304290381e-8" ixz="2.5624287052698965e-7" iyy="0.01" iyz="-2.891156821380605e-7" izz="0.01"/>
</inertial>
<visual>
Expand Down Expand Up @@ -2328,7 +2328,7 @@
</joint>
<link name="l_sole"/>
<joint name="l_sole_fixed_joint" type="fixed">
<origin xyz="0.059629999999999996 0 1.1102230246251565e-16" rpy="0 1.5094274020956558e-31 0"/>
<origin xyz="0.059629999999999996 -1.3877787807814457e-17 -0.024299999999999877" rpy="0 1.5094274020956558e-31 0"/>
<parent link="l_foot_rear"/>
<child link="l_sole"/>
</joint>
Expand Down Expand Up @@ -2694,7 +2694,7 @@
</joint>
<link name="r_sole"/>
<joint name="r_sole_fixed_joint" type="fixed">
<origin xyz="0.05963 -1.3877787807814457e-17 7.771561172376096e-16" rpy="0 3.1371750050201436e-30 0"/>
<origin xyz="0.05963 -1.3877787807814457e-17 -0.02429999999999999" rpy="0 3.1371750050201436e-30 0"/>
<parent link="r_foot_rear"/>
<child link="r_sole"/>
</joint>
Expand Down
4 changes: 2 additions & 2 deletions urdf/ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -280,7 +280,7 @@
</joint>
<link name="r_sole"/>
<joint name="r_sole_fixed_joint" type="fixed">
<origin xyz="0.05963 0 -0.022299999999999986" rpy="0 0 0"/>
<origin xyz="0.05963 0 -0.024299999999999877" rpy="0 0 0"/>
<parent link="r_foot_rear"/>
<child link="r_sole"/>
<dynamics damping="0.1"/>
Expand Down Expand Up @@ -600,7 +600,7 @@
</joint>
<link name="l_sole"/>
<joint name="l_sole_fixed_joint" type="fixed">
<origin xyz="0.05963 0 -0.022299999999999986" rpy="0 0 0"/>
<origin xyz="0.05963 0 -0.024299999999999877" rpy="0 0 0"/>
<parent link="l_foot_rear"/>
<child link="l_sole"/>
<dynamics damping="0.1"/>
Expand Down
4 changes: 2 additions & 2 deletions urdf/ergoCub/robots/ergoCubSN000/model.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -280,7 +280,7 @@
</joint>
<link name="r_sole"/>
<joint name="r_sole_fixed_joint" type="fixed">
<origin xyz="0.05963 0 -0.022299999999999986" rpy="0 0 0"/>
<origin xyz="0.05963 0 -0.024299999999999877" rpy="0 0 0"/>
<parent link="r_foot_rear"/>
<child link="r_sole"/>
<dynamics damping="0.1"/>
Expand Down Expand Up @@ -600,7 +600,7 @@
</joint>
<link name="l_sole"/>
<joint name="l_sole_fixed_joint" type="fixed">
<origin xyz="0.05963 0 -0.022299999999999986" rpy="0 0 0"/>
<origin xyz="0.05963 0 -0.024299999999999877" rpy="0 0 0"/>
<parent link="l_foot_rear"/>
<child link="l_sole"/>
<dynamics damping="0.1"/>
Expand Down
Loading
Loading