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

Fix strange use of angular linear serialization in TorqueEstimationTree #124

Merged
merged 3 commits into from
Jan 7, 2016
Merged

Fix strange use of angular linear serialization in TorqueEstimationTree #124

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
cd248c6
[yarp] Added failing test on KDLtoYarp_twist and KDLtoYarp_wrench met…
traversaro Jan 7, 2016
cd3c99c
[yarp] Fix the angular-linear serialization used in TorqueEstimation
traversaro Jan 7, 2016
732d582
Fix CMake if iCub is used from the build directory
Jan 7, 2016
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
1 change: 1 addition & 0 deletions src/icub/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ target_link_libraries(idyntree-icub LINK_PUBLIC skinDynLib iKin iDyn
target_include_directories(idyntree-icub INTERFACE ${skinDynLib_INCLUDE_DIRS})

include_directories(SYSTEM ${skinDynLib_INCLUDE_DIRS}
${iDyn_INCLUDE_DIRS}
${GSL_INCLUDE_DIRS}
${YARP_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
Expand Down
8 changes: 4 additions & 4 deletions src/icub/src/TorqueEstimationTree.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -663,8 +663,8 @@ void TorqueEstimationTree::buildAb_contacts()
{
//std::cout << " setting b_contact [ " << getSubGraphIndex(link_nmbr) << " ] ";
//std::cout << "to " << b_contacts_subtree[link_nmbr] << std::endl;
b_contacts[getSubGraphIndex(link_nmbr)].setSubvector(0,KDLtoYarp(b_contacts_subtree[link_nmbr].torque));
b_contacts[getSubGraphIndex(link_nmbr)].setSubvector(3,KDLtoYarp(b_contacts_subtree[link_nmbr].force));
b_contacts[getSubGraphIndex(link_nmbr)].setSubvector(0,KDLtoYarp(b_contacts_subtree[link_nmbr].force));
b_contacts[getSubGraphIndex(link_nmbr)].setSubvector(3,KDLtoYarp(b_contacts_subtree[link_nmbr].torque));
//std::cout << " b_contacts [ " << getSubGraphIndex(link_nmbr) << " ] is " << b_contacts[getSubGraphIndex(link_nmbr)].toString() << std::endl;
}

Expand Down Expand Up @@ -848,10 +848,10 @@ void TorqueEstimationTree::store_contacts_results()
} else {
//6 UNKNOWN
assert( unknownInd+2 < x_contacts[sg].size() );
it->setMoment(x_contacts[sg].subVector(unknownInd, unknownInd+2));
it->setForce(x_contacts[sg].subVector(unknownInd, unknownInd+2));
unknownInd += 3;
assert( unknownInd+2 < x_contacts[sg].size() );
it->setForce(x_contacts[sg].subVector(unknownInd, unknownInd+2));
it->setMoment(x_contacts[sg].subVector(unknownInd, unknownInd+2));
unknownInd += 3;

}
Expand Down
64 changes: 64 additions & 0 deletions src/icub/tests/AdjointMatricesConsistencyTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,64 @@
/*
* Copyright (C) 2016 Fondazione Istituto Italiano di Tecnologia
* Author: Silvio Traversaro <silvio.traversaro@iit.it>
*
* CopyPolicy: Released under the terms of the LGPLv2.1 or later, see LGPL.TXT
*/

#include <iDynTree/Core/MatrixFixSize.h>

#include <iDynTree/Core/TestUtils.h>
#include <iDynTree/Core/EigenHelpers.h>

#include <kdl_codyco/KDLConversions.h>
#include <kdl_codyco/regressor_utils.hpp>

#include <iCub/iDynTree/yarp_kdl.h>

#include <iDynTree/Core/Transform.h>

#include <yarp/sig/Matrix.h>

using namespace iDynTree;

int main()
{
// Create a random iDynTree frame
Transform trans = getRandomTransform();

Matrix6x6 adj_idyntree = trans.asAdjointTransform();
yarp::sig::Matrix adj_yarp_kdl_inyarp = KDLtoYarp_twist(ToKDL(trans));
Matrix6x6 adj_yarp_kdl;
toEigen(adj_yarp_kdl) = Eigen::Map<Eigen::Matrix<double,6,6,Eigen::RowMajor> >(adj_yarp_kdl_inyarp.data());
Matrix6x6 adj_eigen_kdl;
toEigen(adj_eigen_kdl) = KDL::CoDyCo::TwistTransformationMatrix(ToKDL(trans));

std::cerr << "Adjoint matrix: "<< std::endl;
std::cerr << "iDynTree : \n" << adj_idyntree.toString() << std::endl;
std::cerr << "YARP : \n" << adj_yarp_kdl.toString() << std::endl;
std::cerr << "Eigen : \n" << adj_eigen_kdl.toString() << std::endl;

Matrix6x6 adj_wrench_idyntree = trans.asAdjointTransformWrench();
yarp::sig::Matrix adj_wrench_yarp_kdl_inyarp = KDLtoYarp_wrench(ToKDL(trans));
Matrix6x6 adj_wrench_yarp_kdl;
toEigen(adj_wrench_yarp_kdl) = Eigen::Map<Eigen::Matrix<double,6,6,Eigen::RowMajor> >(adj_wrench_yarp_kdl_inyarp.data());

Matrix6x6 adj_wrench_eigen_kdl;
toEigen(adj_wrench_eigen_kdl) = KDL::CoDyCo::WrenchTransformationMatrix(ToKDL(trans));

std::cerr << "Wrench Adjoint matrix: "<< std::endl;
std::cerr << "iDynTree : \n" << adj_wrench_idyntree.toString() << std::endl;
std::cerr << "YARP : \n" << adj_wrench_yarp_kdl.toString() << std::endl;
std::cerr << "Eigen : \n" << adj_wrench_eigen_kdl.toString() << std::endl;

ASSERT_EQUAL_MATRIX(adj_idyntree,adj_yarp_kdl);
ASSERT_EQUAL_MATRIX(adj_idyntree,adj_eigen_kdl);

ASSERT_EQUAL_MATRIX(adj_wrench_idyntree,adj_wrench_yarp_kdl);
ASSERT_EQUAL_MATRIX(adj_wrench_idyntree,adj_wrench_eigen_kdl);

return EXIT_SUCCESS;
}



1 change: 1 addition & 0 deletions src/icub/tests/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,3 +27,4 @@ add_icub_test(iDynTreeWholeBodyFTsensorEstimate)
add_icub_test(iDynTreeExportFixedJointsFrames)
add_icub_test(iDynTreeExternalWrenchesEstimation)
add_icub_test(iDynTreeiCubExternalWrenchesEstimation)
add_icub_test(AdjointMatricesConsistency)
4 changes: 2 additions & 2 deletions src/yarp/src/yarp_kdl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -158,7 +158,7 @@ bool KDLtoYarp_twist(const KDL::Frame & kdlFrame, yarp::sig::Matrix & yarpMatrix

yarpMatrix6_6.setSubmatrix(R,0,0);
yarpMatrix6_6.setSubmatrix(R,3,3);
yarpMatrix6_6.setSubmatrix(yarp::math::crossProductMatrix(p)*R,3,0);
yarpMatrix6_6.setSubmatrix(yarp::math::crossProductMatrix(p)*R,0,3);

return true;
}
Expand All @@ -184,7 +184,7 @@ bool KDLtoYarp_wrench(const KDL::Frame &kdlFrame, yarp::sig::Matrix & yarpMatrix

yarpMatrix6_6.setSubmatrix(R,0,0);
yarpMatrix6_6.setSubmatrix(R,3,3);
yarpMatrix6_6.setSubmatrix(yarp::math::crossProductMatrix(p)*R,0,3);
yarpMatrix6_6.setSubmatrix(yarp::math::crossProductMatrix(p)*R,3,0);

return true;
}
Expand Down