Improved performance of Body::setPose() by calling .linear() instead …

…of .rotation().
moveit · Mar 22, 2020 · 4f5a7be · 4f5a7be
1 parent bca6b88
commit 4f5a7be
Showing 1 changed file with 2 additions and 2 deletions.
diff --git a/src/bodies.cpp b/src/bodies.cpp
@@ -315,7 +315,7 @@ void bodies::Cylinder::updateInternalData()
   radiusBSqr_ = length2_ * length2_ + radius2_;
   radiusB_ = sqrt(radiusBSqr_);
 
-  Eigen::Matrix3d basis = pose_.rotation();
+  Eigen::Matrix3d basis = pose_.linear();
   normalB1_ = basis.col(0);
   normalB2_ = basis.col(1);
   normalH_ = basis.col(2);
@@ -546,7 +546,7 @@ void bodies::Box::updateInternalData()
   radius2_ = length2_ * length2_ + width2_ * width2_ + height2_ * height2_;
   radiusB_ = sqrt(radius2_);
 
-  Eigen::Matrix3d basis = pose_.rotation();
+  Eigen::Matrix3d basis = pose_.linear();
   normalL_ = basis.col(0);
   normalW_ = basis.col(1);
   normalH_ = basis.col(2);