diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a8291ca..23cbbe25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required (VERSION 2.8.11) +cmake_minimum_required (VERSION 3.16.3) project(wbc) include(FindPkgConfig) @@ -7,15 +7,19 @@ set(PROJECT_VERSION 0.2) set(API_VERSION ${PROJECT_VERSION}) list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -if(USE_HYRODYN AND USE_PINOCCHIO AND USE_KDL AND USE_EIQUADPROG AND USE_QPSWIFT AND USE_PROXQP) - message("Detected Full install, adding benchmark folder") - add_subdirectory(benchmarks) -endif() + +option(ROBOT_MODEL_RBDL "Also build the RBDL-based robot model, by default only pinocchio is built" OFF) +option(ROBOT_MODEL_KDL "Also build the KDL-based robot model, by default only pinocchio is built" OFF) +option(ROBOT_MODEL_HYRODYN "Also build the HyRoDyn-based robot model, by default only pinocchio is built" OFF) +option(SOLVER_PROXQP "Build the ProxQP-based solver, by default only hls and qpoases are built" OFF) +option(SOLVER_EIQUADPROG "Build the Eiquadprog-based solver, by default only hls and qpoases are built" OFF) +option(SOLVER_QPSWIFT "Build the QPSwift-based solver, by default only hls and qpoases are built" OFF) + add_subdirectory(src) -add_subdirectory(test) add_subdirectory(tutorials) -add_subdirectory(bin) + find_package(Doxygen) + if(DOXYGEN_FOUND) set(doxyfile_in ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile) set(doxyfile ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) diff --git a/benchmarks/CMakeLists.txt b/benchmarks/CMakeLists.txt deleted file mode 100644 index 95fc5260..00000000 --- a/benchmarks/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -find_package(Boost COMPONENTS system filesystem REQUIRED) - -add_subdirectory(robot_models) -add_subdirectory(scenes) -if(USE_QPSWIFT AND USE_EIQUADPROG AND USE_PROXQP) - add_subdirectory(solvers) -endif() diff --git a/benchmarks/benchmarks_common.cpp b/benchmarks/benchmarks_common.cpp deleted file mode 100644 index f92a4b99..00000000 --- a/benchmarks/benchmarks_common.cpp +++ /dev/null @@ -1,166 +0,0 @@ -#include "benchmarks_common.hpp" -#include -#include - -using namespace std; - -namespace wbc{ - -double stdDev(const base::VectorXd& vec){ - return sqrt((vec.array() - vec.mean()).square().sum() / (vec.size() - 1)); -} - -double whiteNoise(const double std_dev) -{ - double rand_no = ( rand() / ( (double)RAND_MAX ) ); - while( rand_no == 0 ) - rand_no = ( rand() / ( (double)RAND_MAX ) ); - - double tmp = cos( ( 2.0 * (double)M_PI ) * rand() / ( (double)RAND_MAX ) ); - return std_dev * sqrt( -2.0 * log( rand_no ) ) * tmp; -} - -base::samples::Joints randomJointState(base::samples::Joints joint_state_in){ - base::samples::Joints joint_state; - joint_state.resize(joint_state_in.size()); - joint_state.names = joint_state_in.names; - for(auto n : joint_state.names){ - joint_state[n].position = joint_state_in[n].position + whiteNoise(1e-4); - joint_state[n].speed = whiteNoise(1e-4); - joint_state[n].acceleration = whiteNoise(1e-4); - } - joint_state.time = base::Time::now(); - - return joint_state; -} - -base::samples::Joints randomJointState(base::JointLimits limits){ - base::samples::Joints joint_state; - joint_state.resize(limits.size()); - joint_state.names = limits.names; - for(auto n : joint_state.names){ - joint_state[n].position = (double(rand())/RAND_MAX)* (limits[n].max.position - limits[n].min.position) + limits[n].min.position; - joint_state[n].speed = whiteNoise(1e-4); - joint_state[n].acceleration = whiteNoise(1e-4); - } - joint_state.time = base::Time::now(); - - return joint_state; -} - -base::samples::RigidBodyStateSE3 randomFloatingBaseState(){ - base::samples::RigidBodyStateSE3 floating_base_state; - floating_base_state.pose.position = base::Vector3d(double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX); - floating_base_state.pose.orientation = Eigen::AngleAxisd(double(rand())/RAND_MAX, Eigen::Vector3d::UnitX()) - * Eigen::AngleAxisd(double(rand())/RAND_MAX, Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(double(rand())/RAND_MAX, Eigen::Vector3d::UnitZ()); - floating_base_state.twist.linear = base::Vector3d(double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX); - floating_base_state.twist.angular = base::Vector3d(double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX); - floating_base_state.acceleration.linear = base::Vector3d(double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX); - floating_base_state.acceleration.angular = base::Vector3d(double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX); - floating_base_state.time = base::Time::now(); - floating_base_state.frame_id = "world"; - return floating_base_state; -} - - -base::samples::RigidBodyStateSE3 randomFloatingBaseState(base::samples::RigidBodyStateSE3 floating_base_state_in){ - base::samples::RigidBodyStateSE3 floating_base_state; - floating_base_state.pose.position = base::Vector3d(floating_base_state_in.pose.position[0]+whiteNoise(1e-4), - floating_base_state_in.pose.position[1]+whiteNoise(1e-4), - floating_base_state_in.pose.position[2]+whiteNoise(1e-4)); - floating_base_state.pose.orientation = Eigen::AngleAxisd(base::getEuler(floating_base_state_in.pose.orientation)[0] + whiteNoise(1e-4), Eigen::Vector3d::UnitX()) - * Eigen::AngleAxisd(base::getEuler(floating_base_state_in.pose.orientation)[1] + whiteNoise(1e-4), Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(base::getEuler(floating_base_state_in.pose.orientation)[2] + whiteNoise(1e-4), Eigen::Vector3d::UnitZ()); - floating_base_state.twist.linear = base::Vector3d(whiteNoise(1e-4), - whiteNoise(1e-4), - whiteNoise(1e-4)); - floating_base_state.twist.angular = base::Vector3d(whiteNoise(1e-4), - whiteNoise(1e-4), - whiteNoise(1e-4)); - floating_base_state.acceleration.linear = base::Vector3d( whiteNoise(1e-4), - whiteNoise(1e-4), - whiteNoise(1e-4)); - floating_base_state.acceleration.angular = base::Vector3d(whiteNoise(1e-4), - whiteNoise(1e-4), - whiteNoise(1e-4)); - floating_base_state.time = base::Time::now(); - floating_base_state.frame_id = "world"; - return floating_base_state; -} - -base::samples::RigidBodyStateSE3 randomConstraintReference(){ - base::samples::RigidBodyStateSE3 ref; - for(int i = 0; i < 3; i++){ - ref.twist.linear[i] = whiteNoise(1e-4); - ref.twist.angular[i] = whiteNoise(1e-4); - ref.acceleration.linear[i] = whiteNoise(1e-4); - ref.acceleration.angular[i] = whiteNoise(1e-4); - } - ref.time = base::Time::now(); - return ref; -} - - -map evaluateWBCSceneRandom(WbcScenePtr scene, int n_samples){ - - base::VectorXd time_scene_update(n_samples); - base::VectorXd time_scene_solve(n_samples); - for(int i = 0; i <= n_samples; i++){ - for(auto w : scene->getWbcConfig()) - scene->setReference(w.name, randomConstraintReference()); - - RobotModelPtr robot_model = scene->getRobotModel(); - base::samples::Joints joint_state = randomJointState(robot_model->jointState(robot_model->independentJointNames())); - base::samples::RigidBodyStateSE3 floating_base_state = randomFloatingBaseState(scene->getRobotModel()->floatingBaseState()); - scene->getRobotModel()->update(joint_state, floating_base_state); - usleep(0.01*1e6); - - if(i==0){ - HierarchicalQP qp = scene->update(); - scene->solve(qp); - } - else{ - base::Time start = base::Time::now(); - HierarchicalQP qp = scene->update(); - time_scene_update[i-1] = (double)(base::Time::now()-start).toMicroseconds()/1000; - - try{ - start = base::Time::now(); - scene->solve(qp); - time_scene_solve[i-1] = (double)(base::Time::now()-start).toMicroseconds()/1000; - } - catch(exception e){ - i--; - continue; - } - } - usleep(0.01*1e6); - } - - map results; - results["scene_update"] = time_scene_update; - results["scene_solve"] = time_scene_solve; - return results; -} - -void toCSV(map res, const string &filename){ - std::fstream myfile; - myfile.open(filename, ios_base::out | ios_base::app); - if(!myfile.is_open()) - throw std::runtime_error("Error opening file " + filename); - - if(myfile.tellg() == 0){ - for(auto it : res) - myfile << it.first << " "; - myfile << "\n"; - } - - for(int i = 0; i < res.begin()->second.size(); i++){ - for(auto it : res) - myfile << res[it.first][i] << " "; - myfile << "\n"; - } -} - -} diff --git a/benchmarks/benchmarks_common.hpp b/benchmarks/benchmarks_common.hpp deleted file mode 100644 index c9c88bac..00000000 --- a/benchmarks/benchmarks_common.hpp +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include -#include -#include -#include - -namespace wbc{ - -double stdDev(const base::VectorXd& vec); -double whiteNoise(const double std_dev); -base::samples::Joints randomJointState(base::samples::Joints joint_state_in); -base::samples::Joints randomJointState(base::JointLimits limits); -base::samples::RigidBodyStateSE3 randomFloatingBaseState(base::samples::RigidBodyStateSE3 floating_base_state_in); -base::samples::RigidBodyStateSE3 randomFloatingBaseState(); -void toCSV(std::map res, const std::string &filename); -base::samples::RigidBodyStateSE3 randomConstraintReference(); -std::map evaluateWBCSceneRandom(WbcScenePtr scene, int n_samples); - -} diff --git a/benchmarks/robot_models/CMakeLists.txt b/benchmarks/robot_models/CMakeLists.txt deleted file mode 100644 index bf15c221..00000000 --- a/benchmarks/robot_models/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -add_executable(benchmark_robot_models benchmark_robot_models.cpp ../benchmarks_common.cpp) -target_link_libraries(benchmark_robot_models - wbc-robot_models-kdl - wbc-robot_models-hyrodyn - wbc-robot_models-pinocchio - wbc-robot_models-rbdl) - diff --git a/benchmarks/robot_models/benchmark_robot_models.cpp b/benchmarks/robot_models/benchmark_robot_models.cpp deleted file mode 100644 index 970c91ee..00000000 --- a/benchmarks/robot_models/benchmark_robot_models.cpp +++ /dev/null @@ -1,367 +0,0 @@ -#include -#include "../benchmarks_common.hpp" -#include "core/RobotModelFactory.hpp" -#include "robot_models/hyrodyn/RobotModelHyrodyn.hpp" -#include "robot_models/kdl/RobotModelKDL.hpp" -#include "robot_models/rbdl/RobotModelRBDL.hpp" -#include "robot_models/pinocchio/RobotModelPinocchio.hpp" - -using namespace wbc; -using namespace std; - -base::VectorXd evalBodyJacobian(RobotModelPtr robot_model, const string &root, const string &tip, int n_samples){ - base::VectorXd results(n_samples); - for(int i = 0; i < n_samples; i++){ - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state = randomFloatingBaseState(robot_model->floatingBaseState()); - robot_model->update(joint_state, floating_base_state); - base::Time start = base::Time::now(); - robot_model->bodyJacobian(root, tip); - results[i] = (double)(base::Time::now()-start).toMicroseconds()/1000; - usleep(0.001*1e6); - } - return results; -} - -base::VectorXd evalSpaceJacobian(RobotModelPtr robot_model, const string &root, const string &tip, int n_samples){ - base::VectorXd results(n_samples); - for(int i = 0; i < n_samples; i++){ - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state = randomFloatingBaseState(robot_model->floatingBaseState()); - robot_model->update(joint_state, floating_base_state); - base::Time start = base::Time::now(); - robot_model->spaceJacobian(root, tip); - results[i] = (double)(base::Time::now()-start).toMicroseconds()/1000; - usleep(0.001*1e6); - } - return results; -} - -base::VectorXd evalFK(RobotModelPtr robot_model, const string &root, const string &tip, int n_samples){ - base::VectorXd results(n_samples); - for(int i = 0; i < n_samples; i++){ - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state = randomFloatingBaseState(robot_model->floatingBaseState()); - robot_model->update(joint_state, floating_base_state); - base::Time start = base::Time::now(); - robot_model->rigidBodyState(root, tip); - results[i] = (double)(base::Time::now()-start).toMicroseconds()/1000; - usleep(0.001*1e6); - } - return results; -} - -base::VectorXd evalBiasForces(RobotModelPtr robot_model, int n_samples){ - base::VectorXd results(n_samples); - for(int i = 0; i < n_samples; i++){ - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state = randomFloatingBaseState(robot_model->floatingBaseState()); - robot_model->update(joint_state, floating_base_state); - base::Time start = base::Time::now(); - robot_model->biasForces(); - results[i] = (double)(base::Time::now()-start).toMicroseconds()/1000; - usleep(0.001*1e6); - } - return results; -} - -base::VectorXd evalJointSpaceInertiaMat(RobotModelPtr robot_model, int n_samples){ - base::VectorXd results(n_samples); - for(int i = 0; i < n_samples; i++){ - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state = randomFloatingBaseState(robot_model->floatingBaseState()); - robot_model->update(joint_state, floating_base_state); - base::Time start = base::Time::now(); - robot_model->jointSpaceInertiaMatrix(); - results[i] = (double)(base::Time::now()-start).toMicroseconds()/1000; - usleep(0.001*1e6); - } - return results; -} - -base::VectorXd evalSpatialAccelerationBias(RobotModelPtr robot_model, const string &root, const string &tip, int n_samples){ - base::VectorXd results(n_samples); - for(int i = 0; i < n_samples; i++){ - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state = randomFloatingBaseState(robot_model->floatingBaseState()); - robot_model->update(joint_state, floating_base_state); - base::Time start = base::Time::now(); - robot_model->spatialAccelerationBias(root, tip); - results[i] = (double)(base::Time::now()-start).toMicroseconds()/1000; - usleep(0.001*1e6); - } - return results; -} - -base::VectorXd evalCoM(RobotModelPtr robot_model, int n_samples){ - base::VectorXd results(n_samples); - for(int i = 0; i < n_samples; i++){ - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state = randomFloatingBaseState(robot_model->floatingBaseState()); - robot_model->update(joint_state, floating_base_state); - base::Time start = base::Time::now(); - robot_model->centerOfMass(); - results[i] = (double)(base::Time::now()-start).toMicroseconds()/1000; - usleep(0.001*1e6); - } - return results; -} - -void printResults(map results){ - cout << "Space Jacobian " << results["space_jac"].mean() << " ms +/- " << stdDev(results["space_jac"]) << endl; - cout << "Body Jacobian " << results["body_jac"].mean() << " ms +/- " << stdDev(results["body_jac"]) << endl; - cout << "Forward Kinematics " << results["FK"].mean() << " ms +/- " << stdDev(results["FK"]) << endl; - cout << "Jnt Inertia Mat " << results["joint_space_inertia_mat"].mean() << " ms +/- " << stdDev(results["joint_space_inertia_mat"]) << endl; - cout << "Bias Forces " << results["bias_forces"].mean() << " ms +/- " << stdDev(results["bias_forces"]) << endl; - cout << "Spatial Acc Bias " << results["spatial_acc_bias"].mean() << " ms +/- " << stdDev(results["spatial_acc_bias"]) << endl; - cout << "CoM " << results["com"].mean() << " ms +/- " << stdDev(results["com"]) << endl; -} - -map evaluateRobotModel(RobotModelPtr robot_model, const string &root, const string &tip, int n_samples){ - map results; - results["space_jac"] = evalSpaceJacobian(robot_model, root, tip, n_samples); - results["body_jac"] = evalBodyJacobian(robot_model, root, tip, n_samples); - results["FK"] = evalFK(robot_model, root, tip, n_samples); - results["joint_space_inertia_mat"] = evalJointSpaceInertiaMat(robot_model, n_samples); - results["bias_forces"] = evalBiasForces(robot_model, n_samples); - results["spatial_acc_bias"] = evalSpatialAccelerationBias(robot_model, root, tip, n_samples); - results["com"] = evalCoM(robot_model, n_samples); - return results; -} - -void runKUKAIiwaBenchmarks(int n_samples){ - cout << " ----------- Evaluating KUKA iiwa model -----------" << endl; - - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; - cfg.submechanism_file = "../../../models/kuka/hyrodyn/kuka_iiwa.yml"; - - RobotModelPtr robot_model_kdl = std::make_shared(); - RobotModelPtr robot_model_hyrodyn = std::make_shared(); - RobotModelPtr robot_model_rbdl = std::make_shared(); - RobotModelPtr robot_model_pinocchio = std::make_shared(); - - if(!robot_model_kdl->configure(cfg)) abort(); - if(!robot_model_hyrodyn->configure(cfg))abort(); - if(!robot_model_pinocchio->configure(cfg))abort(); - if(!robot_model_rbdl->configure(cfg))abort(); - - const std::string root = "kuka_lbr_l_link_0", tip = "kuka_lbr_l_tcp"; - map results_kdl = evaluateRobotModel(robot_model_kdl, root, tip, n_samples); - map results_hyrodyn = evaluateRobotModel(robot_model_hyrodyn, root, tip, n_samples); - map results_pinocchio = evaluateRobotModel(robot_model_pinocchio, root, tip, n_samples); - map results_rbdl = evaluateRobotModel(robot_model_rbdl, root, tip, n_samples); - - toCSV(results_kdl, "results/kuka_iiwa_robot_model_kdl.csv"); - toCSV(results_hyrodyn, "results/kuka_iiwa_robot_model_hyrodyn.csv"); - toCSV(results_pinocchio, "results/kuka_iiwa_robot_model_pinocchio.csv"); - toCSV(results_rbdl, "results/kuka_iiwa_robot_model_rbdl.csv"); - - cout << " ----------- Results RobotModelKDL -----------" << endl; - printResults(results_kdl); - cout << " ----------- Results RobotModelHyrodyn -----------" << endl; - printResults(results_hyrodyn); - cout << " ----------- Results RobotModelPinocchio -----------" << endl; - printResults(results_pinocchio); - cout << " ----------- Results RobotModelRBDL -----------" << endl; - printResults(results_rbdl); -} - -void runRH5SingleLegBenchmarks(int n_samples){ - cout << " ----------- Evaluating RH5 Single Leg model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg.yml"; - - RobotModelPtr robot_model_kdl = std::make_shared(); - RobotModelPtr robot_model_hyrodyn = std::make_shared(); - RobotModelPtr robot_model_hyrodyn_hybrid = std::make_shared(); - RobotModelPtr robot_model_rbdl = std::make_shared(); - RobotModelPtr robot_model_pinocchio = std::make_shared(); - - if(!robot_model_kdl->configure(cfg)) abort(); - if(!robot_model_hyrodyn->configure(cfg))abort(); - if(!robot_model_pinocchio->configure(cfg))abort(); - if(!robot_model_rbdl->configure(cfg))abort(); - cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg_hybrid.yml"; - if(!robot_model_hyrodyn_hybrid->configure(cfg))abort(); - - const std::string root = "RH5_Root_Link", tip = "LLAnkle_FT"; - map results_kdl = evaluateRobotModel(robot_model_kdl, root, tip, n_samples); - map results_hyrodyn = evaluateRobotModel(robot_model_hyrodyn, root, tip, n_samples); - map results_hyrodyn_hybrid = evaluateRobotModel(robot_model_hyrodyn_hybrid, root, tip, n_samples); - map results_pinocchio = evaluateRobotModel(robot_model_pinocchio, root, tip, n_samples); - map results_rbdl = evaluateRobotModel(robot_model_rbdl, root, tip, n_samples); - - toCSV(results_kdl, "results/rh5_single_leg_robot_model_kdl.csv"); - toCSV(results_hyrodyn, "results/rh5_single_leg_robot_model_hyrodyn.csv"); - toCSV(results_hyrodyn_hybrid, "results/rh5_single_leg_robot_model_hyrodyn_hybrid.csv"); - toCSV(results_pinocchio, "results/rh5_single_leg_robot_model_pinocchio.csv"); - toCSV(results_rbdl, "results/rh5_single_leg_robot_model_rbdl.csv"); - - cout << " ----------- Results RobotModelKDL -----------" << endl; - printResults(results_kdl); - cout << " ----------- Results RobotModelHyrodyn -----------" << endl; - printResults(results_hyrodyn); - cout << " ----------- Results RobotModelHyrodyn Hybrid -----------" << endl; - printResults(results_hyrodyn_hybrid); - cout << " ----------- Results RobotModelPinocchio -----------" << endl; - printResults(results_pinocchio); - cout << " ----------- Results RobotModelRBDL -----------" << endl; - printResults(results_rbdl); -} - - -void runRH5LegsBenchmarks(int n_samples){ - cout << " ----------- Evaluating RH5 Legs model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_legs.yml"; - cfg.floating_base = true; - - RobotModelPtr robot_model_kdl = std::make_shared(); - RobotModelPtr robot_model_hyrodyn = std::make_shared(); - RobotModelPtr robot_model_hyrodyn_hybrid = std::make_shared(); - RobotModelPtr robot_model_rbdl = std::make_shared(); - RobotModelPtr robot_model_pinocchio = std::make_shared(); - - if(!robot_model_kdl->configure(cfg)) abort(); - if(!robot_model_hyrodyn->configure(cfg))abort(); - if(!robot_model_pinocchio->configure(cfg))abort(); - if(!robot_model_rbdl->configure(cfg))abort(); - cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs_hybrid.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_legs_hybrid.yml"; - if(!robot_model_hyrodyn_hybrid->configure(cfg))abort(); - - const string root = "world", tip = "LLAnkle_FT"; - map results_kdl = evaluateRobotModel(robot_model_kdl, root, tip, n_samples); - map results_hyrodyn = evaluateRobotModel(robot_model_hyrodyn, root, tip, n_samples); - map results_hyrodyn_hybrid = evaluateRobotModel(robot_model_hyrodyn_hybrid, root, tip, n_samples); - map results_pinocchio = evaluateRobotModel(robot_model_pinocchio, root, tip, n_samples); - map results_rbdl = evaluateRobotModel(robot_model_rbdl, root, tip, n_samples); - - toCSV(results_kdl, "results/rh5_legs_robot_model_kdl.csv"); - toCSV(results_hyrodyn, "results/rh5_legs_robot_model_hyrodyn.csv"); - toCSV(results_hyrodyn_hybrid, "results/rh5_legs_robot_model_hyrodyn_hybrid.csv"); - toCSV(results_pinocchio, "results/rh5_legs_robot_model_pinocchio.csv"); - toCSV(results_rbdl, "results/rh5_legs_robot_model_rbdl.csv"); - - cout << " ----------- Results RobotModelKDL -----------" << endl; - printResults(results_kdl); - cout << " ----------- Results RobotModelHyrodyn -----------" << endl; - printResults(results_hyrodyn); - cout << " ----------- Results RobotModelHyrodyn Hybrid -----------" << endl; - printResults(results_hyrodyn_hybrid); - cout << " ----------- Results RobotModelPinocchio -----------" << endl; - printResults(results_pinocchio); - cout << " ----------- Results RobotModelRBDL -----------" << endl; - printResults(results_rbdl); -} - -void runRH5Benchmarks(int n_samples){ - cout << " ----------- Evaluating RH5 model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5.yml"; - cfg.floating_base = true; - - RobotModelPtr robot_model_kdl = std::make_shared(); - RobotModelPtr robot_model_hyrodyn = std::make_shared(); - RobotModelPtr robot_model_hyrodyn_hybrid = std::make_shared(); - RobotModelPtr robot_model_rbdl = std::make_shared(); - RobotModelPtr robot_model_pinocchio = std::make_shared(); - - if(!robot_model_kdl->configure(cfg)) abort(); - if(!robot_model_hyrodyn->configure(cfg))abort(); - if(!robot_model_pinocchio->configure(cfg))abort(); - if(!robot_model_rbdl->configure(cfg))abort(); - cfg.file_or_string = "../../../models/rh5/urdf/rh5_hybrid.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_hybrid.yml"; - if(!robot_model_hyrodyn_hybrid->configure(cfg))abort(); - - const string root = "world", tip = "LLAnkle_FT"; - map results_kdl = evaluateRobotModel(robot_model_kdl, root, tip, n_samples); - map results_hyrodyn = evaluateRobotModel(robot_model_hyrodyn, root, tip, n_samples); - map results_hyrodyn_hybrid = evaluateRobotModel(robot_model_hyrodyn_hybrid, root, tip, n_samples); - map results_pinocchio = evaluateRobotModel(robot_model_pinocchio, root, tip, n_samples); - map results_rbdl = evaluateRobotModel(robot_model_rbdl, root, tip, n_samples); - - toCSV(results_kdl, "results/rh5_robot_model_kdl.csv"); - toCSV(results_hyrodyn, "results/rh5_robot_model_hyrodyn.csv"); - toCSV(results_hyrodyn_hybrid, "results/rh5_robot_model_hyrodyn_hybrid.csv"); - toCSV(results_pinocchio, "results/rh5_robot_model_pinocchio.csv"); - toCSV(results_rbdl, "results/rh5_robot_model_rbdl.csv"); - - cout << " ----------- Results RobotModelKDL -----------" << endl; - printResults(results_kdl); - cout << " ----------- Results RobotModelHyrodyn -----------" << endl; - printResults(results_hyrodyn); - cout << " ----------- Results RobotModelHyrodyn Hybrid -----------" << endl; - printResults(results_hyrodyn_hybrid); - cout << " ----------- Results RobotModelPinocchio -----------" << endl; - printResults(results_pinocchio); - cout << " ----------- Results RobotModelRBDL -----------" << endl; - printResults(results_rbdl); -} - -void runRH5v2Benchmarks(int n_samples){ - cout << " ----------- Evaluating RH5v2 model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2.urdf"; - cfg.submechanism_file = "../../../models/rh5v2/hyrodyn/rh5v2.yml"; - - RobotModelPtr robot_model_kdl = std::make_shared(); - RobotModelPtr robot_model_hyrodyn = std::make_shared(); - RobotModelPtr robot_model_hyrodyn_hybrid = std::make_shared(); - RobotModelPtr robot_model_rbdl = std::make_shared(); - RobotModelPtr robot_model_pinocchio = std::make_shared(); - - if(!robot_model_kdl->configure(cfg)) abort(); - if(!robot_model_hyrodyn->configure(cfg))abort(); - if(!robot_model_pinocchio->configure(cfg))abort(); - if(!robot_model_rbdl->configure(cfg))abort(); - cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2_hybrid.urdf"; - cfg.submechanism_file = "../../../models/rh5v2/hyrodyn/rh5v2_hybrid.yml"; - if(!robot_model_hyrodyn_hybrid->configure(cfg))abort(); - - const std::string root = "RH5v2_Root_Link", tip = "ALWristFT_Link"; - map results_kdl = evaluateRobotModel(robot_model_kdl, root, tip, n_samples); - map results_hyrodyn = evaluateRobotModel(robot_model_hyrodyn, root, tip, n_samples); - map results_hyrodyn_hybrid = evaluateRobotModel(robot_model_hyrodyn_hybrid, root, tip, n_samples); - map results_pinocchio = evaluateRobotModel(robot_model_pinocchio, root, tip, n_samples); - map results_rbdl = evaluateRobotModel(robot_model_rbdl, root, tip, n_samples); - - toCSV(results_kdl, "results/rh5v2_robot_model_kdl.csv"); - toCSV(results_hyrodyn, "results/rh5v2_robot_model_hyrodyn.csv"); - toCSV(results_hyrodyn_hybrid, "results/rh5v2_robot_model_hyrodyn_hybrid.csv"); - toCSV(results_pinocchio, "results/rh5v2_robot_model_pinocchio.csv"); - toCSV(results_rbdl, "results/rh5v2_robot_model_rbdl.csv"); - - cout << " ----------- Results RobotModelKDL -----------" << endl; - printResults(results_kdl); - cout << " ----------- Results RobotModelHyrodyn -----------" << endl; - printResults(results_hyrodyn); - cout << " ----------- Results RobotModelHyrodyn Hybrid -----------" << endl; - printResults(results_hyrodyn_hybrid); - cout << " ----------- Results RobotModelPinocchio -----------" << endl; - printResults(results_pinocchio); - cout << " ----------- Results RobotModelRBDL -----------" << endl; - printResults(results_rbdl); -} - -void runBenchmarks(int n_samples){ - boost::filesystem::create_directory("results"); - - runKUKAIiwaBenchmarks(n_samples); - runRH5SingleLegBenchmarks(n_samples); - runRH5LegsBenchmarks(n_samples); - runRH5Benchmarks(n_samples); - runRH5v2Benchmarks(n_samples); -} - -int main(){ - srand(time(NULL)); - int n_samples = 1000; - runBenchmarks(n_samples); -} diff --git a/benchmarks/robot_models/plot_results.py b/benchmarks/robot_models/plot_results.py deleted file mode 100644 index a093dfd5..00000000 --- a/benchmarks/robot_models/plot_results.py +++ /dev/null @@ -1,107 +0,0 @@ -import matplotlib.pyplot as plt -import pandas as pd -import numpy as np -from IPython import embed -import sys - -robots = ["rh5_single_leg", - "rh5_legs", - "rh5", - "rh5v2"] -robot_models = ["robot_model_pinocchio", - "robot_model_rbdl", - "robot_model_kdl", - "robot_model_hyrodyn", - "robot_model_hyrodyn_hybrid"] - -if(len(sys.argv) < 2): - raise Exception("Invalid number of command line args. Usage: plot_results.py ") - -folder = sys.argv[1] - -def plotBoxWhysker(data_pinocchio, data_rbdl, data_kdl, data_hyrodyn, data_hyrodyn_hybrid, labels, title): - fig = plt.figure() - plt.title(title, fontsize = 20) - ax = fig.add_subplot(111) - width = 0.1 - - positions_group_pinocchio = np.array(range(len(data_pinocchio)))-2*width - positions_group_rbdl = np.array(range(len(data_rbdl)))-width - positions_group_kdl = np.array(range(len(data_kdl))) - positions_group_hyrodyn = np.array(range(len(data_hyrodyn)))+width - positions_group_hyrodyn_hybrid = np.array(range(len(data_hyrodyn_hybrid)))+width*2 - - data = [data_pinocchio, data_rbdl, data_kdl, data_hyrodyn, data_hyrodyn_hybrid] - positions = [positions_group_pinocchio,positions_group_rbdl,positions_group_kdl,positions_group_hyrodyn,positions_group_hyrodyn_hybrid] - - bplots = [] - for d,p in zip(data,positions): - bplots.append(plt.boxplot(d, positions=p, widths=width,showfliers=False, patch_artist=True)) - - plt.xticks(list(range(len(labels))) , labels, fontsize=20) - plt.grid(True) - - colors = ['blue','orange','green','red','grey'] - for bp,c in zip(bplots,colors): - for box in bp['boxes']: - box.set_facecolor(color=c) - for m in bp['medians']: - m.set_color(color='black') - - ax.legend([bp["boxes"][0] for bp in bplots], ['Pinocchio', 'RBDL', 'KDL', 'Hyrodyn', 'Hyrodyn Hybrid'], loc='upper left', fontsize=20) - plt.xlabel("Robots", fontsize=30, labelpad=20) - plt.ylabel("Time [us]", fontsize=30, labelpad=20) - -def plotRobotModels(robots, robot_models): - # Load CSV - csv_list = {} - for r in robots: - for b in robot_models: - csv_list[r + "_" + b] = pd.read_csv(folder + r + "_" + b + ".csv",sep=" ") - - space_jac_pinocchio = [csv_list[r + "_" + "robot_model_pinocchio"]["space_jac"].values*1000 for r in robots] - space_jac_rbdl = [csv_list[r + "_" + "robot_model_rbdl"]["space_jac"].values*1000 for r in robots] - space_jac_kdl = [csv_list[r + "_" + "robot_model_kdl"]["space_jac"].values*1000 for r in robots] - space_jac_hyrodyn = [csv_list[r + "_" + "robot_model_hyrodyn"]["space_jac"].values*1000 for r in robots] - space_jac_hyrodyn_hybrid = [csv_list[r + "_" + "robot_model_hyrodyn_hybrid"]["space_jac"].values*1000 for r in robots] - - body_jac_pinocchio = [csv_list[r + "_" + "robot_model_pinocchio"]["body_jac"].values*1000 for r in robots] - body_jac_rbdl = [csv_list[r + "_" + "robot_model_rbdl"]["body_jac"].values*1000 for r in robots] - body_jac_kdl = [csv_list[r + "_" + "robot_model_kdl"]["body_jac"].values*1000 for r in robots] - body_jac_hyrodyn = [csv_list[r + "_" + "robot_model_hyrodyn"]["body_jac"].values*1000 for r in robots] - body_jac_hyrodyn_hybrid = [csv_list[r + "_" + "robot_model_hyrodyn_hybrid"]["body_jac"].values*1000 for r in robots] - - - fk_pinocchio = [csv_list[r + "_" + "robot_model_pinocchio"]["FK"].values*1000 for r in robots] - fk_rbdl = [csv_list[r + "_" + "robot_model_rbdl"]["FK"].values*1000 for r in robots] - fk_kdl = [csv_list[r + "_" + "robot_model_kdl"]["FK"].values*1000 for r in robots] - fk_hyrodyn = [csv_list[r + "_" + "robot_model_hyrodyn"]["FK"].values*1000 for r in robots] - fk_hyrodyn_hybrid = [csv_list[r + "_" + "robot_model_hyrodyn_hybrid"]["FK"].values*1000 for r in robots] - - bias_forces_pinocchio = [csv_list[r + "_" + "robot_model_pinocchio"]["bias_forces"].values*1000 for r in robots] - bias_forces_rbdl = [csv_list[r + "_" + "robot_model_rbdl"]["bias_forces"].values*1000 for r in robots] - bias_forces_kdl = [csv_list[r + "_" + "robot_model_kdl"]["bias_forces"].values*1000 for r in robots] - bias_forces_hyrodyn = [csv_list[r + "_" + "robot_model_hyrodyn"]["bias_forces"].values*1000 for r in robots] - bias_forces_hyrodyn_hybrid = [csv_list[r + "_" + "robot_model_hyrodyn_hybrid"]["bias_forces"].values*1000 for r in robots] - - inertia_mat_pinocchio = [csv_list[r + "_" + "robot_model_pinocchio"]["joint_space_inertia_mat"].values*1000 for r in robots] - inertia_mat_rbdl = [csv_list[r + "_" + "robot_model_rbdl"]["joint_space_inertia_mat"].values*1000 for r in robots] - inertia_mat_kdl = [csv_list[r + "_" + "robot_model_kdl"]["joint_space_inertia_mat"].values*1000 for r in robots] - inertia_mat_hyrodyn = [csv_list[r + "_" + "robot_model_hyrodyn"]["joint_space_inertia_mat"].values*1000 for r in robots] - inertia_mat_hyrodyn_hybrid = [csv_list[r + "_" + "robot_model_hyrodyn_hybrid"]["joint_space_inertia_mat"].values*1000 for r in robots] - - com_pinocchio = [csv_list[r + "_" + "robot_model_pinocchio"]["com"].values*1000 for r in robots] - com_rbdl = [csv_list[r + "_" + "robot_model_rbdl"]["com"].values*1000 for r in robots] - com_kdl = [csv_list[r + "_" + "robot_model_kdl"]["com"].values*1000 for r in robots] - com_hyrodyn = [csv_list[r + "_" + "robot_model_hyrodyn"]["com"].values*1000 for r in robots] - com_hyrodyn_hybrid = [csv_list[r + "_" + "robot_model_hyrodyn_hybrid"]["com"].values*1000 for r in robots] - - plotBoxWhysker(space_jac_pinocchio, space_jac_rbdl, space_jac_kdl, space_jac_hyrodyn, space_jac_hyrodyn_hybrid, robots, "Space Jacobian") - plotBoxWhysker(body_jac_pinocchio, body_jac_rbdl, body_jac_kdl, body_jac_hyrodyn, body_jac_hyrodyn_hybrid, robots, "Body Jacobian") - plotBoxWhysker(fk_pinocchio, fk_rbdl, fk_kdl, fk_hyrodyn, fk_hyrodyn_hybrid, robots, "Forward Kinematics (Pose,Twist,Acceleration)") - plotBoxWhysker(bias_forces_pinocchio, bias_forces_rbdl, bias_forces_kdl, bias_forces_hyrodyn, bias_forces_hyrodyn_hybrid, robots, "Bias Forces/Torques") - plotBoxWhysker(inertia_mat_pinocchio, inertia_mat_rbdl, inertia_mat_kdl, inertia_mat_hyrodyn, inertia_mat_hyrodyn_hybrid, robots, "Joint Space Inertia Matrix") - plotBoxWhysker(com_pinocchio, com_rbdl, com_kdl, com_hyrodyn, com_hyrodyn_hybrid, robots, "CoM") - -plotRobotModels(robots, robot_models) -plt.show() diff --git a/benchmarks/robot_models_common.hpp b/benchmarks/robot_models_common.hpp deleted file mode 100644 index 547a87e4..00000000 --- a/benchmarks/robot_models_common.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef ROBOT_MODEL_COMMON_HPP -#define ROBOT_MODEL_COMMON_HPP - -#include - -namespace wbc{ - -RobotModelPtr makeRobotModelKUKAIiwa(const std::string type); -RobotModelPtr makeRobotModelRH5SingleLeg(const std::string type, bool hybrid_model = false); -RobotModelPtr makeRobotModelRH5Legs(const std::string type, bool hybrid_model = false); -RobotModelPtr makeRobotModelRH5(const std::string type, bool hybrid_model = false); -RobotModelPtr makeRobotModelRH5v2(const std::string type, bool hybrid_model = false); - -} - -#endif diff --git a/benchmarks/scenes/CMakeLists.txt b/benchmarks/scenes/CMakeLists.txt deleted file mode 100644 index 11cff1fc..00000000 --- a/benchmarks/scenes/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -add_executable(benchmark_scenes benchmark_scenes.cpp ../benchmarks_common.cpp) -target_link_libraries(benchmark_scenes - wbc-solvers-qpoases - wbc-solvers-qpswift - wbc-solvers-eiquadprog - wbc-solvers-proxqp - wbc-scenes - wbc-robot_models-kdl - wbc-robot_models-hyrodyn - wbc-robot_models-rbdl - wbc-robot_models-pinocchio - Boost::system - Boost::filesystem) - diff --git a/benchmarks/scenes/benchmark_scenes.cpp b/benchmarks/scenes/benchmark_scenes.cpp deleted file mode 100644 index d3345939..00000000 --- a/benchmarks/scenes/benchmark_scenes.cpp +++ /dev/null @@ -1,298 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "../benchmarks_common.hpp" - -using namespace wbc; -using namespace std; - -void printResults(map results){ - cout << "Scene Update " << results["scene_update"].mean() << " ms +/- " << stdDev(results["scene_update"]) << endl; - cout << "Scene Solve " << results["scene_solve"].mean() << " ms +/- " << stdDev(results["scene_solve"]) << endl; -} - -void evaluateVelocitySceneQuadraticCost(map robot_models, map solvers, - const string &root, const string &tip, - int n_samples, string robot_name){ - TaskConfig cart_task("cart_pos_ctrl",0,root,tip,root,1); - for(auto it : robot_models){ - for(auto itt : solvers){ - WbcScenePtr scene = make_shared(it.second, itt.second); - if(!scene->configure({cart_task})) - throw runtime_error("Failed to configure VelocitySceneQuadraticCost"); - map results = evaluateWBCSceneRandom(scene, n_samples); - string filename = "results/" + robot_name + "_vel_" + it.first + "_" + itt.first + ".csv"; - toCSV(results, filename); - cout << " ----------- Results VelocitySceneQuadraticCost (" + it.first + "," + itt.first + ") -----------" << endl; - printResults(results); - } - } -} - -void evaluateAccelerationSceneTSID(map robot_models, map solvers, - const string &root, const string &tip, - int n_samples, string robot_name){ - TaskConfig cart_task("cart_pos_ctrl",0,root,tip,root,1); - for(auto it : robot_models){ - for(auto itt : solvers){ - WbcScenePtr scene = make_shared(it.second, itt.second); - if(!scene->configure({cart_task})) - throw runtime_error("Failed to configure AccelerationSceneTSID"); - map results = evaluateWBCSceneRandom(scene, n_samples); - toCSV(results, "results/" + robot_name + "_acc_" + it.first + "_" + itt.first + ".csv"); - cout << " ----------- Results AccelerationSceneTSID (" + it.first + "," + itt.first + ") -----------" << endl; - printResults(results); - } - } -} - -void evaluateAccelerationSceneReducedTSID(map robot_models, map solvers, - const string &root, const string &tip, - int n_samples, string robot_name){ - TaskConfig cart_task("cart_pos_ctrl",0,root,tip,root,1); - for(auto it : robot_models){ - for(auto itt : solvers){ - WbcScenePtr scene = make_shared(it.second, itt.second); - if(!scene->configure({cart_task})) - throw runtime_error("Failed to configure AccelerationSceneReducedTSID"); - map results = evaluateWBCSceneRandom(scene, n_samples); - toCSV(results, "results/" + robot_name + "_acc_reduced_" + it.first + "_" + itt.first + ".csv"); - cout << " ----------- Results AccelerationSceneReducedTSID (" + it.first + "," + itt.first + ") -----------" << endl; - printResults(results); - } - } -} - -void runKUKAIiwaBenchmarks(int n_samples){ - cout << " ----------- Evaluating KUKA iiwa model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; - cfg.submechanism_file = "../../../models/kuka/hyrodyn/kuka_iiwa.yml"; - const string root = "kuka_lbr_l_link_0", tip = "kuka_lbr_l_tcp"; - const string robot = "kuka_iiwa"; - - map robot_models; - robot_models["kdl"] = make_shared(); - robot_models["hyrodyn"] = make_shared(); - robot_models["pinocchio"] = make_shared(); - robot_models["rbdl"] = make_shared(); - for(auto it : robot_models){ - if(!it.second->configure(cfg)) - abort(); - base::samples::Joints joint_state = randomJointState(it.second->jointLimits()); - it.second->update(joint_state); - } - - map solvers; - solvers["qpoases"] = make_shared(); - solvers["eiquadprog"] = make_shared(); - solvers["qpswift"] = make_shared(); - solvers["proxqp"] = make_shared(); - - evaluateVelocitySceneQuadraticCost(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneTSID(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneReducedTSID(robot_models, solvers, root, tip, n_samples, robot); -} - -void runRH5SingleLegBenchmarks(int n_samples){ - cout << " ----------- Evaluating RH5 Single Leg model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg.yml"; - const string robot = "rh5_single_leg"; - - map robot_models; - robot_models["kdl"] = make_shared(); - robot_models["hyrodyn"] = make_shared(); - robot_models["pinocchio"] = make_shared(); - robot_models["rbdl"] = make_shared(); - for(auto it : robot_models){ - if(!it.second->configure(cfg)) - abort(); - } - robot_models["hyrodyn_hybrid"] = make_shared(); - cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg_hybrid.yml"; - if(!robot_models["hyrodyn_hybrid"]->configure(cfg)) abort(); - for(auto it : robot_models){ - base::samples::Joints joint_state = randomJointState(it.second->jointLimits()); - joint_state.time = base::Time::now(); - it.second->update(joint_state); - } - - map solvers; - solvers["qpoases"] = make_shared(); - solvers["eiquadprog"] = make_shared(); - solvers["qpswift"] = make_shared(); - solvers["proxqp"] = make_shared(); - - const string root = "RH5_Root_Link", tip = "LLAnkle_FT"; - evaluateVelocitySceneQuadraticCost(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneTSID(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneReducedTSID(robot_models, solvers, root, tip, n_samples, robot); -} - -void runRH5LegsBenchmarks(int n_samples){ - cout << " ----------- Evaluating RH5 Legs Model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_legs.yml"; - cfg.floating_base = true; - ActiveContact contact(1,0.6); - contact.wx = 0.2; - contact.wy = 0.08; - cfg.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; - cfg.contact_points.elements = {contact, contact}; - const string robot = "rh5_legs"; - - map robot_models; - robot_models["kdl"] = make_shared(); - robot_models["hyrodyn"] = make_shared(); - robot_models["pinocchio"] = make_shared(); - robot_models["rbdl"] = make_shared(); - for(auto it : robot_models){ - if(!it.second->configure(cfg)) abort(); - } - robot_models["hyrodyn_hybrid"] = make_shared(); - cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs_hybrid.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_legs_hybrid.yml"; - if(!robot_models["hyrodyn_hybrid"]->configure(cfg)) abort(); - for(auto it : robot_models){ - base::samples::Joints joint_state = randomJointState(it.second->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state; - floating_base_state.pose.position = base::Vector3d(-0.0, 0.0, 0.87); - floating_base_state.pose.orientation = base::Orientation(1,0,0,0); - floating_base_state.twist.setZero(); - floating_base_state.acceleration.setZero(); - floating_base_state.time = base::Time::now(); - joint_state.time = base::Time::now(); - it.second->update(joint_state, floating_base_state); - } - - map solvers; - solvers["qpoases"] = make_shared(); - solvers["eiquadprog"] = make_shared(); - solvers["qpswift"] = make_shared(); - solvers["proxqp"] = make_shared(); - - const string root = "world", tip = "LLAnkle_FT"; - evaluateVelocitySceneQuadraticCost(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneTSID(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneReducedTSID(robot_models, solvers, root, tip, n_samples, robot); -} - -void runRH5Benchmarks(int n_samples){ - cout << " ----------- Evaluating RH5 Model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5.yml"; - cfg.floating_base = true; - ActiveContact contact(1,0.6); - contact.wx = 0.2; - contact.wy = 0.08; - cfg.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; - cfg.contact_points.elements = {contact, contact}; - const string robot = "rh5"; - - map robot_models; - robot_models["kdl"] = make_shared(); - robot_models["hyrodyn"] = make_shared(); - robot_models["pinocchio"] = make_shared(); - robot_models["rbdl"] = make_shared(); - for(auto it : robot_models){ - if(!it.second->configure(cfg)) abort(); - } - robot_models["hyrodyn_hybrid"] = make_shared(); - cfg.file_or_string = "../../../models/rh5/urdf/rh5_hybrid.urdf"; - cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_hybrid.yml"; - if(!robot_models["hyrodyn_hybrid"]->configure(cfg)) abort(); - for(auto it : robot_models){ - base::samples::Joints joint_state = randomJointState(it.second->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state; - floating_base_state.pose.position = base::Vector3d(-0.0, 0.0, 0.87); - floating_base_state.pose.orientation = base::Orientation(1,0,0,0); - floating_base_state.twist.setZero(); - floating_base_state.acceleration.setZero(); - floating_base_state.time = base::Time::now(); - joint_state.time = base::Time::now(); - it.second->update(joint_state, floating_base_state); - } - - - map solvers; - solvers["qpoases"] = make_shared(); - solvers["eiquadprog"] = make_shared(); - solvers["qpswift"] = make_shared(); - solvers["proxqp"] = make_shared(); - - const string root = "world", tip = "LLAnkle_FT"; - evaluateVelocitySceneQuadraticCost(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneTSID(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneReducedTSID(robot_models, solvers, root, tip, n_samples, robot); -} - -void runRH5v2Benchmarks(int n_samples){ - cout << " ----------- Evaluating RH5v2 model -----------" << endl; - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2.urdf"; - cfg.submechanism_file = "../../../models/rh5v2/hyrodyn/rh5v2.yml"; - const string robot = "rh5v2"; - - map robot_models; - robot_models["kdl"] = make_shared(); - robot_models["hyrodyn"] = make_shared(); - robot_models["pinocchio"] = make_shared(); - robot_models["rbdl"] = make_shared(); - for(auto it : robot_models){ - if(!it.second->configure(cfg)) abort(); - } - robot_models["hyrodyn_hybrid"] = make_shared(); - cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2_hybrid.urdf"; - cfg.submechanism_file = "../../../models/rh5v2/hyrodyn/rh5v2_hybrid.yml"; - if(!robot_models["hyrodyn_hybrid"]->configure(cfg)) abort(); - for(auto it : robot_models){ - base::samples::Joints joint_state = randomJointState(it.second->jointLimits()); - base::samples::RigidBodyStateSE3 rbs = randomFloatingBaseState(); - joint_state.time = base::Time::now(); - it.second->update(joint_state, rbs); - } - - map solvers; - solvers["qpoases"] = make_shared(); - solvers["eiquadprog"] = make_shared(); - solvers["qpswift"] = make_shared(); - solvers["proxqp"] = make_shared(); - - const string root = "RH5v2_Root_Link", tip = "ALWristFT_Link"; - evaluateVelocitySceneQuadraticCost(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneTSID(robot_models, solvers, root, tip, n_samples, robot); - evaluateAccelerationSceneReducedTSID(robot_models, solvers, root, tip, n_samples, robot); -} - -void runBenchmarks(int n_samples){ - boost::filesystem::create_directory("results"); - - runRH5LegsBenchmarks(n_samples); - runRH5Benchmarks(n_samples); - runKUKAIiwaBenchmarks(n_samples); - runRH5SingleLegBenchmarks(n_samples); - runRH5v2Benchmarks(n_samples); -} - -int -main(){ - - srand(time(NULL)); - int n_samples = 1000; - runBenchmarks(n_samples); -} diff --git a/benchmarks/scenes/plot_results.py b/benchmarks/scenes/plot_results.py deleted file mode 100644 index b9e5dfd0..00000000 --- a/benchmarks/scenes/plot_results.py +++ /dev/null @@ -1,209 +0,0 @@ -import matplotlib.pyplot as plt -import pandas as pd -import numpy as np -import sys -from IPython import embed - -robots = ["rh5_single_leg", - "rh5_legs", - "rh5", - "rh5v2"] -robot_models = ["pinocchio", - "hyrodyn", - "rbdl", - "kdl"] -scenes = ["vel", - "acc", - "acc_reduced"] -solvers = ["qpoases", - "eiquadprog", - "qpswift", - "proxqp"] - -if(len(sys.argv) < 2): - raise Exception("Invalid number of command line args. Usage: plot_results.py ") -folder = sys.argv[1] - -def getAvgSceneUpdateTime(csv_list, robot, robot_model, scene): - n_data = len(csv_list[robot+"_"+scene+"_"+robot_model+"_"+"qpoases"]["scene_update"].values) - avg = np.array([0]*n_data) - for s in solvers: - key = robot+"_"+scene+"_"+robot_model+"_"+s - avg = avg + csv_list[key]["scene_update"].values - avg = avg/len(solvers) - return avg - -def plotSceneUpdateTime(csv_list, wbc_type, title): - fig = plt.figure() - plt.title(title, fontsize = 20) - ax = fig.add_subplot(111) - width = 0.1 - - data_pinocchio = [getAvgSceneUpdateTime(csv_list, r, "pinocchio", wbc_type) for r in robots] - data_kdl = [getAvgSceneUpdateTime(csv_list, r, "kdl", wbc_type) for r in robots] - data_rbdl = [getAvgSceneUpdateTime(csv_list, r, "rbdl", wbc_type) for r in robots] - data_hyrodyn = [getAvgSceneUpdateTime(csv_list, r, "hyrodyn", wbc_type) for r in robots] - - positions_group_pinocchio = np.array(range(len(data_pinocchio)))-width*1.5 - positions_group_kdl = np.array(range(len(data_kdl)))-width*0.5 - positions_group_rbdl = np.array(range(len(data_rbdl)))+width*0.5 - positions_group_hyrodyn = np.array(range(len(data_hyrodyn)))+width*1.5 - - data = [data_pinocchio,data_kdl,data_rbdl,data_hyrodyn] - positions = [positions_group_pinocchio,positions_group_kdl,positions_group_rbdl,positions_group_hyrodyn] - - bplots = [] - for d,p in zip(data,positions): - bplots.append(plt.boxplot(d, positions=p, widths=width,showfliers=False, patch_artist=True)) - - plt.xticks(list(range(len(robots))), robots, fontsize=20) - plt.grid(True) - - colors = ['blue','orange','green','red','grey'] - for bp,c in zip(bplots,colors): - for box in bp['boxes']: - box.set_facecolor(color=c) - for m in bp['medians']: - m.set_color(color='black') - - ax.legend([bp["boxes"][0] for bp in bplots], robot_models, loc='upper left', fontsize=20) - plt.xlabel("Robots", fontsize=30, labelpad=20) - plt.ylabel("Time [ms]", fontsize=30, labelpad=20) - -def getAvgSceneSolveTime(csv_list, robot, solver, scene): - n_data = len(csv_list[robot+"_"+scene+"_"+"pinocchio"+"_"+solver]["scene_update" ].values) - avg = np.array([0]*n_data) - tmp = ["pinocchio"] - for rm in tmp: - key = robot+"_"+scene+"_"+rm+"_"+solver - avg = avg + csv_list[key]["scene_solve"].values - avg = avg/len(tmp) - return avg - -def plotSceneSolveTime(csv_list, wbc_type, title): - fig = plt.figure() - plt.title(title, fontsize = 20) - ax = fig.add_subplot(111) - width = 0.1 - - data_qpoases = [getAvgSceneSolveTime(csv_list, r, "qpoases", wbc_type) for r in robots] - data_eiquadprog = [getAvgSceneSolveTime(csv_list, r, "eiquadprog", wbc_type) for r in robots] - data_qpswift = [getAvgSceneSolveTime(csv_list, r, "qpswift", wbc_type) for r in robots] - data_proxqp = [getAvgSceneSolveTime(csv_list, r, "proxqp", wbc_type) for r in robots] - - positions_group_qpoases = np.array(range(len(data_qpoases)))-width*1.5 - positions_group_eiquadprog = np.array(range(len(data_eiquadprog)))-width*0.5 - positions_group_qpswift = np.array(range(len(data_qpswift)))+width*0.5 - positions_group_proxqp = np.array(range(len(data_proxqp)))+width*1.5 - - data = [data_qpoases,data_eiquadprog,data_qpswift,data_proxqp] - positions = [positions_group_qpoases,positions_group_eiquadprog,positions_group_qpswift,positions_group_proxqp] - - bplots = [] - for d,p in zip(data,positions): - bplots.append(plt.boxplot(d, positions=p, widths=width,showfliers=False, patch_artist=True)) - - plt.xticks(list(range(len(robots))), robots, fontsize=20) - plt.grid(True) - - colors = ['blue','orange','green','red','grey'] - for bp,c in zip(bplots,colors): - for box in bp['boxes']: - box.set_facecolor(color=c) - for m in bp['medians']: - m.set_color(color='black') - - ax.legend([bp["boxes"][0] for bp in bplots], ['QPOases','Eiquadprog', 'QPSwift', 'ProxQP'], loc='upper left', fontsize=20) - plt.xlabel("Robots", fontsize=30, labelpad=20) - plt.ylabel("Time [ms]", fontsize=30, labelpad=20) - -def compareSerialVsHybrid(data1, data2, labels, title): - fig = plt.figure() - plt.title(title, fontsize = 20) - ax = fig.add_subplot(111) - width = 0.2 - - positions_group1 = np.array(range(len(data1)))-width/2 - positions_group2 = np.array(range(len(data2)))+width/2 - - bplot1 = plt.boxplot(data1, - positions=positions_group1, - widths=width, - showfliers=False, - patch_artist=True) - bplot2 = plt.boxplot(data2, - positions=positions_group2, - widths=width, - showfliers=False, - patch_artist=True) - - plt.xticks(list(range(len(labels))) , labels, fontsize=20) - plt.grid(True) - for box in bplot1['boxes']: - box.set_facecolor(color='blue') - for box in bplot2['boxes']: - box.set_facecolor(color='orange') - for m in bplot1['medians']: - m.set_color(color='black') - ax.legend([bplot1["boxes"][0], bplot2["boxes"][0]], ['Serial Model', 'Hybrid Model'], loc='upper left', fontsize=20) - plt.xlabel("Robot Model / WBC Type", fontsize=30, labelpad=20) - plt.ylabel("Time per Cycle [ms]", fontsize=30, labelpad=20) - -def compareScenes(csv_list, title): - fig = plt.figure() - plt.title(title, fontsize = 20) - ax = fig.add_subplot(111) - width = 0.1 - - data_qpoases_vel = [getAvgSceneUpdateTime(csv_list, r, "pinocchio", "vel") + getAvgSceneSolveTime(csv_list, r, "qpoases", "vel") for r in robots] - data_qpoases_acc = [getAvgSceneUpdateTime(csv_list, r, "pinocchio", "acc") + getAvgSceneSolveTime(csv_list, r, "qpoases", "acc") for r in robots] - data_qpoases_accr = [getAvgSceneUpdateTime(csv_list, r, "pinocchio", "acc_reduced") + getAvgSceneSolveTime(csv_list, r, "qpoases", "acc_reduced") for r in robots] - - positions_group_vel = np.array(range(len(data_qpoases_vel)))-width - positions_group_acc = np.array(range(len(data_qpoases_acc))) - positions_group_accr = np.array(range(len(data_qpoases_accr)))+width - - data = [data_qpoases_vel,data_qpoases_acc,data_qpoases_accr] - positions = [positions_group_vel,positions_group_acc,positions_group_accr] - - bplots = [] - for d,p in zip(data,positions): - bplots.append(plt.boxplot(d, positions=p, widths=width,showfliers=False, patch_artist=True)) - - plt.xticks(list(range(len(robots))), robots, fontsize=20) - plt.grid(True) - - colors = ['blue','orange','green','red','grey'] - for bp,c in zip(bplots,colors): - for box in bp['boxes']: - box.set_facecolor(color=c) - for m in bp['medians']: - m.set_color(color='black') - - ax.legend([bp["boxes"][0] for bp in bplots], ['Velocity','TSID', 'TSID reduced'], loc='upper left', fontsize=20) - plt.xlabel("Robots", fontsize=30, labelpad=20) - plt.ylabel("Time [ms]", fontsize=30, labelpad=20) - -def plotScenes(robots, robot_models, solvers, scenes): - csv_list = {} - # load all csv files - for r in robots: - for b in scenes: - for rm in robot_models: - for s in solvers: - key = r + "_" + b + "_" + rm + "_" + s - csv_list[key] = pd.read_csv(folder + "/" + key + ".csv",sep=" ") - - - # Plot update time for different robot models / robots - #compareScenes(csv_list, "Scene Update") - plotSceneUpdateTime(csv_list, "vel", "Scene Update (Velocity)") - plotSceneUpdateTime(csv_list, "acc", "Scene Update (Acceleration)") - - plotSceneSolveTime(csv_list, "vel", "Scene Solve (Velocity)") - plotSceneSolveTime(csv_list, "acc", "Scene Solve (Acceleration)") - #compareSerialVsHybrid(total_serial, total_hybrid, ["RH5v2\nVelocity", "RH5v2\nAcceleration", "RH5 One Leg\nVelocity", "RH5 One Leg\nAcceleration", "RH5 Legs\nVelocity", "RH5 Legs\nAcceleration", "RH5\nVelocity", "RH5\nAcceleration"], "") - - -plotScenes(robots, robot_models, solvers, scenes) -plt.show() diff --git a/benchmarks/solvers/CMakeLists.txt b/benchmarks/solvers/CMakeLists.txt deleted file mode 100644 index 80768554..00000000 --- a/benchmarks/solvers/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -add_executable(benchmark_solvers benchmark_solvers.cpp ../benchmarks_common.cpp) -target_link_libraries(benchmark_solvers - wbc-solvers-qpoases - wbc-solvers-qpswift - wbc-solvers-eiquadprog - wbc-solvers-proxqp - wbc-scenes - wbc-robot_models-pinocchio - Boost::system - Boost::filesystem) - diff --git a/benchmarks/solvers/benchmark_solvers.cpp b/benchmarks/solvers/benchmark_solvers.cpp deleted file mode 100644 index b51e0e02..00000000 --- a/benchmarks/solvers/benchmark_solvers.cpp +++ /dev/null @@ -1,365 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "../benchmarks_common.hpp" - -using namespace std; -using namespace wbc; - -void printResults(map results){ - cout << "Scene Update " << results["scene_update"].mean() << " ms +/- " << stdDev(results["scene_update"]) << endl; - cout << "Scene Solve " << results["scene_solve"].mean() << " ms +/- " << stdDev(results["scene_solve"]) << endl; -} - -map evaluateQPOases(RobotModelPtr robot_model, string root, string tip, int n_samples, string type){ - clog << "Evaluate QPOases solver in " << type << " scene" << endl; - - QPSolverPtr solver = std::make_shared(); - - TaskConfig cart_task("cart_pos_ctrl",0,root,tip,root,1); - WbcScenePtr scene; - if(type == "vel") - scene = std::make_shared(robot_model, solver); - else - scene = std::make_shared(robot_model, solver); - if(!scene->configure({cart_task})) - throw std::runtime_error("Failed to configure evaluateAccelerationSceneReducedTSID"); - - return evaluateWBCSceneRandom(scene, n_samples); -} - -map evaluateQPSwift(RobotModelPtr robot_model, string root, string tip, int n_samples, string type){ - clog << "Evaluate QPSwift solver in " << type << " scene" << endl; - - QPSolverPtr solver = std::make_shared(); - - TaskConfig cart_task("cart_pos_ctrl",0,root,tip,root,1); - WbcScenePtr scene; - if(type == "vel") - scene = std::make_shared(robot_model, solver); - else - scene = std::make_shared(robot_model, solver); - if(!scene->configure({cart_task})) - throw std::runtime_error("Failed to configure evaluateAccelerationSceneReducedTSID"); - - return evaluateWBCSceneRandom(scene, n_samples); -} - -map evaluateEiquadprog(RobotModelPtr robot_model, string root, string tip, int n_samples, string type){ - clog << "Evaluate Eiquadprog solver in " << type << " scene" << endl; - - QPSolverPtr solver = std::make_shared(); - - TaskConfig cart_task("cart_pos_ctrl",0,root,tip,root,1); - WbcScenePtr scene; - if(type == "vel") - scene = std::make_shared(robot_model, solver); - else - scene = std::make_shared(robot_model, solver); - if(!scene->configure({cart_task})) - throw std::runtime_error("Failed to configure evaluateAccelerationSceneReducedTSID"); - - return evaluateWBCSceneRandom(scene, n_samples); -} - -map evaluateProxQP(RobotModelPtr robot_model, string root, string tip, int n_samples, string type){ - clog << "Evaluate ProxQP solver in " << type << " scene" << endl; - - QPSolverPtr solver = std::make_shared(); - - TaskConfig cart_task("cart_pos_ctrl",0,root,tip,root,1); - WbcScenePtr scene; - if(type == "vel") - scene = std::make_shared(robot_model, solver); - else - scene = std::make_shared(robot_model, solver); - if(!scene->configure({cart_task})) - throw std::runtime_error("Failed to configure evaluateAccelerationSceneReducedTSID"); - - return evaluateWBCSceneRandom(scene, n_samples); -} - -void runKUKAIiwaBenchmarks(int n_samples){ - - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; - RobotModelPtr robot_model = std::make_shared(); - if(!robot_model->configure(cfg))abort(); - - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - robot_model->update(joint_state); - - clog << " ----------- Evaluating KUKA iiwa model -----------" << endl; - map results_qp_oases_vel = evaluateQPOases(robot_model, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", n_samples, "vel"); - map results_qp_swift_vel = evaluateQPSwift(robot_model, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", n_samples, "vel"); - map results_proxqp_vel = evaluateProxQP(robot_model, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", n_samples, "vel"); - map results_eiquadprog_vel = evaluateEiquadprog(robot_model, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", n_samples, "vel"); - map results_qp_oases_acc = evaluateQPOases(robot_model, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", n_samples, "acc"); - map results_qp_swift_acc = evaluateQPSwift(robot_model, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", n_samples, "acc"); - map results_proxqp_acc = evaluateProxQP(robot_model, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", n_samples, "acc"); - map results_eiquadprog_acc = evaluateEiquadprog(robot_model, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", n_samples, "acc"); - - toCSV(results_qp_oases_vel, "results/kuka_iiwa_vel_qpoases.csv"); - toCSV(results_qp_swift_vel, "results/kuka_iiwa_vel_qpswift.csv"); - toCSV(results_proxqp_vel, "results/kuka_iiwa_vel_proxqp.csv"); - toCSV(results_eiquadprog_vel, "results/kuka_iiwa_vel_eiquadprog.csv"); - toCSV(results_qp_oases_acc, "results/kuka_iiwa_acc_qpoases.csv"); - toCSV(results_qp_swift_acc, "results/kuka_iiwa_acc_qpswift.csv"); - toCSV(results_proxqp_acc, "results/kuka_iiwa_acc_proxqp.csv"); - toCSV(results_eiquadprog_acc, "results/kuka_iiwa_acc_eiquadprog.csv"); - - cout << " ----------- Results VelocitySceneQuadraticCost (QPOases) -----------" << endl; - printResults(results_qp_oases_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (QPSwift) -----------" << endl; - printResults(results_qp_swift_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (ProxQP) -----------" << endl; - printResults(results_proxqp_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_vel); - cout << " ----------- Results AccelerationSceneReducedTSID (QPOases) -----------" << endl; - printResults(results_qp_oases_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (QPSwift) -----------" << endl; - printResults(results_qp_swift_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (ProxQP) -----------" << endl; - printResults(results_proxqp_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_acc); -} - -void runRH5SingleLegBenchmarks(int n_samples){ - - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg.urdf"; - RobotModelPtr robot_model = std::make_shared(); - if(!robot_model->configure(cfg))abort(); - - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 rbs = randomFloatingBaseState(); - robot_model->update(joint_state, rbs); - - clog << " ----------- Evaluating RH5 Single Leg model -----------" << endl; - map results_qp_oases_vel = evaluateQPOases(robot_model, "RH5_Root_Link", "LLAnkle_FT", n_samples, "vel"); - map results_qp_swift_vel = evaluateQPSwift(robot_model, "RH5_Root_Link", "LLAnkle_FT", n_samples, "vel"); - map results_proxqp_vel = evaluateProxQP(robot_model, "RH5_Root_Link", "LLAnkle_FT", n_samples, "vel"); - map results_eiquadprog_vel = evaluateEiquadprog(robot_model, "RH5_Root_Link", "LLAnkle_FT", n_samples, "vel"); - map results_qp_oases_acc = evaluateQPOases(robot_model, "RH5_Root_Link", "LLAnkle_FT", n_samples, "acc"); - map results_qp_swift_acc = evaluateQPSwift(robot_model, "RH5_Root_Link", "LLAnkle_FT", n_samples, "acc"); - map results_proxqp_acc = evaluateProxQP(robot_model, "RH5_Root_Link", "LLAnkle_FT", n_samples, "acc"); - map results_eiquadprog_acc = evaluateEiquadprog(robot_model, "RH5_Root_Link", "LLAnkle_FT", n_samples, "acc"); - - toCSV(results_qp_oases_vel, "results/rh5_single_leg_vel_qpoases.csv"); - toCSV(results_qp_swift_vel, "results/rh5_single_leg_vel_qpswift.csv"); - toCSV(results_proxqp_vel, "results/rh5_single_leg_vel_proxqp.csv"); - toCSV(results_eiquadprog_vel, "results/rh5_single_leg_vel_eiquadprog.csv"); - toCSV(results_qp_oases_acc, "results/rh5_single_leg_acc_qpoases.csv"); - toCSV(results_qp_swift_acc, "results/rh5_single_leg_acc_qpswift.csv"); - toCSV(results_proxqp_acc, "results/rh5_single_leg_acc_proxqp.csv"); - toCSV(results_eiquadprog_acc, "results/rh5_single_leg_acc_eiquadprog.csv"); - - - cout << " ----------- Results VelocitySceneQuadraticCost (QPOases) -----------" << endl; - printResults(results_qp_oases_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (QPSwift) -----------" << endl; - printResults(results_qp_swift_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (ProxQP) -----------" << endl; - printResults(results_proxqp_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_vel); - cout << " ----------- Results AccelerationSceneReducedTSID (QPOases) -----------" << endl; - printResults(results_qp_oases_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (QPSwift) -----------" << endl; - printResults(results_qp_swift_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (ProxQP) -----------" << endl; - printResults(results_proxqp_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_acc); -} - -void runRH5LegsBenchmarks(int n_samples){ - - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; - cfg.floating_base = true; - ActiveContact contact(1,0.6); - contact.wx = 0.2; - contact.wy = 0.08; - cfg.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; - RobotModelPtr robot_model = std::make_shared(); - if(!robot_model->configure(cfg))abort(); - - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state; - floating_base_state.pose.position = base::Vector3d(-0.0, 0.0, 0.87); - floating_base_state.pose.orientation = base::Orientation(1,0,0,0); - floating_base_state.twist.setZero(); - floating_base_state.acceleration.setZero(); - floating_base_state.time = base::Time::now(); - joint_state.time = base::Time::now(); - robot_model->update(joint_state, floating_base_state); - - clog << " ----------- Evaluating RH5 Legs model -----------" << endl; - map results_qp_oases_vel = evaluateQPOases(robot_model, "world", "LLAnkle_FT", n_samples, "vel"); - map results_qp_swift_vel = evaluateQPSwift(robot_model, "world", "LLAnkle_FT", n_samples, "vel"); - map results_proxqp_vel = evaluateProxQP(robot_model, "world", "LLAnkle_FT", n_samples, "vel"); - map results_eiquadprog_vel = evaluateEiquadprog(robot_model, "world", "LLAnkle_FT", n_samples, "vel"); - map results_qp_oases_acc = evaluateQPOases(robot_model, "world", "LLAnkle_FT", n_samples, "acc"); - map results_qp_swift_acc = evaluateQPSwift(robot_model, "world", "LLAnkle_FT", n_samples, "acc"); - map results_proxqp_acc = evaluateProxQP(robot_model, "world", "LLAnkle_FT", n_samples, "acc"); - map results_eiquadprog_acc = evaluateEiquadprog(robot_model, "world", "LLAnkle_FT", n_samples, "acc"); - - toCSV(results_qp_oases_vel, "results/rh5_legs_vel_qpoases.csv"); - toCSV(results_qp_swift_vel, "results/rh5_legs_vel_qpswift.csv"); - toCSV(results_proxqp_vel, "results/rh5_legs_vel_proxqp.csv"); - toCSV(results_eiquadprog_vel, "results/rh5_legs_vel_eiquadprog.csv"); - toCSV(results_qp_oases_acc, "results/rh5_legs_acc_qpoases.csv"); - toCSV(results_qp_swift_acc, "results/rh5_legs_acc_qpswift.csv"); - toCSV(results_proxqp_acc, "results/rh5_legs_acc_proxqp.csv"); - toCSV(results_eiquadprog_acc, "results/rh5_legs_acc_eiquadprog.csv"); - - cout << " ----------- Results VelocitySceneQuadraticCost (QPOases) -----------" << endl; - printResults(results_qp_oases_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (QPSwift) -----------" << endl; - printResults(results_qp_swift_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (ProxQP) -----------" << endl; - printResults(results_proxqp_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_vel); - cout << " ----------- Results AccelerationSceneReducedTSID (QPOases) -----------" << endl; - printResults(results_qp_oases_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (QPSwift) -----------" << endl; - printResults(results_qp_swift_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (ProxQP) -----------" << endl; - printResults(results_proxqp_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_acc); -} - -void runRH5Benchmarks(int n_samples){ - - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5/urdf/rh5.urdf"; - cfg.floating_base = true; - ActiveContact contact(1,0.6); - contact.wx = 0.2; - contact.wy = 0.08; - cfg.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; - cfg.contact_points.elements = {contact, contact}; - RobotModelPtr robot_model = std::make_shared(); - if(!robot_model->configure(cfg))abort(); - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 floating_base_state; - floating_base_state.pose.position = base::Vector3d(0.0, 0.0, 0.89); - floating_base_state.pose.orientation = base::Orientation(1,0,0,0); - floating_base_state.twist.setZero(); - floating_base_state.acceleration.setZero(); - floating_base_state.time = base::Time::now(); - joint_state.time = base::Time::now(); - robot_model->update(joint_state, floating_base_state); - - clog << " ----------- Evaluating RH5 model -----------" << endl; - map results_qp_oases_vel = evaluateQPOases(robot_model, "world", "LLAnkle_FT", n_samples, "vel"); - map results_qp_swift_vel = evaluateQPSwift(robot_model, "world", "LLAnkle_FT", n_samples, "vel"); - map results_proxqp_vel = evaluateProxQP(robot_model, "world", "LLAnkle_FT", n_samples, "vel"); - map results_eiquadprog_vel = evaluateEiquadprog(robot_model, "world", "LLAnkle_FT", n_samples, "vel"); - map results_qp_oases_acc = evaluateQPOases(robot_model, "world", "LLAnkle_FT", n_samples, "acc"); - map results_qp_swift_acc = evaluateQPSwift(robot_model, "world", "LLAnkle_FT", n_samples, "acc"); - map results_proxqp_acc = evaluateProxQP(robot_model, "world", "LLAnkle_FT", n_samples, "acc"); - map results_eiquadprog_acc = evaluateEiquadprog(robot_model, "world", "LLAnkle_FT", n_samples, "acc"); - - toCSV(results_qp_oases_vel, "results/rh5_vel_qpoases.csv"); - toCSV(results_qp_swift_vel, "results/rh5_vel_qpswift.csv"); - toCSV(results_proxqp_vel, "results/rh5_vel_proxqp.csv"); - toCSV(results_eiquadprog_vel, "results/rh5_vel_eiquadprog.csv"); - toCSV(results_qp_oases_acc, "results/rh5_acc_qpoases.csv"); - toCSV(results_qp_swift_acc, "results/rh5_acc_qpswift.csv"); - toCSV(results_proxqp_acc, "results/rh5_acc_proxqp.csv"); - toCSV(results_eiquadprog_acc, "results/rh5_acc_eiquadprog.csv"); - - cout << " ----------- Results VelocitySceneQuadraticCost (QPOases) -----------" << endl; - printResults(results_qp_oases_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (QPSwift) -----------" << endl; - printResults(results_qp_swift_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (ProxQP) -----------" << endl; - printResults(results_proxqp_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_vel); - cout << " ----------- Results AccelerationSceneReducedTSID (QPOases) -----------" << endl; - printResults(results_qp_oases_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (QPSwift) -----------" << endl; - printResults(results_qp_swift_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (ProxQP) -----------" << endl; - printResults(results_proxqp_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_acc); -} - -void runRH5v2Benchmarks(int n_samples){ - - RobotModelConfig cfg; - cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2.urdf"; - RobotModelPtr robot_model = std::make_shared(); - if(!robot_model->configure(cfg))abort(); - - base::samples::Joints joint_state = randomJointState(robot_model->jointLimits()); - base::samples::RigidBodyStateSE3 rbs = randomFloatingBaseState(); - robot_model->update(joint_state, rbs); - - clog << " ----------- Evaluating RH5v2 model -----------" << endl; - map results_qp_oases_vel = evaluateQPOases(robot_model, "RH5v2_Root_Link", "ALWristFT_Link", n_samples, "vel"); - map results_qp_swift_vel = evaluateQPSwift(robot_model, "RH5v2_Root_Link", "ALWristFT_Link", n_samples, "vel"); - map results_proxqp_vel = evaluateProxQP(robot_model, "RH5v2_Root_Link", "ALWristFT_Link", n_samples, "vel"); - map results_eiquadprog_vel = evaluateEiquadprog(robot_model, "RH5v2_Root_Link", "ALWristFT_Link", n_samples, "vel"); - map results_qp_oases_acc = evaluateQPOases(robot_model, "RH5v2_Root_Link", "ALWristFT_Link", n_samples, "acc"); - map results_qp_swift_acc = evaluateQPSwift(robot_model, "RH5v2_Root_Link", "ALWristFT_Link", n_samples, "acc"); - map results_proxqp_acc = evaluateProxQP(robot_model, "RH5v2_Root_Link", "ALWristFT_Link", n_samples, "acc"); - map results_eiquadprog_acc = evaluateEiquadprog(robot_model, "RH5v2_Root_Link", "ALWristFT_Link", n_samples, "acc"); - - toCSV(results_qp_oases_vel, "results/rh5v2_vel_qpoases.csv"); - toCSV(results_qp_swift_vel, "results/rh5v2_vel_qpswift.csv"); - toCSV(results_proxqp_vel, "results/rh5v2_vel_proxqp.csv"); - toCSV(results_eiquadprog_vel, "results/rh5v2_vel_eiquadprog.csv"); - toCSV(results_qp_oases_acc, "results/rh5v2_acc_qpoases.csv"); - toCSV(results_qp_swift_acc, "results/rh5v2_acc_qpswift.csv"); - toCSV(results_proxqp_acc, "results/rh5v2_acc_proxqp.csv"); - toCSV(results_eiquadprog_acc, "results/rh5v2_acc_eiquadprog.csv"); - - cout << " ----------- Results VelocitySceneQuadraticCost (QPOases) -----------" << endl; - printResults(results_qp_oases_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (QPSwift) -----------" << endl; - printResults(results_qp_swift_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (ProxQP) -----------" << endl; - printResults(results_proxqp_vel); - cout << " ----------- Results VelocitySceneQuadraticCost (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_vel); - cout << " ----------- Results AccelerationSceneReducedTSID (QPOases) -----------" << endl; - printResults(results_qp_oases_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (QPSwift) -----------" << endl; - printResults(results_qp_swift_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (ProxQP) -----------" << endl; - printResults(results_proxqp_acc); - cout << " ----------- Results AccelerationSceneReducedTSID (Eiquadprog) -----------" << endl; - printResults(results_eiquadprog_acc); -} - -void runBenchmarks(int n_samples){ - boost::filesystem::create_directory("results"); - - runRH5Benchmarks(n_samples); - runRH5v2Benchmarks(n_samples); - runKUKAIiwaBenchmarks(n_samples); - runRH5SingleLegBenchmarks(n_samples); - runRH5LegsBenchmarks(n_samples); -} - -int main(){ - srand(time(NULL)); - int n_samples = 1000; - runBenchmarks(n_samples); -} diff --git a/benchmarks/solvers/plot_results.py b/benchmarks/solvers/plot_results.py deleted file mode 100644 index 69dbfdab..00000000 --- a/benchmarks/solvers/plot_results.py +++ /dev/null @@ -1,75 +0,0 @@ -import matplotlib.pyplot as plt -import pandas as pd -import numpy as np -import sys -from IPython import embed - -robots = ["rh5_single_leg", - "rh5_legs", - "rh5", - "rh5v2"] -solvers = ["qpoases", - "eiquadprog", - "qpswift", - "proxqp"] -scenes = ["vel","acc"] - -if(len(sys.argv) < 2): - raise Exception("Invalid number of command line args. Usage: plot_results.py ") -folder = sys.argv[1] - -def getAvgSceneSolveTime(csv_list, robot, wbc_type, solver): - key = robot+"_"+wbc_type+"_"+solver - return csv_list[key]["scene_solve"].values - -def plotSceneSolveTime(csv_list, wbc_type, title): - fig = plt.figure() - plt.title(title, fontsize = 20) - ax = fig.add_subplot(111) - width = 0.1 - - data_qpoases = [getAvgSceneSolveTime(csv_list, r, wbc_type, "qpoases") for r in robots] - data_eiquadprog = [getAvgSceneSolveTime(csv_list, r, wbc_type, "eiquadprog") for r in robots] - data_qpswift = [getAvgSceneSolveTime(csv_list, r, wbc_type, "qpswift") for r in robots] - data_proxqp = [getAvgSceneSolveTime(csv_list, r, wbc_type, "proxqp") for r in robots] - - positions_group_eiquadprog = np.array(range(len(data_eiquadprog)))-1.5*width - positions_group_qpoases = np.array(range(len(data_qpoases)))-0.5*width - positions_group_qpswift = np.array(range(len(data_qpswift)))+0.5*width - positions_group_proxqp = np.array(range(len(data_proxqp)))+1.5*width - - data = [data_eiquadprog,data_qpoases,data_qpswift,data_proxqp] - positions = [positions_group_eiquadprog,positions_group_qpoases,positions_group_qpswift,positions_group_proxqp] - - bplots = [] - for d,p in zip(data,positions): - bplots.append(plt.boxplot(d, positions=p, widths=width,showfliers=False, patch_artist=True)) - - plt.xticks(list(range(len(robots))), robots, fontsize=20) - plt.grid(True) - - colors = ['blue','orange','green','yellow'] - for bp,c in zip(bplots,colors): - for box in bp['boxes']: - box.set_facecolor(color=c) - for m in bp['medians']: - m.set_color(color='black') - - ax.legend([bp["boxes"][0] for bp in bplots], ['Eiquadprog', 'QPOases','QPSwift','proxQP'], loc='upper left', fontsize=20) - plt.xlabel("Robots", fontsize=30, labelpad=20) - plt.ylabel("Time [ms]", fontsize=30, labelpad=20) - -def plotAll(robots, solvers): - csv_list = {} - # load all csv files - for r in robots: - for b in scenes: - for s in solvers: - key = r + "_" + b + "_" + s - csv_list[key] = pd.read_csv(folder + "/" + key + ".csv",sep=" ") - - plotSceneSolveTime(csv_list, "acc", "Scene Solve (Acceleration)") - plotSceneSolveTime(csv_list, "vel", "Scene Solve (Velocity)") - -plotAll(robots, solvers) -plt.show() diff --git a/bin/CMakeLists.txt b/bin/CMakeLists.txt deleted file mode 100644 index 837dba56..00000000 --- a/bin/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -FIND_PACKAGE( Boost COMPONENTS program_options REQUIRED ) - -add_executable(joint_limits_from_urdf joint_limits_from_urdf.cpp) -target_link_libraries(joint_limits_from_urdf wbc-tools Boost::program_options) - -install(TARGETS joint_limits_from_urdf - LIBRARY RUNTIME DESTINATION bin) - - diff --git a/bin/joint_limits_from_urdf.cpp b/bin/joint_limits_from_urdf.cpp deleted file mode 100644 index 817a0703..00000000 --- a/bin/joint_limits_from_urdf.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include -#include -#include -#include - -using namespace wbc; -using namespace std; -namespace po = boost::program_options; - -int main(int argc, char *argv[]){ - argc--; - argv++; - - bool as_yaml = false; - - // Declare the supported options. - po::options_description desc("Usage: joint_limits_from_urdf . Possible options are"); - desc.add_options() - ("help", "produce help message") - ("yml", "print in yaml format") - ; - - po::variables_map vm; - po::store(po::parse_command_line(argc, argv, desc), vm); - po::notify(vm); - - if (vm.count("help")) { - cout << desc << "\n"; - return 1; - } - if(vm.count("yml")) - as_yaml = true; - - if(argc < 1){ - cout << desc << "\n"; - return -1; - } - - std::string filename = argv[0]; - base::JointLimits joint_limits; - URDFTools::jointLimitsFromURDF(filename, joint_limits); - - if(as_yaml){ - printf("joint_limits:\n"); - printf(" names:\n"); - for(auto n : joint_limits.names) printf(" - %s\n", n.c_str()); - printf(" elements:\n"); - printf(" [\n"); - for(auto n : joint_limits.names){ - base::JointLimitRange r = joint_limits[n]; - printf(" {max: {position: %2.6f, speed: %2.6f, effort: %2.6f}, min: {position %2.6f}},\n", r.max.position, r.max.speed, r.max.effort, r.min.position); - } - printf(" ]\n"); - } - else{ - for(auto n : joint_limits.names){ - printf("%25s: \tMax Pos: %2.6f \tMin Pos: %2.6f \tMax Vel: %2.6f \tMax Eff: %2.6f\n", - n.c_str(), joint_limits[n].max.position, joint_limits[n].min.position, joint_limits[n].max.speed, joint_limits[n].max.effort); - } - } - - return 0; -} diff --git a/manifest.xml b/manifest.xml index d28446ba..9dffefef 100644 --- a/manifest.xml +++ b/manifest.xml @@ -30,11 +30,11 @@ - + - + diff --git a/scripts/full_install.sh b/scripts/full_install.sh index ca06b4cc..98917026 100644 --- a/scripts/full_install.sh +++ b/scripts/full_install.sh @@ -91,6 +91,7 @@ make -j8 && sudo make install && cd ../.. # WBC mkdir wbc/build && cd wbc/build -cmake .. -DUSE_EIQUADPROG=1 -DUSE_KDL=1 -DUSE_PINOCCHIO=1 -DUSE_QPSWIFT=1 -DUSE_PROXQP=1 -make -j8 && sudo make install && cd ../.. +cmake .. -DROBOT_MODEL_RBDL=ON -DROBOT_MODEL_KDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DCMAKE_BUILD_TYPE=RELEASE +make -j8 && sudo make install && cd .. + sudo ldconfig diff --git a/scripts/install.sh b/scripts/install.sh index 8d99f428..940e7c25 100644 --- a/scripts/install.sh +++ b/scripts/install.sh @@ -23,12 +23,11 @@ sudo apt-get -y install liburdfdom-headers-dev liburdfdom-dev # Clone WBC repo here to have the patches git clone https://github.com/ARC-OPT/wbc.git -# RBDL -git clone --branch v3.2.1 --recurse-submodules https://github.com/rbdl/rbdl.git -cd rbdl -git apply ../wbc/patches/rbdl.patch --ignore-whitespace +# Pinocchio +git clone --branch v2.6.8 --recurse-submodules https://github.com/stack-of-tasks/pinocchio.git +cd pinocchio mkdir build && cd build -cmake .. -DRBDL_BUILD_ADDON_URDFREADER=ON +cmake .. -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_UNIT_TESTS=OFF -DCMAKE_BUILD_TYPE=RELEASE make -j8 && sudo make install && cd ../.. # If not done yet, setup a ssh key pair using the command `ssh-keygen` and add the @@ -44,6 +43,6 @@ cmake .. && make -j8 && sudo make install && cd ../.. # WBC mkdir wbc/build && cd wbc/build -cmake .. +cmake .. -DCMAKE_BUILD_TYPE=RELEASE make -j8 && sudo make install sudo ldconfig diff --git a/scripts/run_tests.sh b/scripts/run_tests.sh index bc4cfeb1..9f025709 100644 --- a/scripts/run_tests.sh +++ b/scripts/run_tests.sh @@ -1,77 +1,99 @@ # Controllers echo "Testing controllers ..." -cd build/test/controllers +cd build/src +cd controllers/test ./test_pid_controllers ./test_pos_pd_controllers ./test_pot_field_controllers +cd ../.. # Core echo "Testing core library ..." -cd ../core +cd core/test ./test_core +cd ../.. # Robot Models echo "Testing robot models ..." -echo "Testing RobotModelRBDL ..." -cd ../robot_models/rbdl -./test_robot_model_rbdl -cd .. +echo "Testing RobotModelPinocchio ..." +cd robot_models/pinocchio/test +./test_robot_model_pinocchio +cd ../.. if [ -d "kdl" ]; then echo "Testing RobotModelKDL ..." - cd kdl + cd kdl/test ./test_robot_model_kdl - cd .. + cd ../.. fi -if [ -d "pinocchio" ]; then - echo "Testing RobotModelPinocchio ..." - cd ../robot_models/pinocchio - ./test_robot_model_pinocchio - cd .. +if [ -d "rbdl" ]; then + echo "Testing RobotModelRBDL ..." + cd rbdl/test + ./test_robot_model_rbdl + cd ../.. fi if [ -d "hyrodyn" ]; then echo "Testing RobotModelHyrodyn ..." - cd hyrodyn + cd hyrodyn/test ./test_robot_model_hyrodyn - cd .. + cd ../.. fi +cd .. # Scenes echo "Testing scenes ..." -cd ../scenes + echo "Testing VelocityScene ..." +cd scenes/velocity/test ./test_velocity_scene +cd ../.. + echo "Testing VelocitySceneQuadraticCost ..." +cd velocity_qp/test ./test_velocity_scene_quadratic_cost +cd ../.. + + echo "Testing AccelerationScene ..." +cd acceleration/test ./test_acceleration_scene +cd ../.. + echo "Testing AccelerationSceneTSID ..." +cd acceleration_tsid/test ./test_acceleration_scene_tsid +cd ../.. + echo "Testing AccelerationSceneReducedTSID ..." +cd acceleration_reduced_tsid/test ./test_acceleration_scene_reduced_tsid +cd ../../.. # Solvers echo "Testing hls ..." -cd ../solvers/hls +cd solvers/hls/test ./test_hls_solver +cd ../.. + echo "Testing QPOasesSolver ..." -cd ../qpoases +cd qpoases/test ./test_qpoases_solver -cd .. +cd ../.. if [ -d "eiquadprog" ]; then echo "Testing EiquadprogSolver ..." - cd eiquadprog + cd eiquadprog/test ./test_eiquadprog_solver - cd .. + cd ../.. fi if [ -d "proxqp" ]; then echo "Testing ProxQPSolver ..." - cd proxqp + cd proxqp/test ./test_proxqp_solver - cd .. + cd ../.. fi if [ -d "qpswift" ]; then echo "Testing QPSwiftSolver ..." - cd qpswift + cd qpswift/test ./test_qpswift_solver - cd ../../.. + cd ../.. fi +cd .. diff --git a/src/controllers/ActivationFunction.hpp b/src/controllers/ActivationFunction.hpp index 63a86c75..4b2a563e 100644 --- a/src/controllers/ActivationFunction.hpp +++ b/src/controllers/ActivationFunction.hpp @@ -4,7 +4,7 @@ #include #include -namespace ctrl_lib{ +namespace wbc{ enum activationType{NO_ACTIVATION, /** Activation values will always be one */ STEP_ACTIVATION, /** If input > threshold, activation will be one, else 0 */ diff --git a/src/controllers/CMakeLists.txt b/src/controllers/CMakeLists.txt index 8930d085..cd8b4e35 100644 --- a/src/controllers/CMakeLists.txt +++ b/src/controllers/CMakeLists.txt @@ -20,3 +20,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/controllers) + +add_subdirectory(test) diff --git a/src/controllers/CartesianForcePIDController.cpp b/src/controllers/CartesianForcePIDController.cpp index a116c93d..ebbec19c 100644 --- a/src/controllers/CartesianForcePIDController.cpp +++ b/src/controllers/CartesianForcePIDController.cpp @@ -1,6 +1,6 @@ #include "CartesianForcePIDController.hpp" -namespace ctrl_lib { +namespace wbc { CartesianForcePIDController::CartesianForcePIDController() : PIDController(6){ diff --git a/src/controllers/CartesianForcePIDController.hpp b/src/controllers/CartesianForcePIDController.hpp index 553814ce..7d836282 100644 --- a/src/controllers/CartesianForcePIDController.hpp +++ b/src/controllers/CartesianForcePIDController.hpp @@ -5,7 +5,7 @@ #include #include -namespace ctrl_lib { +namespace wbc { /** * @brief CartesianForcePIDController implements a PID Controller on a Wrench data type diff --git a/src/controllers/CartesianPosPDController.cpp b/src/controllers/CartesianPosPDController.cpp index 518999d9..f54d0f04 100644 --- a/src/controllers/CartesianPosPDController.cpp +++ b/src/controllers/CartesianPosPDController.cpp @@ -2,7 +2,7 @@ #include "ControllerTools.hpp" #include -namespace ctrl_lib{ +namespace wbc{ CartesianPosPDController::CartesianPosPDController() : PosPDController(6){ diff --git a/src/controllers/CartesianPosPDController.hpp b/src/controllers/CartesianPosPDController.hpp index bf3657d1..643dbb33 100644 --- a/src/controllers/CartesianPosPDController.hpp +++ b/src/controllers/CartesianPosPDController.hpp @@ -1,10 +1,10 @@ -#ifndef CTRL_LIB_CART_POS_PD_CONTROLLER_HPP -#define CTRL_LIB_CART_POS_PD_CONTROLLER_HPP +#ifndef WBC_CART_POS_PD_CONTROLLER_HPP +#define WBC_CART_POS_PD_CONTROLLER_HPP #include "PosPDController.hpp" #include -namespace ctrl_lib { +namespace wbc { /** * @brief The CartesianPosPDController class implements a PD Controller with feed forward on the RigidBodyStateSE3 type. The following control schemes are available: diff --git a/src/controllers/CartesianPotentialFieldsController.cpp b/src/controllers/CartesianPotentialFieldsController.cpp index ff9589a6..16a24a8a 100644 --- a/src/controllers/CartesianPotentialFieldsController.cpp +++ b/src/controllers/CartesianPotentialFieldsController.cpp @@ -1,7 +1,7 @@ #include "CartesianPotentialFieldsController.hpp" #include -using namespace ctrl_lib; +using namespace wbc; CartesianPotentialFieldsController::CartesianPotentialFieldsController() : PotentialFieldsController(3){ diff --git a/src/controllers/CartesianPotentialFieldsController.hpp b/src/controllers/CartesianPotentialFieldsController.hpp index ee440e90..854f6c36 100644 --- a/src/controllers/CartesianPotentialFieldsController.hpp +++ b/src/controllers/CartesianPotentialFieldsController.hpp @@ -4,7 +4,7 @@ #include "PotentialFieldsController.hpp" #include -namespace ctrl_lib{ +namespace wbc{ /** * @brief The PotentialFieldsController class implements a multi potential field controller in Cartesian space. diff --git a/src/controllers/JointLimitAvoidanceController.cpp b/src/controllers/JointLimitAvoidanceController.cpp index ee47d722..5e779e33 100644 --- a/src/controllers/JointLimitAvoidanceController.cpp +++ b/src/controllers/JointLimitAvoidanceController.cpp @@ -4,7 +4,7 @@ using namespace std; -namespace ctrl_lib { +namespace wbc { JointLimitAvoidanceController::JointLimitAvoidanceController(const base::JointLimits& limits, const base::VectorXd &influence_distance) : diff --git a/src/controllers/JointLimitAvoidanceController.hpp b/src/controllers/JointLimitAvoidanceController.hpp index eacb7fb6..6291ab38 100644 --- a/src/controllers/JointLimitAvoidanceController.hpp +++ b/src/controllers/JointLimitAvoidanceController.hpp @@ -6,7 +6,7 @@ #include #include -namespace ctrl_lib { +namespace wbc { class JointLimitAvoidanceController : public PotentialFieldsController{ protected: diff --git a/src/controllers/JointPosPDController.cpp b/src/controllers/JointPosPDController.cpp index 38ad7925..ab35ed5f 100644 --- a/src/controllers/JointPosPDController.cpp +++ b/src/controllers/JointPosPDController.cpp @@ -1,6 +1,6 @@ #include "JointPosPDController.hpp" -namespace ctrl_lib{ +namespace wbc{ JointPosPDController::JointPosPDController(const std::vector& joint_names) : PosPDController(joint_names.size()), diff --git a/src/controllers/JointPosPDController.hpp b/src/controllers/JointPosPDController.hpp index 15b65dd7..6984a51c 100644 --- a/src/controllers/JointPosPDController.hpp +++ b/src/controllers/JointPosPDController.hpp @@ -1,10 +1,10 @@ -#ifndef CTRL_LIB_JOINT_POS_PD_CONTROLLER_HPP -#define CTRL_LIB_JOINT_POS_PD_CONTROLLER_HPP +#ifndef WBC_JOINT_POS_PD_CONTROLLER_HPP +#define WBC_JOINT_POS_PD_CONTROLLER_HPP #include "PosPDController.hpp" #include -namespace ctrl_lib { +namespace wbc { /** * @brief The JointPosPDController class implements a PD Controller with feed forward on the base-Joints type. The following control schemes are available: diff --git a/src/controllers/JointTorquePIDController.cpp b/src/controllers/JointTorquePIDController.cpp index 5b21e9df..7ccfd64a 100644 --- a/src/controllers/JointTorquePIDController.cpp +++ b/src/controllers/JointTorquePIDController.cpp @@ -1,6 +1,6 @@ #include "JointTorquePIDController.hpp" -namespace ctrl_lib{ +namespace wbc{ JointTorquePIDController::JointTorquePIDController(const std::vector& joint_names) : PIDController(joint_names.size()), diff --git a/src/controllers/JointTorquePIDController.hpp b/src/controllers/JointTorquePIDController.hpp index 3c52a130..21ba19ec 100644 --- a/src/controllers/JointTorquePIDController.hpp +++ b/src/controllers/JointTorquePIDController.hpp @@ -4,7 +4,7 @@ #include "PIDController.hpp" #include -namespace ctrl_lib{ +namespace wbc{ class JointTorquePIDController : public PIDController{ protected: diff --git a/src/controllers/PIDController.cpp b/src/controllers/PIDController.cpp index fadeddde..f488367f 100644 --- a/src/controllers/PIDController.cpp +++ b/src/controllers/PIDController.cpp @@ -2,7 +2,7 @@ using namespace std; -namespace ctrl_lib{ +namespace wbc{ PIDController::PIDController(uint dimension) : dimension(dimension){ diff --git a/src/controllers/PIDController.hpp b/src/controllers/PIDController.hpp index 4793a5c4..4eb63b6e 100644 --- a/src/controllers/PIDController.hpp +++ b/src/controllers/PIDController.hpp @@ -4,7 +4,7 @@ #include #include "PIDCtrlParams.hpp" -namespace ctrl_lib{ +namespace wbc{ /** * @brief The PIDController class implements an n-dimensional PID controller diff --git a/src/controllers/PIDCtrlParams.hpp b/src/controllers/PIDCtrlParams.hpp index 85f8cb8d..5fbd10fd 100644 --- a/src/controllers/PIDCtrlParams.hpp +++ b/src/controllers/PIDCtrlParams.hpp @@ -3,7 +3,7 @@ #include -namespace ctrl_lib{ +namespace wbc{ class PIDCtrlParams{ public: diff --git a/src/controllers/PlanarPotentialField.cpp b/src/controllers/PlanarPotentialField.cpp index e2ab3cc6..81afe5d0 100644 --- a/src/controllers/PlanarPotentialField.cpp +++ b/src/controllers/PlanarPotentialField.cpp @@ -1,6 +1,6 @@ #include "PlanarPotentialField.hpp" -using namespace ctrl_lib; +using namespace wbc; PlanarPotentialField::PlanarPotentialField(const std::string &_name) : PotentialField(3, _name){ diff --git a/src/controllers/PlanarPotentialField.hpp b/src/controllers/PlanarPotentialField.hpp index 73b0cfa3..952425fe 100644 --- a/src/controllers/PlanarPotentialField.hpp +++ b/src/controllers/PlanarPotentialField.hpp @@ -2,7 +2,7 @@ #include "PotentialField.hpp" -namespace ctrl_lib{ +namespace wbc{ class PotentialField; diff --git a/src/controllers/PosPDController.cpp b/src/controllers/PosPDController.cpp index 1c3cdaea..cbb8fa79 100644 --- a/src/controllers/PosPDController.cpp +++ b/src/controllers/PosPDController.cpp @@ -1,6 +1,6 @@ #include "PosPDController.hpp" -namespace ctrl_lib { +namespace wbc { PosPDController::PosPDController(size_t dim_controller) : dim_controller(dim_controller){ diff --git a/src/controllers/PosPDController.hpp b/src/controllers/PosPDController.hpp index 7cec9ce2..8a411b6f 100644 --- a/src/controllers/PosPDController.hpp +++ b/src/controllers/PosPDController.hpp @@ -3,7 +3,7 @@ #include -namespace ctrl_lib { +namespace wbc { /** * @brief The PosPDController class implements the following two control scemes diff --git a/src/controllers/PotentialField.hpp b/src/controllers/PotentialField.hpp index 8555a18e..f6bbb7b8 100644 --- a/src/controllers/PotentialField.hpp +++ b/src/controllers/PotentialField.hpp @@ -5,7 +5,7 @@ #include #include -namespace ctrl_lib{ +namespace wbc{ /** * @brief Base class for potential fields diff --git a/src/controllers/PotentialFieldInfo.hpp b/src/controllers/PotentialFieldInfo.hpp index fd80cebc..f06e8063 100644 --- a/src/controllers/PotentialFieldInfo.hpp +++ b/src/controllers/PotentialFieldInfo.hpp @@ -3,7 +3,7 @@ #include "PotentialField.hpp" -namespace ctrl_lib { +namespace wbc { struct PotentialFieldInfo{ diff --git a/src/controllers/PotentialFieldsController.cpp b/src/controllers/PotentialFieldsController.cpp index 0c90eec9..574339fc 100644 --- a/src/controllers/PotentialFieldsController.cpp +++ b/src/controllers/PotentialFieldsController.cpp @@ -2,7 +2,7 @@ #include #include -using namespace ctrl_lib; +using namespace wbc; using namespace std; PotentialFieldsController::PotentialFieldsController(const uint _dimension) diff --git a/src/controllers/PotentialFieldsController.hpp b/src/controllers/PotentialFieldsController.hpp index 69b203a5..3e58c3c8 100644 --- a/src/controllers/PotentialFieldsController.hpp +++ b/src/controllers/PotentialFieldsController.hpp @@ -4,7 +4,7 @@ #include "PotentialFieldInfo.hpp" #include -namespace ctrl_lib{ +namespace wbc{ /** * @brief Base class for potential field controllers diff --git a/src/controllers/RadialPotentialField.cpp b/src/controllers/RadialPotentialField.cpp index a5d341d9..1b12270b 100644 --- a/src/controllers/RadialPotentialField.cpp +++ b/src/controllers/RadialPotentialField.cpp @@ -1,6 +1,6 @@ #include "RadialPotentialField.hpp" -using namespace ctrl_lib; +using namespace wbc; RadialPotentialField::RadialPotentialField(const uint _dimension, const std::string& _name) : PotentialField(_dimension, _name){ diff --git a/src/controllers/RadialPotentialField.hpp b/src/controllers/RadialPotentialField.hpp index 678b02cd..a2f7a479 100644 --- a/src/controllers/RadialPotentialField.hpp +++ b/src/controllers/RadialPotentialField.hpp @@ -2,7 +2,7 @@ #include "PotentialField.hpp" -namespace ctrl_lib{ +namespace wbc{ /** * @brief Radial Potential field. The computed gradient will be constant on volumnes with diff --git a/test/controllers/CMakeLists.txt b/src/controllers/test/CMakeLists.txt similarity index 76% rename from test/controllers/CMakeLists.txt rename to src/controllers/test/CMakeLists.txt index 04e172da..9dddbcc0 100644 --- a/test/controllers/CMakeLists.txt +++ b/src/controllers/test/CMakeLists.txt @@ -1,15 +1,16 @@ -add_executable(test_pot_field_controllers test_pot_field_controllers.cpp ../suite.cpp) +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_pot_field_controllers test_pot_field_controllers.cpp) target_link_libraries(test_pot_field_controllers wbc-controllers Boost::unit_test_framework) -add_executable(test_pos_pd_controllers test_pos_pd_controllers.cpp ../suite.cpp) +add_executable(test_pos_pd_controllers test_pos_pd_controllers.cpp) target_link_libraries(test_pos_pd_controllers wbc-controllers Boost::unit_test_framework) -add_executable(test_pid_controllers test_pid_controllers.cpp ../suite.cpp) +add_executable(test_pid_controllers test_pid_controllers.cpp) target_link_libraries(test_pid_controllers wbc-controllers Boost::unit_test_framework) diff --git a/test/controllers/test_pid_controllers.cpp b/src/controllers/test/test_pid_controllers.cpp similarity index 96% rename from test/controllers/test_pid_controllers.cpp rename to src/controllers/test/test_pid_controllers.cpp index 98b64a42..e4e3fe64 100644 --- a/test/controllers/test_pid_controllers.cpp +++ b/src/controllers/test/test_pid_controllers.cpp @@ -1,11 +1,13 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include -#include "controllers/JointTorquePIDController.hpp" +#include "../JointTorquePIDController.hpp" #include using namespace std; -using namespace ctrl_lib; +using namespace wbc; BOOST_AUTO_TEST_CASE(configuration_test){ @@ -86,4 +88,3 @@ BOOST_AUTO_TEST_CASE(joint_torque_controller_test){ usleep(dt*1000*1000); } } - diff --git a/test/controllers/test_pos_pd_controllers.cpp b/src/controllers/test/test_pos_pd_controllers.cpp similarity index 97% rename from test/controllers/test_pos_pd_controllers.cpp rename to src/controllers/test/test_pos_pd_controllers.cpp index 6293c7a9..9963613a 100644 --- a/test/controllers/test_pos_pd_controllers.cpp +++ b/src/controllers/test/test_pos_pd_controllers.cpp @@ -1,14 +1,16 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include #include -#include "controllers/CartesianPosPDController.hpp" -#include "controllers/JointPosPDController.hpp" -#include +#include "../CartesianPosPDController.hpp" +#include "../JointPosPDController.hpp" +#include "../ControllerTools.hpp" #include using namespace std; -using namespace ctrl_lib; +using namespace wbc; BOOST_AUTO_TEST_CASE(configuration_test){ @@ -176,4 +178,3 @@ BOOST_AUTO_TEST_CASE(jnt_pos_pd_controller){ usleep(dt*1000*1000); } } - diff --git a/test/controllers/test_pot_field_controllers.cpp b/src/controllers/test/test_pot_field_controllers.cpp similarity index 97% rename from test/controllers/test_pot_field_controllers.cpp rename to src/controllers/test/test_pot_field_controllers.cpp index 694d0482..15c4d5c9 100644 --- a/test/controllers/test_pot_field_controllers.cpp +++ b/src/controllers/test/test_pot_field_controllers.cpp @@ -1,14 +1,16 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include #include -#include "controllers/CartesianPotentialFieldsController.hpp" -#include "controllers/RadialPotentialField.hpp" -#include "controllers/PlanarPotentialField.hpp" -#include "controllers/JointLimitAvoidanceController.hpp" +#include "../CartesianPotentialFieldsController.hpp" +#include "../RadialPotentialField.hpp" +#include "../PlanarPotentialField.hpp" +#include "../JointLimitAvoidanceController.hpp" using namespace std; -using namespace ctrl_lib; +using namespace wbc; void runPotentialFieldController(std::string filename, CartesianPotentialFieldsController* ctrl, @@ -307,4 +309,3 @@ BOOST_AUTO_TEST_CASE(joint_limit_avoidance) usleep(dt*1000*1000); } } - diff --git a/src/core/CMakeLists.txt b/src/core/CMakeLists.txt index cfc80559..2eb3c5b0 100644 --- a/src/core/CMakeLists.txt +++ b/src/core/CMakeLists.txt @@ -21,3 +21,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/core) + +add_subdirectory(test) diff --git a/src/core/RobotModelConfig.hpp b/src/core/RobotModelConfig.hpp index 72ddb74d..352049a1 100644 --- a/src/core/RobotModelConfig.hpp +++ b/src/core/RobotModelConfig.hpp @@ -33,7 +33,7 @@ struct RobotModelConfig{ public: RobotModelConfig(){ floating_base = false; - type = "rbdl"; + type = "pinocchio"; } RobotModelConfig(const std::string& file_or_string, const bool floating_base = false, @@ -41,7 +41,7 @@ struct RobotModelConfig{ const std::string& submechanism_file = "") : file_or_string(file_or_string), submechanism_file(submechanism_file), - type("rbdl"), + type("pinocchio"), floating_base(floating_base), contact_points(contact_points){ @@ -59,7 +59,7 @@ struct RobotModelConfig{ std::string file_or_string; /** Only Hyrodyn models: Absolute path to submechanism file, which describes the kinematic structure including parallel mechanisms.*/ std::string submechanism_file; - /** Model type. Must be the exact name of one of the registered robot model plugins. See src/robot_models for all available plugins. Default is rbdl*/ + /** Model type. Must be the exact name of one of the registered robot model plugins. See src/robot_models for all available plugins. Default is pinocchio*/ std::string type; /** Optional: Attach a virtual 6 DoF floating base to the model: Naming scheme of the joints is currently fixed: * floating_base_trans_x, floating_base_trans_y, floating_base_trans_z, diff --git a/src/core/test/CMakeLists.txt b/src/core/test/CMakeLists.txt new file mode 100644 index 00000000..0577417d --- /dev/null +++ b/src/core/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_core test_core.cpp) +target_link_libraries(test_core + wbc-core + Boost::unit_test_framework) diff --git a/test/core/test_core.cpp b/src/core/test/test_core.cpp similarity index 94% rename from test/core/test_core.cpp rename to src/core/test/test_core.cpp index 52948db3..e00bed32 100644 --- a/test/core/test_core.cpp +++ b/src/core/test/test_core.cpp @@ -1,3 +1,5 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include #include @@ -76,15 +78,15 @@ BOOST_AUTO_TEST_CASE(task_config){ } BOOST_AUTO_TEST_CASE(robot_model_factory){ - BOOST_CHECK_NO_THROW(PluginLoader::loadPlugin("libwbc-robot_models-rbdl.so")); + BOOST_CHECK_NO_THROW(PluginLoader::loadPlugin("libwbc-robot_models-pinocchio.so")); PluginLoader::PluginMap *plugin_map = PluginLoader::getPluginMap(); - BOOST_CHECK(plugin_map->count("libwbc-robot_models-rbdl.so") == 1); + BOOST_CHECK(plugin_map->count("libwbc-robot_models-pinocchio.so") == 1); RobotModelFactory::RobotModelMap *robot_model_map = RobotModelFactory::getRobotModelMap(); - BOOST_CHECK(robot_model_map->count("rbdl") == 1); + BOOST_CHECK(robot_model_map->count("pinocchio") == 1); RobotModel* model; - BOOST_CHECK_NO_THROW(model = RobotModelFactory::createInstance("rbdl")); + BOOST_CHECK_NO_THROW(model = RobotModelFactory::createInstance("pinocchio")); BOOST_CHECK(model != 0); - BOOST_CHECK_NO_THROW(PluginLoader::unloadPlugin("libwbc-robot_models-rbdl.so")); + BOOST_CHECK_NO_THROW(PluginLoader::unloadPlugin("libwbc-robot_models-pinocchio.so")); } BOOST_AUTO_TEST_CASE(qp_solver_factory){ @@ -110,4 +112,3 @@ BOOST_AUTO_TEST_CASE(scene_factory){ BOOST_CHECK_NO_THROW(scene = SceneFactory::createInstance("velocity", robot_model, solver, 1e-3)); BOOST_CHECK(scene != 0); } - diff --git a/src/robot_models/CMakeLists.txt b/src/robot_models/CMakeLists.txt index 590e4ce2..132cd746 100644 --- a/src/robot_models/CMakeLists.txt +++ b/src/robot_models/CMakeLists.txt @@ -1,11 +1,11 @@ -add_subdirectory(rbdl) -if(USE_PINOCCHIO) - add_subdirectory(pinocchio) +add_subdirectory(pinocchio) +if(ROBOT_MODEL_RBDL) + add_subdirectory(rbdl) endif() -if(USE_HYRODYN) +if(ROBOT_MODEL_HYRODYN) add_subdirectory(hyrodyn) endif() -if(USE_KDL) +if(ROBOT_MODEL_KDL) add_subdirectory(kdl) endif() diff --git a/src/robot_models/hyrodyn/CMakeLists.txt b/src/robot_models/hyrodyn/CMakeLists.txt index d25fe175..993ddd2b 100644 --- a/src/robot_models/hyrodyn/CMakeLists.txt +++ b/src/robot_models/hyrodyn/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/robot_models/hyrodyn) + +add_subdirectory(test) diff --git a/test/robot_models/hyrodyn/CMakeLists.txt b/src/robot_models/hyrodyn/test/CMakeLists.txt similarity index 61% rename from test/robot_models/hyrodyn/CMakeLists.txt rename to src/robot_models/hyrodyn/test/CMakeLists.txt index 06e3df93..c8d70644 100644 --- a/test/robot_models/hyrodyn/CMakeLists.txt +++ b/src/robot_models/hyrodyn/test/CMakeLists.txt @@ -1,5 +1,5 @@ -add_executable(test_robot_model_hyrodyn test_robot_model_hyrodyn.cpp ../../suite.cpp ../test_robot_model.cpp) +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_robot_model_hyrodyn test_robot_model_hyrodyn.cpp ../../test/test_robot_model.cpp) target_link_libraries(test_robot_model_hyrodyn wbc-robot_models-hyrodyn Boost::unit_test_framework) - diff --git a/test/robot_models/hyrodyn/test_robot_model_hyrodyn.cpp b/src/robot_models/hyrodyn/test/test_robot_model_hyrodyn.cpp similarity index 79% rename from test/robot_models/hyrodyn/test_robot_model_hyrodyn.cpp rename to src/robot_models/hyrodyn/test/test_robot_model_hyrodyn.cpp index f7de5748..9d6aebfd 100644 --- a/test/robot_models/hyrodyn/test_robot_model_hyrodyn.cpp +++ b/src/robot_models/hyrodyn/test/test_robot_model_hyrodyn.cpp @@ -1,6 +1,8 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include "robot_models/hyrodyn/RobotModelHyrodyn.hpp" #include -#include "../test_robot_model.hpp" +#include "../../test/test_robot_model.hpp" using namespace std; using namespace wbc; @@ -25,8 +27,8 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ "floating_base_rot_x", "floating_base_rot_y", "floating_base_rot_z"}; // Valid config - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; BOOST_CHECK(robot_model.configure(config) == true); for(size_t i = 0; i < robot_model.noOfJoints(); i++){ BOOST_CHECK(joint_names[i] == robot_model.jointNames()[i]); @@ -45,17 +47,17 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ } // Invalid filename - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urd"); + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urd"); BOOST_CHECK(robot_model.configure(config) == false); // Invalid submechanism file - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa.ym"; + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa.ym"; BOOST_CHECK(robot_model.configure(config) == false); // Valid config with floating base - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; config.floating_base = true; BOOST_CHECK(robot_model.configure(config) == true); for(size_t i = 0; i < 6; i++){ @@ -90,15 +92,15 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ } // Config with contact points - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; config.contact_points.names.push_back("kuka_lbr_l_tcp"); config.contact_points.elements.push_back(wbc::ActiveContact(1,0.6)); BOOST_CHECK(robot_model.configure(config) == true); // Config with invalid contact points - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; config.contact_points.names.push_back("XYZ"); config.contact_points.elements.push_back(wbc::ActiveContact(1,0.6)); BOOST_CHECK(robot_model.configure(config) == false); @@ -106,12 +108,12 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ BOOST_AUTO_TEST_CASE(fk){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); RobotModelConfig cfg(urdf_file); - cfg.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; + cfg.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; cfg.floating_base = true; BOOST_CHECK(robot_model->configure(cfg)); @@ -120,12 +122,12 @@ BOOST_AUTO_TEST_CASE(fk){ BOOST_AUTO_TEST_CASE(space_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); RobotModelConfig cfg(urdf_file); - cfg.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; + cfg.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; cfg.floating_base = true; BOOST_CHECK(robot_model->configure(cfg)); @@ -133,12 +135,12 @@ BOOST_AUTO_TEST_CASE(space_jacobian){ } BOOST_AUTO_TEST_CASE(body_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); RobotModelConfig cfg(urdf_file); - cfg.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; + cfg.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; cfg.floating_base = true; BOOST_CHECK(robot_model->configure(cfg)); @@ -146,12 +148,12 @@ BOOST_AUTO_TEST_CASE(body_jacobian){ } BOOST_AUTO_TEST_CASE(com_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); RobotModelConfig cfg(urdf_file); - cfg.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; + cfg.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; cfg.floating_base = false; BOOST_CHECK(robot_model->configure(cfg)); @@ -160,11 +162,11 @@ BOOST_AUTO_TEST_CASE(com_jacobian){ BOOST_AUTO_TEST_CASE(dynamics){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; RobotModelPtr robot_model = make_shared(); RobotModelConfig cfg(urdf_file); - cfg.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; + cfg.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa.yml"; cfg.floating_base = false; BOOST_CHECK(robot_model->configure(cfg)); @@ -182,14 +184,14 @@ BOOST_AUTO_TEST_CASE(compare_serial_vs_hybrid_model){ string tip = "LLAnklePitch_Link"; RobotModelHyrodyn robot_model_hybrid; - RobotModelConfig config_hybrid("../../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"); - config_hybrid.submechanism_file = "../../../../models/rh5/hyrodyn/rh5_single_leg_hybrid.yml"; + RobotModelConfig config_hybrid("../../../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"); + config_hybrid.submechanism_file = "../../../../../models/rh5/hyrodyn/rh5_single_leg_hybrid.yml"; BOOST_CHECK(robot_model_hybrid.configure(config_hybrid) == true); RobotModelHyrodyn robot_model_serial; - RobotModelConfig config_serial("../../../../models/rh5/urdf/rh5_single_leg.urdf"); - config_serial.submechanism_file = "../../../../models/rh5/hyrodyn/rh5_single_leg.yml"; + RobotModelConfig config_serial("../../../../../models/rh5/urdf/rh5_single_leg.urdf"); + config_serial.submechanism_file = "../../../../../models/rh5/hyrodyn/rh5_single_leg.yml"; BOOST_CHECK(robot_model_serial.configure(config_serial) == true); base::samples::Joints joint_state; diff --git a/src/robot_models/kdl/CMakeLists.txt b/src/robot_models/kdl/CMakeLists.txt index 2b3dd15b..1d0f7fb4 100644 --- a/src/robot_models/kdl/CMakeLists.txt +++ b/src/robot_models/kdl/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/robot_models/kdl) + +add_subdirectory(test) diff --git a/test/robot_models/kdl/CMakeLists.txt b/src/robot_models/kdl/test/CMakeLists.txt similarity index 62% rename from test/robot_models/kdl/CMakeLists.txt rename to src/robot_models/kdl/test/CMakeLists.txt index fdf675c5..e7d3147c 100644 --- a/test/robot_models/kdl/CMakeLists.txt +++ b/src/robot_models/kdl/test/CMakeLists.txt @@ -1,5 +1,5 @@ -add_executable(test_robot_model_kdl test_robot_model_kdl.cpp ../../suite.cpp ../test_robot_model.cpp) +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_robot_model_kdl test_robot_model_kdl.cpp ../../test/test_robot_model.cpp) target_link_libraries(test_robot_model_kdl wbc-robot_models-kdl Boost::unit_test_framework) - diff --git a/test/robot_models/kdl/test_robot_model_kdl.cpp b/src/robot_models/kdl/test/test_robot_model_kdl.cpp similarity index 85% rename from test/robot_models/kdl/test_robot_model_kdl.cpp rename to src/robot_models/kdl/test/test_robot_model_kdl.cpp index 5a702939..0e3756a6 100644 --- a/test/robot_models/kdl/test_robot_model_kdl.cpp +++ b/src/robot_models/kdl/test/test_robot_model_kdl.cpp @@ -1,13 +1,15 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include -#include "robot_models/kdl/RobotModelKDL.hpp" -#include "robot_models/kdl/KinematicChainKDL.hpp" -#include "core/RobotModelConfig.hpp" +#include "../RobotModelKDL.hpp" +#include "../KinematicChainKDL.hpp" +#include #include #include #include -#include "tools/URDFTools.hpp" +#include #include -#include "../test_robot_model.hpp" +#include "../../test/test_robot_model.hpp" using namespace std; using namespace wbc; @@ -32,7 +34,7 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ "floating_base_rot_x", "floating_base_rot_y", "floating_base_rot_z"}; // Valid config - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); BOOST_CHECK(robot_model.configure(config) == true); for(size_t i = 0; i < robot_model.noOfJoints(); i++){ BOOST_CHECK(joint_names[i] == robot_model.jointNames()[i]); @@ -55,7 +57,7 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ } // Invalid filename - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urd"); + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urd"); BOOST_CHECK(robot_model.configure(config) == false); // Empty filename @@ -63,8 +65,8 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ BOOST_CHECK(robot_model.configure(config) == false); // Valid config with floating base - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; config.floating_base = true; BOOST_CHECK(robot_model.configure(config) == true); for(size_t i = 0; i < 6; i++){ @@ -99,13 +101,13 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ } // Config with contact points - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); config.contact_points.names.push_back("kuka_lbr_l_tcp"); config.contact_points.elements.push_back(ActiveContact(1,0.6)); BOOST_CHECK(robot_model.configure(config) == true); // Config with invalid contact points - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); config.contact_points.names.push_back("XYZ"); config.contact_points.elements.push_back(ActiveContact(1,0.6)); BOOST_CHECK(robot_model.configure(config) == false); @@ -113,7 +115,7 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ } BOOST_AUTO_TEST_CASE(fk){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -126,7 +128,7 @@ BOOST_AUTO_TEST_CASE(fk){ BOOST_AUTO_TEST_CASE(space_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -138,7 +140,7 @@ BOOST_AUTO_TEST_CASE(space_jacobian){ } BOOST_AUTO_TEST_CASE(body_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -150,7 +152,7 @@ BOOST_AUTO_TEST_CASE(body_jacobian){ } BOOST_AUTO_TEST_CASE(com_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -163,7 +165,7 @@ BOOST_AUTO_TEST_CASE(com_jacobian){ BOOST_AUTO_TEST_CASE(dynamics){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; RobotModelPtr robot_model = make_shared(); RobotModelConfig cfg(urdf_file); diff --git a/src/robot_models/pinocchio/CMakeLists.txt b/src/robot_models/pinocchio/CMakeLists.txt index ea2ba992..288fe291 100644 --- a/src/robot_models/pinocchio/CMakeLists.txt +++ b/src/robot_models/pinocchio/CMakeLists.txt @@ -27,3 +27,4 @@ CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/robot_models/pinocchio) +add_subdirectory(test) diff --git a/test/robot_models/pinocchio/CMakeLists.txt b/src/robot_models/pinocchio/test/CMakeLists.txt similarity index 71% rename from test/robot_models/pinocchio/CMakeLists.txt rename to src/robot_models/pinocchio/test/CMakeLists.txt index e8fec11b..c1f48e0f 100644 --- a/test/robot_models/pinocchio/CMakeLists.txt +++ b/src/robot_models/pinocchio/test/CMakeLists.txt @@ -1,4 +1,5 @@ -add_executable(test_robot_model_pinocchio test_robot_model_pinocchio.cpp ../../suite.cpp ../test_robot_model.cpp) +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_robot_model_pinocchio test_robot_model_pinocchio.cpp ../../test/test_robot_model.cpp) target_link_libraries(test_robot_model_pinocchio wbc-robot_models-pinocchio Boost::unit_test_framework diff --git a/test/robot_models/pinocchio/test_robot_model_pinocchio.cpp b/src/robot_models/pinocchio/test/test_robot_model_pinocchio.cpp similarity index 88% rename from test/robot_models/pinocchio/test_robot_model_pinocchio.cpp rename to src/robot_models/pinocchio/test/test_robot_model_pinocchio.cpp index e1d3f6ab..2e02c02b 100644 --- a/test/robot_models/pinocchio/test_robot_model_pinocchio.cpp +++ b/src/robot_models/pinocchio/test/test_robot_model_pinocchio.cpp @@ -1,8 +1,10 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include -#include "robot_models/pinocchio/RobotModelPinocchio.hpp" -#include "core/RobotModelConfig.hpp" -#include "tools/URDFTools.hpp" -#include "../test_robot_model.hpp" +#include "../RobotModelPinocchio.hpp" +#include +#include +#include "../../test/test_robot_model.hpp" using namespace std; using namespace wbc; @@ -26,7 +28,7 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ std::vector floating_base_names = {"floating_base_trans_x", "floating_base_trans_y", "floating_base_trans_z", "floating_base_rot_x", "floating_base_rot_y", "floating_base_rot_z"}; // Valid config - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); BOOST_CHECK(robot_model.configure(config) == true); for(size_t i = 0; i < robot_model.noOfJoints(); i++){ @@ -46,7 +48,7 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ } // Valid config with floating base - config = RobotModelConfig("../../../../models/kuka/urdf/kuka_iiwa.urdf"); + config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); config.submechanism_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; config.floating_base = true; BOOST_CHECK(robot_model.configure(config) == true); @@ -84,7 +86,7 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ BOOST_AUTO_TEST_CASE(fk){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -97,7 +99,7 @@ BOOST_AUTO_TEST_CASE(fk){ BOOST_AUTO_TEST_CASE(space_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -109,7 +111,7 @@ BOOST_AUTO_TEST_CASE(space_jacobian){ } BOOST_AUTO_TEST_CASE(body_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -121,7 +123,7 @@ BOOST_AUTO_TEST_CASE(body_jacobian){ } BOOST_AUTO_TEST_CASE(com_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -134,7 +136,7 @@ BOOST_AUTO_TEST_CASE(com_jacobian){ BOOST_AUTO_TEST_CASE(dynamics){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; RobotModelPtr robot_model = make_shared(); RobotModelConfig cfg(urdf_file); diff --git a/src/robot_models/rbdl/CMakeLists.txt b/src/robot_models/rbdl/CMakeLists.txt index 16184a65..e444cc5d 100644 --- a/src/robot_models/rbdl/CMakeLists.txt +++ b/src/robot_models/rbdl/CMakeLists.txt @@ -29,3 +29,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/robot_models/rbdl) + +add_subdirectory(test) diff --git a/test/robot_models/rbdl/CMakeLists.txt b/src/robot_models/rbdl/test/CMakeLists.txt similarity index 61% rename from test/robot_models/rbdl/CMakeLists.txt rename to src/robot_models/rbdl/test/CMakeLists.txt index 0a389f48..6ad23705 100644 --- a/test/robot_models/rbdl/CMakeLists.txt +++ b/src/robot_models/rbdl/test/CMakeLists.txt @@ -1,5 +1,5 @@ -add_executable(test_robot_model_rbdl test_robot_model_rbdl.cpp ../../suite.cpp ../test_robot_model.cpp) +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_robot_model_rbdl test_robot_model_rbdl.cpp ../../test/test_robot_model.cpp) target_link_libraries(test_robot_model_rbdl wbc-robot_models-rbdl Boost::unit_test_framework) - diff --git a/test/robot_models/rbdl/test_robot_model_rbdl.cpp b/src/robot_models/rbdl/test/test_robot_model_rbdl.cpp similarity index 91% rename from test/robot_models/rbdl/test_robot_model_rbdl.cpp rename to src/robot_models/rbdl/test/test_robot_model_rbdl.cpp index c520937d..c02b6bf7 100644 --- a/test/robot_models/rbdl/test_robot_model_rbdl.cpp +++ b/src/robot_models/rbdl/test/test_robot_model_rbdl.cpp @@ -1,8 +1,10 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include #include "tools/URDFTools.hpp" #include "robot_models/rbdl/RobotModelRBDL.hpp" -#include "../test_robot_model.hpp" +#include "../../test/test_robot_model.hpp" using namespace std; using namespace wbc; @@ -25,7 +27,7 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ // Valid config RobotModelRBDL robot_model; - RobotModelConfig cfg("../../../../models/rh5/urdf/rh5.urdf"); + RobotModelConfig cfg("../../../../../models/rh5/urdf/rh5.urdf"); BOOST_CHECK(robot_model.configure(cfg)); for(size_t i = 0; i < robot_model.noOfJoints(); i++){ BOOST_CHECK(joint_names[i] == robot_model.jointNames()[i]); @@ -79,7 +81,7 @@ BOOST_AUTO_TEST_CASE(configure_and_update){ } BOOST_AUTO_TEST_CASE(fk){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -91,7 +93,7 @@ BOOST_AUTO_TEST_CASE(fk){ } BOOST_AUTO_TEST_CASE(space_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -103,7 +105,7 @@ BOOST_AUTO_TEST_CASE(space_jacobian){ } BOOST_AUTO_TEST_CASE(body_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -115,7 +117,7 @@ BOOST_AUTO_TEST_CASE(body_jacobian){ } BOOST_AUTO_TEST_CASE(com_jacobian){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; string tip_frame = "kuka_lbr_l_tcp"; RobotModelPtr robot_model = make_shared(); @@ -128,7 +130,7 @@ BOOST_AUTO_TEST_CASE(com_jacobian){ BOOST_AUTO_TEST_CASE(dynamics){ - string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; RobotModelPtr robot_model = make_shared(); RobotModelConfig cfg(urdf_file); diff --git a/src/robot_models/test/CMakeLists.txt b/src/robot_models/test/CMakeLists.txt new file mode 100644 index 00000000..2661fca1 --- /dev/null +++ b/src/robot_models/test/CMakeLists.txt @@ -0,0 +1,8 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_model_consistency test_model_consistency.cpp test_robot_model.cpp) +target_link_libraries(test_model_consistency + wbc-robot_models-hyrodyn + wbc-robot_models-kdl + wbc-robot_models-rbdl + wbc-robot_models-pinocchio + Boost::unit_test_framework) diff --git a/test/robot_models/test_model_consistency.cpp b/src/robot_models/test/test_model_consistency.cpp similarity index 95% rename from test/robot_models/test_model_consistency.cpp rename to src/robot_models/test/test_model_consistency.cpp index 05566f4f..32c9ed50 100644 --- a/test/robot_models/test_model_consistency.cpp +++ b/src/robot_models/test/test_model_consistency.cpp @@ -1,3 +1,5 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include "robot_models/hyrodyn/RobotModelHyrodyn.hpp" #include "robot_models/rbdl/RobotModelRBDL.hpp" @@ -294,12 +296,12 @@ void compareRobotModels(RobotModelConfig cfg, string tip_frame, bool verbose = f BOOST_AUTO_TEST_CASE(fixed_base){ - vector urdf_files = {"../../../models/rh5/urdf/rh5_single_leg.urdf", - "../../../models/kuka/urdf/kuka_iiwa.urdf", - "../../../models/rh5v2/urdf/rh5v2.urdf"}; - vector sub_mec_files = {"../../../models/rh5/hyrodyn/rh5_single_leg.yml", - "../../../models/kuka/hyrodyn/kuka_iiwa.yml", - "../../../models/rh5v2/hyrodyn/rh5v2.yml"}; + vector urdf_files = {"../../../../models/rh5/urdf/rh5_single_leg.urdf", + "../../../../models/kuka/urdf/kuka_iiwa.urdf", + "../../../../models/rh5v2/urdf/rh5v2.urdf"}; + vector sub_mec_files = {"../../../../models/rh5/hyrodyn/rh5_single_leg.yml", + "../../../../models/kuka/hyrodyn/kuka_iiwa.yml", + "../../../../models/rh5v2/hyrodyn/rh5v2.yml"}; vector tip_frames = {"LLAnkle_FT", "kuka_lbr_l_tcp", "ALWristFT_Link"}; bool verbose = false; @@ -316,10 +318,11 @@ BOOST_AUTO_TEST_CASE(fixed_base){ } } +/* BOOST_AUTO_TEST_CASE(floating_base){ - string urdf_file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; - string sub_mec_file = "../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; + string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string sub_mec_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; string tip_frame = "kuka_lbr_l_tcp"; bool verbose = false; @@ -329,3 +332,4 @@ BOOST_AUTO_TEST_CASE(floating_base){ compareRobotModels(cfg, tip_frame, verbose); } +*/ diff --git a/test/robot_models/test_robot_model.cpp b/src/robot_models/test/test_robot_model.cpp similarity index 100% rename from test/robot_models/test_robot_model.cpp rename to src/robot_models/test/test_robot_model.cpp diff --git a/test/robot_models/test_robot_model.hpp b/src/robot_models/test/test_robot_model.hpp similarity index 95% rename from test/robot_models/test_robot_model.hpp rename to src/robot_models/test/test_robot_model.hpp index b814b6ce..34590367 100644 --- a/test/robot_models/test_robot_model.hpp +++ b/src/robot_models/test/test_robot_model.hpp @@ -3,7 +3,7 @@ #include #include -#include "core/RobotModel.hpp" +#include namespace wbc { void printRbs(base::samples::RigidBodyStateSE3 rbs); diff --git a/src/scenes/acceleration/CMakeLists.txt b/src/scenes/acceleration/CMakeLists.txt index 18c9d451..0e8a056d 100644 --- a/src/scenes/acceleration/CMakeLists.txt +++ b/src/scenes/acceleration/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/scenes/acceleration) + +add_subdirectory(test) diff --git a/src/scenes/acceleration/test/CMakeLists.txt b/src/scenes/acceleration/test/CMakeLists.txt new file mode 100644 index 00000000..5ed48b06 --- /dev/null +++ b/src/scenes/acceleration/test/CMakeLists.txt @@ -0,0 +1,7 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_acceleration_scene test_acceleration_scene.cpp) +target_link_libraries(test_acceleration_scene + wbc-scenes-acceleration + wbc-robot_models-pinocchio + wbc-solvers-qpoases + Boost::unit_test_framework) diff --git a/test/scenes/test_acceleration_scene.cpp b/src/scenes/acceleration/test/test_acceleration_scene.cpp similarity index 90% rename from test/scenes/test_acceleration_scene.cpp rename to src/scenes/acceleration/test/test_acceleration_scene.cpp index fce77212..98b64ff8 100644 --- a/test/scenes/test_acceleration_scene.cpp +++ b/src/scenes/acceleration/test/test_acceleration_scene.cpp @@ -1,5 +1,7 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/acceleration/AccelerationScene.hpp" #include "solvers/qpoases/QPOasesSolver.hpp" @@ -13,9 +15,9 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK_EQUAL(robot_model->configure(config), true); base::samples::Joints joint_state; diff --git a/src/scenes/acceleration_reduced_tsid/CMakeLists.txt b/src/scenes/acceleration_reduced_tsid/CMakeLists.txt index 4f3fd8dd..e21affaf 100644 --- a/src/scenes/acceleration_reduced_tsid/CMakeLists.txt +++ b/src/scenes/acceleration_reduced_tsid/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/scenes/acceleration_reduced_tsid) + +add_subdirectory(test) diff --git a/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt b/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt new file mode 100644 index 00000000..13b0404d --- /dev/null +++ b/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt @@ -0,0 +1,7 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_acceleration_scene_reduced_tsid test_acceleration_scene_reduced_tsid.cpp) +target_link_libraries(test_acceleration_scene_reduced_tsid + wbc-scenes-acceleration_reduced_tsid + wbc-robot_models-pinocchio + wbc-solvers-qpoases + Boost::unit_test_framework) diff --git a/test/scenes/test_acceleration_scene_reduced_tsid.cpp b/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp similarity index 94% rename from test/scenes/test_acceleration_scene_reduced_tsid.cpp rename to src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp index c5e7f153..a89ca7ec 100644 --- a/test/scenes/test_acceleration_scene_reduced_tsid.cpp +++ b/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp @@ -1,5 +1,7 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.hpp" #include "solvers/qpoases/QPOasesSolver.hpp" @@ -13,9 +15,9 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; + config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; config.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; wbc::ActiveContact contact(1,0.6); diff --git a/src/scenes/acceleration_tsid/CMakeLists.txt b/src/scenes/acceleration_tsid/CMakeLists.txt index 25ad80d7..fd853325 100644 --- a/src/scenes/acceleration_tsid/CMakeLists.txt +++ b/src/scenes/acceleration_tsid/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/scenes/acceleration_tsid) + +add_subdirectory(test) diff --git a/src/scenes/acceleration_tsid/test/CMakeLists.txt b/src/scenes/acceleration_tsid/test/CMakeLists.txt new file mode 100644 index 00000000..1cd2c9c8 --- /dev/null +++ b/src/scenes/acceleration_tsid/test/CMakeLists.txt @@ -0,0 +1,7 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_acceleration_scene_tsid test_acceleration_scene_tsid.cpp) +target_link_libraries(test_acceleration_scene_tsid + wbc-scenes-acceleration_tsid + wbc-robot_models-pinocchio + wbc-solvers-qpoases + Boost::unit_test_framework) diff --git a/test/scenes/test_acceleration_scene_tsid.cpp b/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp similarity index 92% rename from test/scenes/test_acceleration_scene_tsid.cpp rename to src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp index b6b57c31..51338f9a 100644 --- a/test/scenes/test_acceleration_scene_tsid.cpp +++ b/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp @@ -1,5 +1,7 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/acceleration_tsid/AccelerationSceneTSID.hpp" #include "solvers/qpoases/QPOasesSolver.hpp" @@ -13,9 +15,9 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; + config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; config.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; wbc::ActiveContact contact(1,0.6); diff --git a/src/scenes/velocity/CMakeLists.txt b/src/scenes/velocity/CMakeLists.txt index 38e148d7..59adfedc 100644 --- a/src/scenes/velocity/CMakeLists.txt +++ b/src/scenes/velocity/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/scenes/velocity) + +add_subdirectory(test) diff --git a/src/scenes/velocity/test/CMakeLists.txt b/src/scenes/velocity/test/CMakeLists.txt new file mode 100644 index 00000000..4173c66d --- /dev/null +++ b/src/scenes/velocity/test/CMakeLists.txt @@ -0,0 +1,8 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_velocity_scene test_velocity_scene.cpp) +target_link_libraries(test_velocity_scene + wbc-scenes-velocity + wbc-robot_models-pinocchio + wbc-solvers-hls + Boost::unit_test_framework) + diff --git a/test/scenes/test_velocity_scene.cpp b/src/scenes/velocity/test/test_velocity_scene.cpp similarity index 88% rename from test/scenes/test_velocity_scene.cpp rename to src/scenes/velocity/test/test_velocity_scene.cpp index 8cf2ba72..c5a27ac5 100644 --- a/test/scenes/test_velocity_scene.cpp +++ b/src/scenes/velocity/test/test_velocity_scene.cpp @@ -1,5 +1,7 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/velocity/VelocityScene.hpp" #include "solvers/hls/HierarchicalLSSolver.hpp" @@ -14,9 +16,9 @@ BOOST_AUTO_TEST_CASE(configuration_test){ TaskConfig cart_task("cart_pos_ctrl_left", 0, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", "kuka_lbr_l_link_0"); - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK_EQUAL(robot_model->configure(config), true); QPSolverPtr solver = std::make_shared(); VelocityScene wbc_scene(robot_model, solver, 1e-3); @@ -45,9 +47,9 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK(robot_model->configure(config)); base::samples::Joints joint_state; @@ -91,11 +93,3 @@ BOOST_AUTO_TEST_CASE(simple_test){ BOOST_CHECK(fabs(status[0].y_ref[i+3] - status[0].y_solution[i]) < 1e5); } } - - - - - - - - diff --git a/src/scenes/velocity_qp/CMakeLists.txt b/src/scenes/velocity_qp/CMakeLists.txt index a5307e99..953e4f67 100644 --- a/src/scenes/velocity_qp/CMakeLists.txt +++ b/src/scenes/velocity_qp/CMakeLists.txt @@ -25,3 +25,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/wbc/scenes/velocity_qp) + +add_subdirectory(test) diff --git a/src/scenes/velocity_qp/test/CMakeLists.txt b/src/scenes/velocity_qp/test/CMakeLists.txt new file mode 100644 index 00000000..ddd621c7 --- /dev/null +++ b/src/scenes/velocity_qp/test/CMakeLists.txt @@ -0,0 +1,8 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_velocity_scene_quadratic_cost test_velocity_scene_quadratic_cost.cpp) +target_link_libraries(test_velocity_scene_quadratic_cost + wbc-scenes-velocity_qp + wbc-robot_models-pinocchio + wbc-solvers-qpoases + Boost::unit_test_framework) + diff --git a/test/scenes/test_velocity_scene_quadratic_cost.cpp b/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp similarity index 92% rename from test/scenes/test_velocity_scene_quadratic_cost.cpp rename to src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp index 56749013..9bc9f2c2 100644 --- a/test/scenes/test_velocity_scene_quadratic_cost.cpp +++ b/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp @@ -1,5 +1,7 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/velocity_qp/VelocitySceneQP.hpp" #include "solvers/qpoases/QPOasesSolver.hpp" @@ -13,9 +15,9 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; + config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; config.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; config.contact_points.elements = {ActiveContact(1,0.6),ActiveContact(1,0.6)}; diff --git a/src/solvers/CMakeLists.txt b/src/solvers/CMakeLists.txt index bfbec159..de02170d 100644 --- a/src/solvers/CMakeLists.txt +++ b/src/solvers/CMakeLists.txt @@ -1,12 +1,11 @@ -set(HEADERS qp_solver.hpp) add_subdirectory(qpoases) add_subdirectory(hls) -if(USE_EIQUADPROG) +if(SOLVER_EIQUADPROG) add_subdirectory(eiquadprog) endif() -if(USE_QPSWIFT) +if(SOLVER_QPSWIFT) add_subdirectory(qpswift) endif() -if(USE_PROXQP) +if(SOLVER_PROXQP) add_subdirectory(proxqp) endif() diff --git a/src/solvers/eiquadprog/CMakeLists.txt b/src/solvers/eiquadprog/CMakeLists.txt index 0bc07ef6..c58a7129 100644 --- a/src/solvers/eiquadprog/CMakeLists.txt +++ b/src/solvers/eiquadprog/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/${PROJECT_NAME}/solvers/eiquadprog) + +add_subdirectory(test) diff --git a/test/solvers/eiquadprog/CMakeLists.txt b/src/solvers/eiquadprog/test/CMakeLists.txt similarity index 68% rename from test/solvers/eiquadprog/CMakeLists.txt rename to src/solvers/eiquadprog/test/CMakeLists.txt index 8cdb9573..df6e5dae 100644 --- a/test/solvers/eiquadprog/CMakeLists.txt +++ b/src/solvers/eiquadprog/test/CMakeLists.txt @@ -1,4 +1,5 @@ -add_executable(test_eiquadprog_solver test_eiquadprog_solver.cpp ../../suite.cpp) +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_eiquadprog_solver test_eiquadprog_solver.cpp) target_link_libraries(test_eiquadprog_solver wbc-solvers-eiquadprog Boost::unit_test_framework) diff --git a/test/solvers/eiquadprog/test_eiquadprog_solver.cpp b/src/solvers/eiquadprog/test/test_eiquadprog_solver.cpp similarity index 98% rename from test/solvers/eiquadprog/test_eiquadprog_solver.cpp rename to src/solvers/eiquadprog/test/test_eiquadprog_solver.cpp index d6c48b90..c47b35b6 100644 --- a/test/solvers/eiquadprog/test_eiquadprog_solver.cpp +++ b/src/solvers/eiquadprog/test/test_eiquadprog_solver.cpp @@ -1,8 +1,10 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include #include -#include "core/QuadraticProgram.hpp" -#include "solvers/eiquadprog/EiquadprogSolver.hpp" +#include +#include "../EiquadprogSolver.hpp" using namespace wbc; using namespace std; diff --git a/src/solvers/hls/CMakeLists.txt b/src/solvers/hls/CMakeLists.txt index 54279b15..d53f97bf 100644 --- a/src/solvers/hls/CMakeLists.txt +++ b/src/solvers/hls/CMakeLists.txt @@ -20,3 +20,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/${PROJECT_NAME}/solvers/hls) + +add_subdirectory(test) diff --git a/src/solvers/hls/test/CMakeLists.txt b/src/solvers/hls/test/CMakeLists.txt new file mode 100644 index 00000000..2abd7c80 --- /dev/null +++ b/src/solvers/hls/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_hls_solver test_hls_solver.cpp) +target_link_libraries(test_hls_solver + wbc-solvers-hls + Boost::unit_test_framework) diff --git a/test/solvers/hls/test_hls_solver.cpp b/src/solvers/hls/test/test_hls_solver.cpp similarity index 97% rename from test/solvers/hls/test_hls_solver.cpp rename to src/solvers/hls/test/test_hls_solver.cpp index dc5d470e..ef1c1f89 100644 --- a/test/solvers/hls/test_hls_solver.cpp +++ b/src/solvers/hls/test/test_hls_solver.cpp @@ -1,3 +1,5 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include "solvers/hls/HierarchicalLSSolver.hpp" #include "core/QuadraticProgram.hpp" diff --git a/src/solvers/proxqp/CMakeLists.txt b/src/solvers/proxqp/CMakeLists.txt index c0cb88b4..f1884c1a 100644 --- a/src/solvers/proxqp/CMakeLists.txt +++ b/src/solvers/proxqp/CMakeLists.txt @@ -26,3 +26,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/${PROJECT_NAME}/solvers/proxqp) + +add_subdirectory(test) diff --git a/src/solvers/proxqp/test/CMakeLists.txt b/src/solvers/proxqp/test/CMakeLists.txt new file mode 100644 index 00000000..0237d8c8 --- /dev/null +++ b/src/solvers/proxqp/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_proxqp_solver test_proxqp_solver.cpp) +target_link_libraries(test_proxqp_solver + wbc-solvers-proxqp + ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY}) diff --git a/test/solvers/proxqp/test_proxqp_solver.cpp b/src/solvers/proxqp/test/test_proxqp_solver.cpp similarity index 98% rename from test/solvers/proxqp/test_proxqp_solver.cpp rename to src/solvers/proxqp/test/test_proxqp_solver.cpp index f59a7e30..e17770dd 100644 --- a/test/solvers/proxqp/test_proxqp_solver.cpp +++ b/src/solvers/proxqp/test/test_proxqp_solver.cpp @@ -1,8 +1,10 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include #include -#include "core/QuadraticProgram.hpp" -#include "solvers/proxqp/ProxQPSolver.hpp" +#include +#include "../ProxQPSolver.hpp" using namespace wbc; using namespace std; @@ -172,7 +174,7 @@ BOOST_AUTO_TEST_CASE(solver_proxqp_with_inequality_constraints) long useconds = end.tv_usec - start.tv_usec; Eigen::VectorXd test = A*solver_output; - + for(uint j = 0; j < NO_JOINTS; ++j) BOOST_CHECK((qp.lower_y(j)-1e-9) <= test(j) && test(j) <= (qp.upper_y(j)+1e-9)); } diff --git a/src/solvers/qpoases/CMakeLists.txt b/src/solvers/qpoases/CMakeLists.txt index e75a195e..f6fb4c1a 100644 --- a/src/solvers/qpoases/CMakeLists.txt +++ b/src/solvers/qpoases/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/${PROJECT_NAME}/solvers/qpoases) + +add_subdirectory(test) diff --git a/src/solvers/qpoases/test/CMakeLists.txt b/src/solvers/qpoases/test/CMakeLists.txt new file mode 100644 index 00000000..97d8aa29 --- /dev/null +++ b/src/solvers/qpoases/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_qpoases_solver test_qpoases_solver.cpp) +target_link_libraries(test_qpoases_solver + wbc-solvers-qpoases + Boost::unit_test_framework) diff --git a/test/solvers/qpoases/test_qpoases_solver.cpp b/src/solvers/qpoases/test/test_qpoases_solver.cpp similarity index 99% rename from test/solvers/qpoases/test_qpoases_solver.cpp rename to src/solvers/qpoases/test/test_qpoases_solver.cpp index 4ad0b52b..295fa892 100644 --- a/test/solvers/qpoases/test_qpoases_solver.cpp +++ b/src/solvers/qpoases/test/test_qpoases_solver.cpp @@ -1,3 +1,5 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include #include @@ -280,4 +282,4 @@ BOOST_AUTO_TEST_CASE(solver_qpoases_bounded) for(uint j = 0; j < NO_JOINTS; ++j) BOOST_CHECK((qp.lower_x(j)-1e-9) <= solver_output(j) && solver_output(j) <= (qp.upper_x(j)+1e-9)); -} \ No newline at end of file +} diff --git a/src/solvers/qpswift/CMakeLists.txt b/src/solvers/qpswift/CMakeLists.txt index 829457fc..e5cff08c 100644 --- a/src/solvers/qpswift/CMakeLists.txt +++ b/src/solvers/qpswift/CMakeLists.txt @@ -24,3 +24,5 @@ install(TARGETS ${TARGET_NAME} CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) INSTALL(FILES ${HEADERS} DESTINATION include/${PROJECT_NAME}/solvers/qpswift) + +add_subdirectory(test) diff --git a/src/solvers/qpswift/QPSwiftSolver.cpp b/src/solvers/qpswift/QPSwiftSolver.cpp index 042502d5..69b00ce3 100644 --- a/src/solvers/qpswift/QPSwiftSolver.cpp +++ b/src/solvers/qpswift/QPSwiftSolver.cpp @@ -1,5 +1,5 @@ #include "QPSwiftSolver.hpp" -#include +#include #include #include @@ -23,14 +23,14 @@ QPSwiftSolver::~QPSwiftSolver(){ } void QPSwiftSolver::toQpSwift(const wbc::QuadraticProgram &qp){ - - G.setZero(); + + G.setZero(); P = qp.H; c = qp.g; A = qp.A; b = qp.b; - + // create inequalities matrix (inequalities constraints + bounds) G.middleRows(0, qp.nin) = qp.C; G.middleRows(qp.nin, qp.nin) = -qp.C; @@ -71,11 +71,11 @@ void QPSwiftSolver::solve(const wbc::HierarchicalQP &hierarchical_qp, base::Vect const wbc::QuadraticProgram &qp = hierarchical_qp[0]; - if(!configured){ + if(!configured){ // Count equality / inequality constraints n_dec = qp.nq; n_eq = qp.neq; - n_ineq = 2 * (qp.nin + qp.upper_x.size()); + n_ineq = 2 * (qp.nin + qp.upper_x.size()); A.resize(n_eq, n_dec); b.resize(n_eq); diff --git a/src/solvers/qpswift/test/CMakeLists.txt b/src/solvers/qpswift/test/CMakeLists.txt new file mode 100644 index 00000000..1e09d643 --- /dev/null +++ b/src/solvers/qpswift/test/CMakeLists.txt @@ -0,0 +1,5 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) +add_executable(test_qpswift_solver test_qpswift_solver.cpp) +target_link_libraries(test_qpswift_solver + wbc-solvers-qpswift + Boost::unit_test_framework) diff --git a/test/solvers/qpswift/test_qpswift_solver.cpp b/src/solvers/qpswift/test/test_qpswift_solver.cpp similarity index 97% rename from test/solvers/qpswift/test_qpswift_solver.cpp rename to src/solvers/qpswift/test/test_qpswift_solver.cpp index 4da48f04..bdfff7af 100644 --- a/test/solvers/qpswift/test_qpswift_solver.cpp +++ b/src/solvers/qpswift/test/test_qpswift_solver.cpp @@ -1,8 +1,10 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN #include #include #include -#include "core/QuadraticProgram.hpp" -#include "solvers/qpswift/QPSwiftSolver.hpp" +#include +#include "../QPSwiftSolver.hpp" using namespace wbc; using namespace std; @@ -24,7 +26,7 @@ BOOST_AUTO_TEST_CASE(solver_qp_swift_without_constraints) wbc::QuadraticProgram qp; qp.resize(NO_JOINTS, NO_EQ_CONSTRAINTS, NO_IN_CONSTRAINTS, WITH_BOUNDS); - + // qp.lower_y.resize(0); // qp.upper_y.resize(0); @@ -43,7 +45,7 @@ BOOST_AUTO_TEST_CASE(solver_qp_swift_without_constraints) qp.H = A.transpose()*A; qp.g = -(A.transpose()*y).transpose(); - + qp.lower_x.setConstant(-1000); qp.upper_x.setConstant(1000); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt deleted file mode 100644 index 94f7e384..00000000 --- a/test/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) - -add_subdirectory(controllers) -add_subdirectory(core) -add_subdirectory(robot_models) -add_subdirectory(scenes) -add_subdirectory(solvers) diff --git a/test/core/CMakeLists.txt b/test/core/CMakeLists.txt deleted file mode 100644 index 49cf5e09..00000000 --- a/test/core/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -add_executable(test_core test_core.cpp ../suite.cpp) -target_link_libraries(test_core - wbc-core - Boost::unit_test_framework) diff --git a/test/robot_models/CMakeLists.txt b/test/robot_models/CMakeLists.txt deleted file mode 100644 index 7f5045e1..00000000 --- a/test/robot_models/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -add_subdirectory(rbdl) -if(USE_PINOCCHIO) - add_subdirectory(pinocchio) -endif() -if(USE_HYRODYN) - add_subdirectory(hyrodyn) -endif() -if(USE_KDL) - add_subdirectory(kdl) -endif() - - -if(USE_HYRODYN AND USE_KDL AND USE_PINOCCHIO) - - add_executable(test_model_consistency test_model_consistency.cpp ../suite.cpp test_robot_model.cpp) - target_link_libraries(test_model_consistency - wbc-robot_models-hyrodyn - wbc-robot_models-kdl - wbc-robot_models-rbdl - wbc-robot_models-pinocchio - Boost::unit_test_framework) -endif() diff --git a/test/scenes/CMakeLists.txt b/test/scenes/CMakeLists.txt deleted file mode 100644 index 493d8413..00000000 --- a/test/scenes/CMakeLists.txt +++ /dev/null @@ -1,36 +0,0 @@ -add_executable(test_velocity_scene test_velocity_scene.cpp ../suite.cpp) -target_link_libraries(test_velocity_scene - wbc-scenes-velocity - wbc-robot_models-rbdl - wbc-solvers-hls - Boost::unit_test_framework) - -add_executable(test_velocity_scene_quadratic_cost test_velocity_scene_quadratic_cost.cpp ../suite.cpp) -target_link_libraries(test_velocity_scene_quadratic_cost - wbc-scenes-velocity_qp - wbc-robot_models-rbdl - wbc-solvers-qpoases - Boost::unit_test_framework) - -add_executable(test_acceleration_scene test_acceleration_scene.cpp ../suite.cpp) -target_link_libraries(test_acceleration_scene - wbc-scenes-acceleration - wbc-robot_models-rbdl - wbc-solvers-qpoases - Boost::unit_test_framework) - -add_executable(test_acceleration_scene_tsid test_acceleration_scene_tsid.cpp ../suite.cpp) -target_link_libraries(test_acceleration_scene_tsid - wbc-scenes-acceleration_tsid - wbc-robot_models-rbdl - wbc-solvers-qpoases - Boost::unit_test_framework) - -add_executable(test_acceleration_scene_reduced_tsid test_acceleration_scene_reduced_tsid.cpp ../suite.cpp) -target_link_libraries(test_acceleration_scene_reduced_tsid - wbc-scenes-acceleration_reduced_tsid - wbc-robot_models-rbdl - wbc-solvers-qpoases - Boost::unit_test_framework) - - diff --git a/test/solvers/CMakeLists.txt b/test/solvers/CMakeLists.txt deleted file mode 100644 index ed1e1b0a..00000000 --- a/test/solvers/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -add_subdirectory(hls) -add_subdirectory(qpoases) -if(USE_EIQUADPROG) - add_subdirectory(eiquadprog) -endif() -if(USE_QPSWIFT) - add_subdirectory(qpswift) -endif() -if(USE_PROXQP) - add_subdirectory(proxqp) -endif() diff --git a/test/solvers/hls/CMakeLists.txt b/test/solvers/hls/CMakeLists.txt deleted file mode 100644 index 895bef02..00000000 --- a/test/solvers/hls/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -add_executable(test_hls_solver test_hls_solver.cpp ../../suite.cpp) -target_link_libraries(test_hls_solver - wbc-solvers-hls - Boost::unit_test_framework) diff --git a/test/solvers/proxqp/CMakeLists.txt b/test/solvers/proxqp/CMakeLists.txt deleted file mode 100644 index bf7cda73..00000000 --- a/test/solvers/proxqp/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -add_executable(test_proxqp_solver test_proxqp_solver.cpp ../../suite.cpp) -target_link_libraries(test_proxqp_solver - wbc-solvers-proxqp - ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY}) diff --git a/test/solvers/qpoases/CMakeLists.txt b/test/solvers/qpoases/CMakeLists.txt deleted file mode 100644 index 915a172c..00000000 --- a/test/solvers/qpoases/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -add_executable(test_qpoases_solver test_qpoases_solver.cpp ../../suite.cpp) -target_link_libraries(test_qpoases_solver - wbc-solvers-qpoases - Boost::unit_test_framework) diff --git a/test/solvers/qpswift/CMakeLists.txt b/test/solvers/qpswift/CMakeLists.txt deleted file mode 100644 index a5038a54..00000000 --- a/test/solvers/qpswift/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -add_executable(test_qpswift_solver test_qpswift_solver.cpp ../../suite.cpp) -target_link_libraries(test_qpswift_solver - wbc-solvers-qpswift - Boost::unit_test_framework) diff --git a/test/suite.cpp b/test/suite.cpp deleted file mode 100644 index 0b47303b..00000000 --- a/test/suite.cpp +++ /dev/null @@ -1,3 +0,0 @@ -#define BOOST_TEST_DYN_LINK -#define BOOST_TEST_MAIN -#include diff --git a/tutorials/kuka_iiwa/CMakeLists.txt b/tutorials/kuka_iiwa/CMakeLists.txt index d486fd77..50208ea3 100644 --- a/tutorials/kuka_iiwa/CMakeLists.txt +++ b/tutorials/kuka_iiwa/CMakeLists.txt @@ -2,26 +2,26 @@ add_executable(cart_pos_ctrl_hls cart_pos_ctrl_hls.cpp) target_link_libraries(cart_pos_ctrl_hls wbc-solvers-hls wbc-scenes-velocity - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-controllers) add_executable(cart_pos_ctrl_qpoases cart_pos_ctrl_qpoases.cpp) target_link_libraries(cart_pos_ctrl_qpoases wbc-solvers-qpoases wbc-scenes-velocity_qp - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-controllers) add_executable(cart_pos_ctrl_hls_weights cart_pos_ctrl_hls_weights.cpp) target_link_libraries(cart_pos_ctrl_hls_weights wbc-solvers-hls wbc-scenes-velocity - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-controllers) add_executable(cart_pos_ctrl_hls_hierarchies cart_pos_ctrl_hls_hierarchies.cpp) target_link_libraries(cart_pos_ctrl_hls_hierarchies wbc-solvers-hls wbc-scenes-velocity - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-controllers) diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp index df0e4967..c678c537 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include #include @@ -37,7 +37,7 @@ using namespace wbc; int main(){ // Create a robot model. Each robot model is derived from a common RobotModel class, as it will be passed to the WBC scene later on. - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting // the path to the URDF file. In doing so, WBC will assume: @@ -78,7 +78,7 @@ int main(){ // v_d = kd*v_r + kp(x_r - x). // // As we don't use feed forward velocity here, we can ignore the factor kd. - ctrl_lib::CartesianPosPDController controller; + CartesianPosPDController controller; controller.setPGain(base::Vector6d::Constant(1)); // Set kp // Choose an initial joint state. For velocity-based WBC only the current position of all joint has to be passed diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp index e86005ed..ffe361a1 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -15,7 +15,7 @@ using namespace wbc; */ int main(int argc, char** argv){ - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting // the path to the URDF file. In doing so, WBC will assume: @@ -63,9 +63,9 @@ int main(int argc, char** argv){ return -1; // Configure the controllers, one Cartesian position controller, one joint Position controller - ctrl_lib::CartesianPosPDController cart_controller; + CartesianPosPDController cart_controller; cart_controller.setPGain(base::Vector6d::Constant(1)); // Set kp - ctrl_lib::JointPosPDController jnt_controller(jnt_task.joint_names); + JointPosPDController jnt_controller(jnt_task.joint_names); base::VectorXd p_gain(1); p_gain.setConstant(1); jnt_controller.setPGain(p_gain); diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp index a11f1fc6..8c8bf81b 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -14,7 +14,7 @@ using namespace wbc; */ int main(){ - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting // the path to the URDF file. In doing so, WBC will assume: @@ -64,7 +64,7 @@ int main(){ // v_d = kd*v_r + kp(x_r - x). // // As we don't use feed forward velocity here, we can ignore the factor kd. - ctrl_lib::CartesianPosPDController controller; + CartesianPosPDController controller; controller.setPGain(base::Vector6d::Constant(1)); // Set kp // Choose an initial joint state. For velocity-based WBC only the current position of all joint has to be passed diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp index 813b03f9..bd9898a3 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -39,7 +39,7 @@ int main(){ double dt = 0.01; // Create a robot model. Each robot model is derived from a common RobotModel class, as it will be passed to the WBC scene later on. - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting // the path to the URDF file. In doing so, WBC will assume: @@ -83,7 +83,7 @@ int main(){ // v_d = kd*v_r + kp(x_r - x). // // As we don't use feed forward velocity here, we can ignore the factor kd. - ctrl_lib::CartesianPosPDController controller; + CartesianPosPDController controller; controller.setPGain(base::Vector6d::Constant(1)); // Choose an initial joint state. For velocity-based WBC only the current position of all joint has to be passed @@ -110,6 +110,7 @@ int main(){ while((setpoint.pose.position - feedback.pose.position).norm() > 1e-4){ // Update the robot model. WBC will only work if at least one joint state with valid timestamp has been passed to the robot model + auto s = std::chrono::high_resolution_clock::now(); robot_model->update(joint_state); // Update controller. The feedback is the pose of the tip link described in ref_frame link @@ -125,7 +126,6 @@ int main(){ // Solve the QP. The output is the joint velocity that achieves the task space velocity demanded by the controller, i.e., // this joint velocity will drive the end effector to the reference x_r - auto s = std::chrono::high_resolution_clock::now(); solver_output = scene.solve(hqp); auto e = std::chrono::high_resolution_clock::now(); double solve_time = std::chrono::duration_cast(e-s).count(); diff --git a/tutorials/rh5/CMakeLists.txt b/tutorials/rh5/CMakeLists.txt index 6880d4d7..209e5aa0 100644 --- a/tutorials/rh5/CMakeLists.txt +++ b/tutorials/rh5/CMakeLists.txt @@ -3,16 +3,16 @@ target_link_libraries(rh5_single_leg wbc-solvers-qpoases wbc-controllers wbc-scenes-velocity_qp - wbc-robot_models-rbdl) + wbc-robot_models-pinocchio) add_executable(rh5_legs_floating_base rh5_legs_floating_base.cpp) target_link_libraries(rh5_legs_floating_base wbc-solvers-qpoases wbc-scenes-velocity_qp wbc-controllers - wbc-robot_models-rbdl) + wbc-robot_models-pinocchio) -if(USE_HYRODYN) +if(ROBOT_MODEL_HYRODYN) add_executable(rh5_single_leg_hybrid rh5_single_leg_hybrid.cpp) target_link_libraries(rh5_single_leg_hybrid wbc-solvers-qpoases diff --git a/tutorials/rh5/rh5_legs_floating_base.cpp b/tutorials/rh5/rh5_legs_floating_base.cpp index b0bf0a50..54e2bf4e 100644 --- a/tutorials/rh5/rh5_legs_floating_base.cpp +++ b/tutorials/rh5/rh5_legs_floating_base.cpp @@ -1,15 +1,16 @@ #include -#include +#include #include #include #include #include #include +#include using namespace wbc; using namespace std; using namespace qpOASES; -using namespace ctrl_lib; +using namespace wbc; /** * Velocity-based example, Cartesian position control on two 6 dof legs of the RH5 humanoid including floating base and two contact points (feet). @@ -44,7 +45,7 @@ int main(){ double dt = 0.001; // Create robot model - RobotModelPtr robot_model = std::make_shared(); + RobotModelPtr robot_model = std::make_shared(); // Configure a serial robot model with floating base and two contact points: {"LLAnkle_FT", "LRAnkle_FT"}. // Note that the joint names have to contain {"floating_base_trans_x", "floating_base_trans_y", "floating_base_trans_z", @@ -126,6 +127,9 @@ int main(){ base::commands::Joints solver_output; while((setpoint.pose.position - feedback.pose.position).norm() > 1e-4){ + + auto s = std::chrono::high_resolution_clock::now(); + // Update the robot model. WBC will only work if at least one joint state with valid timestamp has been passed to the robot model. // Note that you have to pass the floating base state as well now! robot_model->update(joint_state, floating_base_state); @@ -151,6 +155,10 @@ int main(){ joint_state[i].speed = solver_output[i].speed; } + + auto e = std::chrono::high_resolution_clock::now(); + double solve_time = std::chrono::duration_cast(e-s).count(); + // Update floating base pose, use an over-simplistic estimation base::Pose t1 = robot_model->rigidBodyState("world", "RH5_Root_Link").pose; base::Pose t2 = robot_model->rigidBodyState("world", "LLAnkle_FT").pose; @@ -166,6 +174,7 @@ int main(){ cout<<"Solver output: "; cout< -#include +#include #include #include #include @@ -8,7 +8,7 @@ using namespace wbc; using namespace std; using namespace qpOASES; -using namespace ctrl_lib; +using namespace wbc; /** * Velocity-based example, Cartesian position control on 6 dof leg of the RH5 humanoid (fixed base/fully actuated, serial robot model). In the example the following problem is solved: @@ -39,7 +39,7 @@ int main(){ double dt = 0.001; // Create Hyrodyn based robot model. In this case, we use the Hyrodyn-based model, which allows handling of parallel mechanisms. - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the model. We pass a serial model description to RobotModelHyrodyn, which means that the solution will be identical // to the one obtained by RobotModelPinocchio. RobotModelHyrodyn assumes that you specify correctly all joints (actuated and unactuated). diff --git a/tutorials/rh5/rh5_single_leg_hybrid.cpp b/tutorials/rh5/rh5_single_leg_hybrid.cpp index dab5514c..67324f06 100644 --- a/tutorials/rh5/rh5_single_leg_hybrid.cpp +++ b/tutorials/rh5/rh5_single_leg_hybrid.cpp @@ -7,7 +7,6 @@ using namespace wbc; using namespace std; using namespace qpOASES; -using namespace ctrl_lib; /** * Velocity-based example, exact same problem as in the rh5_single_leg example. The only difference is that here we use the hybrid robot model, i.e. we @@ -23,7 +22,7 @@ int main(){ double dt = 0.001; // Create Hyrodyn based robot model. In this case, we use the Hyrodyn-based model, which allows handling of parallel mechanisms. - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the full robot model, containing all linear actuators and parallel mechanisms. RobotModelConfig config; diff --git a/tutorials/rh5v2/CMakeLists.txt b/tutorials/rh5v2/CMakeLists.txt index 167ef654..0b736d1d 100644 --- a/tutorials/rh5v2/CMakeLists.txt +++ b/tutorials/rh5v2/CMakeLists.txt @@ -3,9 +3,9 @@ target_link_libraries(rh5v2 wbc-solvers-qpoases wbc-scenes-acceleration_tsid wbc-controllers - wbc-robot_models-rbdl) + wbc-robot_models-pinocchio) -if(USE_HYRODYN) +if(ROBOT_MODEL_HYRODYN) add_executable(rh5v2_hybrid rh5v2_hybrid.cpp) target_link_libraries(rh5v2_hybrid wbc-solvers-qpoases diff --git a/tutorials/rh5v2/rh5v2.cpp b/tutorials/rh5v2/rh5v2.cpp index 7b3fa008..dad440a0 100644 --- a/tutorials/rh5v2/rh5v2.cpp +++ b/tutorials/rh5v2/rh5v2.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -7,7 +7,7 @@ using namespace std; using namespace wbc; -using namespace ctrl_lib; +using namespace wbc; /** * Acceleration-based example, Cartesian position control of the RH5v2 arms (fixed base, no contacts). @@ -45,7 +45,7 @@ int main() double dt = 0.01; // Create robot model, use hyrodyn-based model in this case - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure robot model. We configure a serial model without parallel mechanisms. // Blacklist the finger joints, as they are not relevant and may slow down the solver diff --git a/tutorials/rh5v2/rh5v2_hybrid.cpp b/tutorials/rh5v2/rh5v2_hybrid.cpp index 73071af2..ad5e12a3 100644 --- a/tutorials/rh5v2/rh5v2_hybrid.cpp +++ b/tutorials/rh5v2/rh5v2_hybrid.cpp @@ -6,7 +6,6 @@ using namespace std; using namespace wbc; -using namespace ctrl_lib; /** * Acceleration-based example, same problem as in the rh5v2 example. The only difference is that here we use the hybrid robot model, i.e. we