Skip to content

Commit

Permalink
Merge pull request #26 from roahmlab/unit_test_dev
Browse files Browse the repository at this point in the history 
Unit test dev
  • Loading branch information
@Cfather
Cfather authored Oct 4, 2024
2 parents 395b81a + a86cea0 commit 09672db
Show file tree
Hide file tree
Showing 7 changed files with 171 additions and 165 deletions.
9 changes: 9 additions & 0 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,15 @@ if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()

include(CTest)
set(CTEST_OUTPUT_ON_FAILURE TRUE)
set(CTEST_CUSTOM_TESTS_IGNORE "")

set(CTEST_JUNIT_OUTPUT_PATH "${CMAKE_BINARY_DIR}/TestResults")
set(CTEST_CUSTOM_TESTS_IGNORE "")

enable_testing()

# set(CMAKE_CXX_FLAGS "-Wall -Wextra")
set(CMAKE_CXX_FLAGS_DEBUG "-g -fopenmp")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fopenmp")
Expand Down
73 changes: 17 additions & 56 deletions Tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
# Define common libraries
set(COMMON_LIBRARIES
trajlib
IDlib
Expand All @@ -9,67 +10,27 @@ set(COMMON_LIBRARIES
coinhsl
)

add_executable(ForwardKinematics_test
# Define a function to add tests
function(add_raptor_test test_name source_file)
add_executable(${test_name} ${source_file})
target_link_libraries(${test_name} PRIVATE ${COMMON_LIBRARIES})
target_compile_options(${test_name} PRIVATE ${PINOCCHIO_FLAGS})
add_test(NAME ${test_name} COMMAND ${test_name})
set_tests_properties(${test_name} PROPERTIES WORKING_DIRECTORY "${CMAKE_BINARY_DIR}")
endfunction()

# Add individual tests
add_raptor_test(ForwardKinematics_test
KinematicsDynamics/TestForwardKinematicsSolver.cpp)

target_link_libraries(ForwardKinematics_test PUBLIC
IDlib
${BOOST_LIBRARIES}
pinocchio::pinocchio)

target_compile_options(ForwardKinematics_test PUBLIC
${PINOCCHIO_FLAGS})

add_executable(ForwardKinematicsGradient_test
add_raptor_test(ForwardKinematicsGradient_test
KinematicsDynamics/TestForwardKinematicsRPYDerivatives.cpp)

target_link_libraries(ForwardKinematicsGradient_test PUBLIC
IDlib
Optlib
Conslib
ipopt
coinhsl
${BOOST_LIBRARIES}
pinocchio::pinocchio)

target_compile_options(ForwardKinematicsGradient_test PUBLIC
${PINOCCHIO_FLAGS})

add_executable(KinematicsConstraints_test
add_raptor_test(KinematicsConstraints_test
Constraints/TestKinematicsConstraints.cpp)

target_link_libraries(KinematicsConstraints_test PUBLIC
trajlib
IDlib
Conslib
${BOOST_LIBRARIES}
pinocchio::pinocchio)

target_compile_options(KinematicsConstraints_test PUBLIC
${PINOCCHIO_FLAGS})

add_executable(CustomizedInverseDynamics_test
add_raptor_test(CustomizedInverseDynamics_test
KinematicsDynamics/TestCustomizedInverseDynamics.cpp)

target_link_libraries(CustomizedInverseDynamics_test PUBLIC
trajlib
IDlib
Conslib
${BOOST_LIBRARIES}
pinocchio::pinocchio)

target_compile_options(CustomizedInverseDynamics_test PUBLIC
${PINOCCHIO_FLAGS})

add_executable(RegressorInverseDynamics_test

add_raptor_test(RegressorInverseDynamics_test
KinematicsDynamics/TestRegressorInverseDynamics.cpp)

target_link_libraries(RegressorInverseDynamics_test PUBLIC
trajlib
IDlib
Conslib
${BOOST_LIBRARIES}
pinocchio::pinocchio)

target_compile_options(RegressorInverseDynamics_test PUBLIC
${PINOCCHIO_FLAGS})
85 changes: 54 additions & 31 deletions Tests/Constraints/TestKinematicsConstraints.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,24 @@
#define BOOST_TEST_MODULE KinematicsConstraintsTest
#include <boost/test/included/unit_test.hpp>

#include "KinematicsConstraints.h"
// #include "Plain.h"
#include "Polynomials.h"
#include <chrono>

using namespace RAPTOR;

int main() {
BOOST_AUTO_TEST_SUITE(KinematicsConstraintsTest)

// test gradient
BOOST_AUTO_TEST_CASE(owngradientTest)
{
// Define robot model
// const std::string urdf_filename = "../Robots/digit-v3/digit-v3-armfixedspecific-floatingbase-springfixed.urdf";
const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf";

pinocchio::Model model;
pinocchio::urdf::buildModel(urdf_filename, model);

// std::shared_ptr<Trajectories> trajPtr_ = std::make_shared<Plain>(model.nv);
std::shared_ptr<Trajectories> trajPtr_ = std::make_shared<Polynomials>(2.0, 10, model.nv, TimeDiscretization::Chebyshev, 3);
ForwardKinematicsSolver fkSolver(&model);

Expand All @@ -26,57 +31,75 @@ int main() {

KinematicsConstraints kc(trajPtr_, &model, end, 6, fkSolver.getTransform());

// simple test when difference is small
Eigen::VectorXd z_test = z.array() + 1e-6;
kc.compute(z_test, false);
std::cout << kc.g << std::endl << std::endl;

// simple test when difference is large
z_test = z.array() + 1.0;
auto start_clock = std::chrono::high_resolution_clock::now();
kc.compute(z_test, true, true);
auto end_clock = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end_clock - start_clock);
std::cout << "Time taken (including gradient and hessian): " << duration.count() << " microseconds" << std::endl;
kc.compute(z, true, false);

std::cout << kc.g << std::endl << std::endl;

// test gradient
const Eigen::MatrixXd J_analytical = kc.pg_pz;
Eigen::MatrixXd J_numerical = Eigen::MatrixXd::Zero(J_analytical.rows(), J_analytical.cols());
for (int i = 0; i < z_test.size(); i++) {
Eigen::VectorXd q_plus = z_test;
for (int i = 0; i < z.size(); i++) {
Eigen::VectorXd q_plus = z;
q_plus(i) += 1e-7;
kc.compute(q_plus, false);
const Eigen::VectorXd g_plus = kc.g;
Eigen::VectorXd q_minus = z_test;
Eigen::VectorXd q_minus = z;
q_minus(i) -= 1e-7;
kc.compute(q_minus, false);
const Eigen::VectorXd g_minus = kc.g;
J_numerical.col(i) = (g_plus - g_minus) / 2e-7;
}

// std::cout << "Analytical gradient: " << std::endl << J_analytical << std::endl << std::endl;
// std::cout << "Numerical gradient: " << std::endl << J_numerical << std::endl << std::endl;
std::cout << J_analytical - J_numerical << std::endl << std::endl;
BOOST_CHECK_SMALL((J_analytical - J_numerical).norm(), 1e-6);
}

// test hessian
BOOST_AUTO_TEST_CASE(ownHessianTest){
const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf";

pinocchio::Model model;
pinocchio::urdf::buildModel(urdf_filename, model);

// std::shared_ptr<Trajectories> trajPtr_ = std::make_shared<Plain>(model.nv);
std::shared_ptr<Trajectories> trajPtr_ = std::make_shared<Polynomials>(2.0, 10, model.nv, TimeDiscretization::Chebyshev, 3);
ForwardKinematicsSolver fkSolver(&model);

// compute a valid transform using forward kinematics
std::srand(std::time(nullptr));
Eigen::VectorXd z = 2 * M_PI * Eigen::VectorXd::Random(trajPtr_->varLength).array() - M_PI;
int start = 0;
int end = model.getJointId("joint_7");
fkSolver.compute(start, end, z);

KinematicsConstraints kc(trajPtr_, &model, end, 6, fkSolver.getTransform());

kc.compute(z, true, true);

// test hessian
Eigen::Array<Eigen::MatrixXd, 1, Eigen::Dynamic> H_analytical = kc.pg_pz_pz;
for (int i = 0; i < z_test.size(); i++) {
Eigen::VectorXd q_plus = z_test;

// params for error checking
bool hasError = false;
double max_diff = 0.0;
std::stringstream error_message;

for (int i = 0; i < z.size(); i++) {
Eigen::VectorXd q_plus = z;
q_plus(i) += 1e-7;
kc.compute(q_plus, true, false);
const Eigen::MatrixXd J_plus = kc.pg_pz;
Eigen::VectorXd q_minus = z_test;
Eigen::VectorXd q_minus = z;
q_minus(i) -= 1e-7;
kc.compute(q_minus, true, false);
const Eigen::MatrixXd J_minus = kc.pg_pz;
const Eigen::MatrixXd H_numerical_row = (J_plus - J_minus) / 2e-7;

for (int j = 0; j < 3; j++) {
std::cout << H_analytical(j).row(i) - H_numerical_row.row(j) << std::endl;
double diff = (H_analytical(j).row(i) - H_numerical_row.row(j) ).norm();
if (diff > 1e-6){
hasError = true;
if (diff >max_diff) max_diff = diff;
error_message << "error found at i=" << i << ", j=" << j
<< " with difference: " << diff << "\n";
}
}
}

return 0;
BOOST_CHECK_MESSAGE(!hasError, "Hessian discrepancies found:\n" + error_message.str());
}
BOOST_AUTO_TEST_SUITE_END()
65 changes: 33 additions & 32 deletions Tests/KinematicsDynamics/TestCustomizedInverseDynamics.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,6 @@
#define BOOST_TEST_MODULE CustomizedInverseDynamicsTest
#include <boost/test/included/unit_test.hpp>

#include "pinocchio/algorithm/model.hpp"

#include "CustomizedInverseDynamics.h"
Expand All @@ -6,7 +9,10 @@

using namespace RAPTOR;

int main() {
BOOST_AUTO_TEST_SUITE(InverseDynamicsTestSuite)
// Test without fixed joints
BOOST_AUTO_TEST_CASE(test_inverse_dynamics_without_fixed_joints)
{
// define robot model
const std::string urdf_filename = "../Robots/kinova-gen3/kinova_grasp.urdf";

Expand All @@ -28,33 +34,38 @@ int main() {
Eigen::VectorXd z = Eigen::VectorXd::Random(trajPtr->varLength);
trajPtr->compute(z);

// Test without fixed joints
// compute inverse dynamics using pinocchio
auto start_clock = std::chrono::high_resolution_clock::now();
for (int i = 0; i < trajPtr->N; i++) {
pinocchio::rnea(model, data,
trajPtr->q(0), trajPtr->q_d(0), trajPtr->q_dd(0));
}
auto stop_clock = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::nanoseconds>(stop_clock - start_clock);
std::cout << "Pinocchio ID: " << duration.count() << " nanoseconds" << std::endl;

// compute inverse dynamics using RAPTOR
std::shared_ptr<CustomizedInverseDynamics> cidPtr =
std::make_shared<CustomizedInverseDynamics>(
model, trajPtr);

start_clock = std::chrono::high_resolution_clock::now();
cidPtr->compute(z, false);
stop_clock = std::chrono::high_resolution_clock::now();
duration = std::chrono::duration_cast<std::chrono::nanoseconds>(stop_clock - start_clock);
std::cout << "RAPTOR ID: " << duration.count() << " nanoseconds" << std::endl;

// compare the results
std::cout << "Pinocchio: " << data.tau.transpose() << std::endl;
std::cout << "RAPTOR: " << cidPtr->tau(0).transpose() << std::endl;
//check the error
BOOST_CHECK_SMALL((data.tau - cidPtr->tau(0)).norm(), 1e-10);
}

// Test with fixed joints
BOOST_AUTO_TEST_CASE(test_inverse_dynamics_with_fixed_joints)
{
// define robot model
const std::string urdf_filename = "../Robots/kinova-gen3/kinova_grasp.urdf";

pinocchio::Model model;
pinocchio::urdf::buildModel(urdf_filename, model);

model.rotorInertia.setZero();
model.damping.setZero();
model.friction.setZero();

std::shared_ptr<Trajectories> trajPtr =
std::make_shared<BezierCurves>(
Eigen::VectorXd::LinSpaced(5, 0, 1), model.nq, 5);

Eigen::VectorXi jtype = convertPinocchioJointType(model);
jtype(jtype.size() - 1) = 0; // fix the last joint

Expand All @@ -63,35 +74,25 @@ int main() {
pinocchio::buildReducedModel(model, list_of_joints_to_lock_by_id, Eigen::VectorXd::Zero(model.nv), model_reduced);
pinocchio::Data data_reduced(model_reduced);

z = Eigen::VectorXd::Random(trajPtr->varLength);
Eigen::VectorXd z = Eigen::VectorXd::Random(trajPtr->varLength);

trajPtr = std::make_shared<BezierCurves>(
Eigen::VectorXd::LinSpaced(5, 0, 1), model_reduced.nq, 5);
trajPtr->compute(z);

// compute inverse dynamics using pinocchio
start_clock = std::chrono::high_resolution_clock::now();
for (int i = 0; i < trajPtr->N; i++) {
pinocchio::rnea(model_reduced, data_reduced,
trajPtr->q(0).head(model_reduced.nq),
trajPtr->q_d(0).head(model_reduced.nv),
trajPtr->q_dd(0).head(model_reduced.nv));
}
stop_clock = std::chrono::high_resolution_clock::now();
duration = std::chrono::duration_cast<std::chrono::nanoseconds>(stop_clock - start_clock);
std::cout << "Pinocchio ID (fixed joints): " << duration.count() << " nanoseconds" << std::endl;

// compute inverse dynamics using RAPTOR
cidPtr = std::make_shared<CustomizedInverseDynamics>(
std::shared_ptr<CustomizedInverseDynamics> cidPtr = std::make_shared<CustomizedInverseDynamics>(
model, trajPtr, jtype);
auto start = std::chrono::high_resolution_clock::now();
cidPtr->compute(z, true);
auto end = std::chrono::high_resolution_clock::now();
duration = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);
std::cout << "RAPTOR ID (fixed joints): " << duration.count() << " nanoseconds" << std::endl;

// compare the results
std::cout << "Pinocchio (fixed joints): " << data_reduced.tau.transpose() << std::endl;
std::cout << "RAPTOR (fixed joints): " << cidPtr->tau(0).transpose() << std::endl;

return 0;
}
//check the error
BOOST_CHECK_SMALL((data_reduced.tau - cidPtr->tau(0)).norm(), 1e-10);
}
BOOST_AUTO_TEST_SUITE_END()
Loading

0 comments on commit 09672db

Please sign in to comment.