From 1079ddabfd2824a925a933fa177fc050209c2a1e Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 May 2019 15:56:21 +0200 Subject: [PATCH 1/3] moveit_core kinematics_metrics - library as shared --- moveit_core/kinematics_metrics/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt index 1fa339f946..f9bbab3819 100644 --- a/moveit_core/kinematics_metrics/CMakeLists.txt +++ b/moveit_core/kinematics_metrics/CMakeLists.txt @@ -1,6 +1,6 @@ set(MOVEIT_LIB_NAME moveit_kinematics_metrics) -add_library(${MOVEIT_LIB_NAME} src/kinematics_metrics.cpp) +add_library(${MOVEIT_LIB_NAME} SHARED src/kinematics_metrics.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(${MOVEIT_LIB_NAME} From ecb2350a2979cddfb8d087e72aebf51dd5c0e457 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 24 May 2019 15:57:26 +0200 Subject: [PATCH 2/3] moveit_core kinematics_metrics - Fixing CMakeLists.txt Co-authored-by: vmayoral --- moveit_core/kinematics_metrics/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt index f9bbab3819..c01e437ec5 100644 --- a/moveit_core/kinematics_metrics/CMakeLists.txt +++ b/moveit_core/kinematics_metrics/CMakeLists.txt @@ -9,6 +9,11 @@ ament_target_dependencies(${MOVEIT_LIB_NAME} urdfdom_headers visualization_msgs) +target_link_libraries(${MOVEIT_LIB_NAME} + moveit_robot_model + moveit_robot_state +) + install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib) From a62cd41e67fba8701ccb03b4fe3b20c254694e8b Mon Sep 17 00:00:00 2001 From: Mike Lautman Date: Fri, 31 May 2019 18:01:35 -0600 Subject: [PATCH 3/3] clang --- .../kinematics_metrics/src/kinematics_metrics.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index 50dda46b0a..5b1f34894d 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -42,7 +42,6 @@ namespace kinematics_metrics { - rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("kinematics_metrics"); double KinematicsMetrics::getJointLimitsPenalty(const robot_state::RobotState& state, @@ -222,7 +221,8 @@ 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){ + for (int i = 0; i < singular_values.rows(); ++i) + { RCLCPP_DEBUG(LOGGER, "Singular value: %d %f", i, singular_values(i, 0)); } @@ -233,8 +233,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){ - RCLCPP_DEBUG(LOGGER, "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(); }