diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index e45ece7127..aef740bb0c 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -93,7 +93,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS controller_manager/include distance_field/include collision_distance_field/include - # dynamics_solver/include + dynamics_solver/include kinematics_base/include kinematics_metrics/include robot_model/include @@ -108,7 +108,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS profiler/include sensor_manager/include trajectory_processing/include - # utils/include + utils/include logging/include ) @@ -200,7 +200,7 @@ add_subdirectory(trajectory_processing) add_subdirectory(distance_field) add_subdirectory(collision_distance_field) add_subdirectory(kinematics_metrics) -# add_subdirectory(dynamics_solver) +add_subdirectory(dynamics_solver) add_subdirectory(utils) # ament_package() diff --git a/moveit_core/dynamics_solver/CMakeLists.txt b/moveit_core/dynamics_solver/CMakeLists.txt index a285cf07bb..4365257555 100644 --- a/moveit_core/dynamics_solver/CMakeLists.txt +++ b/moveit_core/dynamics_solver/CMakeLists.txt @@ -3,11 +3,14 @@ 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 ${rclcpp_LIBRARIES} ${rmw_implementation_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) 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..1000ab9206 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,16 +42,20 @@ #include #include -#include -#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 @@ -68,7 +72,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 +90,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 977e417d16..3a620a5d8b 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -49,13 +49,13 @@ namespace 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 +65,7 @@ inline geometry_msgs::Vector3 transformVector(const Eigen::Isometry3d& transform } 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 +74,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 +82,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 +91,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 +99,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 +107,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 +136,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 +201,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 +216,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 +250,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 +262,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 +272,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 +281,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 +306,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 +317,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