Skip to content
This repository was archived by the owner on Nov 28, 2025. It is now read-only.

Commit 5bda36b

Browse files
olivier-stasseolivier-stasse
committed
Fix Eigen::Index -> Eigen::VectorXd::Index
1 parent e7abd1c commit 5bda36b

8 files changed

+51
-51
lines changed

‎src/base-estimator.cpp‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -574,8 +574,8 @@ namespace dynamicgraph
574574

575575
const Eigen::VectorXd& qj= m_joint_positionsSIN(iter); //n+6
576576
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
577-
assert(qj.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
578-
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
577+
assert(qj.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
578+
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
579579

580580
/* convert sot to pinocchio joint order */
581581
m_robot_util->joints_sot_to_urdf(qj, m_q_pin.tail(m_robot_util->m_nbJoints));
@@ -602,7 +602,7 @@ namespace dynamicgraph
602602
SEND_WARNING_STREAM_MSG("Cannot compute signal q before initialization!");
603603
return s;
604604
}
605-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
605+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
606606
s.resize(m_robot_util->m_nbJoints+6);
607607

608608
const Eigen::VectorXd & qj = m_joint_positionsSIN(iter); //n+6
@@ -631,7 +631,7 @@ namespace dynamicgraph
631631
wR = 0.0;
632632
}
633633

634-
assert(qj.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
634+
assert(qj.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_positions");
635635

636636
// if both weights are zero set them to a small positive value to avoid division by zero
637637
if(wR==0.0 && wL==0.0)
@@ -820,7 +820,7 @@ namespace dynamicgraph
820820
SEND_WARNING_STREAM_MSG("Cannot compute signal q_lf before initialization!");
821821
return s;
822822
}
823-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
823+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
824824
s.resize(m_robot_util->m_nbJoints+6);
825825

826826
const Eigen::VectorXd & q = m_qSOUT(iter);
@@ -837,7 +837,7 @@ namespace dynamicgraph
837837
SEND_WARNING_STREAM_MSG("Cannot compute signal q_rf before initialization!");
838838
return s;
839839
}
840-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
840+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
841841
s.resize(m_robot_util->m_nbJoints+6);
842842

843843
const Eigen::VectorXd & q = m_qSOUT(iter);
@@ -854,7 +854,7 @@ namespace dynamicgraph
854854
SEND_WARNING_STREAM_MSG("Cannot compute signal q_imu before initialization!");
855855
return s;
856856
}
857-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
857+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
858858
s.resize(m_robot_util->m_nbJoints+6);
859859

860860
const Eigen::VectorXd & q = m_qSOUT(iter);
@@ -968,7 +968,7 @@ namespace dynamicgraph
968968
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
969969
return s;
970970
}
971-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
971+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
972972
s.resize(m_robot_util->m_nbJoints+6);
973973

974974
m_kinematics_computationsSINNER(iter);
@@ -981,7 +981,7 @@ namespace dynamicgraph
981981
const Eigen::Vector3d& gyr_imu = m_gyroscopeSIN(iter);
982982
const Vector6 & dftrf = m_dforceRLEGSIN(iter);
983983
const Vector6 & dftlf = m_dforceLLEGSIN(iter);
984-
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
984+
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
985985

986986
// if the weights are not specified by the user through the input signals w_lf, w_rf
987987
// then compute them

‎src/control-manager.cpp‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -259,15 +259,15 @@ namespace dynamicgraph
259259
}
260260

