Skip to content

Commit da6de53

Browse files
hyujunhyujun
committed
perf: Implement performance optimizations by pre-allocating memory, reducing temporary object creation, and enabling aggressive compiler flags.
1 parent bdaa1da commit da6de53

File tree

6 files changed

+232
-214
lines changed

6 files changed

+232
-214
lines changed

‎include/kdl/LieDynamics.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -157,6 +157,7 @@ namespace HYUMotionDynamics{
157157
VectorXd VdotBase;
158158

159159
MatrixXd mM_Tmp;
160+
Adjoint adj_tmp;
160161
MatrixXd mG;
161162

162163
MatrixXi ChainMatrix;

‎include/kdl/PoEKinematics.h‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ namespace HYUMotionKinematics {
2323
* @version 1.2.0
2424
*/
2525
class PoEKinematics : public HYUMotionBase::LieOperator {
26+
// - [/] Phase 1: PoEKinematics Dynamic Memory Allocation & Inverse Optimizationr
2627
public:
2728
/**
2829
* @brief PoEKinematics class constructor
@@ -225,6 +226,7 @@ namespace HYUMotionKinematics {
225226

226227
SE3 SE3_Tmp;
227228
se3 se3_Tmp;
229+
Adjoint adj_tmp;
228230

229231
VectorXi Arr[2];
230232

@@ -246,6 +248,18 @@ namespace HYUMotionKinematics {
246248
MatrixXd Mat_Tmp;
247249
VectorXd Vec_Tmp;
248250

251+
// Pre-allocated variables for WeightpInvJacobian to avoid dynamic allocation
252+
MatrixXd mJ_right1, mJ_left1;
253+
MatrixXd mP1, mP2;
254+
MatrixXd mJ_left, mJ_right;
255+
MatrixXd mJ1_right, mJ2_right, mW_right, mW_inv_right, mY_right, mY_inv_right;
256+
MatrixXd mZ11_right, mZ12_right, mZ21_right, mZ22_right, mJ_WpInv_right;
257+
MatrixXd mJ1_left, mJ2_left, mW_left, mW_inv_left, mY_left, mY_inv_left;
258+
MatrixXd mZ11_left, mZ12_left, mZ21_left, mZ22_left, mJ_WpInv_left;
259+
260+
// Pre-allocated JacobiSVD for pseudo-inverse
261+
Eigen::JacobiSVD<MatrixXd> mpInvSVD;
262+
249263
Quaterniond q;
250264

251265
double WpInv_epsilon_left;

‎src/kdl/CMakeLists.txt‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,3 +21,11 @@ target_link_libraries(${PROJECT_NAME} PUBLIC OpenMP::OpenMP_CXX)
2121
# Eigen3
2222
find_package(Eigen3 3.0 REQUIRED NO_MODULE)
2323
target_link_libraries(${PROJECT_NAME} PUBLIC Eigen3::Eigen)
24+
25+
# Build Optimizations for Real-Time Performance
26+
target_compile_options(${PROJECT_NAME} PRIVATE
27+
-O3
28+
-march=native
29+
-funroll-loops
30+
# -ffast-math # Uncomment after verifying it doesn't break Lie group precision (e.g., LogSO3)
31+
)

‎src/kdl/LieDynamics.cpp‎

Lines changed: 12 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,8 @@ namespace HYUMotionDynamics{
102102
this->Gamma_mat.setZero();
103103
for (int i = 1; i < this->m_DoF; ++i)
104104
{
105-
this->Gamma_mat.block<6, 6>(6 * i, 6 * (i - 1)).noalias() += LieOperator::AdjointMatrix(GetTMat(i+1, i));
105+
LieOperator::AdjointMatrix(GetTMat(i+1, i), adj_tmp);
106+
this->Gamma_mat.block<6, 6>(6 * i, 6 * (i - 1)).noalias() += adj_tmp;
106107
}
107108
return;
108109
}
@@ -163,7 +164,8 @@ namespace HYUMotionDynamics{
163164
{
164165
if(!(k>0 && ChainMatrix(k-1,j) == 1))
165166
{
166-
L_mat.block(6*j, 6*i, 6, 6).noalias() += LieOperator::AdjointMatrix(GetTMat(j+1,i+1));
167+
LieOperator::AdjointMatrix(GetTMat(j+1,i+1), adj_tmp);
168+
L_mat.block(6*j, 6*i, 6, 6).noalias() += adj_tmp;
167169
}
168170
}
169171
}
@@ -249,23 +251,16 @@ namespace HYUMotionDynamics{
249251

250252
void Liedynamics::M_Matrix( MatrixXd &_M )
251253
{
252-
_M.setZero();
253-
254-
Matrix<double,1,1> tmp;
255-
for(int i=0; i<m_DoF; i++)
254+
_M.setZero(m_DoF, m_DoF);
255+
mM_Tmp.setZero(6*m_DoF, m_DoF); // Re-use member variable
256+
257+
for(int k=0; k<m_DoF; k++)
256258
{
257-
for(int j=i; j<m_DoF; j++)
258-
{
259-
tmp.setZero();
260-
for(int k=i; k<m_DoF; k++)
261-
{
262-
tmp.noalias() += LA_mat.block(6*k,i,6,1).transpose()*GIner[k]*LA_mat.block(6*k,j,6,1);
263-
}
264-
_M(i,j) = tmp(0,0);
265-
if(i != j)
266-
_M(j,i) = _M(i,j);
267-
}
259+
mM_Tmp.block(6*k, 0, 6, m_DoF).noalias() = GIner[k] * LA_mat.block(6*k, 0, 6, m_DoF);
268260
}
261+
262+
_M.triangularView<Eigen::Upper>() = LA_mat.transpose() * mM_Tmp;
263+
_M.triangularView<Eigen::StrictlyLower>() = _M.transpose();
269264
}
270265

271266
void Liedynamics::C_Matrix( MatrixXd &_C )

‎src/kdl/LieOperator.cpp‎

Lines changed: 40 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -79,7 +79,8 @@ namespace HYUMotionBase {
7979
AdjointRes.setZero();
8080
AdjointRes.block<3,3>(0,0) = _SE3.block(0, 0, 3, 3);
8181
AdjointRes.block<3,3>(3,3) = _SE3.block(0, 0, 3, 3);
82-
AdjointRes.block<3,3>(3,0).noalias() += SkewMatrix(_SE3.block(0, 3, 3, 1))*_SE3.block(0, 0, 3, 3);
82+
SkewMatrix(_SE3.block(0, 3, 3, 1), Mat3dRes);
83+
AdjointRes.block<3,3>(3,0).noalias() += Mat3dRes*_SE3.block(0, 0, 3, 3);
8384
return AdjointRes;
8485
}
8586

@@ -88,7 +89,8 @@ namespace HYUMotionBase {
8889
_TargetAdjoint.setZero();
8990
_TargetAdjoint.block<3, 3>(0, 0) = _SE3.block(0, 0, 3, 3);
9091
_TargetAdjoint.block<3, 3>(3, 3) = _SE3.block(0, 0, 3, 3);
91-
_TargetAdjoint.block<3, 3>(3, 0).noalias() += SkewMatrix(_SE3.block(0, 3, 3, 1))*_SE3.block(0, 0, 3, 3);
92+
SkewMatrix(_SE3.block(0, 3, 3, 1), Mat3dRes);
93+
_TargetAdjoint.block<3, 3>(3, 0).noalias() += Mat3dRes*_SE3.block(0, 0, 3, 3);
9294

9395
return;
9496
}
@@ -114,20 +116,22 @@ namespace HYUMotionBase {
114116
adjoint LieOperator::adjointMatrix( const se3 &_se3 )
115117
{
116118
adjointRes.setZero();
117-
118-
adjointRes.block(0, 0, 3, 3) = SkewMatrix(_se3.head(3));
119-
adjointRes.block(3, 0, 3, 3) = SkewMatrix(_se3.tail(3));
120-
adjointRes.block(3, 3, 3, 3) = SkewMatrix(_se3.head(3));
119+
SkewMatrix(_se3.head(3), Mat3dRes);
120+
adjointRes.block(0, 0, 3, 3) = Mat3dRes;
121+
adjointRes.block(3, 3, 3, 3) = Mat3dRes;
122+
SkewMatrix(_se3.tail(3), Mat3dRes);
123+
adjointRes.block(3, 0, 3, 3) = Mat3dRes;
121124
return adjointRes;
122125
}
123126

124127
void LieOperator::adjointMatrix( const se3 &_se3, adjoint &_Targetadjoint )
125128
{
126129
_Targetadjoint.setZero();
127-
128-
_Targetadjoint.block(0, 0, 3, 3) = SkewMatrix(_se3.head(3));
129-
_Targetadjoint.block(3, 0, 3, 3) = SkewMatrix(_se3.tail(3));
130-
_Targetadjoint.block(3, 3, 3, 3) = SkewMatrix(_se3.head(3));
130+
SkewMatrix(_se3.head(3), Mat3dRes);
131+
_Targetadjoint.block(0, 0, 3, 3) = Mat3dRes;
132+
_Targetadjoint.block(3, 3, 3, 3) = Mat3dRes;
133+
SkewMatrix(_se3.tail(3), Mat3dRes);
134+
_Targetadjoint.block(3, 0, 3, 3) = Mat3dRes;
131135
return;
132136
}
133137

@@ -224,36 +228,46 @@ namespace HYUMotionBase {
224228

225229
SE3 LieOperator::SE3Matrix( const se3 &_Twist, const double &_q )
226230
{
227-
Matrix3d i = Matrix3d::Identity();
228231
Mat4dRes.setZero();
229-
Mat4dRes.block(0, 0, 3, 3) = i;
230-
Mat4dRes.block(0, 0, 3, 3).noalias() += sin(_q)*SkewMatrix(_Twist.head(3));
232+
Mat4dRes.block(0, 0, 3, 3).setIdentity();
233+
SkewMatrix(_Twist.head(3), Mat3dRes);
234+
Mat4dRes.block(0, 0, 3, 3).noalias() += sin(_q)*Mat3dRes;
231235
double cos_tmp = 1.0 - cos(_q);
232236
double sin_tmp = _q - sin(_q);
233-
Mat4dRes.block(0, 0, 3, 3).noalias() += cos_tmp*SkewMatrixSquare(_Twist.head(3));
237+
234238
Matrix3d Mat3_tmp = Eigen::Matrix3d::Zero();
235-
Mat3_tmp.noalias() += i*_q;
236-
Mat3_tmp.noalias() += cos_tmp*SkewMatrix(_Twist.head(3));
237-
Mat3_tmp.noalias() += sin_tmp*SkewMatrixSquare(_Twist.head(3));
238-
Mat4dRes.block(0, 3, 3, 1).noalias() += Mat3_tmp*_Twist.tail(3);
239+
SkewMatrixSquare(_Twist.head(3), Mat3_tmp);
240+
Mat4dRes.block(0, 0, 3, 3).noalias() += cos_tmp*Mat3_tmp;
241+
242+
Matrix3d Mat3_tmp2 = Eigen::Matrix3d::Identity();
243+
Mat3_tmp2 *= _q;
244+
Mat3_tmp2.noalias() += cos_tmp*Mat3dRes;
245+
Mat3_tmp2.noalias() += sin_tmp*Mat3_tmp;
246+
247+
Mat4dRes.block(0, 3, 3, 1).noalias() += Mat3_tmp2*_Twist.tail(3);
239248
Mat4dRes.block(3, 0, 1, 4) << 0, 0, 0, 1;
240249
return Mat4dRes;
241250
}
242251

243252
void LieOperator::SE3Matrix( const se3 &_Twist, const double &_q, SE3 &_TargetSE3 )
244253
{
245-
Matrix3d i = Matrix3d::Identity();
246254
_TargetSE3.setZero();
247-
_TargetSE3.block(0, 0, 3, 3) = i;
248-
_TargetSE3.block(0, 0, 3, 3).noalias() += sin(_q)*SkewMatrix(_Twist.head(3));
255+
_TargetSE3.block(0, 0, 3, 3).setIdentity();
256+
SkewMatrix(_Twist.head(3), Mat3dRes);
257+
_TargetSE3.block(0, 0, 3, 3).noalias() += sin(_q)*Mat3dRes;
249258
double cos_tmp = 1.0 - cos(_q);
250259
double sin_tmp = _q - sin(_q);
251-
_TargetSE3.block(0, 0, 3, 3).noalias() += cos_tmp*SkewMatrixSquare(_Twist.head(3));
260+
252261
Matrix3d Mat3_tmp = Eigen::Matrix3d::Zero();
253-
Mat3_tmp.noalias() += i*_q;
254-
Mat3_tmp.noalias() += cos_tmp*SkewMatrix(_Twist.head(3));
255-
Mat3_tmp.noalias() += sin_tmp*SkewMatrixSquare(_Twist.head(3));
256-
_TargetSE3.block(0, 3, 3, 1).noalias() += Mat3_tmp*_Twist.tail(3);
262+
SkewMatrixSquare(_Twist.head(3), Mat3_tmp);
263+
_TargetSE3.block(0, 0, 3, 3).noalias() += cos_tmp*Mat3_tmp;
264+
265+
Matrix3d Mat3_tmp2 = Eigen::Matrix3d::Identity();
266+
Mat3_tmp2 *= _q;
267+
Mat3_tmp2.noalias() += cos_tmp*Mat3dRes;
268+
Mat3_tmp2.noalias() += sin_tmp*Mat3_tmp;
269+
270+
_TargetSE3.block(0, 3, 3, 1).noalias() += Mat3_tmp2*_Twist.tail(3);
257271
_TargetSE3.block(3, 0, 1, 4) << 0, 0, 0, 1;
258272
}
259273

0 commit comments

Comments
 (0)