Skip to content

GitLab

Commit 5bda36bd authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Fix Eigen::Index -> Eigen::VectorXd::Index

parent e7abd1c0
Pipeline #4526 failed with stage
in 11 minutes and 46 seconds
Hide whitespace changes
Inline Side-by-side
src/base-estimator.cpp
View file @ 5bda36bd
......@@ -574,8 +574,8 @@ namespace dynamicgraph
const Eigen::VectorXd& qj= m_joint_positionsSIN(iter); //n+6
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(qj.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
assert(qj.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* convert sot to pinocchio joint order */
m_robot_util->joints_sot_to_urdf(qj, m_q_pin.tail(m_robot_util->m_nbJoints));
......@@ -602,7 +602,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & qj = m_joint_positionsSIN(iter); //n+6
......@@ -631,7 +631,7 @@ namespace dynamicgraph
wR = 0.0;
}
assert(qj.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
assert(qj.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
// if both weights are zero set them to a small positive value to avoid division by zero
if(wR==0.0 && wL==0.0)
......@@ -820,7 +820,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_lf before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -837,7 +837,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_rf before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -854,7 +854,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal q_imu before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
const Eigen::VectorXd & q = m_qSOUT(iter);
......@@ -968,7 +968,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -981,7 +981,7 @@ namespace dynamicgraph
const Eigen::Vector3d& gyr_imu = m_gyroscopeSIN(iter);
const Vector6 & dftrf = m_dforceRLEGSIN(iter);
const Vector6 & dftlf = m_dforceLLEGSIN(iter);
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
// if the weights are not specified by the user through the input signals w_lf, w_rf
// then compute them
......
src/control-manager.cpp
View file @ 5bda36bd
......@@ -259,7 +259,7 @@ namespace dynamicgraph
}
const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
assert(ctrl.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(ctrl.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
if(m_jointCtrlModesCountDown[i]==0)
s(i) = ctrl(i);
......@@ -267,7 +267,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()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(ctrl_prev.size()==static_cast<Eigen::VectorXd::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)+
......@@ -502,7 +502,7 @@ namespace dynamicgraph
getClassName()+"("+getName()+")::input(bool)::emergencyStop_"+name));
// register the new signals and add the new signal dependecy
Eigen::Index i = m_emergencyStopSIN.size()-1;
Eigen::VectorXd::Index i = m_emergencyStopSIN.size()-1;
m_u_safeSOUT.addDependency(*m_emergencyStopSIN[i]);
Entity::signalRegistration(*m_emergencyStopSIN[i]);
}
......@@ -515,7 +515,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot set joint name from joint id before initialization!");
return;
}
m_robot_util->set_name_to_id(jointName,static_cast<Eigen::Index>(jointId));
m_robot_util->set_name_to_id(jointName,static_cast<Eigen::VectorXd::Index>(jointId));
}
void ControlManager::setJointLimitsFromId( const double &jointId,
......@@ -553,7 +553,7 @@ namespace dynamicgraph
return;
}
m_robot_util->m_force_util.set_name_to_force_id(forceName,static_cast<Eigen::Index>(forceId));
m_robot_util->m_force_util.set_name_to_force_id(forceName,static_cast<Eigen::VectorXd::Index>(forceId));
}
void ControlManager::setJoints(const dg::Vector & urdf_to_sot)
......
src/current-controller.cpp
View file @ 5bda36bd
......@@ -226,7 +226,7 @@ namespace dynamicgraph
const dynamicgraph::Vector& i_real = m_i_realSOUT(iter);
const dynamicgraph::Vector& in_out_gain = m_in_out_gainSIN(iter);
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)
......
src/device-torque-ctrl.cpp
View file @ 5bda36bd
......@@ -281,7 +281,7 @@ void DeviceTorqueCtrl::computeForwardDynamics()
m_dvBar, m_numericalDamping);
// compute base of null space of constraint Jacobian
Eigen::Index r = (m_Jc_svd.singularValues().array()>1e-8).count();
Eigen::VectorXd::Index r = (m_Jc_svd.singularValues().array()>1e-8).count();
m_Z = m_Jc_svd.matrixV().rightCols(m_nj+6-r);
// compute constrained accelerations ddq_c = (Z^T*M*Z)^{-1}*Z^T*(S^T*tau - h - M*ddqBar)
......
src/free-flyer-locator.cpp
View file @ 5bda36bd
......@@ -147,8 +147,8 @@ namespace dynamicgraph
const Eigen::VectorXd& q= m_base6d_encodersSIN(iter); //n+6
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* convert sot to pinocchio joint order */
m_robot_util->joints_sot_to_urdf(q.tail(m_robot_util->m_nbJoints), m_q_pin.tail(m_robot_util->m_nbJoints));
......@@ -168,7 +168,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal base6dFromFoot_encoders before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -176,7 +176,7 @@ namespace dynamicgraph
getProfiler().start(PROFILE_FREE_FLYER_COMPUTATION);
{
const Eigen::VectorXd& q= m_base6d_encodersSIN(iter); //n+6
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
/* Compute kinematic and return q with freeflyer */
const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
......@@ -232,7 +232,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
s.resize(m_robot_util->m_nbJoints+6);
m_kinematics_computationsSINNER(iter);
......@@ -240,7 +240,7 @@ namespace dynamicgraph
getProfiler().start(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
{
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
/* Compute foot velocities */
const Frame & f_lf = m_model->frames[m_left_foot_id];
......
src/inverse-dynamics-balance-controller.cpp
View file @ 5bda36bd
......@@ -542,10 +542,10 @@ namespace dynamicgraph
const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0);
const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0);
assert(kp_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(kd_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(rotor_inertias_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(gear_ratios_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(kp_posture.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
assert(kd_posture.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
assert(rotor_inertias_sot.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
assert(gear_ratios_sot.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
m_w_hands = m_w_handsSIN(0);
m_w_com = m_w_comSIN(0);
......@@ -666,7 +666,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()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
......@@ -715,7 +715,7 @@ namespace dynamicgraph
SEND_WARNING_STREAM_MSG("Cannot compute signal tau_des before initialization!");
return s;
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
getProfiler().start(PROFILE_TAU_DES_COMPUTATION);
......@@ -770,18 +770,18 @@ namespace dynamicgraph
m_w_feetSIN(iter);
m_active_joints_checkedSINNER(iter);
const VectorN6& q_sot = m_qSIN(iter);
assert(q_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6));
assert(q_sot.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6));
const VectorN6& v_sot = m_vSIN(iter);
assert(v_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6));
assert(v_sot.size()==static_cast<Eigen::VectorXd::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()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(q_ref.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
const VectorN& dq_ref = m_posture_ref_velSIN(iter);
assert(dq_ref.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(dq_ref.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
assert(ddq_ref.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(ddq_ref.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
const Vector6& kp_contact = m_kp_constraintsSIN(iter);
const Vector6& kd_contact = m_kd_constraintsSIN(iter);
......@@ -789,13 +789,13 @@ namespace dynamicgraph
const Vector3& kd_com = m_kd_comSIN(iter);
const VectorN& kp_posture = m_kp_postureSIN(iter);
assert(kp_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(kp_posture.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
const VectorN& kd_posture = m_kd_postureSIN(iter);
assert(kd_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(kd_posture.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
const VectorN& kp_pos = m_kp_posSIN(iter);
assert(kp_pos.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(kp_pos.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
const VectorN& kd_pos = m_kd_posSIN(iter);
assert(kd_pos.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
assert(kd_pos.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
/*const double & w_hands = m_w_handsSIN(iter);*/
const double & w_com = m_w_comSIN(iter);
......
src/joint-trajectory-generator.cpp
View file @ 5bda36bd
......@@ -258,7 +258,7 @@ namespace dynamicgraph
if(m_firstIter)
{
const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN(iter);
if(base6d_encoders.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
if(base6d_encoders.size()!=static_cast<Eigen::VectorXd::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()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
......@@ -321,7 +321,7 @@ namespace dynamicgraph
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
{
......@@ -344,7 +344,7 @@ namespace dynamicgraph
}
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
if(m_status[0]==JTG_TEXT_FILE)
......
src/position-controller.cpp
View file @ 5bda36bd
......@@ -151,16 +151,16 @@ namespace dynamicgraph
const VectorN& qRef = m_qRefSIN(iter); // n
const VectorN& dqRef = m_dqRefSIN(iter); // n
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dq");
assert(qRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
assert(dqRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dqRef");
assert(Kp.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
assert(Kd.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dq");
assert(qRef.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
assert(dqRef.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dqRef");
assert(Kp.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
assert(Kd.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
m_pwmDes = Kp.cwiseProduct(qRef-q.tail(m_robot_util->m_nbJoints)) + Kd.cwiseProduct(dqRef-dq);
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
s = m_pwmDes;
}
......@@ -179,10 +179,10 @@ namespace dynamicgraph
const VectorN6& q = m_base6d_encodersSIN(iter); //n+6
const VectorN& qRef = m_qRefSIN(iter); // n
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(qRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
assert(qRef.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
s.resize(m_robot_util->m_nbJoints);
s = qRef - q.tail(m_robot_util->m_nbJoints);
......
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