261261
const dynamicgraph::Vector& ctrl = (*m_ctrlInputsSIN[cm_id])(iter);
262-
assert(ctrl.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
262+
assert(ctrl.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
263263

264264
if(m_jointCtrlModesCountDown[i]==0)
265265
s(i) = ctrl(i);
266266
else
267267
{
268268
cm_id_prev = m_jointCtrlModes_previous[i].id;
269269
const dynamicgraph::Vector& ctrl_prev = (*m_ctrlInputsSIN[cm_id_prev])(iter);
270-
assert(ctrl_prev.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
270+
assert(ctrl_prev.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
271271

272272
double alpha = m_jointCtrlModesCountDown[i]/CTRL_MODE_TRANSITION_TIME_STEP;
273273
// SEND_MSG("Joint "+toString(i)+" changing ctrl mode from "+toString(cm_id_prev)+
@@ -502,7 +502,7 @@ namespace dynamicgraph
502502
getClassName()+"("+getName()+")::input(bool)::emergencyStop_"+name));
503503

504504
// register the new signals and add the new signal dependecy
505-
Eigen::Index i = m_emergencyStopSIN.size()-1;
505+
Eigen::VectorXd::Index i = m_emergencyStopSIN.size()-1;
506506
m_u_safeSOUT.addDependency(*m_emergencyStopSIN[i]);
507507
Entity::signalRegistration(*m_emergencyStopSIN[i]);
508508
}
@@ -515,7 +515,7 @@ namespace dynamicgraph
515515
SEND_WARNING_STREAM_MSG("Cannot set joint name from joint id before initialization!");
516516
return;
517517
}
518-
m_robot_util->set_name_to_id(jointName,static_cast<Eigen::Index>(jointId));
518+
m_robot_util->set_name_to_id(jointName,static_cast<Eigen::VectorXd::Index>(jointId));
519519
}
520520

521521
void ControlManager::setJointLimitsFromId( const double &jointId,
@@ -553,7 +553,7 @@ namespace dynamicgraph
553553
return;
554554
}
555555

556-
m_robot_util->m_force_util.set_name_to_force_id(forceName,static_cast<Eigen::Index>(forceId));
556+
m_robot_util->m_force_util.set_name_to_force_id(forceName,static_cast<Eigen::VectorXd::Index>(forceId));
557557
}
558558

559559
void ControlManager::setJoints(const dg::Vector & urdf_to_sot)

‎src/current-controller.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -226,7 +226,7 @@ namespace dynamicgraph
226226
const dynamicgraph::Vector& i_real = m_i_realSOUT(iter);
227227
const dynamicgraph::Vector& in_out_gain = m_in_out_gainSIN(iter);
228228

229-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
229+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
230230
s.resize(m_robot_util->m_nbJoints);
231231

232232
for(unsigned int i=0; i<m_robot_util->m_nbJoints; i++)

‎src/device-torque-ctrl.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -281,7 +281,7 @@ void DeviceTorqueCtrl::computeForwardDynamics()
281281
m_dvBar, m_numericalDamping);
282282

283283
// compute base of null space of constraint Jacobian
284-
Eigen::Index r = (m_Jc_svd.singularValues().array()>1e-8).count();
284+
Eigen::VectorXd::Index r = (m_Jc_svd.singularValues().array()>1e-8).count();
285285
m_Z = m_Jc_svd.matrixV().rightCols(m_nj+6-r);
286286

287287
// compute constrained accelerations ddq_c = (Z^T*M*Z)^{-1}*Z^T*(S^T*tau - h - M*ddqBar)

‎src/free-flyer-locator.cpp‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -147,8 +147,8 @@ namespace dynamicgraph
147147

148148
const Eigen::VectorXd& q= m_base6d_encodersSIN(iter); //n+6
149149
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
150-
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
151-
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
150+
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
151+
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
152152

153153
/* convert sot to pinocchio joint order */
154154
m_robot_util->joints_sot_to_urdf(q.tail(m_robot_util->m_nbJoints), m_q_pin.tail(m_robot_util->m_nbJoints));
@@ -168,15 +168,15 @@ namespace dynamicgraph
168168
SEND_WARNING_STREAM_MSG("Cannot compute signal base6dFromFoot_encoders before initialization!");
169169
return s;
170170
}
171-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
171+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
172172
s.resize(m_robot_util->m_nbJoints+6);
173173

