From 9342051c4db3013c4eeff2fc6ad4819236c7075c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 31 Mar 2019 19:28:51 +0200 Subject: [PATCH 1/2] kinematics_metrics, port to ROS 2 --- moveit_core/kinematics_metrics/CMakeLists.txt | 13 ++++++++----- .../kinematics_metrics/kinematics_metrics.h | 4 +++- .../kinematics_metrics/src/kinematics_metrics.cpp | 15 +++++++++------ 3 files changed, 20 insertions(+), 12 deletions(-) diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt index 8438c8641d..1fa339f946 100644 --- a/moveit_core/kinematics_metrics/CMakeLists.txt +++ b/moveit_core/kinematics_metrics/CMakeLists.txt @@ -3,11 +3,14 @@ set(MOVEIT_LIB_NAME moveit_kinematics_metrics) add_library(${MOVEIT_LIB_NAME} src/kinematics_metrics.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_state ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES}) -add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(${MOVEIT_LIB_NAME} + moveit_robot_state + urdf + urdfdom_headers + visualization_msgs) install(TARGETS ${MOVEIT_LIB_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(DIRECTORY include/ DESTINATION include) diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index c92a7d5063..2874a46166 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -39,12 +39,14 @@ #include #include - +#include "rclcpp/rclcpp.hpp" /** @brief Namespace for kinematics metrics */ namespace kinematics_metrics { MOVEIT_CLASS_FORWARD(KinematicsMetrics); +rclcpp::Logger logger = rclcpp::get_logger("kinematics_metrics"); + /** * \brief Compute different kinds of metrics for kinematics evaluation. Currently includes * manipulability. diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index 71289f831d..e7c7ea587f 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -123,7 +123,7 @@ bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& st manipulability_index = 1.0; for (unsigned int i = 0; i < singular_values.rows(); ++i) { - ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(logger, "Singular value: %d %f", i, singular_values(i, 0)); manipulability_index *= singular_values(i, 0); } // Get manipulability index @@ -146,7 +146,7 @@ bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& st manipulability_index = 1.0; for (unsigned int i = 0; i < singular_values.rows(); ++i) { - ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(logger, "Singular value: %d %f", i, singular_values(i, 0)); manipulability_index *= singular_values(i, 0); } // Get manipulability index @@ -218,8 +218,10 @@ bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group); Eigen::JacobiSVD svdsolver(jacobian.topLeftCorner(3, jacobian.cols())); Eigen::MatrixXd singular_values = svdsolver.singularValues(); - for (int i = 0; i < singular_values.rows(); ++i) - ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0)); + for (int i = 0; i < singular_values.rows(); ++i){ + RCLCPP_DEBUG(logger, "Singular value: %d %f", i, singular_values(i, 0)); + } + manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff(); } else @@ -227,8 +229,9 @@ bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group); Eigen::JacobiSVD svdsolver(jacobian); Eigen::MatrixXd singular_values = svdsolver.singularValues(); - for (int i = 0; i < singular_values.rows(); ++i) - ROS_DEBUG_NAMED("kinematics_metrics", "Singular value: %d %f", i, singular_values(i, 0)); + for (int i = 0; i < singular_values.rows(); ++i){ + RCLCPP_DEBUG(logger, "Singular value: %d %f", i, singular_values(i, 0)); + } manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff(); } return true; From cffeeaa1d9062a8767959229c987428819662e4a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 31 Mar 2019 19:35:06 +0200 Subject: [PATCH 2/2] kinematics_metrics, adapt logging to https://github.com/ros-planning/moveit2/issues/21 --- .../moveit/kinematics_metrics/kinematics_metrics.h | 4 +--- .../kinematics_metrics/src/kinematics_metrics.cpp | 12 ++++++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h index 2874a46166..c92a7d5063 100644 --- a/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h +++ b/moveit_core/kinematics_metrics/include/moveit/kinematics_metrics/kinematics_metrics.h @@ -39,14 +39,12 @@ #include #include -#include "rclcpp/rclcpp.hpp" + /** @brief Namespace for kinematics metrics */ namespace kinematics_metrics { MOVEIT_CLASS_FORWARD(KinematicsMetrics); -rclcpp::Logger logger = rclcpp::get_logger("kinematics_metrics"); - /** * \brief Compute different kinds of metrics for kinematics evaluation. Currently includes * manipulability. diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index e7c7ea587f..50dda46b0a 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -38,9 +38,13 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" namespace kinematics_metrics { + +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("kinematics_metrics"); + double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& state, const robot_model::JointModelGroup* joint_model_group) const { @@ -123,7 +127,7 @@ bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& st manipulability_index = 1.0; for (unsigned int i = 0; i < singular_values.rows(); ++i) { - RCLCPP_DEBUG(logger, "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); manipulability_index *= singular_values(i, 0); } // Get manipulability index @@ -146,7 +150,7 @@ bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState& st manipulability_index = 1.0; for (unsigned int i = 0; i < singular_values.rows(); ++i) { - RCLCPP_DEBUG(logger, "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); manipulability_index *= singular_values(i, 0); } // Get manipulability index @@ -219,7 +223,7 @@ bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, Eigen::JacobiSVD svdsolver(jacobian.topLeftCorner(3, jacobian.cols())); Eigen::MatrixXd singular_values = svdsolver.singularValues(); for (int i = 0; i < singular_values.rows(); ++i){ - RCLCPP_DEBUG(logger, "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); } manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff(); @@ -230,7 +234,7 @@ bool KinematicsMetrics::getManipulability(const robot_state::RobotState& state, Eigen::JacobiSVD svdsolver(jacobian); Eigen::MatrixXd singular_values = svdsolver.singularValues(); for (int i = 0; i < singular_values.rows(); ++i){ - RCLCPP_DEBUG(logger, "Singular value: %d %f", i, singular_values(i, 0)); + RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); } manipulability = penalty * singular_values.minCoeff() / singular_values.maxCoeff(); }