Avoid extra copy when assigning std::array to KDL::JntArray

Author: rhaschke

franka_gazebo/src/model_kdl.cpp

@@ -122,7 +122,7 @@ std::array<double, 16> ModelKDL::pose(
 
   KDL::ChainFkSolverPos_recursive solver(chain);
 
-  kq.data = Eigen::Matrix<double, 7, 1>(q.data());
+  kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());
 
   int error = solver.JntToCart(kq, kp, segment(frame));
   if (error != KDL::SolverI::E_NOERROR) {
@@ -146,7 +146,7 @@ std::array<double, 42> ModelKDL::bodyJacobian(
     const {
   KDL::JntArray kq;
   KDL::Jacobian J(7);  // NOLINT(readability-identifier-naming)
-  kq.data = Eigen::Matrix<double, 7, 1>(q.data());
+  kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());
 
   // Augment the chain with the two virtual frames 'EE' and 'K'
   KDL::Chain chain = this->chain_;  // copy
@@ -189,7 +189,7 @@ std::array<double, 42> ModelKDL::zeroJacobian(
     const {
   KDL::JntArray kq;
   KDL::Jacobian J(7);  // NOLINT(readability-identifier-naming)
-  kq.data = Eigen::Matrix<double, 7, 1>(q.data());
+  kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());
 
   // Augment the chain with the two virtual frames 'EE' and 'K'
   KDL::Chain chain = this->chain_;  // copy
@@ -222,7 +222,7 @@ std::array<double, 49> ModelKDL::mass(
     const {
   KDL::JntArray kq;
   KDL::JntSpaceInertiaMatrix M(7);  // NOLINT(readability-identifier-naming)
-  kq.data = Eigen::Matrix<double, 7, 1>(q.data());
+  kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());
 
   KDL::Chain chain = this->chain_;  // copy
   augmentFrame("load", F_x_Ctotal, m_total, I_total, chain);
@@ -247,8 +247,8 @@ std::array<double, 7> ModelKDL::coriolis(
     const std::array<double, 3>& F_x_Ctotal)  // NOLINT(readability-identifier-naming)
     const {
   KDL::JntArray kq, kdq, kc(7);
-  kq.data = Eigen::Matrix<double, 7, 1>(q.data());
-  kdq.data = Eigen::Matrix<double, 7, 1>(dq.data());
+  kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());
+  kdq.data = Eigen::Matrix<double, 7, 1>::Map(dq.data());
 
   KDL::Chain chain = this->chain_;  // copy
   augmentFrame("load", F_x_Ctotal, m_total, I_total, chain);
@@ -272,7 +272,7 @@ std::array<double, 7> ModelKDL::gravity(
     const std::array<double, 3>& gravity_earth) const {
   KDL::JntArray kq, kg(7);
   KDL::Vector grav(gravity_earth[0], gravity_earth[1], gravity_earth[2]);
-  kq.data = Eigen::Matrix<double, 7, 1>(q.data());
+  kq.data = Eigen::Matrix<double, 7, 1>::Map(q.data());
 
   KDL::Chain chain = this->chain_;  // copy
   augmentFrame("load", F_x_Ctotal, m_total, {1, 0, 0, 0, 1, 0, 0, 0, 1}, chain);