diff --git a/CHANGELOG.md b/CHANGELOG.md index b2c4a7f4609f47bf1813dadb61b6c0e1b2e1b587..7595df671fe2feb82e85135ef915cf16f174b64b 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 a09f668201808ebb883174639db2f8c38ea8ddce..feec755dacd55f45fd4d434d0e425944b0b92cf5 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 06ae8f69eea3364bd7d08bbcc9977c56449dc1f2..4e02d493d92a6206ac07ca73487de1e1ed165b5c 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 a9f36a688150df18b3001b8eb6dadfd80cc21e9e..ff8cb56d2a60b2ca943a5588203fe6ff1e3160e0 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 71f68269626adf75d634958e795da20a33be2e8c..9c37a4f4e1c6d6e967afb7a9e95516639d8384af 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 8371b712e13542f14e783ed6fbacaf1407c6b9d5..d3c4cdc258eae5b500b85fa3bfa0fd171ca42fd1 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 6170154323cabdd02642b5054dc4c239c2cf31f1..c608b30f906c2b029b4c278f852dd60e124e9790 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