Unverified Commit 90409a65 authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Merge pull request #50 from nim65s/topic/delete-shell

Remove shell
parents d621620a 82d1317a
Pipeline #1354 passed with stage
in 24 minutes and 8 seconds
......@@ -58,8 +58,8 @@ namespace dynamicgraph {
{
typedef AdmittanceController EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
......@@ -102,15 +102,12 @@ namespace dynamicgraph {
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
Eigen::VectorXd m_u; /// control (i.e. motor currents)
bool m_firstIter;
......@@ -157,7 +154,7 @@ namespace dynamicgraph {
// tsid::math::Vector3 m_zmp_RF; /// 3d zmp left foot
// tsid::math::Vector3 m_zmp; /// 3d global zmp
}; // class AdmittanceController
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
......
......@@ -58,7 +58,7 @@ namespace dynamicgraph {
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/** Compute s12 as an intermediate transform between s1 and s2 SE3 transforms**/
void se3Interp(const se3::SE3 & s1, const se3::SE3 & s2, const double alpha, se3::SE3 & s12);
......@@ -70,13 +70,13 @@ namespace dynamicgraph {
/** Convert from Transformation Matrix to Roll, Pitch, Yaw */
void matrixToRpy(const Eigen::Matrix3d & M, Eigen::Vector3d & rpy);
/** Multiply to quaternions stored in (w,x,y,z) format */
void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2, Eigen::Vector4d & q12);
/** Rotate a point or a vector by a quaternion stored in (w,x,y,z) format */
void pointRotationByQuaternion(const Eigen::Vector3d & point,const Eigen::Vector4d & quat, Eigen::Vector3d & rotatedPoint);
class SOTBASEESTIMATOR_EXPORT BaseEstimator
:public::dynamicgraph::Entity
{
......@@ -91,7 +91,7 @@ namespace dynamicgraph {
typedef boost::math::normal normal;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
......@@ -99,7 +99,7 @@ namespace dynamicgraph {
BaseEstimator( const std::string & name );
void init(const double & dt, const std::string& urdfFile);
void set_fz_stable_windows_size(const int & ws);
void set_alpha_w_filter(const double & a);
void set_alpha_DC_acc(const double & a);
......@@ -168,15 +168,12 @@ namespace dynamicgraph {
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_reset_foot_pos; /// true after the command resetFootPositions is called
......@@ -214,12 +211,12 @@ namespace dynamicgraph {
double m_alpha_DC_acc; /// alpha parameter for DC blocker filter on acceleration data
double m_alpha_DC_vel; /// alpha parameter for DC blocker filter on velocity data
double m_alpha_w_filter; /// filter parameter to filter weights (1st order low pass filter)
double m_w_lf_filtered; /// filtered weight of the estimation coming from the left foot
double m_w_rf_filtered; /// filtered weight of the estimation coming from the right foot
se3::Model m_model; /// Pinocchio robot model
se3::Data *m_data; /// Pinocchio robot data
se3::SE3 m_oMff_lf; /// world-to-base transformation obtained through left foot
......@@ -228,10 +225,10 @@ namespace dynamicgraph {
SE3 m_oMrfs; /// transformation from world to right foot sole
Vector7 m_oMlfs_xyzquat;
Vector7 m_oMrfs_xyzquat;
SE3 m_oMlfs_default_ref;/// Default reference for left foot pose to use if no ref is pluged
SE3 m_oMrfs_default_ref;/// Default reference for right foot pose to use if no ref is pluged
SE3 m_oMlfs_default_ref;/// Default reference for left foot pose to use if no ref is pluged
SE3 m_oMrfs_default_ref;/// Default reference for right foot pose to use if no ref is pluged
normal m_normal; /// Normal distribution
bool m_isFirstIterFlag; /// flag to detect first iteration and initialise velocity filters
SE3 m_sole_M_ftSens; /// foot sole to F/T sensor transformation
......@@ -246,14 +243,14 @@ namespace dynamicgraph {
Eigen::VectorXd m_v_sot; /// robot velocities according to SoT convention
Matrix3 m_oRchest; /// chest orientation in the world from angular fusion
Matrix3 m_oRff; /// base orientation in the world
/* Filter buffers*/
Vector3 m_last_vel;
Vector3 m_last_DCvel;
Vector3 m_last_DCacc;
}; // class BaseEstimator
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
......
......@@ -63,8 +63,8 @@ namespace dynamicgraph {
{
typedef FreeFlyerLocator EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
......@@ -83,16 +83,13 @@ namespace dynamicgraph {
/// base6d_encoders with base6d in RPY
DECLARE_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector);
/// n+6 robot velocities
DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
/* --- COMMANDS --- */
void displayRobotUtil();
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
......@@ -100,10 +97,10 @@ namespace dynamicgraph {
}
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
se3::Model *m_model; /// Pinocchio robot model
se3::Data *m_data; /// Pinocchio robot data
se3::Data *m_data; /// Pinocchio robot data
se3::SE3 m_Mff; /// SE3 Transform from center of feet to base
se3::SE3 m_w_M_lf;
se3::SE3 m_w_M_rf;
......@@ -117,7 +114,7 @@ namespace dynamicgraph {
RobotUtil * m_robot_util;
}; // class FreeFlyerLocator
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
......
......@@ -76,9 +76,7 @@ namespace dynamicgraph {
protected:
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
/* --- METHODS --- */
void update_offset_impl(int iter);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
......
......@@ -68,14 +68,14 @@ namespace dynamicgraph {
{
typedef InverseDynamicsBalanceController EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
InverseDynamicsBalanceController( const std::string & name );
void init(const double& dt,
void init(const double& dt,
const std::string& robotRef);
void updateComOffset();
void removeRightFootContact(const double& transitionTime);
......@@ -147,7 +147,7 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(M, dynamicgraph::Matrix);
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
......@@ -175,16 +175,13 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(left_foot_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector);
/// This signal copies active_joints only if it changes from a all false or to an all false value
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
......
......@@ -58,8 +58,8 @@ namespace dynamicgraph {
{
typedef JointTrajectoryGenerator EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
public:
/* --- CONSTRUCTOR ---- */
JointTrajectoryGenerator( const std::string & name );
......@@ -154,18 +154,15 @@ namespace dynamicgraph {
* @param forceName A string identifying the force to stop.
* */
void stopForce(const std::string& forceName);
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[JointTrajectoryGenerator-"+name+"] "+msg, t, file, line);
}
protected:
enum JTG_Status
{
......@@ -212,7 +209,7 @@ namespace dynamicgraph {
bool isForceInRange(unsigned int id, int axis, double f);
}; // class JointTrajectoryGenerator
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
......
......@@ -88,9 +88,7 @@ namespace dynamicgraph {
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
/* --- METHODS --- */
float invSqrt(float x);
void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) ;
......
......@@ -67,8 +67,8 @@ namespace dynamicgraph {
{
typedef NdTrajectoryGenerator EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
public:
/* --- CONSTRUCTOR ---- */
NdTrajectoryGenerator( const std::string & name );
......@@ -94,7 +94,7 @@ namespace dynamicgraph {
void setSpline(const std::string& filename, const double& timeToInitConf);
void startSpline();
/** Print the current value of the specified component. */
void getValue(const int& id);
......@@ -145,15 +145,12 @@ namespace dynamicgraph {
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
}
protected:
enum JTG_Status
{
......@@ -187,7 +184,7 @@ namespace dynamicgraph {
parametriccurves::Spline<double, Eigen::Dynamic>* m_splineTrajGen;
}; // class NdTrajectoryGenerator
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
......
......@@ -56,8 +56,8 @@ namespace dynamicgraph {
{
typedef PositionController EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
......@@ -84,15 +84,12 @@ namespace dynamicgraph {
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[PositionController-"+name+"] "+msg, t, file, line);
}
protected:
RobotUtil * m_robot_util; /// Robot Util
Eigen::VectorXd m_pwmDes;
......@@ -105,7 +102,7 @@ namespace dynamicgraph {
Eigen::VectorXd m_q, m_dq;
}; // class PositionController
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
......
......@@ -58,8 +58,8 @@ namespace dynamicgraph {
{
typedef SE3TrajectoryGenerator EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
public:
/* --- CONSTRUCTOR ---- */
SE3TrajectoryGenerator( const std::string & name );
......@@ -135,15 +135,12 @@ namespace dynamicgraph {
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[SE3TrajectoryGenerator-"+name+"] "+msg, t, file, line);
}
protected:
enum TG_Status
{
......@@ -181,7 +178,7 @@ namespace dynamicgraph {
TextFileTrajectoryGenerator* m_textFileTrajGen;
}; // class SE3TrajectoryGenerator
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
......
......@@ -89,9 +89,6 @@ namespace dynamicgraph {
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
......
......@@ -430,23 +430,6 @@ namespace dynamicgraph
catch (ExceptionSignal e) {}
}
void AdmittanceController::commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os )
{
if( cmdLine == "help" )
{
os << "sotAdmittanceController:\n"
<< "\t -." << std::endl;
Entity::commandLine(cmdLine, cmdArgs, os);
}
else
{
Entity::commandLine(cmdLine,cmdArgs,os);
}
}
//**************************************************************************************************
VectorXd svdSolveWithDamping(const JacobiSVD<MatrixXd>& A, const VectorXd &b, double damping)
{
......@@ -473,7 +456,7 @@ namespace dynamicgraph
return res;
}
} // namespace torquecontrol
} // namespace sot
} // namespace dynamicgraph
......
......@@ -75,7 +75,7 @@ namespace dynamicgraph
rpy(0) = atan2(M(2,1), M(2,2)); // gamma
}
}
void quanternionMult(const Eigen::Vector4d & q1, const Eigen::Vector4d & q2, Eigen::Vector4d & q12)
{
q12(0) = q2(0)*q1(0)-q2(1)*q1(1)-q2(2)*q1(2)-q2(3)*q1(3);
......@@ -105,7 +105,7 @@ namespace dynamicgraph
m_imu_quaternionSIN << m_forceLLEGSIN << m_forceRLEGSIN << m_dforceLLEGSIN << m_dforceRLEGSIN << \
m_w_lf_inSIN << m_w_rf_inSIN << m_K_fb_feet_posesSIN << m_lf_ref_xyzquatSIN << m_rf_ref_xyzquatSIN << m_accelerometerSIN << m_gyroscopeSIN
#define OUTPUT_SIGNALS m_qSOUT << m_vSOUT << m_q_lfSOUT << m_q_rfSOUT << m_q_imuSOUT << \
m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT << m_w_rf_filteredSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << \
m_w_lfSOUT << m_w_rfSOUT << m_w_lf_filteredSOUT << m_w_rf_filteredSOUT << m_lf_xyzquatSOUT << m_rf_xyzquatSOUT << \
m_v_acSOUT << m_a_acSOUT << m_v_kinSOUT << m_v_imuSOUT << m_v_gyrSOUT << m_v_flexSOUT
/// Define EntityClassName here rather than in the header file
......@@ -198,15 +198,15 @@ namespace dynamicgraph
addCommand("set_alpha_w_filter",
makeCommandVoid1(*this, &BaseEstimator::set_alpha_w_filter,
docCommandVoid1("Set the filter parameter to filter weights",
"double")));
"double")));
addCommand("set_alpha_DC_acc",
makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_acc,
docCommandVoid1("Set the filter parameter for removing DC from accelerometer data",
"double")));
"double")));
addCommand("set_alpha_DC_vel",
makeCommandVoid1(*this, &BaseEstimator::set_alpha_DC_vel,
docCommandVoid1("Set the filter parameter for removing DC from velocity integrated from acceleration",
"double")));
"double")));
addCommand("reset_foot_positions",
makeCommandVoid0(*this, &BaseEstimator::reset_foot_positions,
docCommandVoid0("Reset the position of the feet.")));
......@@ -307,11 +307,11 @@ namespace dynamicgraph
m_v_gyr.setZero(m_robot_util->m_nbJoints+6);
m_sole_M_ftSens = SE3(Matrix3::Identity(),
-Eigen::Map<const Vector3>(&m_robot_util->m_foot_util.m_Right_Foot_Force_Sensor_XYZ(0)));
m_last_vel.setZero();
m_last_DCvel.setZero();
m_last_DCacc.setZero(); //this is to replace by acceleration at 1st iteration
m_alpha_DC_acc = 0.9995;
m_alpha_DC_vel = 0.9995;
m_alpha_w_filter = 1.0;
......@@ -353,7 +353,7 @@ namespace dynamicgraph
return SEND_MSG("alpha should be in [0..1]", MSG_TYPE_ERROR);
m_alpha_DC_acc = a;
}
void BaseEstimator::set_alpha_DC_vel(const double& a)
{
if(a<0.0 || a>1.0)
......@@ -526,7 +526,7 @@ namespace dynamicgraph
//save this poses to use it if no ref is provided
m_oMlfs_default_ref = m_oMlfs;
m_oMrfs_default_ref = m_oMrfs;
sendMsg("Reference pos of left foot:\n"+toString(m_oMlfs), MSG_TYPE_INFO);
sendMsg("Reference pos of right foot:\n"+toString(m_oMrfs), MSG_TYPE_INFO);
......@@ -600,7 +600,7 @@ namespace dynamicgraph
}
if(s.size()!=m_robot_util->m_nbJoints+6)
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & qj = m_joint_positionsSIN(iter); //n+6
const Eigen::Vector4d & quatIMU_vec = m_imu_quaternionSIN(iter);
const Vector6 & ftrf = m_forceRLEGSIN(iter);
......@@ -653,15 +653,15 @@ namespace dynamicgraph
kinematics_estimation(ftlf, m_K_lf, m_oMlfs, m_left_foot_id, m_oMff_lf, oMlfa, lfsMff);
// get rpy
const SE3 ffMchest(m_data->oMf[m_IMU_body_id]); // transform between freeflyer and body attached to IMU sensor
const SE3 ffMchest(m_data->oMf[m_IMU_body_id]); // transform between freeflyer and body attached to IMU sensor
const SE3 chestMff(ffMchest.inverse()); // transform between body attached to IMU sensor and freeflyer
Vector3 rpy_chest, rpy_chest_lf, rpy_chest_rf, rpy_chest_imu; // orientation of the body which imu is attached to. (fusion, from left kine, from right kine, from imu)
matrixToRpy((m_oMff_lf*ffMchest).rotation(), rpy_chest_lf);
matrixToRpy((m_oMff_rf*ffMchest).rotation(), rpy_chest_rf);
Eigen::Quaternion<double> quatIMU(quatIMU_vec[0], quatIMU_vec[1], quatIMU_vec[2], quatIMU_vec[3]);
matrixToRpy(quatIMU.toRotationMatrix(), rpy_chest_imu);
matrixToRpy(quatIMU.toRotationMatrix(), rpy_chest_imu);
// average (we do not take into account the IMU yaw)
double wSum = wL + wR + m_w_imu;
......@@ -684,10 +684,10 @@ namespace dynamicgraph
base_se3_to_sot(m_q_pin.head<3>(), m_oRff, m_q_sot.head<6>());
s = m_q_sot;
// store estimation of the base pose in SE3 format
const SE3 oMff_est(m_oRff, m_q_pin.head<3>());
// feedback on feet poses
if(m_K_fb_feet_posesSIN.isPlugged())
{
......@@ -708,7 +708,7 @@ namespace dynamicgraph
SE3 rightDrift_delta;
se3Interp(leftDrift ,SE3::Identity(),K_fb*wR,leftDrift_delta);
se3Interp(rightDrift,SE3::Identity(),K_fb*wL,rightDrift_delta);
// if a feet is not stable on the ground (aka flying), fully update his position
// if a feet is not stable on the ground (aka flying), fully update his position
if (m_right_foot_is_stable == false)
rightDrift_delta = rightDrift;
if (m_left_foot_is_stable == false)
......@@ -944,7 +944,7 @@ namespace dynamicgraph
s = m_w_rf_filtered;
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(w_lf_filtered, double)
{
if(!m_initSucceeded)
......@@ -1023,14 +1023,14 @@ namespace dynamicgraph
Vector6 v_kin_r = -ffMrf.act(v_rf_local).toVector(); //this is the velocity of the base in the frame of the base.
v_kin_r.head<3>() = m_oRff * v_kin_r.head<3>();
v_kin_r.segment<3>(3) = m_oRff * v_kin_r.segment<3>(3);
m_v_kin.head<6>() = (wR*v_kin_r + wL*v_kin_l)/(wL+wR);
/* Compute velocity induced by the flexibility */
Vector6 v_flex_l;
Vector6 v_flex_r;
v_flex_l << -dftlf[0]/m_K_lf(0), -dftlf[1]/m_K_lf(1), -dftlf[2]/m_K_lf(2),
-dftlf[3]/m_K_lf(3), -dftlf[4]/m_K_lf(4), -dftlf[5]/m_K_lf(5);
-dftlf[3]/m_K_lf(3), -dftlf[4]/m_K_lf(4), -dftlf[5]/m_K_lf(5);
v_flex_r << -dftrf[0]/m_K_rf(0), -dftrf[1]/m_K_rf(1), -dftrf[2]/m_K_rf(<