From 30bf7fed31be51714c3e415a992b3226eb345ad8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?No=C3=ABlie=20Ramuzat?= Date: Thu, 28 Feb 2019 10:59:32 +0100 Subject: [PATCH] [tools] Change namespace Vector[1|6]d to dynamic_graph::sot Move a part of vector-conversions.hh to matrix-geometry.hh in sot-core to share the tools Use the namespace dynamic_graph::sot with a short cut (dg::sot) for VectorXd instead of Eigen:: for consistency. --- include/sot/torque_control/base-estimator.hh | 4 +- .../torque_control/nd-trajectory-generator.hh | 2 +- .../utils/vector-conversions.hh | 46 ------------------- src/admittance-controller.cpp | 17 +++---- src/device-torque-ctrl.cpp | 5 +- src/free-flyer-locator.cpp | 5 +- src/inverse-dynamics-balance-controller.cpp | 9 ++-- src/madgwickahrs.cpp | 3 +- 8 files changed, 25 insertions(+), 66 deletions(-) diff --git a/include/sot/torque_control/base-estimator.hh b/include/sot/torque_control/base-estimator.hh index 6675b06..6ec4965 100644 --- a/include/sot/torque_control/base-estimator.hh +++ b/include/sot/torque_control/base-estimator.hh @@ -85,8 +85,8 @@ namespace dynamicgraph { typedef Eigen::Vector2d Vector2; typedef Eigen::Vector3d Vector3; typedef Eigen::Vector4d Vector4; - typedef Eigen::Vector6d Vector6; - typedef Eigen::Vector7d Vector7; + typedef dynamicgraph::sot::Vector6d Vector6; + typedef dynamicgraph::sot::Vector7d Vector7; typedef Eigen::Matrix3d Matrix3; typedef boost::math::normal normal; diff --git a/include/sot/torque_control/nd-trajectory-generator.hh b/include/sot/torque_control/nd-trajectory-generator.hh index b7d3e9d..c7a9469 100644 --- a/include/sot/torque_control/nd-trajectory-generator.hh +++ b/include/sot/torque_control/nd-trajectory-generator.hh @@ -173,7 +173,7 @@ namespace dynamicgraph { bool m_splineReady; /// true if the spline has been successfully loaded. std::vector m_status; /// status of the component - std::vector* > m_currentTrajGen; + std::vector* > m_currentTrajGen; std::vector* > m_noTrajGen; std::vector* > m_minJerkTrajGen; std::vector* > m_sinTrajGen; diff --git a/include/sot/torque_control/utils/vector-conversions.hh b/include/sot/torque_control/utils/vector-conversions.hh index b16dc21..bd60e72 100644 --- a/include/sot/torque_control/utils/vector-conversions.hh +++ b/include/sot/torque_control/utils/vector-conversions.hh @@ -21,52 +21,6 @@ #include #include -namespace Eigen -{ - #define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \ - /** \ingroup matrixtypedefs */ \ - typedef Matrix Matrix##SizeSuffix##TypeSuffix; \ - /** \ingroup matrixtypedefs */ \ - typedef Matrix Vector##SizeSuffix##TypeSuffix; \ - /** \ingroup matrixtypedefs */ \ - typedef Matrix RowVector##SizeSuffix##TypeSuffix; - - #define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \ - /** \ingroup matrixtypedefs */ \ - typedef Matrix Matrix##Size##X##TypeSuffix; \ - /** \ingroup matrixtypedefs */ \ - typedef Matrix Matrix##X##Size##TypeSuffix; - - #define EIGEN_MAKE_TYPEDEFS_ALL_SIZES(Type, TypeSuffix) \ - EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 1, 1) \ - EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 5, 5) \ - EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 6, 6) \ - EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, 7, 7) \ - EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 1) \ - EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 5) \ - EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 6) \ - EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, 7) - - EIGEN_MAKE_TYPEDEFS_ALL_SIZES(int, i) - EIGEN_MAKE_TYPEDEFS_ALL_SIZES(float, f) - EIGEN_MAKE_TYPEDEFS_ALL_SIZES(double, d) - EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex, cf) - EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex, cd) - - #undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES - #undef EIGEN_MAKE_TYPEDEFS - - typedef Matrix MatrixRXd; - typedef Map SigMatrixXd; - typedef Map SigVectorXd; - typedef const Map const_SigMatrixXd; - typedef const Map const_SigVectorXd; - - typedef Eigen::Ref RefVector; - typedef const Eigen::Ref& ConstRefVector; - typedef Eigen::Ref RefMatrix; - typedef const Eigen::Ref ConstRefMatrix; -} #define EIGEN_CONST_VECTOR_FROM_STD_VECTOR(name,signal) \ Eigen::const_SigVectorXd name \ diff --git a/src/admittance-controller.cpp b/src/admittance-controller.cpp index 4a2a8b3..96cc160 100644 --- a/src/admittance-controller.cpp +++ b/src/admittance-controller.cpp @@ -35,6 +35,7 @@ namespace dynamicgraph using namespace tsid; using namespace tsid::math; using namespace tsid::tasks; + using namespace dg::sot; #define PROFILE_DQ_DES_COMPUTATION "Admittance control computation" @@ -229,8 +230,8 @@ namespace dynamicgraph getProfiler().start(PROFILE_DQ_DES_COMPUTATION); { - const Eigen::Vector6d v_des_LF = m_vDesLeftFootSOUT(iter); - const Eigen::Vector6d v_des_RF = m_vDesRightFootSOUT(iter); + const dg::sot::Vector6d v_des_LF = m_vDesLeftFootSOUT(iter); + const dg::sot::Vector6d v_des_RF = m_vDesRightFootSOUT(iter); const Vector& q_sot = m_encodersSIN(iter); // n // const Vector& dq_sot = m_jointsVelocitiesSIN(iter); // n //const Vector& qMask = m_controlledJointsSIN(iter); // n @@ -291,9 +292,9 @@ namespace dynamicgraph const Vector6d& f_sat = m_force_integral_saturationSIN(iter); const Vector6d& dz = m_force_integral_deadzoneSIN(iter); - Eigen::Vector6d err = fRef - f; - Eigen::Vector6d err_filt = fRef - f_filt; - Eigen::Vector6d v_des = -kp.cwiseProduct(err_filt); + dg::sot::Vector6d err = fRef - f; + dg::sot::Vector6d err_filt = fRef - f_filt; + dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt); for(int i=0; i<6; i++) { @@ -339,9 +340,9 @@ namespace dynamicgraph const Vector6d& f_sat = m_force_integral_saturationSIN(iter); const Vector6d& dz = m_force_integral_deadzoneSIN(iter); - Eigen::Vector6d err = fRef - f; - Eigen::Vector6d err_filt = fRef - f_filt; - Eigen::Vector6d v_des = -kp.cwiseProduct(err_filt); + dg::sot::Vector6d err = fRef - f; + dg::sot::Vector6d err_filt = fRef - f_filt; + dg::sot::Vector6d v_des = -kp.cwiseProduct(err_filt); for(int i=0; i<6; i++) { diff --git a/src/device-torque-ctrl.cpp b/src/device-torque-ctrl.cpp index baab580..e803dcc 100644 --- a/src/device-torque-ctrl.cpp +++ b/src/device-torque-ctrl.cpp @@ -32,6 +32,7 @@ using namespace dynamicgraph; using namespace sot::torque_control; using namespace tsid; using namespace tsid::tasks; +using namespace dynamicgraph::sot; typedef tsid::math::ConstraintBase ConstraintBase; @@ -134,8 +135,8 @@ void DeviceTorqueCtrl::init(const double& dt, const std::string& robotRef) m_nj = m_robot_util->m_nbJoints; - const Eigen::Vector6d& kp_contact = m_kp_constraintsSIN(0); - const Eigen::Vector6d& kd_contact = m_kd_constraintsSIN(0); + const dynamicgraph::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0); + const dynamicgraph::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0); const Vector rotor_inertias = m_rotor_inertiasSIN(0); const Vector gear_ratios = m_gear_ratiosSIN(0); const std::string & urdfFile = m_robot_util->m_urdf_filename; diff --git a/src/free-flyer-locator.cpp b/src/free-flyer-locator.cpp index debc1b0..0b83569 100644 --- a/src/free-flyer-locator.cpp +++ b/src/free-flyer-locator.cpp @@ -34,8 +34,9 @@ namespace dynamicgraph using namespace dynamicgraph::command; using namespace std; using namespace se3; + using namespace dynamicgraph::sot; - typedef Eigen::Vector6d Vector6; + typedef dynamicgraph::sot::Vector6d Vector6; #define PROFILE_FREE_FLYER_COMPUTATION "Free-flyer position computation" #define PROFILE_FREE_FLYER_VELOCITY_COMPUTATION "Free-flyer velocity computation" @@ -214,7 +215,7 @@ namespace dynamicgraph s.resize(6); //~ const se3::SE3 & iMo = m_data->oMi[31].inverse(); const Eigen::AngleAxisd aa(m_Mff.rotation()); - Eigen::Vector6d freeflyer; + dynamicgraph::sot::Vector6d freeflyer; freeflyer << m_Mff.translation(), aa.axis() * aa.angle(); // due to distance from ankle to ground diff --git a/src/inverse-dynamics-balance-controller.cpp b/src/inverse-dynamics-balance-controller.cpp index 69836a2..2d7add7 100644 --- a/src/inverse-dynamics-balance-controller.cpp +++ b/src/inverse-dynamics-balance-controller.cpp @@ -48,6 +48,7 @@ namespace dynamicgraph using namespace tsid::contacts; using namespace tsid::tasks; using namespace tsid::solvers; + using namespace dg::sot; typedef SolverHQuadProgRT<60,36,34> SolverHQuadProgRT60x36x34; typedef SolverHQuadProgRT<48,30,17> SolverHQuadProgRT48x30x17; @@ -397,12 +398,12 @@ namespace dynamicgraph const Eigen::Matrix& contactPoints = m_contact_pointsSIN(0); const Eigen::Vector3d& contactNormal = m_contact_normalSIN(0); // const Eigen::VectorXd w_forceReg = m_weight_contact_forcesSIN(0); - const Eigen::Vector6d& kp_contact = m_kp_constraintsSIN(0); - const Eigen::Vector6d& kd_contact = m_kd_constraintsSIN(0); + const dg::sot::Vector6d& kp_contact = m_kp_constraintsSIN(0); + const dg::sot::Vector6d& kd_contact = m_kd_constraintsSIN(0); const Eigen::Vector3d& kp_com = m_kp_comSIN(0); const Eigen::Vector3d& kd_com = m_kd_comSIN(0); - const Eigen::Vector6d& kp_feet = m_kp_feetSIN(0); - const Eigen::Vector6d& kd_feet = m_kd_feetSIN(0); + const dg::sot::Vector6d& kp_feet = m_kp_feetSIN(0); + const dg::sot::Vector6d& kd_feet = m_kd_feetSIN(0); const VectorN& kp_posture = m_kp_postureSIN(0); const VectorN& kd_posture = m_kd_postureSIN(0); const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0); diff --git a/src/madgwickahrs.cpp b/src/madgwickahrs.cpp index 58e3dec..9d23b06 100644 --- a/src/madgwickahrs.cpp +++ b/src/madgwickahrs.cpp @@ -30,8 +30,9 @@ namespace dynamicgraph using namespace dg; using namespace dg::command; using namespace std; + using namespace dg::sot; - typedef Eigen::Vector6d Vector6; + typedef dg::sot::Vector6d Vector6; #define PROFILE_MADGWICKAHRS_COMPUTATION "MadgwickAHRS computation"