Skip to content

Commit

Permalink
Fix Eigen::Index -> Eigen::VectorXd::Index
Browse files Browse the repository at this point in the history
  • Loading branch information
@olivier-stasse
olivier-stasse committed Apr 16, 2019
1 parent e7abd1c commit 5bda36b
Show file tree
Hide file tree
Showing 8 changed files with 51 additions and 51 deletions.
18 changes: 9 additions & 9 deletions src/base-estimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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
Expand Down
10 changes: 5 additions & 5 deletions src/control-manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -259,15 +259,15 @@ 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);
else
{
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)+
Expand Down Expand Up @@ -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]);
}
Expand All @@ -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,
Expand Down Expand Up @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion src/current-controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++)
Expand Down
2 changes: 1 addition & 1 deletion src/device-torque-ctrl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
12 changes: 6 additions & 6 deletions src/free-flyer-locator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
Expand All @@ -168,15 +168,15 @@ 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);

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());
Expand Down Expand Up @@ -232,15 +232,15 @@ 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);

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];
Expand Down
30 changes: 15 additions & 15 deletions src/inverse-dynamics-balance-controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -770,32 +770,32 @@ 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);
const Vector3& kp_com = m_kp_comSIN(iter);
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);
Expand Down
8 changes: 4 additions & 4 deletions src/joint-trajectory-generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()) + " " +
Expand All @@ -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)
Expand Down Expand Up @@ -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)
{
Expand All @@ -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)
Expand Down
20 changes: 10 additions & 10 deletions src/position-controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -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);

Expand Down

0 comments on commit 5bda36b

Please sign in to comment.