Commit a2ac1247 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Merge branch 'devel' into origin-master

parents 72b0f9fe ab2ed89d
Pipeline #4527 failed with stage
in 8 minutes and 4 seconds
Subproject commit dc8b946d456d2c41ad12b819111b005148c68031
Subproject commit 679949e28a91c334edf3588753cf294aec2fc4c7
Subproject commit 1d9aeca25e970d2d967fd5be0fb93fe961db121b
Subproject commit 61e5574a0615706aab06986f6aecf665ddc31141
......@@ -108,7 +108,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("["+name+"] "+msg, t, file, line);
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
......@@ -146,7 +146,7 @@ namespace dynamicgraph {
tsid::math::Vector6 m_v_RF_int;
tsid::math::Vector6 m_v_LF_int;
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
// tsid::math::Vector3 m_zmp_des_LF; /// 3d desired zmp left foot
// tsid::math::Vector3 m_zmp_des_RF; /// 3d desired zmp left foot
......
......@@ -174,14 +174,14 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("["+name+"] "+msg, t, file, line);
Entity::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
double m_dt; /// sampling time step
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
bool m_left_foot_is_stable; /// True if left foot as been stable for the last 'm_fz_stable_windows_size' samples
bool m_right_foot_is_stable; /// True if right foot as been stable for the last 'm_fz_stable_windows_size' samples
......
......@@ -88,9 +88,9 @@ namespace dynamicgraph {
/* --- CONSTRUCTOR ---- */
ControlManager( const std::string & name);
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
/// Initialize
/// @param dt: control interval
/// @param urdfFile: path to the URDF model of the robot
void init(const double & dt,
const std::string & urdfFile,
const std::string & robotRef);
......@@ -112,35 +112,39 @@ namespace dynamicgraph {
/* --- COMMANDS --- */
/// Commands related to the control mode.
/// Commands related to the control mode.
void addCtrlMode(const std::string& name);
void ctrlModes();
void getCtrlMode(const std::string& jointName);
void setCtrlMode(const std::string& jointName, const std::string& ctrlMode);
void setCtrlMode(const int jid, const CtrlMode& cm);
void resetProfiler();
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const double & jointId);
void setJointLimitsFromId(const double &jointId,
const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const double &jointId,
const dynamicgraph::Vector &lq,
const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName,
const double & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
/// Commands related to joint name and joint id
void setNameToId(const std::string& jointName, const double & jointId);
void setJointLimitsFromId(const double &jointId,
const double &lq, const double &uq);
/// Command related to ForceUtil
void setForceLimitsFromId(const double &jointId,
const dynamicgraph::Vector &lq,
const dynamicgraph::Vector &uq);
void setForceNameToForceId(const std::string& forceName,
const double & forceId);
/// Commands related to FootUtil
void setRightFootSoleXYZ(const dynamicgraph::Vector &);
void setRightFootForceSensorXYZ(const dynamicgraph::Vector &);
void setFootFrameName(const std::string &, const std::string &);
void setFootFrameName(const std::string &, const std::string &);
void setImuJointName(const std::string &);
void displayRobotUtil();
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
void displayRobotUtil();
/// Commands related to HandUtil
void setHandFrameName(const std::string &, const std::string &);
/// Set the mapping between urdf and sot.
void setJoints(const dynamicgraph::Vector &);
void setStreamPrintPeriod(const double & s);
void setSleepTime(const double &seconds);
......@@ -151,11 +155,11 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[ControlManager-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[ControlManager-"+name+"] "+msg, t, file, line);
}
protected:
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
tsid::robots::RobotWrapper * m_robot;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
......
......@@ -115,11 +115,11 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[CurrentController-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[CurrentController-"+name+"] "+msg, t, file, line);
}
protected:
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_emergency_stop_triggered;
double m_dt; /// control loop time period
......
......@@ -34,7 +34,7 @@ namespace dynamicgraph {
#define ALL_INPUT_SIGNALS m_pos_desSIN << m_pos_motor_measureSIN \
<< m_pos_joint_measureSIN << m_dx_measureSIN \
<< m_tau_measureSIN << m_temp_measureSIN
<< m_tau_measureSIN << m_temp_measureSIN << m_tau_desSIN
#define ALL_OUTPUT_SIGNALS m_tauSOUT
......@@ -49,7 +49,8 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(pos_joint_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dx_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tau_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(temp_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(temp_measure, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(tau, dynamicgraph::Vector);
protected:
......
......@@ -98,7 +98,7 @@ namespace dynamicgraph
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[DeviceTorqueCtrl] "+msg, t, file, line);
Entity::sendMsg("[DeviceTorqueCtrl] "+msg, t, file, line);
}
/// \brief Current integration step.
......@@ -176,7 +176,7 @@ namespace dynamicgraph
DIST normalDistribution_;
GEN normalRandomNumberGenerator_;
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
};
} // end namespace torque_control
......
......@@ -106,7 +106,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("["+name+"] "+msg, t, file, line);
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
public: /* --- ENTITY INHERITANCE --- */
......
......@@ -97,7 +97,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[FreeFlyerLocator-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[FreeFlyerLocator-"+name+"] "+msg, t, file, line);
}
protected:
......@@ -115,7 +115,7 @@ namespace dynamicgraph {
Eigen::VectorXd m_v_pin; /// robot velocities according to pinocchio convention
Eigen::VectorXd m_v_sot; /// robot velocities according to SoT convention
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
}; // class FreeFlyerLocator
......
......@@ -84,7 +84,7 @@ namespace dynamicgraph {
void update_offset_impl(int iter);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[ImuOffsetCompensation-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[ImuOffsetCompensation-"+name+"] "+msg, t, file, line);
}
protected:
......
......@@ -85,6 +85,10 @@ namespace dynamicgraph {
void removeLeftFootContact(const double& transitionTime);
void addRightFootContact(const double& transitionTime);
void addLeftFootContact(const double& transitionTime);
void addTaskRightHand(/*const double& transitionTime*/);
void removeTaskRightHand(const double& transitionTime);
void addTaskLeftHand(/*const double& transitionTime*/);
void removeTaskLeftHand(const double& transitionTime);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
......@@ -96,6 +100,12 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(lf_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lf_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lf_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rh_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rh_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(rh_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lh_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lh_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(lh_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
......@@ -113,6 +123,8 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_feet, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_feet, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_hands, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_hands, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
......@@ -120,6 +132,7 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(w_com, double);
DECLARE_SIGNAL_IN(w_feet, double);
DECLARE_SIGNAL_IN(w_hands, double);
DECLARE_SIGNAL_IN(w_posture, double);
DECLARE_SIGNAL_IN(w_base_orientation, double);
DECLARE_SIGNAL_IN(w_torques, double);
......@@ -172,10 +185,16 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_hand_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_hand_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_hand_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_hand_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_hand_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_hand_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_acc_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_acc_des, dynamicgraph::Vector);
......@@ -188,7 +207,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("["+name+"] "+msg, t, file, line);
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
......@@ -210,9 +229,29 @@ namespace dynamicgraph {
ContactState m_contactState;
double m_contactTransitionTime; /// end time of the current contact transition (if any)
enum RightHandState
{
TASK_RIGHT_HAND_ON = 0,
/*TASK_RIGHT_HAND_TRANSITION = 1,*/
TASK_RIGHT_HAND_OFF = 1
};
RightHandState m_rightHandState;
enum LeftHandState
{
TASK_LEFT_HAND_ON = 0,
/*TASK_LEFT_HAND_TRANSITION = 1,*/
TASK_LEFT_HAND_OFF = 1
};
LeftHandState m_leftHandState;
/*double m_handsTransitionTime;*/ /// end time of the current transition (if any)
int m_frame_id_rf; /// frame id of right foot
int m_frame_id_lf; /// frame id of left foot
int m_frame_id_rh; /// frame id of right hand
int m_frame_id_lh; /// frame id of left hand
/// tsid
tsid::robots::RobotWrapper * m_robot;
tsid::solvers::SolverHQPBase * m_hqpSolver;
......@@ -221,19 +260,26 @@ namespace dynamicgraph {
tsid::InverseDynamicsFormulationAccForce * m_invDyn;
tsid::contacts::Contact6d * m_contactRF;
tsid::contacts::Contact6d * m_contactLF;
tsid::contacts::Contact6d * m_contactRH;
tsid::contacts::Contact6d * m_contactLH;
tsid::tasks::TaskComEquality * m_taskCom;
tsid::tasks::TaskSE3Equality * m_taskRF;
tsid::tasks::TaskSE3Equality * m_taskLF;
tsid::tasks::TaskSE3Equality * m_taskRH;
tsid::tasks::TaskSE3Equality * m_taskLH;
tsid::tasks::TaskJointPosture * m_taskPosture;
tsid::tasks::TaskJointPosture * m_taskBlockedJoints;
tsid::trajectories::TrajectorySample m_sampleCom;
tsid::trajectories::TrajectorySample m_sampleRF;
tsid::trajectories::TrajectorySample m_sampleLF;
tsid::trajectories::TrajectorySample m_sampleRH;
tsid::trajectories::TrajectorySample m_sampleLH;
tsid::trajectories::TrajectorySample m_samplePosture;
double m_w_com;
double m_w_posture;
double m_w_hands;
tsid::math::Vector m_dv_sot; /// desired accelerations (sot order)
tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order)
......@@ -262,7 +308,7 @@ namespace dynamicgraph {
tsid::math::Vector6 m_v_LF_int;
unsigned int m_timeLast;
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
}; // class InverseDynamicsBalanceController
} // namespace torque_control
......
......@@ -124,11 +124,11 @@ namespace dynamicgraph {
// Eigen::VectorXd m_dqDesIntegral; /// integral of the desired velocity
Eigen::VectorXd m_dqErrIntegral; /// integral of the velocity error
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("["+name+"] "+msg, t, file, line);
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
public:
......
......@@ -163,7 +163,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[JointTrajectoryGenerator-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[JointTrajectoryGenerator-"+name+"] "+msg, t, file, line);
}
protected:
......@@ -182,7 +182,7 @@ namespace dynamicgraph {
bool m_firstIter; /// true if it is the first iteration, false otherwise
double m_dt; /// control loop time period
RobotUtil *m_robot_util;
RobotUtilShrPtr m_robot_util;
std::vector<int> m_iterForceSignals;
......
......@@ -97,7 +97,7 @@ namespace dynamicgraph {
//void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[MadgwickAHRS-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[MadgwickAHRS-"+name+"] "+msg, t, file, line);
}
protected:
......
......@@ -148,7 +148,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[NdTrajectoryGenerator-"+name+"] "+msg, t, file, line);
}
protected:
......
......@@ -117,7 +117,7 @@ namespace dynamicgraph {
protected:
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("["+name+"] "+msg, t, file, line);
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
public: /* --- ENTITY INHERITANCE --- */
......
......@@ -89,11 +89,11 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[PositionController-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[PositionController-"+name+"] "+msg, t, file, line);
}
protected:
RobotUtil * m_robot_util; /// Robot Util
RobotUtilShrPtr m_robot_util; /// Robot Util
Eigen::VectorXd m_pwmDes;
bool m_initSucceeded; /// true if the entity has been successfully initialized
double m_dt; /// control loop time period
......
......@@ -140,7 +140,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("[SE3TrajectoryGenerator-"+name+"] "+msg, t, file, line);
Entity::sendMsg("[SE3TrajectoryGenerator-"+name+"] "+msg, t, file, line);
}
protected:
......
......@@ -94,7 +94,7 @@ namespace dynamicgraph {
DECLARE_SIGNAL_OUT(jointTorquesEstimated, dynamicgraph::Vector);
protected:
RobotUtil * m_robot_util;
RobotUtilShrPtr m_robot_util;
pinocchio::Model m_model; /// Pinocchio robot model
pinocchio::Data *m_data; /// Pinocchio robot data
int n_iterations; //Number of iterations to consider
......@@ -114,7 +114,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
sendMsg("["+name+"] "+msg, t, file, line);
Entity::sendMsg("["+name+"] "+msg, t, file, line);
}
private:
......
Markdown is supported
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