From f2dd70841fe5805c7230eb8a0fb1aedd4e2e5f5d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Sun, 31 Mar 2019 19:45:11 +0200 Subject: [PATCH] Adapt dynamics_solver to logging https://github.com/ros-planning/moveit2/issues/21 --- .../moveit/dynamics_solver/dynamics_solver.h | 5 -- .../dynamics_solver/src/dynamics_solver.cpp | 52 ++++++++++--------- 2 files changed, 28 insertions(+), 29 deletions(-) diff --git a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h index 1000ab9206..bee9199caa 100644 --- a/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h +++ b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h @@ -44,18 +44,13 @@ #include #include #include - #include -#include "rclcpp/rclcpp.hpp" - - /** \brief This namespace includes the dynamics_solver library */ namespace dynamics_solver { MOVEIT_CLASS_FORWARD(DynamicsSolver); -rclcpp::Logger logger = rclcpp::get_logger("dynamics_solver"); /** * This solver currently computes the required torques given a * joint configuration, velocities, accelerations and external wrenches diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index f38e6faacf..943fb69339 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -44,9 +44,13 @@ #include #include #include +#include "rclcpp/rclcpp.hpp" namespace dynamics_solver { + +rclcpp::Logger LOGGER = rclcpp::get_logger("moveit").get_child("dynamics_solver"); + namespace { inline geometry_msgs::msg::Vector3 transformVector(const Eigen::Affine3d& transform, const geometry_msgs::msg::Vector3& vector) @@ -74,7 +78,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode if (!joint_model_group_->isChain()) { - RCLCPP_ERROR(logger, "Group '%s' is not a chain. Will not initialize dynamics solver", + RCLCPP_ERROR(LOGGER, "Group '%s' is not a chain. Will not initialize dynamics solver", group_name.c_str()); joint_model_group_ = nullptr; return; @@ -82,7 +86,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode if (!joint_model_group_->getMimicJointModels().empty()) { - RCLCPP_ERROR(logger, "Group '%s' has a mimic joint. Will not initialize dynamics solver", + RCLCPP_ERROR(LOGGER, "Group '%s' has a mimic joint. Will not initialize dynamics solver", group_name.c_str()); joint_model_group_ = nullptr; return; @@ -91,7 +95,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode const robot_model::JointModel* joint = joint_model_group_->getJointRoots()[0]; if (!joint->getParentLinkModel()) { - RCLCPP_ERROR(logger, "Group '%s' does not have a parent link", group_name.c_str()); + RCLCPP_ERROR(LOGGER, "Group '%s' does not have a parent link", group_name.c_str()); joint_model_group_ = nullptr; return; } @@ -99,7 +103,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode base_name_ = joint->getParentLinkModel()->getName(); tip_name_ = joint_model_group_->getLinkModelNames().back(); - RCLCPP_DEBUG(logger, "Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str()); + RCLCPP_DEBUG(LOGGER, "Base name: '%s', Tip name: '%s'", base_name_.c_str(), tip_name_.c_str()); const urdf::ModelInterfaceSharedPtr urdf_model = robot_model_->getURDF(); const srdf::ModelConstSharedPtr srdf_model = robot_model_->getSRDF(); @@ -107,13 +111,13 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode if (!kdl_parser::treeFromUrdfModel(*urdf_model, tree)) { - RCLCPP_ERROR(logger, "Could not initialize tree object"); + RCLCPP_ERROR(LOGGER, "Could not initialize tree object"); joint_model_group_ = nullptr; return; } if (!tree.getChain(base_name_, tip_name_, kdl_chain_)) { - RCLCPP_ERROR(logger, "Could not initialize chain object"); + RCLCPP_ERROR(LOGGER, "Could not initialize chain object"); joint_model_group_ = nullptr; return; } @@ -136,7 +140,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode KDL::Vector gravity(gravity_vector.x, gravity_vector.y, gravity_vector.z); // \todo Not sure if KDL expects the negative of this (Sachin) gravity_ = gravity.Norm(); - RCLCPP_DEBUG(logger, "Gravity norm set to %f", gravity_); + RCLCPP_DEBUG(LOGGER, "Gravity norm set to %f", gravity_); chain_id_solver_.reset(new KDL::ChainIdSolver_RNE(kdl_chain_, gravity)); } @@ -147,33 +151,33 @@ bool DynamicsSolver::getTorques(const std::vector& joint_angles, const s { if (!joint_model_group_) { - RCLCPP_DEBUG(logger, "Did not construct DynamicsSolver object properly. " + RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. " "Check error logs."); return false; } if (joint_angles.size() != num_joints_) { - RCLCPP_ERROR(logger, "Joint angles vector should be size %d", num_joints_); + RCLCPP_ERROR(LOGGER, "Joint angles vector should be size %d", num_joints_); return false; } if (joint_velocities.size() != num_joints_) { - RCLCPP_ERROR(logger, "Joint velocities vector should be size %d", num_joints_); + RCLCPP_ERROR(LOGGER, "Joint velocities vector should be size %d", num_joints_); return false; } if (joint_accelerations.size() != num_joints_) { - RCLCPP_ERROR(logger, "Joint accelerations vector should be size %d", num_joints_); + RCLCPP_ERROR(LOGGER, "Joint accelerations vector should be size %d", num_joints_); return false; } if (wrenches.size() != num_segments_) { - RCLCPP_ERROR(logger, "Wrenches vector should be size %d", num_segments_); + RCLCPP_ERROR(LOGGER, "Wrenches vector should be size %d", num_segments_); return false; } if (torques.size() != num_joints_) { - RCLCPP_ERROR(logger, "Torques vector should be size %d", num_joints_); + RCLCPP_ERROR(LOGGER, "Torques vector should be size %d", num_joints_); return false; } @@ -201,7 +205,7 @@ bool DynamicsSolver::getTorques(const std::vector& joint_angles, const s if (chain_id_solver_->CartToJnt(kdl_angles, kdl_velocities, kdl_accelerations, kdl_wrenches, kdl_torques) < 0) { - RCLCPP_ERROR(logger, "Something went wrong computing torques"); + RCLCPP_ERROR(LOGGER, "Something went wrong computing torques"); return false; } @@ -216,13 +220,13 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub { if (!joint_model_group_) { - RCLCPP_DEBUG(logger, "Did not construct DynamicsSolver object properly. " + RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. " "Check error logs."); return false; } if (joint_angles.size() != num_joints_) { - RCLCPP_ERROR(logger, "Joint angles vector should be size %d", num_joints_); + RCLCPP_ERROR(LOGGER, "Joint angles vector should be size %d", num_joints_); return false; } std::vector joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0); @@ -250,7 +254,7 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub wrenches.back().force = transformVector(transform, wrenches.back().force); wrenches.back().torque = transformVector(transform, wrenches.back().torque); - RCLCPP_DEBUG(logger, "New wrench (local frame): %f %f %f", wrenches.back().force.x, + RCLCPP_DEBUG(LOGGER, "New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, wrenches.back().force.z); if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, torques)) @@ -262,9 +266,9 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub double payload_joint = std::max((max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i]), (-max_torques_[i] - zero_torques[i]) / (torques[i] - zero_torques[i])); // because we set the payload to 1.0 - RCLCPP_DEBUG(logger, "Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i], + RCLCPP_DEBUG(LOGGER, "Joint: %d, Actual Torque: %f, Max Allowed: %f, Gravity: %f", i, torques[i], max_torques_[i], zero_torques[i]); - RCLCPP_DEBUG(logger, "Joint: %d, Payload Allowed (N): %f", i, payload_joint); + RCLCPP_DEBUG(LOGGER, "Joint: %d, Payload Allowed (N): %f", i, payload_joint); if (payload_joint < min_payload) { min_payload = payload_joint; @@ -272,7 +276,7 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub } } payload = min_payload / gravity_; - RCLCPP_DEBUG(logger, "Max payload (kg): %f", payload); + RCLCPP_DEBUG(LOGGER, "Max payload (kg): %f", payload); return true; } @@ -281,18 +285,18 @@ bool DynamicsSolver::getPayloadTorques(const std::vector& joint_angles, { if (!joint_model_group_) { - RCLCPP_DEBUG(logger, "Did not construct DynamicsSolver object properly. " + RCLCPP_DEBUG(LOGGER, "Did not construct DynamicsSolver object properly. " "Check error logs."); return false; } if (joint_angles.size() != num_joints_) { - RCLCPP_ERROR(logger, "Joint angles vector should be size %d", num_joints_); + RCLCPP_ERROR(LOGGER, "Joint angles vector should be size %d", num_joints_); return false; } if (joint_torques.size() != num_joints_) { - RCLCPP_ERROR(logger, "Joint torques vector should be size %d", num_joints_); + RCLCPP_ERROR(LOGGER, "Joint torques vector should be size %d", num_joints_); return false; } std::vector joint_velocities(num_joints_, 0.0), joint_accelerations(num_joints_, 0.0); @@ -306,7 +310,7 @@ bool DynamicsSolver::getPayloadTorques(const std::vector& joint_angles, wrenches.back().force = transformVector(transform, wrenches.back().force); wrenches.back().torque = transformVector(transform, wrenches.back().torque); - RCLCPP_DEBUG(logger, "New wrench (local frame): %f %f %f", wrenches.back().force.x, + RCLCPP_DEBUG(LOGGER, "New wrench (local frame): %f %f %f", wrenches.back().force.x, wrenches.back().force.y, wrenches.back().force.z); return getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, joint_torques);