From 9b2622251e784100f799125beea2a4d56fd4327a Mon Sep 17 00:00:00 2001 From: dmronga Date: Fri, 23 Feb 2024 09:33:53 +0100 Subject: [PATCH] Remove ctrl_lib namespace and replace with wbc for better code consistency --- src/controllers/ActivationFunction.hpp | 2 +- src/controllers/CartesianForcePIDController.cpp | 2 +- src/controllers/CartesianForcePIDController.hpp | 2 +- src/controllers/CartesianPosPDController.cpp | 2 +- src/controllers/CartesianPosPDController.hpp | 6 +++--- src/controllers/CartesianPotentialFieldsController.cpp | 2 +- src/controllers/CartesianPotentialFieldsController.hpp | 2 +- src/controllers/JointLimitAvoidanceController.cpp | 2 +- src/controllers/JointLimitAvoidanceController.hpp | 2 +- src/controllers/JointPosPDController.cpp | 2 +- src/controllers/JointPosPDController.hpp | 6 +++--- src/controllers/JointTorquePIDController.cpp | 2 +- src/controllers/JointTorquePIDController.hpp | 2 +- src/controllers/PIDController.cpp | 2 +- src/controllers/PIDController.hpp | 2 +- src/controllers/PIDCtrlParams.hpp | 2 +- src/controllers/PlanarPotentialField.cpp | 2 +- src/controllers/PlanarPotentialField.hpp | 2 +- src/controllers/PosPDController.cpp | 2 +- src/controllers/PosPDController.hpp | 2 +- src/controllers/PotentialField.hpp | 2 +- src/controllers/PotentialFieldInfo.hpp | 2 +- src/controllers/PotentialFieldsController.cpp | 2 +- src/controllers/PotentialFieldsController.hpp | 2 +- src/controllers/RadialPotentialField.cpp | 2 +- src/controllers/RadialPotentialField.hpp | 2 +- src/controllers/test/test_pid_controllers.cpp | 2 +- src/controllers/test/test_pos_pd_controllers.cpp | 2 +- src/controllers/test/test_pot_field_controllers.cpp | 2 +- tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp | 2 +- tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp | 4 ++-- tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp | 2 +- tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp | 2 +- tutorials/rh5/rh5_legs_floating_base.cpp | 2 +- tutorials/rh5/rh5_single_leg.cpp | 2 +- tutorials/rh5/rh5_single_leg_hybrid.cpp | 1 - tutorials/rh5v2/rh5v2.cpp | 2 +- tutorials/rh5v2/rh5v2_hybrid.cpp | 1 - 38 files changed, 41 insertions(+), 43 deletions(-) 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/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/src/controllers/test/test_pid_controllers.cpp b/src/controllers/test/test_pid_controllers.cpp index 6e781512..e4e3fe64 100644 --- a/src/controllers/test/test_pid_controllers.cpp +++ b/src/controllers/test/test_pid_controllers.cpp @@ -7,7 +7,7 @@ #include using namespace std; -using namespace ctrl_lib; +using namespace wbc; BOOST_AUTO_TEST_CASE(configuration_test){ diff --git a/src/controllers/test/test_pos_pd_controllers.cpp b/src/controllers/test/test_pos_pd_controllers.cpp index 8d10c9aa..9963613a 100644 --- a/src/controllers/test/test_pos_pd_controllers.cpp +++ b/src/controllers/test/test_pos_pd_controllers.cpp @@ -10,7 +10,7 @@ #include using namespace std; -using namespace ctrl_lib; +using namespace wbc; BOOST_AUTO_TEST_CASE(configuration_test){ diff --git a/src/controllers/test/test_pot_field_controllers.cpp b/src/controllers/test/test_pot_field_controllers.cpp index e77912d2..15c4d5c9 100644 --- a/src/controllers/test/test_pot_field_controllers.cpp +++ b/src/controllers/test/test_pot_field_controllers.cpp @@ -10,7 +10,7 @@ #include "../JointLimitAvoidanceController.hpp" using namespace std; -using namespace ctrl_lib; +using namespace wbc; void runPotentialFieldController(std::string filename, CartesianPotentialFieldsController* ctrl, diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp index 1ce93825..c678c537 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp @@ -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 cbb31c17..ffe361a1 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp @@ -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 366a2026..8c8bf81b 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp @@ -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 55f02319..bd9898a3 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp @@ -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 diff --git a/tutorials/rh5/rh5_legs_floating_base.cpp b/tutorials/rh5/rh5_legs_floating_base.cpp index 74d33368..54e2bf4e 100644 --- a/tutorials/rh5/rh5_legs_floating_base.cpp +++ b/tutorials/rh5/rh5_legs_floating_base.cpp @@ -10,7 +10,7 @@ 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). diff --git a/tutorials/rh5/rh5_single_leg.cpp b/tutorials/rh5/rh5_single_leg.cpp index 163ea21e..66bae74d 100644 --- a/tutorials/rh5/rh5_single_leg.cpp +++ b/tutorials/rh5/rh5_single_leg.cpp @@ -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: diff --git a/tutorials/rh5/rh5_single_leg_hybrid.cpp b/tutorials/rh5/rh5_single_leg_hybrid.cpp index 90abfdf0..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 diff --git a/tutorials/rh5v2/rh5v2.cpp b/tutorials/rh5v2/rh5v2.cpp index 169228da..dad440a0 100644 --- a/tutorials/rh5v2/rh5v2.cpp +++ b/tutorials/rh5v2/rh5v2.cpp @@ -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). 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