Skip to content

Commit 69f0f85

Browse files
hyujunhyujun
committed
refactor: pre-allocate temporary matrices and vectors to optimize kinematics and dynamics computations
1 parent 5879f1b commit 69f0f85

File tree

4 files changed

+100
-32
lines changed

4 files changed

+100
-32
lines changed

‎include/kdl/LieDynamics.h‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,17 @@ namespace HYUMotionDynamics{
160160
Adjoint adj_tmp;
161161
MatrixXd mG;
162162

163+
MatrixXd mC_Temp1;
164+
MatrixXd mC_Temp2;
165+
MatrixXd mC_Temp3;
166+
MatrixXd mC_Temp4;
167+
MatrixXd mC_Temp5;
168+
VectorXd mC_VecTemp1;
169+
VectorXd mC_VecTemp2;
170+
VectorXd mC_VecTemp3;
171+
MatrixXd mTask_M;
172+
VectorXd mTask_G;
173+
163174
MatrixXi ChainMatrix;
164175

165176
MatrixXd Jacobian_mat;

‎include/kdl/PoEKinematics.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -225,6 +225,7 @@ namespace HYUMotionKinematics {
225225
int JointEndNum[2];
226226

227227
SE3 SE3_Tmp;
228+
SE3 SE3_Tmp2;
228229
se3 se3_Tmp;
229230
Adjoint adj_tmp;
230231

@@ -247,6 +248,8 @@ namespace HYUMotionKinematics {
247248

248249
MatrixXd Mat_Tmp;
249250
VectorXd Vec_Tmp;
251+
VectorXd msvInv;
252+
MatrixXd mIdentity_6N;
250253

251254
// Pre-allocated variables for WeightpInvJacobian to avoid dynamic allocation
252255
MatrixXd mJ_right1, mJ_left1;

‎src/kdl/LieDynamics.cpp‎

Lines changed: 60 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,17 @@ namespace HYUMotionDynamics{
2525
this->V.setZero(6*this->m_DoF);
2626
this->VdotBase.setZero(6*this->m_DoF);
2727

28+
this->mC_Temp1.setZero(6*this->m_DoF, 6*this->m_DoF);
29+
this->mC_Temp2.setZero(6*this->m_DoF, 6*this->m_DoF);
30+
this->mC_Temp3.setZero(6*this->m_DoF, 6*this->m_DoF);
31+
this->mC_Temp4.setZero(6*this->m_DoF, 6*this->m_DoF);
32+
this->mC_Temp5.setZero(6*this->m_DoF, this->m_DoF);
33+
this->mC_VecTemp1.setZero(6*this->m_DoF);
34+
this->mC_VecTemp2.setZero(6*this->m_DoF);
35+
this->mC_VecTemp3.setZero(6*this->m_DoF);
36+
this->mTask_M.setZero(this->m_DoF, this->m_DoF);
37+
this->mTask_G.setZero(this->m_DoF);
38+
2839
grav.setZero(6);
2940
grav << 0, 0, 0, 0, 0, 9.8;
3041
}
@@ -64,6 +75,17 @@ namespace HYUMotionDynamics{
6475
V.setZero(6*this->m_DoF);
6576
VdotBase.setZero(6*this->m_DoF);
6677

78+
mC_Temp1.setZero(6*this->m_DoF, 6*this->m_DoF);
79+
mC_Temp2.setZero(6*this->m_DoF, 6*this->m_DoF);
80+
mC_Temp3.setZero(6*this->m_DoF, 6*this->m_DoF);
81+
mC_Temp4.setZero(6*this->m_DoF, 6*this->m_DoF);
82+
mC_Temp5.setZero(6*this->m_DoF, this->m_DoF);
83+
mC_VecTemp1.setZero(6*this->m_DoF);
84+
mC_VecTemp2.setZero(6*this->m_DoF);
85+
mC_VecTemp3.setZero(6*this->m_DoF);
86+
mTask_M.setZero(this->m_DoF, this->m_DoF);
87+
mTask_G.setZero(this->m_DoF);
88+
6789
grav.setZero(6);
6890
grav << 0, 0, 0, 0, 0, 9.8;
6991
}
@@ -266,20 +288,37 @@ namespace HYUMotionDynamics{
266288
void Liedynamics::C_Matrix( MatrixXd &_C )
267289
{
268290
_C.setZero(this->m_DoF, this->m_DoF);
269-
_C = -LA_mat.transpose()*(Iner_mat*L_mat*ad_Aqd*Gamma_mat - ad_V.transpose()*Iner_mat)*LA_mat;
291+
mC_Temp1.noalias() = Iner_mat * L_mat;
292+
mC_Temp2.noalias() = mC_Temp1 * ad_Aqd;
293+
mC_Temp3.noalias() = mC_Temp2 * Gamma_mat;
294+
mC_Temp4.noalias() = ad_V.transpose() * Iner_mat;
295+
mC_Temp3.noalias() -= mC_Temp4;
296+
mC_Temp5.noalias() = mC_Temp3 * LA_mat;
297+
_C.noalias() = -LA_mat.transpose() * mC_Temp5;
270298
}
271299

272300
void Liedynamics::C_Vector( VectorXd &_C, const VectorXd &_qdot )
273301
{
274302
_C.setZero(this->m_DoF);
275-
_C.noalias() += (-LA_mat.transpose()*((Iner_mat*L_mat*ad_Aqd*Gamma_mat - ad_V.transpose()*Iner_mat)*(LA_mat*_qdot)));
303+
mC_VecTemp1.noalias() = LA_mat * _qdot;
304+
mC_VecTemp2.noalias() = Gamma_mat * mC_VecTemp1;
305+
mC_VecTemp3.noalias() = ad_Aqd * mC_VecTemp2;
306+
mC_VecTemp2.noalias() = L_mat * mC_VecTemp3;
307+
mC_VecTemp3.noalias() = Iner_mat * mC_VecTemp2;
308+
309+
mC_VecTemp2.noalias() = Iner_mat * mC_VecTemp1;
310+
mC_VecTemp1.noalias() = ad_V.transpose() * mC_VecTemp2;
311+
312+
mC_VecTemp2.noalias() = mC_VecTemp3 - mC_VecTemp1;
313+
_C.noalias() += -LA_mat.transpose() * mC_VecTemp2;
276314
}
277315

278316
void Liedynamics::G_Matrix( VectorXd &_G )
279317
{
280318
_G.setZero();
281-
282-
_G.noalias() += (LA_mat.transpose()*(Iner_mat*(L_mat*VdotBase)));
319+
mC_VecTemp1.noalias() = L_mat * VdotBase;
320+
mC_VecTemp2.noalias() = Iner_mat * mC_VecTemp1;
321+
_G.noalias() += LA_mat.transpose() * mC_VecTemp2;
283322
}
284323

285324
void Liedynamics::MG_Mat_Joint( MatrixXd &_M, VectorXd&_G )
@@ -290,33 +329,39 @@ namespace HYUMotionDynamics{
290329

291330
void Liedynamics::M_Mat_Task(MatrixXd &_Mx, MatrixXd &_pInv)
292331
{
293-
MatrixXd M;
294-
M_Matrix(M);
332+
M_Matrix(this->mTask_M);
295333
_Mx.setZero(6*this->m_NumChain, 6*this->m_NumChain);
296-
_Mx.noalias() += _pInv.transpose()*M*_pInv;
334+
_Mx.noalias() += _pInv.transpose() * this->mTask_M * _pInv;
297335
}
298336

299337
void Liedynamics::MG_Mat_Task(MatrixXd &_Mx, VectorXd &_Gx)
300338
{
301-
MatrixXd M;
302-
VectorXd G;
303-
M_Matrix(M);
304-
G_Matrix(G);
339+
M_Matrix(this->mTask_M);
340+
G_Matrix(this->mTask_G);
305341
GetAnalyticJacobian(this->Jacobian_mat);
306342
GetpinvJacobian(this->pinvJacobian_mat);
307343

308344
_Mx.setZero(6*this->m_NumChain, 6*this->m_NumChain);
309-
_Mx.noalias() += pinvJacobian_mat.transpose()*M*pinvJacobian_mat;
345+
_Mx.noalias() += pinvJacobian_mat.transpose() * this->mTask_M * pinvJacobian_mat;
310346

311347
_Gx.setZero(6*this->m_NumChain);
312-
_Gx.noalias() += pinvJacobian_mat.transpose()*G;
348+
_Gx.noalias() += pinvJacobian_mat.transpose() * this->mTask_G;
313349
}
314350

315351
void Liedynamics::Mdot_Matrix( MatrixXd &_Mdot )
316352
{
317353
_Mdot.setZero(m_DoF, m_DoF);
318-
_Mdot.noalias() += -LA_mat.transpose()*Gamma_mat.transpose()*ad_Aqd.transpose()*L_mat.transpose()*Iner_mat*LA_mat;
319-
_Mdot.noalias() += -LA_mat.transpose()*Iner_mat*L_mat*ad_Aqd*Gamma_mat*LA_mat;
354+
mC_Temp5.noalias() = Iner_mat * LA_mat;
355+
mC_Temp1.noalias() = L_mat.transpose() * mC_Temp5;
356+
mC_Temp2.noalias() = ad_Aqd.transpose() * mC_Temp1;
357+
mC_Temp3.noalias() = Gamma_mat.transpose() * mC_Temp2;
358+
_Mdot.noalias() += -LA_mat.transpose() * mC_Temp3;
359+
360+
mC_Temp1.noalias() = Gamma_mat * LA_mat;
361+
mC_Temp2.noalias() = ad_Aqd * mC_Temp1;
362+
mC_Temp3.noalias() = L_mat * mC_Temp2;
363+
mC_Temp4.noalias() = Iner_mat * mC_Temp3;
364+
_Mdot.noalias() += -LA_mat.transpose() * mC_Temp4;
320365
}
321366

322367
}

‎src/kdl/PoEKinematics.cpp‎

Lines changed: 26 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,11 @@
33
namespace HYUMotionKinematics {
44

55
PoEKinematics::PoEKinematics():m_NumChain(1),m_DoF(6),WpInv_epsilon_left(0.001), WpInv_epsilon_right(0.001)
6-
{
76
this->mBodyJacobian.setZero(6*this->m_NumChain, this->m_DoF);
87
this->mSpaceJacobian.setZero(6*this->m_NumChain, this->m_DoF);
98
this->mAnalyticJacobian.setZero(6*this->m_NumChain, this->m_DoF);
9+
this->mIdentity_6N.setIdentity(6*this->m_NumChain, 6*this->m_NumChain);
10+
this->msvInv.setZero(this->m_DoF);
1011
}
1112

1213
PoEKinematics::PoEKinematics( const MatrixXi &_ChainMat ):WpInv_epsilon_left(0.001), WpInv_epsilon_right(0.001)
@@ -57,6 +58,8 @@ namespace HYUMotionKinematics {
5758
this->mBodyJacobian.setZero(6*this->m_NumChain, this->m_DoF);
5859
this->mSpaceJacobian.setZero(6*this->m_NumChain, this->m_DoF);
5960
this->mAnalyticJacobian.setZero(6*this->m_NumChain, this->m_DoF);
61+
this->mIdentity_6N.setIdentity(6*this->m_NumChain, 6*this->m_NumChain);
62+
this->msvInv.setZero(this->m_DoF);
6063
}
6164

6265
PoEKinematics::~PoEKinematics()
@@ -149,7 +152,8 @@ namespace HYUMotionKinematics {
149152
}
150153
else
151154
{
152-
mSpaceJacobian.block(6*i, j, 6, 1).noalias() += AdjointMatrix(SE3_Tmp)*v_se3[j];
155+
AdjointMatrix(SE3_Tmp, adj_tmp);
156+
mSpaceJacobian.block(6*i, j, 6, 1).noalias() += adj_tmp*v_se3[j];
153157
}
154158
SE3_Tmp*=Exp_S[j];
155159
}
@@ -162,8 +166,10 @@ namespace HYUMotionKinematics {
162166
mBodyJacobian.setZero();
163167
for(int i=0; i < this->m_NumChain; i++)
164168
{
169+
inverse_SE3(T[0][JointEndNum[i]], SE3_Tmp2);
170+
AdjointMatrix(SE3_Tmp2, adj_tmp);
165171
mBodyJacobian.block(6*i,0,6,this->m_DoF).noalias() +=
166-
AdjointMatrix(inverse_SE3(T[0][JointEndNum[i]]))*mSpaceJacobian.block(6*i,0,6,this->m_DoF);
172+
adj_tmp*mSpaceJacobian.block(6*i,0,6,this->m_DoF);
167173
}
168174
}
169175

@@ -259,12 +265,12 @@ namespace HYUMotionKinematics {
259265
mpInvSVD.compute(mAnalyticJacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
260266
double tolerance = 1e-5;
261267
auto sv = mpInvSVD.singularValues();
262-
Eigen::VectorXd svInv = sv;
268+
msvInv = sv;
263269
for (int i = 0; i < sv.size(); ++i) {
264-
if (sv(i) > tolerance) svInv(i) = 1.0 / sv(i);
265-
else svInv(i) = 0.0;
270+
if (sv(i) > tolerance) msvInv(i) = 1.0 / sv(i);
271+
else msvInv(i) = 0.0;
266272
}
267-
mpInvJacobin.noalias() = mpInvSVD.matrixV() * svInv.asDiagonal() * mpInvSVD.matrixU().transpose();
273+
mpInvJacobin.noalias() = mpInvSVD.matrixV() * msvInv.asDiagonal() * mpInvSVD.matrixU().transpose();
268274
}
269275

270276
void PoEKinematics::GetpinvJacobian( MatrixXd &_pinvJacobian )
@@ -294,10 +300,10 @@ namespace HYUMotionKinematics {
294300
{
295301
Mat_Tmp.setZero(6*m_NumChain, 6*m_NumChain);
296302
Mat_Tmp.noalias() += mAnalyticJacobian*mAnalyticJacobian.transpose();
297-
Mat_Tmp.noalias() += sigma*Eigen::MatrixXd::Identity(6*m_NumChain, 6*m_NumChain);
303+
Mat_Tmp.noalias() += sigma*mIdentity_6N;
298304

299305
mDampedpInvJacobian.setZero( m_DoF,6*m_NumChain );
300-
mDampedpInvJacobian.noalias() += mAnalyticJacobian.transpose()*Mat_Tmp.ldlt().solve(Eigen::MatrixXd::Identity(6*m_NumChain, 6*m_NumChain));
306+
mDampedpInvJacobian.noalias() += mAnalyticJacobian.transpose()*Mat_Tmp.ldlt().solve(mIdentity_6N);
301307
}
302308

303309
void PoEKinematics::GetDampedpInvJacobian( MatrixXd &_DampedpInvJacobian )
@@ -311,10 +317,10 @@ namespace HYUMotionKinematics {
311317
size_t r = _TargetMatrix.rows();
312318
Mat_Tmp.setZero(r, r);
313319
Mat_Tmp.noalias() += _TargetMatrix*_TargetMatrix.transpose();
314-
Mat_Tmp.noalias() += sigma*Eigen::MatrixXd::Identity(r, r);
320+
Mat_Tmp.noalias() += sigma*mIdentity_6N.block(0,0,r,r);
315321

316322
mDampedpInvJacobian.setZero( m_DoF,6*m_NumChain );
317-
mDampedpInvJacobian.noalias() += _TargetMatrix.transpose()*Mat_Tmp.ldlt().solve(Eigen::MatrixXd::Identity(r, r));
323+
mDampedpInvJacobian.noalias() += _TargetMatrix.transpose()*Mat_Tmp.ldlt().solve(mIdentity_6N.block(0,0,r,r));
318324
}
319325

320326
void PoEKinematics::GetDampedpInvJacobian(MatrixXd &_TargetMat, MatrixXd &_DampedpInvJacobian)
@@ -325,12 +331,12 @@ namespace HYUMotionKinematics {
325331

326332
void PoEKinematics::BlockpInvJacobian( Matrix<double, 6, Dynamic> &_Jacobian1, Matrix<double, 6, Dynamic> &_Jacobian2 )
327333
{
328-
mP1 = MatrixXd::Identity(16, 16);
334+
mP1.setIdentity(16, 16);
329335
Mat_Tmp.noalias() = _Jacobian1*_Jacobian1.transpose();
330-
mP1.noalias() += -_Jacobian1.transpose()*Mat_Tmp.ldlt().solve(MatrixXd::Identity(6,6))*_Jacobian1;
331-
mP2 = MatrixXd::Identity(16, 16);
336+
mP1.noalias() += -_Jacobian1.transpose()*Mat_Tmp.ldlt().solve(mIdentity_6N.block(0,0,6,6))*_Jacobian1;
337+
mP2.setIdentity(16, 16);
332338
Mat_Tmp.noalias() = _Jacobian2*_Jacobian2.transpose();
333-
mP2.noalias() += -_Jacobian2.transpose()*Mat_Tmp.ldlt().solve(MatrixXd::Identity(6,6))*_Jacobian2;
339+
mP2.noalias() += -_Jacobian2.transpose()*Mat_Tmp.ldlt().solve(mIdentity_6N.block(0,0,6,6))*_Jacobian2;
334340

335341
mBlockpInvJacobian.setZero(m_DoF, 6*m_NumChain);
336342
mBlockpInvJacobian.block(0,0,16,6) = (_Jacobian1*mP2).completeOrthogonalDecomposition().pseudoInverse();
@@ -544,7 +550,9 @@ namespace HYUMotionKinematics {
544550
mRelativeJacobian.setZero(6, m_DoF);
545551
if( From == 0 && To == 1 )
546552
{
547-
RelJacTmp.block(0, 2, 6, 7).noalias() += -AdjointMatrix(inverse_SE3(T[0][JointEndNum[To]]))*mSpaceJacobian.block(0, 2, 6, 7);
553+
inverse_SE3(T[0][JointEndNum[To]], SE3_Tmp2);
554+
AdjointMatrix(SE3_Tmp2, adj_tmp);
555+
RelJacTmp.block(0, 2, 6, 7).noalias() += -adj_tmp*mSpaceJacobian.block(0, 2, 6, 7);
548556
RelJacTmp.block(0, 9, 6, 7) = mBodyJacobian.block(6, 9, 6, 7);
549557
Body2Analyic.block(3,3,3,3).noalias() += GetForwardKinematicsSO3(JointEndNum[From]).transpose()*GetForwardKinematicsSO3(JointEndNum[To]);
550558
Body2Analyic.block(0,0,3,3) = Body2Analyic.block(3,3,3,3);
@@ -564,7 +572,8 @@ namespace HYUMotionKinematics {
564572
mRelativeJacobianDot.setZero(6,m_DoF);
565573
adjoint adTmp;
566574
Adjoint AdTmp;
567-
AdTmp = AdjointMatrix(inverse_SE3(T[0][JointEndNum[1]]));
575+
inverse_SE3(T[0][JointEndNum[1]], SE3_Tmp2);
576+
AdjointMatrix(SE3_Tmp2, AdTmp);
568577
for(int i=3; i<=8; i++)
569578
{
570579
adTmp = adjointMatrix(mSpaceJacobian.col(i).segment(0,6));

0 commit comments

Comments
 (0)