Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
sot-torque-control
Commits
5bda36bd
Commit
5bda36bd
authored
Apr 16, 2019
by
Olivier Stasse
Browse files
Fix Eigen::Index -> Eigen::VectorXd::Index
parent
e7abd1c0
Pipeline
#4526
failed with stage
in 11 minutes and 46 seconds
Changes
8
Pipelines
1
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
);
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment