Commit 6c0da11e authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Fix warning messages + improve handling error in control-manager

parent 9750072d
......@@ -101,10 +101,10 @@ namespace dynamicgraph {
}
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
volatile float m_beta = betaDef; /// 2 * proportional gain (Kp)
volatile float m_q0 = 1.0f, m_q1 = 0.0f, m_q2 = 0.0f, m_q3 = 0.0f; /// quaternion of sensor frame
float m_sampleFreq = 512.0f; /// sample frequency in Hz
bool m_initSucceeded; /// true if the entity has been successfully initialized
volatile float m_beta; /// 2 * proportional gain (Kp)
volatile float m_q0, m_q1, m_q2 , m_q3; /// quaternion of sensor frame
float m_sampleFreq; /// sample frequency in Hz
}; // class MadgwickAHRS
} // namespace torque_control
......
......@@ -254,7 +254,7 @@ namespace dynamicgraph
}
const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
assert(ctrl.size()==m_robot_util->m_nbJoints);
assert(ctrl.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
if(m_jointCtrlModesCountDown[i]==0)
s(i) = ctrl(i);
......@@ -262,7 +262,7 @@ namespace dynamicgraph
{
cm_id_prev = m_jointCtrlModes_previous[i].id;
const dynamicgraph::Vector& ctrl_prev = (*m_ctrlInputsSIN[cm_id_prev])(iter);
assert(ctrl_prev.size()==m_robot_util->m_nbJoints);
assert(ctrl_prev.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
double alpha = m_jointCtrlModesCountDown[i]/CTRL_MODE_TRANSITION_TIME_STEP;
// SEND_MSG("Joint "+toString(i)+" changing ctrl mode from "+toString(cm_id_prev)+
......@@ -281,7 +281,7 @@ namespace dynamicgraph
}
getProfiler().stop(PROFILE_PWM_DESIRED_COMPUTATION);
usleep(1e6*m_sleep_time);
usleep(static_cast<unsigned int>(1e6*m_sleep_time));
if(m_sleep_time>=0.1)
{
for(unsigned int i=0; i<m_ctrlInputsSIN.size(); i++)
......@@ -310,7 +310,7 @@ namespace dynamicgraph
const dynamicgraph::Vector& i_real = m_i_measuredSIN(iter);
const dynamicgraph::Vector& tau_predicted = m_tau_predictedSIN(iter);
for(int i=0;i<m_emergencyStopSIN.size();i++)
for(std::size_t i=0;i<m_emergencyStopSIN.size();i++)
{
if ((*m_emergencyStopSIN[i]).isPlugged() && (*m_emergencyStopSIN[i])(iter))
{
......@@ -497,20 +497,20 @@ namespace dynamicgraph
getClassName()+"("+getName()+")::input(bool)::emergencyStop_"+name));
// register the new signals and add the new signal dependecy
unsigned int i = m_emergencyStopSIN.size()-1;
Eigen::Index i = m_emergencyStopSIN.size()-1;
m_u_safeSOUT.addDependency(*m_emergencyStopSIN[i]);
Entity::signalRegistration(*m_emergencyStopSIN[i]);
}
void ControlManager::setNameToId(const std::string &jointName,
const double & jointId)
const double & jointId)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot set joint name from joint id before initialization!");
return;
}
m_robot_util->set_name_to_id(jointName,jointId);
m_robot_util->set_name_to_id(jointName,static_cast<Eigen::Index>(jointId));
}
void ControlManager::setJointLimitsFromId( const double &jointId,
......@@ -548,7 +548,7 @@ namespace dynamicgraph
return;
}
m_robot_util->m_force_util.set_name_to_force_id(forceName,forceId);
m_robot_util->m_force_util.set_name_to_force_id(forceName,static_cast<Eigen::Index>(forceId));
}
void ControlManager::setJoints(const dg::Vector & urdf_to_sot)
......@@ -659,7 +659,7 @@ namespace dynamicgraph
bool ControlManager::convertJointNameToJointId(const std::string& name, unsigned int& id)
{
// Check if the joint name exists
pinocchio::Model::JointIndex jid = m_robot_util->get_id_from_name(name);
sot::Index jid = m_robot_util->get_id_from_name(name);
if (jid<0)
{
SEND_MSG("The specified joint name does not exist: "+name, MSG_TYPE_ERROR);
......
......@@ -60,10 +60,10 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL_OUT(accelerometer_out, dynamicgraph::Vector, m_accelerometer_inSIN)
,CONSTRUCT_SIGNAL_OUT(gyrometer_out, dynamicgraph::Vector, m_gyrometer_inSIN)
,m_initSucceeded(false)
,m_dt(static_cast<float>(0.001))
,m_dt(0.001f)
,m_update_cycles_left(0)
,m_update_cycles(0)
,m_a_gyro_DC_blocker(static_cast<float>(1.0))
,m_a_gyro_DC_blocker(1.0f)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
......
......@@ -523,7 +523,7 @@ namespace dynamicgraph
/** Copy active_joints only if a valid transition occurs. (From all OFF) or (To all OFF)**/
DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector)
{
if(s.size()!=m_robot_util->m_nbJoints)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
......@@ -549,7 +549,7 @@ namespace dynamicgraph
blocked_joints(i) = 0.0;
SEND_MSG("Blocked joints: "+toString(blocked_joints.transpose()), MSG_TYPE_INFO);
m_taskBlockedJoints->mask(blocked_joints);
TrajectorySample ref_zero(m_robot_util->m_nbJoints);
TrajectorySample ref_zero(static_cast<unsigned int>(m_robot_util->m_nbJoints));
m_taskBlockedJoints->setReference(ref_zero);
m_invDyn->addMotionTask(*m_taskBlockedJoints, 1.0, 0);
}
......@@ -560,7 +560,7 @@ namespace dynamicgraph
m_enabled = false ;
}
if (m_enabled == false)
for(int i=0; i<m_robot_util->m_nbJoints; i++)
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
s(i)=false;
return s;
}
......@@ -572,7 +572,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal tau_des before initialization!");
return s;
}
if(s.size()!=m_robot_util->m_nbJoints)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
getProfiler().start(PROFILE_TAU_DES_COMPUTATION);
......@@ -619,18 +619,18 @@ namespace dynamicgraph
m_w_feetSIN(iter);
m_active_joints_checkedSINNER(iter);
const VectorN6& q_sot = m_qSIN(iter);
assert(q_sot.size()==m_robot_util->m_nbJoints+6);
assert(q_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6));
const VectorN6& v_sot = m_vSIN(iter);
assert(v_sot.size()==m_robot_util->m_nbJoints+6);
assert(v_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6));
const Vector3& x_com_ref = m_com_ref_posSIN(iter);
const Vector3& dx_com_ref = m_com_ref_velSIN(iter);
const Vector3& ddx_com_ref = m_com_ref_accSIN(iter);
const VectorN& q_ref = m_posture_ref_posSIN(iter);
assert(q_ref.size()==m_robot_util->m_nbJoints);
assert(q_ref.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
const VectorN& dq_ref = m_posture_ref_velSIN(iter);
assert(dq_ref.size()==m_robot_util->m_nbJoints);
assert(dq_ref.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
assert(ddq_ref.size()==m_robot_util->m_nbJoints);
assert(ddq_ref.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
const Vector6& kp_contact = m_kp_constraintsSIN(iter);
const Vector6& kd_contact = m_kd_constraintsSIN(iter);
......@@ -638,13 +638,13 @@ namespace dynamicgraph
const Vector3& kd_com = m_kd_comSIN(iter);
const VectorN& kp_posture = m_kp_postureSIN(iter);
assert(kp_posture.size()==m_robot_util->m_nbJoints);
assert(kp_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
const VectorN& kd_posture = m_kd_postureSIN(iter);
assert(kd_posture.size()==m_robot_util->m_nbJoints);
assert(kd_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
const VectorN& kp_pos = m_kp_posSIN(iter);
assert(kp_pos.size()==m_robot_util->m_nbJoints);
assert(kp_pos.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
const VectorN& kd_pos = m_kd_posSIN(iter);
assert(kd_pos.size()==m_robot_util->m_nbJoints);
assert(kd_pos.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
const double & w_com = m_w_comSIN(iter);
const double & w_posture = m_w_postureSIN(iter);
......@@ -740,16 +740,16 @@ namespace dynamicgraph
m_contactRF->setReference(H_rf);
SEND_MSG("Setting right foot reference to "+toString(H_rf), MSG_TYPE_DEBUG);
}
else if(m_timeLast != iter-1)
else if(m_timeLast != static_cast<unsigned int>(iter-1))
{
SEND_MSG("Last time "+toString(m_timeLast)+" is not current time-1: "+toString(iter), MSG_TYPE_ERROR);
if(m_timeLast == iter)
if(m_timeLast == static_cast<unsigned int>(iter))
{
s = m_tau_sot;
return s;
}
}
m_timeLast = iter;
m_timeLast = static_cast<unsigned int>(iter);
const HQPData & hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
getProfiler().stop(PROFILE_PREPARE_INV_DYN);
......@@ -782,7 +782,7 @@ namespace dynamicgraph
return s;
}
getStatistics().store("active inequalities", sol.activeSet.size());
getStatistics().store("active inequalities", static_cast<double>(sol.activeSet.size()));
getStatistics().store("solver iterations", sol.iterations);
if(ddx_com_ref.norm()>1e-3)
getStatistics().store("com ff ratio", ddx_com_ref.norm()/m_taskCom->getConstraint().vector().norm());
......@@ -1237,7 +1237,7 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal right_foot_vel before initialization!");
SEND_WARNING_STREAM_MSG("Cannot compute signal right_foot_vel before initialization! iter:" +toString(iter));
return s;
}
pinocchio::Motion v;
......
......@@ -264,7 +264,9 @@ namespace dynamicgraph
s.resize(m_robot_util->m_nbJoints);
for(int i=0; i<(int)m_robot_util->m_nbJoints; i++)
s(i) = motorModel.smoothSign(dq[i], 0.1, polySignDq[i]); //TODO Use Eigen binaryexpr
s(i) = motorModel.smoothSign(dq[i], 0.1,
static_cast<unsigned int>(polySignDq[i]));
//TODO Use Eigen binaryexpr
return s;
}
......
......@@ -61,9 +61,9 @@ namespace dynamicgraph
,CONSTRUCT_SIGNAL(fLeftFoot, OUT, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL(fRightHand, OUT, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL(fLeftHand, OUT, dynamicgraph::Vector)
,m_initSucceeded(false)
,m_firstIter(true)
,m_initSucceeded(false)
,m_robot_util(RefVoidRobotUtil())
,m_robot_util(RefVoidRobotUtil())
{
BIND_SIGNAL_TO_FUNCTION(fRightFoot, OUT, dynamicgraph::Vector);
BIND_SIGNAL_TO_FUNCTION(fLeftFoot, OUT, dynamicgraph::Vector);
......@@ -215,7 +215,7 @@ namespace dynamicgraph
m_currentTrajGen.resize(m_robot_util->m_nbJoints);
m_noTrajGen.resize(m_robot_util->m_nbJoints);
for(int i=0; i<m_robot_util->m_nbJoints; i++)
for(long unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
{
m_minJerkTrajGen[i] = new MinimumJerkTrajectoryGenerator(dt,5.0,1);
m_sinTrajGen[i] = new SinusoidTrajectoryGenerator(dt,5.0,1);
......@@ -258,7 +258,7 @@ namespace dynamicgraph
if(m_firstIter)
{
const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN(iter);
if(base6d_encoders.size()!=m_robot_util->m_nbJoints+6)
if(base6d_encoders.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
{
SEND_ERROR_STREAM_MSG("Unexpected size of signal base6d_encoder " +
toString(base6d_encoders.size()) + " " +
......@@ -271,7 +271,7 @@ namespace dynamicgraph
m_firstIter = false;
}
if(s.size()!=m_robot_util->m_nbJoints)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
......@@ -316,13 +316,12 @@ namespace dynamicgraph
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization!");
SEND_WARNING_STREAM_MSG("Cannot compute signal positionDes before initialization! iter: " + toString(iter) );
return s;
}
const dynamicgraph::Vector& q = m_qSOUT(iter);
if(s.size()!=m_robot_util->m_nbJoints)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
{
......@@ -344,9 +343,8 @@ namespace dynamicgraph
return s;
}
const dynamicgraph::Vector& q = m_qSOUT(iter);
if(s.size()!=m_robot_util->m_nbJoints)
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
......@@ -365,7 +363,7 @@ namespace dynamicgraph
{
// SEND_MSG("Compute force right foot iter "+toString(iter), MSG_TYPE_DEBUG);
generateReferenceForceSignal("fRightFoot",
m_robot_util->m_force_util.get_force_id_right_foot(),
static_cast<int>(m_robot_util->m_force_util.get_force_id_right_foot()),
s, iter);
return s;
}
......@@ -373,7 +371,7 @@ namespace dynamicgraph
DEFINE_SIGNAL_OUT_FUNCTION(fLeftFoot, dynamicgraph::Vector)
{
generateReferenceForceSignal("fLeftFoot",
m_robot_util->m_force_util.get_force_id_left_foot(),
static_cast<int>(m_robot_util->m_force_util.get_force_id_left_foot()),
s, iter);
return s;
}
......@@ -381,8 +379,8 @@ namespace dynamicgraph
DEFINE_SIGNAL_OUT_FUNCTION(fRightHand, dynamicgraph::Vector)
{
generateReferenceForceSignal("fRightHand",
m_robot_util->
m_force_util.get_force_id_right_hand(),
static_cast<int>(m_robot_util->
m_force_util.get_force_id_right_hand()),
s, iter);
return s;
}
......@@ -390,8 +388,8 @@ namespace dynamicgraph
DEFINE_SIGNAL_OUT_FUNCTION(fLeftHand, dynamicgraph::Vector)
{
generateReferenceForceSignal("fLeftHand",
m_robot_util->
m_force_util.get_force_id_left_hand(),
static_cast<int>(m_robot_util->
m_force_util.get_force_id_left_hand()),
s, iter);
return s;
}
......@@ -788,7 +786,7 @@ namespace dynamicgraph
convertJointNameToJointId(const std::string& name, unsigned int& id)
{
// Check if the joint name exists
int jid = m_robot_util->get_id_from_name(name);
sot::Index jid = m_robot_util->get_id_from_name(name);
if (jid<0)
{
SEND_MSG("The specified joint name does not exist", MSG_TYPE_ERROR);
......
......@@ -57,6 +57,11 @@ namespace dynamicgraph
m_accelerometerSIN)
,m_initSucceeded(false)
,m_beta(betaDef)
,m_q0(1.0f)
,m_q1(0.0f)
,m_q2(0.0f)
,m_q3(0.0f)
,m_sampleFreq(512.0f)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
......@@ -77,7 +82,7 @@ namespace dynamicgraph
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
m_sampleFreq=1.0f/dt;
m_sampleFreq=static_cast<float>(1.0f/dt);
m_initSucceeded = true;
}
......@@ -85,7 +90,7 @@ namespace dynamicgraph
{
if(beta<0.0 || beta>1.0)
return SEND_MSG("Beta must be in [0,1]", MSG_TYPE_ERROR);
m_beta = beta;
m_beta = static_cast<float>(beta);
}
/* ------------------------------------------------------------------- */
......@@ -105,8 +110,12 @@ namespace dynamicgraph
getProfiler().start(PROFILE_MADGWICKAHRS_COMPUTATION);
{
// Update state with new measurment
madgwickAHRSupdateIMU( gyroscope(0), gyroscope(1), gyroscope(2),
accelerometer(0), accelerometer(1), accelerometer(2));
madgwickAHRSupdateIMU( static_cast<float>(gyroscope(0)),
static_cast<float>(gyroscope(1)),
static_cast<float>(gyroscope(2)),
static_cast<float>(accelerometer(0)),
static_cast<float>(accelerometer(1)),
static_cast<float>(accelerometer(2)));
if(s.size()!=4)
s.resize(4);
s(0) = m_q0;
......
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