From a514135cf0d25dba8ea67c415669dc8ed37a3987 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 9 Jan 2023 11:49:16 +0100 Subject: [PATCH 1/9] Enable test for model generation --- CMakeLists.txt | 11 +- tests/CMakeLists.txt | 18 ++ tests/ergocub-model-test.cpp | 504 +++++++++++++++++++++++++++++++++++ urdf/CMakeLists.txt | 1 - 4 files changed, 528 insertions(+), 6 deletions(-) create mode 100644 tests/CMakeLists.txt create mode 100644 tests/ergocub-model-test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 677dd7e3..4ea24e47 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,11 +22,12 @@ option(ERGOCUB_MODEL_GENERATE_SIMMECHANICS "Generate models using the model gene option(ERGOCUB_MODEL_COPY_TO_SRC "Copy to sources the generated urdf files" OFF) option(BUILD_TESTING "Run tests for the generated models" OFF) +set(BUILD_PREFIX "ergoCub") add_subdirectory(urdf) # TODO to be defined -#if (BUILD_TESTING) -# include( CTest ) -# enable_testing() -# add_subdirectory(tests) -#endif() +if (BUILD_TESTING) + include( CTest ) + enable_testing() + add_subdirectory(tests) +endif() diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt new file mode 100644 index 00000000..5ef7d3e4 --- /dev/null +++ b/tests/CMakeLists.txt @@ -0,0 +1,18 @@ +# Tests for generated models +# We use iDynTree & YARP for the tests +find_package(iDynTree REQUIRED) +find_package(YARP REQUIRED) + +add_executable(ergocub-model-test ergocub-model-test.cpp) +target_link_libraries(ergocub-model-test ${iDynTree_LIBRARIES} YARP::YARP_os) + +macro(add_ergocub_model_test yarpRobotName) + add_test(NAME ${yarpRobotName}ConsistencyCheck + COMMAND ergocub-model-test --model ${CMAKE_BINARY_DIR}/${BUILD_PREFIX}/robots/${yarpRobotName}/model.urdf) +endmacro() + +# Model generated with simmechanics +if( ERGOCUB_MODEL_GENERATE_SIMMECHANICS ) + add_ergocub_model_test(ergoCubGazeboV1) + add_ergocub_model_test(ergoCubSN000) +endif() diff --git a/tests/ergocub-model-test.cpp b/tests/ergocub-model-test.cpp new file mode 100644 index 00000000..6a1a08cd --- /dev/null +++ b/tests/ergocub-model-test.cpp @@ -0,0 +1,504 @@ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +inline bool checkDoubleAreEqual(const double & val1, + const double & val2, + const double tol) +{ + if( std::fabs(val1-val2) > tol ) + { + return false; + } + + return true; +} + + +template +bool checkVectorAreEqual(const VectorType1 & dir1, + const VectorType2 & dir2, + const double tol) +{ + if( dir1.size() != dir2.size() ) + { + return false; + } + + for(int i=0; i < dir1.size(); i++) + { + if( std::fabs(dir1(i)-dir2(i)) > tol ) + { + return false; + } + } + return true; +} + +template +bool checkMatrixAreEqual(const MatrixType1 & mat1, + const MatrixType2 & mat2, + const double tol) +{ + if( mat1.rows() != mat2.rows() || + mat1.cols() != mat2.cols() ) + { + return false; + } + + for(int i=0; i < mat1.rows(); i++) + { + for(int j=0; j < mat1.cols(); j++ ) + { + if( std::fabs(mat1(i,j)-mat2(i,j)) > tol ) + { + return false; + } + } + } + return true; +} + +bool checkTransformAreEqual(const iDynTree::Transform & t1, + const iDynTree::Transform & t2, + const double tol) +{ + return checkMatrixAreEqual(t1.getRotation(),t2.getRotation(),tol) && + checkVectorAreEqual(t1.getPosition(),t2.getPosition(),tol); +} + + +bool getAxisInRootLink(iDynTree::KinDynComputations & comp, + const std::string jointName, + iDynTree::Axis & axisInRootLink) +{ + iDynTree::LinkIndex rootLinkIdx = comp.getFrameIndex("root_link"); + + if( rootLinkIdx == iDynTree::FRAME_INVALID_INDEX ) + { + std::cerr << "ergocub-model-test error: impossible to find root_link in model" << std::endl; + return false; + } + + iDynTree::JointIndex jntIdx = comp.getRobotModel().getJointIndex(jointName); + + if( jntIdx == iDynTree::JOINT_INVALID_INDEX ) + { + std::cerr << "ergocub-model-test error: impossible to find " << jointName << " in model" << std::endl; + return false; + } + + iDynTree::LinkIndex childLinkIdx = comp.getRobotModel().getJoint(jntIdx)->getSecondAttachedLink(); + + // Check that the joint are actually revolute as all the joints in iCub + const iDynTree::RevoluteJoint * revJoint = dynamic_cast(comp.getRobotModel().getJoint(jntIdx)); + + if( !revJoint ) + { + std::cerr << "ergocub-model-test error: " << jointName << " is not revolute " << std::endl; + return false; + } + + if( !revJoint->hasPosLimits() ) + { + std::cerr << "ergocub-model-test error: " << jointName << " is a continous joint" << std::endl; + return false; + } + + axisInRootLink = comp.getRelativeTransform(rootLinkIdx,childLinkIdx)*(revJoint->getAxis(childLinkIdx)); + + return true; +} + +bool checkBaseLink(iDynTree::KinDynComputations & comp) +{ + iDynTree::LinkIndex rootLinkIdx = comp.getFrameIndex("root_link"); + + if( rootLinkIdx == iDynTree::FRAME_INVALID_INDEX ) + { + std::cerr << "ergocub-model-test error: impossible to find root_link in model" << std::endl; + return false; + } + + iDynTree::LinkIndex base_linkIdx = comp.getFrameIndex("base_link"); + + if( rootLinkIdx == iDynTree::FRAME_INVALID_INDEX ) + { + std::cerr << "ergocub-model-test error: impossible to find base_link in model" << std::endl; + return false; + } + + if( comp.getRobotModel().getFrameLink(base_linkIdx) != rootLinkIdx ) + { + std::cerr << "ergocub-model-test error: base_link is not attached to root_link" << std::endl; + return false; + } + + if( !checkTransformAreEqual(comp.getRobotModel().getFrameTransform(base_linkIdx),iDynTree::Transform::Identity(),1e-6) ) + { + std::cerr << "ergocub-model-test error: base_link <---> root_link transform is not an identity" << std::endl; + return false; + } + + std::cerr << "ergocub-model-test : base_link test performed correctly " << std::endl; + + return true; +} + +bool checkSolesAreParallel(iDynTree::KinDynComputations & comp) +{ + iDynTree::LinkIndex rootLinkIdx = comp.getFrameIndex("root_link"); + + if( rootLinkIdx == iDynTree::FRAME_INVALID_INDEX ) + { + std::cerr << "ergocub-model-test error: impossible to find root_link in model" << std::endl; + return false; + } + + iDynTree::LinkIndex l_sole = comp.getFrameIndex("l_sole"); + + if( rootLinkIdx == iDynTree::FRAME_INVALID_INDEX ) + { + std::cerr << "ergocub-model-test error: impossible to find frame l_sole in model" << std::endl; + return false; + } + + iDynTree::LinkIndex r_sole = comp.getFrameIndex("r_sole"); + + if( rootLinkIdx == iDynTree::FRAME_INVALID_INDEX ) + { + std::cerr << "ergocub-model-test error: impossible to find frame r_sole in model" << std::endl; + return false; + } + + iDynTree::Transform root_H_l_sole = comp.getRelativeTransform(rootLinkIdx,l_sole); + iDynTree::Transform root_H_r_sole = comp.getRelativeTransform(rootLinkIdx,r_sole); + + // 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); + + if( !checkDoubleAreEqual(l_sole_height,r_sole_height,1e-5) ) + { + std::cerr << "ergocub-model-test error: l_sole_height is " << l_sole_height << ", while r_sole_height is " << r_sole_height << " (diff : " << std::fabs(l_sole_height-r_sole_height) << " )" << std::endl; + return false; + } + + // x should also be equal + double l_sole_x = root_H_l_sole.getPosition().getVal(0); + double r_sole_x = root_H_r_sole.getPosition().getVal(0); + + // The increased threshold is a workaround for https://github.com/robotology/icub-model-generator/issues/125 + if( !checkDoubleAreEqual(l_sole_x,r_sole_x, 2e-4) ) + { + std::cerr << "ergocub-model-test error: l_sole_x is " << l_sole_x << ", while r_sole_x is " << r_sole_x << " (diff : " << std::fabs(l_sole_x-r_sole_x) << " )" << std::endl; + return false; + } + + // y should be simmetric + double l_sole_y = root_H_l_sole.getPosition().getVal(1); + double r_sole_y = root_H_r_sole.getPosition().getVal(1); + + // The increased threshold is a workaround for https://github.com/robotology/icub-model-generator/issues/125 + if( !checkDoubleAreEqual(l_sole_y,-r_sole_y,1e-4) ) + { + std::cerr << "ergocub-model-test error: l_sole_y is " << l_sole_y << ", while r_sole_y is " << r_sole_y << " while they should be simmetric (diff : " << std::fabs(l_sole_y+r_sole_y) << " )" << std::endl; + return false; + } + + + std::cerr << "ergocub-model-test : sole are parallel test performed correctly " << std::endl; + + return true; +} + + + +bool checkAxisDirections(iDynTree::KinDynComputations & comp) +{ + + std::vector axisNames; + std::vector expectedDirectionInRootLink; + axisNames.push_back("torso_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-1,0,0)); + axisNames.push_back("l_hip_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,-1,0)); + axisNames.push_back("r_hip_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,-1,0)); + axisNames.push_back("r_hip_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-1,0,0)); + axisNames.push_back("r_hip_yaw"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,0,-1)); + axisNames.push_back("r_knee"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,-1,0)); + axisNames.push_back("r_ankle_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,1,0)); + axisNames.push_back("r_ankle_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-1,0,0)); + axisNames.push_back("l_hip_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(1,0,0)); + axisNames.push_back("l_hip_yaw"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,0,1)); + axisNames.push_back("l_knee"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,-1,0)); + axisNames.push_back("l_ankle_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,1,0)); + axisNames.push_back("l_ankle_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(1,0,0)); + axisNames.push_back("torso_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,1,0)); + axisNames.push_back("torso_yaw"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,0,-1)); + axisNames.push_back("l_shoulder_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0.250563,0.935113,0.250563)); + axisNames.push_back("r_shoulder_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.250563,0.935113,-0.250563)); + axisNames.push_back("neck_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0,-1,0)); + axisNames.push_back("neck_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(1,0,0)); + axisNames.push_back("neck_yaw"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-1.62555e-21,-1.1e-15,1)); + axisNames.push_back("r_shoulder_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.961047,-0.271447,-0.0520081)); + // axisNames.push_back("r_shoulder_yaw"); + // expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.0713662,0.0619303,0.995526)); + // axisNames.push_back("r_elbow"); + // expectedDirectionInRootLink.push_back(iDynTree::Direction(0.267012,-0.960459,0.0788901)); + axisNames.push_back("r_wrist_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0.961047,0.271447,0.0520081)); + axisNames.push_back("r_wrist_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0.250563,-0.935113,0.250563)); + axisNames.push_back("r_wrist_yaw"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.116648,0.227771,0.966702)); + axisNames.push_back("l_shoulder_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0.961047,-0.271447,0.0520081)); + // axisNames.push_back("l_shoulder_yaw"); + // expectedDirectionInRootLink.push_back(iDynTree::Direction(0.0713662,0.0619303,-0.995526)); + // axisNames.push_back("l_elbow"); + // expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.267012,-0.960459,-0.0788901)); + axisNames.push_back("l_wrist_roll"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.961047,0.271447,-0.0520081)); + axisNames.push_back("l_wrist_pitch"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.250563,-0.935113,-0.250563)); + axisNames.push_back("l_wrist_yaw"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0.116648,0.227771,-0.966702)); + + for(int i=0; i < axisNames.size(); i++) + { + std::string axisToCheck = axisNames[i]; + iDynTree::Axis axisInRootLink; + iDynTree::Direction expectedDirection = expectedDirectionInRootLink[i]; + bool getAxisOk = getAxisInRootLink(comp,axisToCheck,axisInRootLink); + + if( !getAxisOk ) { + return false; + } + + if( !checkVectorAreEqual(axisInRootLink.getDirection(),expectedDirection,1e-5) ) + { + std::cerr << "ergocub-model-test error:" << axisToCheck << " got direction of " << axisInRootLink.getDirection().toString() + << " instead of expected " << expectedDirection.toString() << std::endl; + return false; + } + } + + return true; +} + +/** + * All the iCub have a odd and not null number of F/T sensors. + */ +bool checkFTSensorsAreOddAndNotNull(iDynTree::ModelLoader & mdlLoader) +{ + int nrOfFTSensors = mdlLoader.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); + + if( nrOfFTSensors == 0 ) + { + std::cerr << "ergocub-model-test error: no F/T sensor found in the model" << std::endl; + return false; + } + + if( nrOfFTSensors % 2 == 0 ) + { + std::cerr << "ergocub-model-test : even number of F/T sensor found in the model" << std::endl; + return false; + } + + + return true; +} + +/** + * All the iCub have a even and not null number of F/T sensors. + */ +bool checkFTSensorsAreEvenAndNotNull(iDynTree::ModelLoader & mdlLoader) +{ + int nrOfFTSensors = mdlLoader.sensors().getNrOfSensors(iDynTree::SIX_AXIS_FORCE_TORQUE); + + if( nrOfFTSensors == 0 ) + { + std::cerr << "ergocub-model-test error: no F/T sensor found in the model" << std::endl; + return false; + } + + if( nrOfFTSensors % 2 == 1 ) + { + std::cerr << "ergocub-model-test : odd number of F/T sensor found in the model" << std::endl; + return false; + } + + + return true; +} + + +bool checkFTSensorIsCorrectlyOriented(iDynTree::KinDynComputations & comp, + const iDynTree::Rotation& expected, + const std::string& sensorName) +{ + // Depending on the ergocub model, the sensor could be absent + if (!comp.model().isFrameNameUsed(sensorName)) + { + return true; + } + + iDynTree::Rotation actual = comp.getRelativeTransform("root_link", sensorName).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 : Expected transform : " << expected.toString() << std::endl; + std::cerr << "ergocub-model-test : Actual transform : " << actual.toString() << std::endl; + return false; + } + + return true; +} + + +bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp) +{ + + iDynTree::Rotation rootLink_R_sensorFrameLeftArmExpected = + iDynTree::Rotation(-0.267012, -0.961047, 0.0713662, + -0.960459, 0.271447, 0.0619303, + -0.0788901, -0.0520081, -0.995526); + iDynTree::Rotation rootLink_R_sensorFrameRightArmExpected = + iDynTree::Rotation(-0.267012, 0.961047, 0.0713662, + 0.960459, 0.271447, -0.0619303, + -0.0788901, 0.0520081, -0.995526); + + iDynTree::Rotation rootLink_R_sensorFrameExpectedFoot = + iDynTree::Rotation(-0.5, 0.866025, 0, + -0.866025, -0.5, 0, + 0, 0, 1); + + iDynTree::Rotation rootLink_R_sensorFrameExpectedLeg = + iDynTree::Rotation(-0.866025, -0.5, 0, + -0.5, 0.866025, 0, + 0, 0, -1); + + iDynTree::Rotation rootLink_L_sensorFrameExpectedLeg = + iDynTree::Rotation(-0.866025, 0.5, 0, + 0.5, 0.866025, 0, + 0, 0, -1); + + bool ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameLeftArmExpected, "l_arm_ft_sensor"); + ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameRightArmExpected, "r_arm_ft_sensor") && ok; + ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_L_sensorFrameExpectedLeg, "l_leg_ft_sensor") && ok; + ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedLeg, "r_leg_ft_sensor") && ok; + ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_rear_ft_sensor") && ok; + ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_rear_ft_sensor") && ok; + ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "l_foot_front_ft_sensor") && ok; + ok = checkFTSensorIsCorrectlyOriented(comp, rootLink_R_sensorFrameExpectedFoot, "r_foot_front_ft_sensor") && ok; + return ok; +} + +int main(int argc, char ** argv) +{ + yarp::os::Property prop; + + // Parse command line options + prop.fromCommand(argc,argv); + + // Skip upper body tests (to support testing iCubHeidelberg01 model) + bool skipUpperBody = prop.check("skipUpperBody"); + + // Get model name + if( ! prop.find("model").isString() ) + { + std::cerr << "ergocub-model-test error: model option not found" << std::endl; + return EXIT_FAILURE; + } + + const std::string modelPath = prop.find("model").asString().c_str(); + + iDynTree::ModelLoader mdlLoader; + mdlLoader.loadModelFromFile(modelPath); + + // Open the model + iDynTree::KinDynComputations comp; + const bool modelLoaded = comp.loadRobotModel(mdlLoader.model()); + + if( !modelLoaded ) + { + std::cerr << "ergocub-model-test error: impossible to load model from " << modelLoaded << std::endl; + return EXIT_FAILURE; + } + + iDynTree::Vector3 grav; + grav.zero(); + iDynTree::JointPosDoubleArray qj(comp.getRobotModel()); + iDynTree::JointDOFsDoubleArray dqj(comp.getRobotModel()); + qj.zero(); + dqj.zero(); + + comp.setRobotState(qj,dqj,grav); + + // Check axis + if( !checkAxisDirections(comp) ) + { + return EXIT_FAILURE; + } + // Check if base_link exist, and check that is a frame attached to root_link and if its + // transform is the idyn + if( !checkBaseLink(comp) ) + { + return EXIT_FAILURE; + } + + + // Check if l_sole/r_sole have the same distance from the root_link + // if( !checkSolesAreParallel(comp) ) + // { + // return EXIT_FAILURE; + // } + + // Now some test that test the sensors + // The ft sensors orientation respect to the root_link are different to iCubV2 and they are under investigation. + if( !checkFTSensorsAreEvenAndNotNull(mdlLoader) ) + { + return EXIT_FAILURE; + } + + // if (!checkFTSensorsAreCorrectlyOriented(comp)) + // { + // return EXIT_FAILURE; + // } + + + std::cerr << "Check for model " << modelPath << " concluded correctly!" << std::endl; + + return EXIT_SUCCESS; +} diff --git a/urdf/CMakeLists.txt b/urdf/CMakeLists.txt index 9d7daf9b..85eeebe9 100644 --- a/urdf/CMakeLists.txt +++ b/urdf/CMakeLists.txt @@ -4,7 +4,6 @@ # This software may be modified and distributed under the terms of the # BSD-3-Clause license. See the accompanying LICENSE file for details. -set(BUILD_PREFIX "ergoCub") set(GAZEBO_SUPPORTED_MODELS "") From 4213185c42f770e9401295ba7d3c552d44a00680 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 9 Jan 2023 11:50:09 +0100 Subject: [PATCH 2/9] urdf: Add reversed axis after running unit tests --- .../data/ergocub/ERGOCUB_all_options.yaml | 11 +++++++++-- .../data/ergocub/ERGOCUB_all_options_gazebo.yaml.in | 13 ++++++++++--- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml index 506fdd01..6d1bbc19 100644 --- a/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml +++ b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options.yaml @@ -1031,7 +1031,15 @@ reverseRotationAxis: r_wrist_yaw r_wrist_pitch r_wrist_roll -# r_hip_yaw + torso_roll + r_hip_roll + r_hip_yaw + r_ankle_roll + l_knee + l_ankle_pitch + torso_pitch + torso_yaw + neck_yaw # r_hip_roll # r_knee # r_ankle_roll @@ -1042,7 +1050,6 @@ reverseRotationAxis: # l_wrist_yaw # torso_yaw # torso_pitch -# torso_roll # neck_yaw XMLBlobs: diff --git a/urdf/simmechanics/data/ergocub/ERGOCUB_all_options_gazebo.yaml.in b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options_gazebo.yaml.in index 4279c915..c56c0292 100644 --- a/urdf/simmechanics/data/ergocub/ERGOCUB_all_options_gazebo.yaml.in +++ b/urdf/simmechanics/data/ergocub/ERGOCUB_all_options_gazebo.yaml.in @@ -1228,8 +1228,16 @@ reverseRotationAxis: r_wrist_yaw r_wrist_pitch r_wrist_roll -# r_hip_yaw -# r_hip_roll + torso_roll + r_hip_roll + r_hip_yaw + r_ankle_roll + l_knee + l_ankle_pitch + torso_pitch + torso_yaw + neck_yaw +# # r_knee # r_ankle_roll # l_ankle_pitch @@ -1239,7 +1247,6 @@ reverseRotationAxis: # l_wrist_yaw # torso_yaw # torso_pitch -# torso_roll # neck_yaw XMLBlobs: From 81938cf05294513384fcca3c537a1f546871d10c Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 9 Jan 2023 11:53:38 +0100 Subject: [PATCH 3/9] Add unit test to generate_model.yml --- .github/workflows/generate_models.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/generate_models.yml b/.github/workflows/generate_models.yml index f31753ff..45886d3b 100644 --- a/.github/workflows/generate_models.yml +++ b/.github/workflows/generate_models.yml @@ -64,9 +64,10 @@ jobs: # Prepare ergocub-software build mkdir build cd build - cmake -DERGOCUB_MODEL_GENERATE_SIMMECHANICS=BOOL:ON -DERGOCUB_MODEL_COPY_TO_SRC=BOOL:ON .. + cmake -DERGOCUB_MODEL_GENERATE_SIMMECHANICS=BOOL:ON -DERGOCUB_MODEL_COPY_TO_SRC=BOOL:ON -DBUILD_TESTING=BOOL:ON .. # Build and run make VERBOSE=1 + ctest --output-on-failure - name: Create Pull Request id: cpr From 200222fd0330ad5762788faba78e0f82535cf6ba Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 9 Jan 2023 14:33:41 +0100 Subject: [PATCH 4/9] generate-models: enable also for PRs, disable PR generated from PR --- .github/workflows/generate_models.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/generate_models.yml b/.github/workflows/generate_models.yml index 45886d3b..2c4e8796 100644 --- a/.github/workflows/generate_models.yml +++ b/.github/workflows/generate_models.yml @@ -4,6 +4,9 @@ name: Automatically generate models and commit to the repo # Controls when the workflow will run on: + pull_request: + paths: + - 'urdf/simmechanics/**' push: # Triggers the workflow on push or pull request events but only for the master branch branches: @@ -70,6 +73,7 @@ jobs: ctest --output-on-failure - name: Create Pull Request + if: ${{ github.event_name == 'push' }} id: cpr uses: peter-evans/create-pull-request@main with: From 9feea5f437b7a849a05b3b624d70c1ac60ef34af Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 9 Jan 2023 14:52:49 +0100 Subject: [PATCH 5/9] generate-models: add default shell --- .github/workflows/generate_models.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/generate_models.yml b/.github/workflows/generate_models.yml index 2c4e8796..f22d0ee5 100644 --- a/.github/workflows/generate_models.yml +++ b/.github/workflows/generate_models.yml @@ -16,11 +16,12 @@ on: # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: - # This workflow contains a single job called "build" generate_models: # The type of runner that the job will run on runs-on: ubuntu-latest - + defaults: + run: + shell: bash -el {0} # Steps represent a sequence of tasks that will be executed as part of the job steps: - uses: actions/checkout@v2 From 6e0b4b161d6d4f33519510b9f0c68d49b00f8e94 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 9 Jan 2023 15:03:23 +0100 Subject: [PATCH 6/9] generate-models: add default shell (in a different point) --- .github/workflows/generate_models.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/workflows/generate_models.yml b/.github/workflows/generate_models.yml index f22d0ee5..43c6779a 100644 --- a/.github/workflows/generate_models.yml +++ b/.github/workflows/generate_models.yml @@ -14,14 +14,15 @@ on: paths: - 'urdf/simmechanics/**' +defaults: + run: + shell: bash -el {0} + # A workflow run is made up of one or more jobs that can run sequentially or in parallel jobs: generate_models: # The type of runner that the job will run on runs-on: ubuntu-latest - defaults: - run: - shell: bash -el {0} # Steps represent a sequence of tasks that will be executed as part of the job steps: - uses: actions/checkout@v2 From 9940ef3e96b4e7c5c91fa953acdddc8027b6e05d Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 9 Jan 2023 15:08:00 +0100 Subject: [PATCH 7/9] generate-models: fix default shell --- .github/workflows/generate_models.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/generate_models.yml b/.github/workflows/generate_models.yml index 43c6779a..8514b1b6 100644 --- a/.github/workflows/generate_models.yml +++ b/.github/workflows/generate_models.yml @@ -34,7 +34,6 @@ jobs: channel-priority: true - name: Dependencies - shell: bash run: | # Workaround for https://github.com/conda-incubator/setup-miniconda/issues/186 conda config --remove channels defaults From f1bae1dd20f89b202ed117f645250588fb4b4b02 Mon Sep 17 00:00:00 2001 From: Nicogene Date: Mon, 9 Jan 2023 15:10:00 +0100 Subject: [PATCH 8/9] generate-models: fix missing conda deps --- .github/workflows/generate_models.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/generate_models.yml b/.github/workflows/generate_models.yml index 8514b1b6..0b40ccaf 100644 --- a/.github/workflows/generate_models.yml +++ b/.github/workflows/generate_models.yml @@ -38,7 +38,7 @@ jobs: # Workaround for https://github.com/conda-incubator/setup-miniconda/issues/186 conda config --remove channels defaults # Actual dependencies - mamba install python=3.8 yarp idyntree ruby + mamba install python=3.8 yarp idyntree ruby cmake compilers make - name: Generate models run: | From 7bb9b061d9e7d729e2c616ba292a0afd4b34b92c Mon Sep 17 00:00:00 2001 From: Nicogene Date: Tue, 10 Jan 2023 11:37:42 +0100 Subject: [PATCH 9/9] ergocub-model-test: re-enable ft sensor orientation check and some joints --- tests/ergocub-model-test.cpp | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/tests/ergocub-model-test.cpp b/tests/ergocub-model-test.cpp index 6a1a08cd..7710c8c6 100644 --- a/tests/ergocub-model-test.cpp +++ b/tests/ergocub-model-test.cpp @@ -271,10 +271,10 @@ bool checkAxisDirections(iDynTree::KinDynComputations & comp) expectedDirectionInRootLink.push_back(iDynTree::Direction(-1.62555e-21,-1.1e-15,1)); axisNames.push_back("r_shoulder_roll"); expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.961047,-0.271447,-0.0520081)); - // axisNames.push_back("r_shoulder_yaw"); - // expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.0713662,0.0619303,0.995526)); - // axisNames.push_back("r_elbow"); - // expectedDirectionInRootLink.push_back(iDynTree::Direction(0.267012,-0.960459,0.0788901)); + axisNames.push_back("r_shoulder_yaw"); + expectedDirectionInRootLink.push_back(iDynTree::Direction( -0.116648,0.227771,0.966702)); + axisNames.push_back("r_elbow"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(0.250563,-0.935113,0.250563)); axisNames.push_back("r_wrist_roll"); expectedDirectionInRootLink.push_back(iDynTree::Direction(0.961047,0.271447,0.0520081)); axisNames.push_back("r_wrist_pitch"); @@ -283,10 +283,10 @@ bool checkAxisDirections(iDynTree::KinDynComputations & comp) expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.116648,0.227771,0.966702)); axisNames.push_back("l_shoulder_roll"); expectedDirectionInRootLink.push_back(iDynTree::Direction(0.961047,-0.271447,0.0520081)); - // axisNames.push_back("l_shoulder_yaw"); - // expectedDirectionInRootLink.push_back(iDynTree::Direction(0.0713662,0.0619303,-0.995526)); - // axisNames.push_back("l_elbow"); - // expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.267012,-0.960459,-0.0788901)); + axisNames.push_back("l_shoulder_yaw"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.116648,-0.227771,0.966702)); + axisNames.push_back("l_elbow"); + expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.250563,-0.935113,-0.250563)); axisNames.push_back("l_wrist_roll"); expectedDirectionInRootLink.push_back(iDynTree::Direction(-0.961047,0.271447,-0.0520081)); axisNames.push_back("l_wrist_pitch"); @@ -391,13 +391,13 @@ bool checkFTSensorsAreCorrectlyOriented(iDynTree::KinDynComputations & comp) { iDynTree::Rotation rootLink_R_sensorFrameLeftArmExpected = - iDynTree::Rotation(-0.267012, -0.961047, 0.0713662, - -0.960459, 0.271447, 0.0619303, - -0.0788901, -0.0520081, -0.995526); + iDynTree::Rotation(-0.250563, -0.961047, 0.116648, + -0.935113, 0.271447, 0.227771, + -0.250563, -0.0520081, -0.966702); iDynTree::Rotation rootLink_R_sensorFrameRightArmExpected = - iDynTree::Rotation(-0.267012, 0.961047, 0.0713662, - 0.960459, 0.271447, -0.0619303, - -0.0788901, 0.0520081, -0.995526); + iDynTree::Rotation(-0.250563, 0.961047, 0.116648, + 0.935113, 0.271447, -0.227771, + -0.250563, 0.0520081, -0.966702); iDynTree::Rotation rootLink_R_sensorFrameExpectedFoot = iDynTree::Rotation(-0.5, 0.866025, 0, @@ -492,10 +492,10 @@ int main(int argc, char ** argv) return EXIT_FAILURE; } - // if (!checkFTSensorsAreCorrectlyOriented(comp)) - // { - // return EXIT_FAILURE; - // } + if (!checkFTSensorsAreCorrectlyOriented(comp)) + { + return EXIT_FAILURE; + } std::cerr << "Check for model " << modelPath << " concluded correctly!" << std::endl;