Commit 44979b79 authored by Noëlie Ramuzat's avatar Noëlie Ramuzat Committed by Olivier Stasse
Browse files

[tools] Change namespace Vector[1|6] 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.
parent 185a3811
......@@ -88,8 +88,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;
......
......@@ -173,7 +173,7 @@ namespace dynamicgraph {
bool m_splineReady; /// true if the spline has been successfully loaded.
std::vector<JTG_Status> m_status; /// status of the component
std::vector<parametriccurves::AbstractCurve<double, Eigen::Vector1d>* > m_currentTrajGen;
std::vector<parametriccurves::AbstractCurve<double, dynamicgraph::sot::Vector1d>* > m_currentTrajGen;
std::vector<parametriccurves::Constant<double, 1>* > m_noTrajGen;
std::vector<parametriccurves::MinimumJerk<double, 1>* > m_minJerkTrajGen;
std::vector<parametriccurves::InfiniteSinusoid<double,1>* > m_sinTrajGen;
......
......@@ -21,52 +21,6 @@
#include <fstream>
#include <sstream>
namespace Eigen
{
#define EIGEN_MAKE_TYPEDEFS(Type, TypeSuffix, Size, SizeSuffix) \
/** \ingroup matrixtypedefs */ \
typedef Matrix<Type, Size, Size> Matrix##SizeSuffix##TypeSuffix; \
/** \ingroup matrixtypedefs */ \
typedef Matrix<Type, Size, 1> Vector##SizeSuffix##TypeSuffix; \
/** \ingroup matrixtypedefs */ \
typedef Matrix<Type, 1, Size> RowVector##SizeSuffix##TypeSuffix;
#define EIGEN_MAKE_FIXED_TYPEDEFS(Type, TypeSuffix, Size) \
/** \ingroup matrixtypedefs */ \
typedef Matrix<Type, Size, Dynamic> Matrix##Size##X##TypeSuffix; \
/** \ingroup matrixtypedefs */ \
typedef Matrix<Type, Dynamic, Size> 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<float>, cf)
EIGEN_MAKE_TYPEDEFS_ALL_SIZES(std::complex<double>, cd)
#undef EIGEN_MAKE_TYPEDEFS_ALL_SIZES
#undef EIGEN_MAKE_TYPEDEFS
typedef Matrix<double,Dynamic,Dynamic,RowMajor> MatrixRXd;
typedef Map<MatrixRXd> SigMatrixXd;
typedef Map<VectorXd> SigVectorXd;
typedef const Map<const MatrixRXd> const_SigMatrixXd;
typedef const Map<const VectorXd> const_SigVectorXd;
typedef Eigen::Ref<Eigen::VectorXd> RefVector;
typedef const Eigen::Ref<const Eigen::VectorXd>& ConstRefVector;
typedef Eigen::Ref<Eigen::MatrixXd> RefMatrix;
typedef const Eigen::Ref<const Eigen::MatrixXd> ConstRefMatrix;
}
#define EIGEN_CONST_VECTOR_FROM_STD_VECTOR(name,signal) \
Eigen::const_SigVectorXd name \
......
......@@ -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++)
{
......
......@@ -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;
......
......@@ -35,7 +35,7 @@ namespace dynamicgraph
using namespace std;
using namespace pinocchio;
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 +214,7 @@ namespace dynamicgraph
s.resize(6);
//~ const pinocchio::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
......
......@@ -49,6 +49,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;
......@@ -400,12 +401,12 @@ namespace dynamicgraph
const Eigen::Matrix<double, 3, 4>& 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);
......
......@@ -29,8 +29,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"
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment