Skip to content

Commit

Permalink
Removed TestKinematics
Browse files Browse the repository at this point in the history
  • Loading branch information
@EnricoMingo
EnricoMingo committed Feb 9, 2024
1 parent 17b03c2 commit 4b714e2
Show file tree
Hide file tree
Showing 2 changed files with 47 additions and 70 deletions.
5 changes: 0 additions & 5 deletions tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -345,11 +345,6 @@ add_test(NAME OpenSoT_task_velocity_Postural COMMAND testPosturalVelocityTask)
# add_dependencies(testCoMForceTask OpenSoT)
# add_test(NAME OpenSoT_task_force_CoM COMMAND testCoMForceTask)

# ADD_EXECUTABLE(testKinematics utils/TestKinematics.cpp)
# TARGET_LINK_LIBRARIES(testKinematics ${TestLibs})
# add_dependencies(testKinematics OpenSoT)
# add_test(NAME OpenSoT_utils_test_kinematics COMMAND testKinematics)

# if(${qpSWIFT_FOUND})
# ADD_EXECUTABLE(testqpSWIFTSolver solvers/TestqpSWIFT.cpp)
# TARGET_LINK_LIBRARIES(testqpSWIFTSolver ${TestLibs} qpSWIFT::qpSWIFT-static)
Expand Down
112 changes: 47 additions & 65 deletions tests/utils/TestKinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,26 +2,18 @@
#include <memory>
#include <xbot2_interface/xbotinterface2.h>

#include "../common.h"