174174
m_kinematics_computationsSINNER(iter);
175175

176176
getProfiler().start(PROFILE_FREE_FLYER_COMPUTATION);
177177
{
178178
const Eigen::VectorXd& q= m_base6d_encodersSIN(iter); //n+6
179-
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
179+
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
180180

181181
/* Compute kinematic and return q with freeflyer */
182182
const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
@@ -232,15 +232,15 @@ namespace dynamicgraph
232232
SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
233233
return s;
234234
}
235-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
235+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
236236
s.resize(m_robot_util->m_nbJoints+6);
237237

238238
m_kinematics_computationsSINNER(iter);
239239

240240
getProfiler().start(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
241241
{
242242
const Eigen::VectorXd& dq= m_joint_velocitiesSIN(iter);
243-
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
243+
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal joint_velocities");
244244

245245
/* Compute foot velocities */
246246
const Frame & f_lf = m_model->frames[m_left_foot_id];

‎src/inverse-dynamics-balance-controller.cpp‎

Lines changed: 15 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -542,10 +542,10 @@ namespace dynamicgraph
542542
const VectorN& rotor_inertias_sot = m_rotor_inertiasSIN(0);
543543
const VectorN& gear_ratios_sot = m_gear_ratiosSIN(0);
544544

545-
assert(kp_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
546-
assert(kd_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
547-
assert(rotor_inertias_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
548-
assert(gear_ratios_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
545+
assert(kp_posture.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
546+
assert(kd_posture.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
547+
assert(rotor_inertias_sot.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
548+
assert(gear_ratios_sot.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
549549

550550
m_w_hands = m_w_handsSIN(0);
551551
m_w_com = m_w_comSIN(0);
@@ -666,7 +666,7 @@ namespace dynamicgraph
666666
/** Copy active_joints only if a valid transition occurs. (From all OFF) or (To all OFF)**/
667667
DEFINE_SIGNAL_INNER_FUNCTION(active_joints_checked, dynamicgraph::Vector)
668668
{
669-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
669+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
670670
s.resize(m_robot_util->m_nbJoints);
671671

672672
const Eigen::VectorXd& active_joints_sot = m_active_jointsSIN(iter);
@@ -715,7 +715,7 @@ namespace dynamicgraph
715715
SEND_WARNING_STREAM_MSG("Cannot compute signal tau_des before initialization!");
716716
return s;
717717
}
718-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
718+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
719719
s.resize(m_robot_util->m_nbJoints);
720720

721721
getProfiler().start(PROFILE_TAU_DES_COMPUTATION);
@@ -770,32 +770,32 @@ namespace dynamicgraph
770770
m_w_feetSIN(iter);
771771
m_active_joints_checkedSINNER(iter);
772772
const VectorN6& q_sot = m_qSIN(iter);
773-
assert(q_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6));
773+
assert(q_sot.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6));
774774
const VectorN6& v_sot = m_vSIN(iter);
775-
assert(v_sot.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6));
775+
assert(v_sot.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6));
776776
const Vector3& x_com_ref = m_com_ref_posSIN(iter);
777777
const Vector3& dx_com_ref = m_com_ref_velSIN(iter);
778778
const Vector3& ddx_com_ref = m_com_ref_accSIN(iter);
779779
const VectorN& q_ref = m_posture_ref_posSIN(iter);
780-
assert(q_ref.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
780+
assert(q_ref.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
781781
const VectorN& dq_ref = m_posture_ref_velSIN(iter);
782-
assert(dq_ref.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
782+
assert(dq_ref.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
783783
const VectorN& ddq_ref = m_posture_ref_accSIN(iter);
784-
assert(ddq_ref.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
784+
assert(ddq_ref.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
785785

786786
const Vector6& kp_contact = m_kp_constraintsSIN(iter);
787787
const Vector6& kd_contact = m_kd_constraintsSIN(iter);
788788
const Vector3& kp_com = m_kp_comSIN(iter);
789789
const Vector3& kd_com = m_kd_comSIN(iter);
790790

791791
const VectorN& kp_posture = m_kp_postureSIN(iter);
792-
assert(kp_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
792+
assert(kp_posture.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
793793
const VectorN& kd_posture = m_kd_postureSIN(iter);
794-
assert(kd_posture.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
794+
assert(kd_posture.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
795795
const VectorN& kp_pos = m_kp_posSIN(iter);
796-
assert(kp_pos.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
796+
assert(kp_pos.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
797797
const VectorN& kd_pos = m_kd_posSIN(iter);
798-
assert(kd_pos.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints));
798+
assert(kd_pos.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints));
799799

800800
/*const double & w_hands = m_w_handsSIN(iter);*/
801801
const double & w_com = m_w_comSIN(iter);

‎src/joint-trajectory-generator.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -258,7 +258,7 @@ namespace dynamicgraph
258258
if(m_firstIter)
259259
{
260260
const dynamicgraph::Vector& base6d_encoders = m_base6d_encodersSIN(iter);
261-
if(base6d_encoders.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6))
261+
if(base6d_encoders.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6))
262262
{
263263
SEND_ERROR_STREAM_MSG("Unexpected size of signal base6d_encoder " +
264264
toString(base6d_encoders.size()) + " " +
@@ -271,7 +271,7 @@ namespace dynamicgraph
271271
m_firstIter = false;
272272
}
273273

274-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
274+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
275275
s.resize(m_robot_util->m_nbJoints);
276276

277277
if(m_status[0]==JTG_TEXT_FILE)
@@ -321,7 +321,7 @@ namespace dynamicgraph
321321
}
322322

323323

324-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
324+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
325325
s.resize(m_robot_util->m_nbJoints);
326326
if(m_status[0]==JTG_TEXT_FILE)
327327
{
@@ -344,7 +344,7 @@ namespace dynamicgraph
344344
}
345345

346346

347-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
347+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
348348
s.resize(m_robot_util->m_nbJoints);
349349

350350
if(m_status[0]==JTG_TEXT_FILE)

‎src/position-controller.cpp‎

Lines changed: 10 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -151,16 +151,16 @@ namespace dynamicgraph
151151
const VectorN& qRef = m_qRefSIN(iter); // n
152152
const VectorN& dqRef = m_dqRefSIN(iter); // n
153153

154-
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
155-
assert(dq.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dq");
156-
assert(qRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
157-
assert(dqRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dqRef");
158-
assert(Kp.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
159-
assert(Kd.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
154+
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
155+
assert(dq.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dq");
156+
assert(qRef.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
157+
assert(dqRef.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal dqRef");
158+
assert(Kp.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
159+
assert(Kd.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal Kd");
160160

161161
m_pwmDes = Kp.cwiseProduct(qRef-q.tail(m_robot_util->m_nbJoints)) + Kd.cwiseProduct(dqRef-dq);
162162

163-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
163+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
164164
s.resize(m_robot_util->m_nbJoints);
165165
s = m_pwmDes;
166166
}
@@ -179,10 +179,10 @@ namespace dynamicgraph
179179

180180
const VectorN6& q = m_base6d_encodersSIN(iter); //n+6
181181
const VectorN& qRef = m_qRefSIN(iter); // n
182-
assert(q.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
183-
assert(qRef.size()==static_cast<Eigen::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
182+
assert(q.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints+6) && "Unexpected size of signal base6d_encoder");
183+
assert(qRef.size()==static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) && "Unexpected size of signal qRef");
184184

185-
if(s.size()!=static_cast<Eigen::Index>(m_robot_util->m_nbJoints))
185+
if(s.size()!=static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints))
186186
s.resize(m_robot_util->m_nbJoints);
187187
s = qRef - q.tail(m_robot_util->m_nbJoints);
188188

0 commit comments

Comments
 (0)