Merge pull request #60 in SWDEV/franka_ros from fix-includes to develop

* commit '65ee6f46':
  Fix formatting, remove use of boost pointer
  Fix includes for Eigen3 in franka_example_controllers

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
 

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

franka_example_controllers/include/franka_example_controllers/cartesian_impedance_example_controller.h

@@ -13,8 +13,7 @@
 #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 <Eigen/Dense>
 
 #include <franka_example_controllers/compliance_paramConfig.h>
 #include <franka_hw/franka_model_interface.h>
@@ -60,7 +59,7 @@ class CartesianImpedanceExampleController : public controller_interface::MultiIn
       const Eigen::Matrix<double, 7, 1>& gravity);
 
   // Dynamic reconfigure
-  boost::scoped_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
+  std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
       dynamic_server_compliance_param_;
   ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
   void complianceParamCallback(franka_example_controllers::compliance_paramConfig& config,

franka_example_controllers/include/franka_example_controllers/force_example_controller.h

@@ -7,19 +7,16 @@
 #include <vector>
 
 #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 <Eigen/Core>
 
-#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 {
 
@@ -48,8 +45,7 @@ class ForceExampleController : public controller_interface::MultiInterfaceContro
   Eigen::Matrix<double, 7, 1> tau_error_;
 
   // Dynamic reconfigure
-  boost::scoped_ptr<
-      dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>
+  std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::desired_mass_paramConfig>>
       dynamic_server_desired_mass_param_;
   ros::NodeHandle dynamic_reconfigure_desired_mass_param_node_;
   void desiredMassParamCallback(franka_example_controllers::desired_mass_paramConfig& config,

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>

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() *

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