diff --git a/moveit_core/dynamics_solver/CMakeLists.txt b/moveit_core/dynamics_solver/CMakeLists.txt index 61324a752d..51c3ae24f3 100644 --- a/moveit_core/dynamics_solver/CMakeLists.txt +++ b/moveit_core/dynamics_solver/CMakeLists.txt @@ -3,11 +3,17 @@ set(MOVEIT_LIB_NAME moveit_dynamics_solver) add_library(${MOVEIT_LIB_NAME} src/dynamics_solver.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 + orocos_kdl + visualization_msgs + kdl_parser) 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/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h b/moveit_core/dynamics_solver/include/moveit/dynamics_solver/dynamics_solver.h index 3e73f8b036..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 @@ -42,9 +42,8 @@ #include #include -#include -#include - +#include +#include #include /** \brief This namespace includes the dynamics_solver library */ @@ -68,7 +67,7 @@ class DynamicsSolver * @return False if initialization failed */ DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, - const geometry_msgs::Vector3& gravity_vector); + const geometry_msgs::msg::Vector3& gravity_vector); /** * @brief Get the torques (the order of all input and output is the same @@ -86,7 +85,7 @@ class DynamicsSolver * @return False if any of the input vectors are of the wrong size */ bool getTorques(const std::vector& joint_angles, const std::vector& joint_velocities, - const std::vector& joint_accelerations, const std::vector& wrenches, + const std::vector& joint_accelerations, const std::vector& wrenches, std::vector& torques) const; /** diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 94f106ea9a..d5588c1c5b 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -44,18 +44,22 @@ #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::Vector3 transformVector(const Eigen::Isometry3d& transform, const geometry_msgs::Vector3& vector) +inline geometry_msgs::msg::Vector3 transformVector(const Eigen::Isometry3d& transform, const geometry_msgs::msg::Vector3& vector) { Eigen::Vector3d p; p = Eigen::Vector3d(vector.x, vector.y, vector.z); p = transform.rotation() * p; - geometry_msgs::Vector3 result; + geometry_msgs::msg::Vector3 result; result.x = p.x(); result.y = p.y(); result.z = p.z(); @@ -65,7 +69,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform } // namespace DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name, - const geometry_msgs::Vector3& gravity_vector) + const geometry_msgs::msg::Vector3& gravity_vector) { robot_model_ = robot_model; joint_model_group_ = robot_model_->getJointModelGroup(group_name); @@ -74,7 +78,7 @@ DynamicsSolver::DynamicsSolver(const robot_model::RobotModelConstPtr& robot_mode if (!joint_model_group_->isChain()) { - ROS_ERROR_NAMED("dynamics_solver", "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()) { - ROS_ERROR_NAMED("dynamics_solver", "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()) { - ROS_ERROR_NAMED("dynamics_solver", "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(); - ROS_DEBUG_NAMED("dynamics_solver", "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)) { - ROS_ERROR_NAMED("dynamics_solver", "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_)) { - ROS_ERROR_NAMED("dynamics_solver", "Could not initialize chain object"); + RCLCPP_ERROR(LOGGER, "Could not initialize chain object"); joint_model_group_ = nullptr; return; } @@ -136,44 +140,44 @@ 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(); - ROS_DEBUG_NAMED("dynamics_solver", "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)); } bool DynamicsSolver::getTorques(const std::vector& joint_angles, const std::vector& joint_velocities, const std::vector& joint_accelerations, - const std::vector& wrenches, std::vector& torques) const + const std::vector& wrenches, std::vector& torques) const { if (!joint_model_group_) { - ROS_DEBUG_NAMED("dynamics_solver", "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_) { - ROS_ERROR_NAMED("dynamics_solver", "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_) { - ROS_ERROR_NAMED("dynamics_solver", "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_) { - ROS_ERROR_NAMED("dynamics_solver", "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_) { - ROS_ERROR_NAMED("dynamics_solver", "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_) { - ROS_ERROR_NAMED("dynamics_solver", "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) { - ROS_ERROR_NAMED("dynamics_solver", "Something went wrong computing torques"); + RCLCPP_ERROR(LOGGER, "Something went wrong computing torques"); return false; } @@ -216,19 +220,19 @@ bool DynamicsSolver::getMaxPayload(const std::vector& joint_angles, doub { if (!joint_model_group_) { - ROS_DEBUG_NAMED("dynamics_solver", "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_) { - ROS_ERROR_NAMED("dynamics_solver", "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); std::vector torques(num_joints_, 0.0), zero_torques(num_joints_, 0.0); - std::vector wrenches(num_segments_); + std::vector wrenches(num_segments_); if (!getTorques(joint_angles, joint_velocities, joint_accelerations, wrenches, zero_torques)) return false; @@ -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); - ROS_DEBUG_NAMED("dynamics_solver", "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 - ROS_DEBUG_NAMED("dynamics_solver", "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]); - ROS_DEBUG_NAMED("dynamics_solver", "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_; - ROS_DEBUG_NAMED("dynamics_solver", "Max payload (kg): %f", payload); + RCLCPP_DEBUG(LOGGER, "Max payload (kg): %f", payload); return true; } @@ -281,22 +285,22 @@ bool DynamicsSolver::getPayloadTorques(const std::vector& joint_angles, { if (!joint_model_group_) { - ROS_DEBUG_NAMED("dynamics_solver", "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_) { - ROS_ERROR_NAMED("dynamics_solver", "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_) { - ROS_ERROR_NAMED("dynamics_solver", "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); - std::vector wrenches(num_segments_); + std::vector wrenches(num_segments_); state_->setJointGroupPositions(joint_model_group_, joint_angles); const Eigen::Isometry3d& base_frame = state_->getFrameTransform(base_name_); @@ -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); - ROS_DEBUG_NAMED("dynamics_solver", "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); @@ -317,4 +321,4 @@ const std::vector& DynamicsSolver::getMaxTorques() const return max_torques_; } -} // end of namespace dynamics_solver \ No newline at end of file +} // end of namespace dynamics_solver