Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[tools] Change namespace Vector[1|6]d to dynamic_graph::sot #58

Closed
wants to merge 1 commit into from
Closed

[tools] Change namespace Vector[1|6]d to dynamic_graph::sot #58

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions include/sot/torque_control/base-estimator.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down
2 changes: 1 addition & 1 deletion include/sot/torque_control/nd-trajectory-generator.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
46 changes: 0 additions & 46 deletions include/sot/torque_control/utils/vector-conversions.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 \
Expand Down
17 changes: 9 additions & 8 deletions src/admittance-controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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++)
{
Expand Down Expand Up @@ -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++)
{
Expand Down
5 changes: 3 additions & 2 deletions src/device-torque-ctrl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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;
Expand Down
5 changes: 3 additions & 2 deletions src/free-flyer-locator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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
Expand Down
9 changes: 5 additions & 4 deletions src/inverse-dynamics-balance-controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -397,12 +398,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);
Expand Down
3 changes: 2 additions & 1 deletion src/madgwickahrs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down