From 421694de2c8f26cdf2eb7466377d96c278aa37f7 Mon Sep 17 00:00:00 2001
From: Florian Walch <florian.walch@franka.de>
Date: Thu, 18 Jan 2018 18:26:44 +0100
Subject: [PATCH] Fix includes for Eigen3 in franka_example_controllers
---
CHANGELOG.md | 1 +
franka_example_controllers/CMakeLists.txt | 4 +++-
.../cartesian_impedance_example_controller.h | 4 ++--
.../force_example_controller.h | 12 ++++------
franka_example_controllers/package.xml | 1 +
...cartesian_impedance_example_controller.cpp | 4 ++--
.../src/pseudo_inversion.h | 24 +++++++------------
7 files changed, 23 insertions(+), 27 deletions(-)
diff --git a/CHANGELOG.md b/CHANGELOG.md
index b2c4a7f..7595df6 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -7,6 +7,7 @@
* Added `m_ee`, `F_x_Cee`, `I_ee`, `m_total`, `F_x_Ctotal` and `I_total`
to the robot state
* Use O_T_EE_d in examples
+ * Fix includes for Eigen3 in `franka_example_controllers`
## 0.1.2 - 2017-10-10
diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt
index a09f668..feec755 100644
--- a/franka_example_controllers/CMakeLists.txt
+++ b/franka_example_controllers/CMakeLists.txt
@@ -16,6 +16,8 @@ find_package(catkin REQUIRED COMPONENTS
dynamic_reconfigure
)
+find_package(Eigen3 REQUIRED)
+
add_message_files(FILES
JointTorqueComparison.msg
)
@@ -33,7 +35,7 @@ catkin_package(
CATKIN_DEPENDS controller_interface franka_hw pluginlib
)
-include_directories(include ${catkin_INCLUDE_DIRS})
+include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
add_library(franka_example_controllers
src/cartesian_pose_example_controller.cpp
diff --git a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
index 06ae8f6..4e02d49 100644
--- a/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h
@@ -6,6 +6,8 @@
#include <string>
#include <vector>
+#include <Eigen/Dense>
+#include <boost/scoped_ptr.hpp>
#include <controller_interface/multi_interface_controller.h>
#include <dynamic_reconfigure/server.h>
#include <geometry_msgs/PoseStamped.h>
@@ -13,8 +15,6 @@
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
#include <ros/time.h>
-#include <boost/scoped_ptr.hpp>
-#include <eigen3/Eigen/Dense>
#include <franka_example_controllers/compliance_paramConfig.h>
#include <franka_hw/franka_model_interface.h>
diff --git a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
index a9f36a6..ff8cb56 100644
--- a/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
+++ b/franka_example_controllers/include/franka_example_controllers/force_example_controller.h
@@ -6,20 +6,18 @@
#include <string>
#include <vector>
+#include <Eigen/Core>
+#include <boost/scoped_ptr.hpp>
#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
#include <ros/time.h>
-#include <franka_hw/franka_model_interface.h>
-#include <franka_hw/franka_state_interface.h>
-
-#include <eigen3/Eigen/Core>
-
-#include <dynamic_reconfigure/server.h>
#include <franka_example_controllers/desired_mass_paramConfig.h>
-#include <boost/scoped_ptr.hpp>
namespace franka_example_controllers {
diff --git a/franka_example_controllers/package.xml b/franka_example_controllers/package.xml
index 71f6826..9c37a4f 100644
--- a/franka_example_controllers/package.xml
+++ b/franka_example_controllers/package.xml
@@ -16,6 +16,7 @@
<build_depend>roscpp</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
+ <build_depend>eigen</build_depend>
<depend>controller_interface</depend>
<depend>franka_hw</depend>
diff --git a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
index 8371b71..d3c4cdc 100644
--- a/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
+++ b/franka_example_controllers/src/cartesian_impedance_example_controller.cpp
@@ -5,10 +5,10 @@
#include <cmath>
#include <controller_interface/controller_base.h>
+#include <franka/robot_state.h>
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
-#include <franka/robot_state.h>
#include "pseudo_inversion.h"
namespace franka_example_controllers {
@@ -174,7 +174,7 @@ void CartesianImpedanceExampleController::update(const ros::Time& /*time*/,
// pseudoinverse for nullspace handling
// kinematic pseuoinverse
Eigen::MatrixXd jacobian_transpose_pinv;
- pseudo_inverse(jacobian.transpose(), jacobian_transpose_pinv);
+ pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv);
// Cartesian PD control with damping ratio = 1
tau_task << jacobian.transpose() *
diff --git a/franka_example_controllers/src/pseudo_inversion.h b/franka_example_controllers/src/pseudo_inversion.h
index 6170154..c608b30 100644
--- a/franka_example_controllers/src/pseudo_inversion.h
+++ b/franka_example_controllers/src/pseudo_inversion.h
@@ -5,28 +5,22 @@
// between damped and not)
// returns the pseudo inverted matrix M_pinv_
-#ifndef PSEUDO_INVERSION_H
-#define PSEUDO_INVERSION_H
+#pragma once
-#include <eigen3/Eigen/Core>
-#include <eigen3/Eigen/LU>
-#include <eigen3/Eigen/SVD>
-using namespace Eigen;
+#include <Eigen/Core>
+#include <Eigen/LU>
+#include <Eigen/SVD>
-inline void pseudo_inverse(const Eigen::MatrixXd& M_,
- Eigen::MatrixXd& M_pinv_,
- bool damped = true) {
+inline void pseudoInverse(const Eigen::MatrixXd& M_, Eigen::MatrixXd& M_pinv_, bool damped = true) {
double lambda_ = damped ? 0.2 : 0.0;
- JacobiSVD<MatrixXd> svd(M_, ComputeFullU | ComputeFullV);
- JacobiSVD<MatrixXd>::SingularValuesType sing_vals_ = svd.singularValues();
- MatrixXd S_ = M_; // copying the dimensions of M_, its content is not needed.
+ Eigen::JacobiSVD<Eigen::MatrixXd> svd(M_, Eigen::ComputeFullU | Eigen::ComputeFullV);
+ Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType sing_vals_ = svd.singularValues();
+ Eigen::MatrixXd S_ = M_; // copying the dimensions of M_, its content is not needed.
S_.setZero();
for (int i = 0; i < sing_vals_.size(); i++)
S_(i, i) = (sing_vals_(i)) / (sing_vals_(i) * sing_vals_(i) + lambda_ * lambda_);
- M_pinv_ = MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose());
+ M_pinv_ = Eigen::MatrixXd(svd.matrixV() * S_.transpose() * svd.matrixU().transpose());
}
-
-#endif // PSEUDO_INVERSION_H
--
GitLab