namespace{
class testKinematics: public ::testing::Test
class testKinematics: public TestBase
{
protected:
XBot::ModelInterface::Ptr _model_ptr;
std::string _path_to_cfg;

testKinematics()
testKinematics():TestBase("coman_floating_base")
{
std::string relative_path = OPENSOT_TEST_PATH "configs/coman/configs/config_coman_floating_base.yaml";

_path_to_cfg = relative_path;

_model_ptr = XBot::ModelInterface::getModel(_path_to_cfg);

if(_model_ptr)
std::cout<<"pointer address: "<<_model_ptr.get()<<std::endl;
else
std::cout<<"pointer is NULL "<<_model_ptr.get()<<std::endl;
}

virtual ~testKinematics() {
Expand Down Expand Up @@ -68,16 +60,6 @@ double getRandomAngle(const double min, const double max)
return (double)rand()/RAND_MAX * (max-min) + min;
}

Eigen::VectorXd getRandomAngles(const Eigen::VectorXd &min, const Eigen::VectorXd &max, const int size)
{
initializeIfNeeded();
Eigen::VectorXd q(size);
assert(min.size() >= size);
assert(max.size() >= size);
for(unsigned int i = 0; i < size; ++i)
q(i) = getRandomAngle(min[i],max[i]);
return q;
}

Eigen::Matrix3d skew(const Eigen::Vector3d& p)
{
Expand Down Expand Up @@ -122,12 +104,12 @@ TEST_F(testKinematics, testFloatingBaseJacobian)

//std::cout<<"# Joints: "<<_model_ptr->getJointNum()<<std::endl;

_model_ptr->setJointPosition(getRandomAngles(qmin, qmax, qmin.size()));
_model_ptr->setJointPosition(_model_ptr->generateRandomQ());
_model_ptr->update();

urdf::ModelInterface urdf_model = _model_ptr->getUrdf();
urdf::ModelConstSharedPtr urdf_model = _model_ptr->getUrdf();
std::vector<urdf::LinkSharedPtr> links;
urdf_model.getLinks(links);
urdf_model->getLinks(links);

std::string base_link = "Waist";
std::string distal_link = links[getRandomInt(0, links.size()-1)]->name; //"RSoftHand";
Expand Down Expand Up @@ -166,59 +148,59 @@ TEST_F(testKinematics, testFloatingBaseJacobian)

}

TEST_F(testKinematics, testRelativeJacobian)
{
for(unsigned int k = 0; k < 100; ++k)
{
Eigen::VectorXd qmin, qmax;
_model_ptr->getJointLimits(qmin, qmax);
//TEST_F(testKinematics, testRelativeJacobian)
//{
// for(unsigned int k = 0; k < 100; ++k)
// {
// Eigen::VectorXd qmin, qmax;
// _model_ptr->getJointLimits(qmin, qmax);

//std::cout<<"# Joints: "<<_model_ptr->getJointNum()<<std::endl;
// //std::cout<<"# Joints: "<<_model_ptr->getJointNum()<<std::endl;

_model_ptr->setJointPosition(getRandomAngles(qmin, qmax, qmin.size()));
_model_ptr->update();
// _model_ptr->setJointPosition(getRandomAngles(qmin, qmax, qmin.size()));
// _model_ptr->update();

urdf::ModelInterface urdf_model = _model_ptr->getUrdf();
std::vector<urdf::LinkSharedPtr> links;
urdf_model.getLinks(links);
// urdf::ModelInterface urdf_model = _model_ptr->getUrdf();
// std::vector<urdf::LinkSharedPtr> links;
// urdf_model.getLinks(links);

std::string base_link = links[getRandomInt(0, links.size()-1)]->name; //"LSoftHand";
std::string distal_link = links[getRandomInt(0, links.size()-1)]->name; //"RSoftHand";
//std::cout<<"base_link = "<<base_link<<" distal_link = "<<distal_link<<std::endl;
// std::string base_link = links[getRandomInt(0, links.size()-1)]->name; //"LSoftHand";
// std::string distal_link = links[getRandomInt(0, links.size()-1)]->name; //"RSoftHand";
// //std::cout<<"base_link = "<<base_link<<" distal_link = "<<distal_link<<std::endl;

Eigen::MatrixXd J; _model_ptr->getRelativeJacobian(distal_link, base_link, J); //Jacobian of distal_link in base_link expressed in base_link
// Eigen::MatrixXd J; _model_ptr->getRelativeJacobian(distal_link, base_link, J); //Jacobian of distal_link in base_link expressed in base_link

//std::cout<<"Jacobian of "<<distal_link<<" wrt "<<base_link<<" expressed in "<<base_link<<": "<<std::endl<<J<<std::endl;
// //std::cout<<"Jacobian of "<<distal_link<<" wrt "<<base_link<<" expressed in "<<base_link<<": "<<std::endl<<J<<std::endl;


Eigen::Affine3d wTb; _model_ptr->getPose(base_link, wTb); //Pose of base_link in world
Eigen::Affine3d wTd; _model_ptr->getPose(distal_link, wTd); //Pose of distal_link in world
// Eigen::Affine3d wTb; _model_ptr->getPose(base_link, wTb); //Pose of base_link in world
// Eigen::Affine3d wTd; _model_ptr->getPose(distal_link, wTd); //Pose of distal_link in world

Eigen::MatrixXd Jdistal; _model_ptr->getJacobian(distal_link, Jdistal); //Jacobian of distal_link expressed in world
Eigen::MatrixXd Jbase; _model_ptr->getJacobian(base_link, Jbase); //Jacobian of base_link expressed in world
// Eigen::MatrixXd Jdistal; _model_ptr->getJacobian(distal_link, Jdistal); //Jacobian of distal_link expressed in world
// Eigen::MatrixXd Jbase; _model_ptr->getJacobian(base_link, Jbase); //Jacobian of base_link expressed in world

Eigen::Matrix6d Adj1; Adj1.setZero();
Adj1.block(0,0,3,3) = wTb.linear().transpose();
Adj1.block(3,3,3,3) = wTb.linear().transpose();
Eigen::MatrixXd J1 = Adj1*Jdistal; //Jacobian of distal_link expressed in base_link
//std::cout<<"J1: "<<std::endl<<J1<<std::endl;
// Eigen::Matrix6d Adj1; Adj1.setZero();
// Adj1.block(0,0,3,3) = wTb.linear().transpose();
// Adj1.block(3,3,3,3) = wTb.linear().transpose();
// Eigen::MatrixXd J1 = Adj1*Jdistal; //Jacobian of distal_link expressed in base_link
// //std::cout<<"J1: "<<std::endl<<J1<<std::endl;

Eigen::Vector3d p = wTd.translation() - wTb.translation(); //relative position between distal_link and base_link expressed in world
// Eigen::Vector3d p = wTd.translation() - wTb.translation(); //relative position between distal_link and base_link expressed in world

Eigen::Matrix6d Adj2; Adj2.setZero();
Adj2.block(0,0,3,3) = wTb.linear().transpose();
Adj2.block(0,3,3,3) = -wTb.linear().transpose()*skew(p);
Adj2.block(3,3,3,3) = wTb.linear().transpose();
//std::cout<<"Adj2: "<<std::endl<<Adj2<<std::endl;
Eigen::MatrixXd J2 = Adj2*Jbase; //Jacobian of base_link expressed in base_link computed at distal_link
//std::cout<<"J2: "<<std::endl<<J2<<std::endl;
// Eigen::Matrix6d Adj2; Adj2.setZero();
// Adj2.block(0,0,3,3) = wTb.linear().transpose();
// Adj2.block(0,3,3,3) = -wTb.linear().transpose()*skew(p);
// Adj2.block(3,3,3,3) = wTb.linear().transpose();
// //std::cout<<"Adj2: "<<std::endl<<Adj2<<std::endl;
// Eigen::MatrixXd J2 = Adj2*Jbase; //Jacobian of base_link expressed in base_link computed at distal_link
// //std::cout<<"J2: "<<std::endl<<J2<<std::endl;

Eigen::MatrixXd JR = J1 - J2;
//std::cout<<"Computed Jacobian of "<<distal_link<<" wrt "<<base_link<<" expressed in "<<base_link<<": "<<std::endl<<JR<<std::endl;
// Eigen::MatrixXd JR = J1 - J2;
// //std::cout<<"Computed Jacobian of "<<distal_link<<" wrt "<<base_link<<" expressed in "<<base_link<<": "<<std::endl<<JR<<std::endl;

EXPECT_MATRIX_NEAR(J, JR, 1e-9);
}
}
// EXPECT_MATRIX_NEAR(J, JR, 1e-9);
// }
//}

}

Expand Down

0 comments on commit 4b714e2

Please sign in to comment.