diff --git a/CMakeLists.txt b/CMakeLists.txt index 6bc123b..4e1f06e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,9 @@ -cmake_minimum_required(VERSION 3.10.2) -project(moveit_drake) +cmake_minimum_required(VERSION 3.22) +project(moveit_drake LANGUAGES CXX) + +list(APPEND CMAKE_PREFIX_PATH /opt/drake/lib) +find_package(drake REQUIRED) + find_package(ament_cmake REQUIRED) # Common cmake code applied to all moveit packages @@ -7,10 +11,15 @@ find_package(moveit_common REQUIRED) moveit_package() find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(class_loader REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(moveit_core REQUIRED) +find_package(moveit_msgs REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(moveit_visual_tools REQUIRED) +find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rviz_visual_tools REQUIRED) find_package(warehouse_ros REQUIRED) @@ -24,6 +33,16 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp rviz_visual_tools warehouse_ros) + +include_directories(include) + +add_library(moveit_diff_ik_plugin SHARED src/diff_ik_plugin.cpp) + +ament_target_dependencies(moveit_diff_ik_plugin rclcpp pluginlib moveit_core + moveit_msgs EIGEN3) + +target_link_libraries(moveit_diff_ik_plugin drake::drake) + add_subdirectory(demo) ament_package() diff --git a/README.md b/README.md index 458f794..f9be3d2 100644 --- a/README.md +++ b/README.md @@ -35,4 +35,4 @@ ros2 launch moveit_drake pipeline_testbench.launch.py ### Development -- Use [pre-commit to format your code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks) \ No newline at end of file +- Use [pre-commit to format your code](https://moveit.ros.org/documentation/contributing/code/#pre-commit-formatting-checks) diff --git a/include/moveit/drake/diff_ik_plugin.hpp b/include/moveit/drake/diff_ik_plugin.hpp new file mode 100644 index 0000000..29f0e6a --- /dev/null +++ b/include/moveit/drake/diff_ik_plugin.hpp @@ -0,0 +1,119 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr + Desc: TODO */ + +#pragma once + +// ROS2 +#include + +// System +#include + +// ROS msgs +#include +#include +#include + +// MoveIt +#include +#include + +namespace diff_ik_plugin +{ +/** + * @brief TODO + */ +class DiffIKPlugin : public kinematics::KinematicsBase +{ +public: + DiffIKPlugin(); + + bool + getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK( + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; + + bool searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, std::vector& solution, + const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(), + const moveit::core::RobotState* context_state = nullptr) const override; + + bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, + std::vector& poses) const override; + + bool initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_name, + const std::vector& tip_frames, double search_discretization) override; + + /** + * @brief Return all the joint names in the order they are used internally + */ + const std::vector& getJointNames() const override; + + /** + * @brief Return all the link names in the order they are represented internally + */ + const std::vector& getLinkNames() const override; + +protected: + bool setRedundantJoints(const std::vector& redundant_joint_indices) override; +}; +} // namespace diff_ik_plugin diff --git a/moveit_drake.repos b/moveit_drake.repos index 74092e4..ee4f931 100644 --- a/moveit_drake.repos +++ b/moveit_drake.repos @@ -22,4 +22,4 @@ repositories: moveit_benchmark_resources: type: git url: https://github.com/moveit/moveit_benchmark_resources - version: main \ No newline at end of file + version: main diff --git a/package.xml b/package.xml index 9948a2f..b4d9553 100644 --- a/package.xml +++ b/package.xml @@ -9,21 +9,23 @@ BSD ament_cmake + moveit_common + class_loader + eigen + generate_parameter_library moveit_core + moveit_msgs + moveit_ros_planning moveit_visual_tools + pluginlib rviz_visual_tools - eigen - moveit_common - moveit_ros_planning - controller_manager gripper_controllers joint_state_broadcaster joint_state_publisher joint_trajectory_controller - moveit moveit_resources_panda_moveit_config moveit_configs_utils moveit_py diff --git a/src/diff_ik_plugin.cpp b/src/diff_ik_plugin.cpp new file mode 100644 index 0000000..5820386 --- /dev/null +++ b/src/diff_ik_plugin.cpp @@ -0,0 +1,234 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Sebastian Jahr */ + +#include +#include +#include +#include + +// Eigen +#include +#include + +// Drake +#include + +// register SRVKinematics as a KinematicsBase implementation +CLASS_LOADER_REGISTER_CLASS(diff_ik_plugin::DiffIKPlugin, kinematics::KinematicsBase) + +namespace diff_ik_plugin +{ + +using namespace drake::multibody; +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.kinematics.diff_ik_plugin"); +} +} // namespace + +DiffIKPlugin::DiffIKPlugin() +{ +} + +bool DiffIKPlugin::initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model, + const std::string& group_name, const std::string& base_frame, + const std::vector& tip_frames, double search_discretization) +{ + // TODO + return true; +} + +bool DiffIKPlugin::setRedundantJoints(const std::vector& redundant_joints) +{ + // TODO + return true; +} + +bool DiffIKPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + std::vector consistency_limits; + + return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, consistency_limits, solution, IKCallbackFn(), + error_code, options); +} + +bool DiffIKPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + double timeout, std::vector& solution, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + std::vector consistency_limits; + + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, + options); +} + +bool DiffIKPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, + std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code, + options); +} + +bool DiffIKPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + double timeout, std::vector& solution, + const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + std::vector consistency_limits; + return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, + options); +} + +bool DiffIKPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, + double timeout, const std::vector& consistency_limits, + std::vector& solution, const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& options) const +{ + // Convert single pose into a vector of one pose + std::vector ik_poses; + ik_poses.push_back(ik_pose); + + return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, + options); +} + +bool DiffIKPlugin::searchPositionIK(const std::vector& ik_poses, + const std::vector& ik_seed_state, double /*timeout*/, + const std::vector& /*consistency_limits*/, std::vector& solution, + const IKCallbackFn& solution_callback, + moveit_msgs::msg::MoveItErrorCodes& error_code, + const kinematics::KinematicsQueryOptions& /*options*/, + const moveit::core::RobotState* context_state) const +{ + // TODO + using namespace drake::multibody; + + // TODO Use multibody plant? + + // TODO: This explicitly assumes that num_positions == num_velocities, which + // would not be true in the case of e.g. quaternion axes. How does MoveIt + // handle a reduced-DoF tangent space? + const size_t N = joint_model_group_->getActiveVariableCount(); + + Eigen::VectorXd q_current; + current_state_->copyJointGroupPositions(joint_model_group_, q_current); + + Eigen::VectorXd v_current; + current_state_->copyJointGroupVelocities(joint_model_group_, v_current); + + // TODO: The DoDifferentialInverseKinematics function can handle many + // addition costs and constraints, e.g.: + // - joint position limits + // - end-effector velocity/acceleration constraints + // - nominal posture/centering cost + DifferentialInverseKinematicsParameters params(N, N); + + // TODO: How do I get the real value from MoveIt? + // params.set_time_step(0.01); + + // TODO: Hardcoded velocity and acceleration limits for demonstration + // purposes. These should be drawn from the MoveIt/Servo configuration + // instead. + const Eigen::VectorXd v_max = Eigen::VectorXd::Constant(N, 1.0); + const Eigen::VectorXd vdot_max = Eigen::VectorXd::Constant(N, 1.0); + params.set_joint_velocity_limits({ -v_max, v_max }); + params.set_joint_acceleration_limits({ -vdot_max, vdot_max }); + + double error = std::numeric_limits::infinity(); + // TODO: Expose as parameter + redefine termination criteria + while (error > 0.1) + { + // Error between target pose and FK of current pose + + // Calculate Jacobian + // TODO: Unclear the exact specifications of this Jacobian: w.r.t. what + // reference frame? Analytical or geometric Jacobian? Seems to work OK but + // should be clarified. + const Eigen::MatrixXd& jacobian; + context_state.getJacobian(joint_model_group_ /* reference position? */); + + // Calculate generalized velocities + const auto result = DoDifferentialInverseKinematics(q_current, v_current, error, jacobian, params); + + // Process result + switch (result.status) + { + case DifferentialInverseKinematicsStatus::kSolutionFound: + // Update + break; + + case DifferentialInverseKinematicsStatus::kNoSolutionFound: + RCLCPP_WARN(LOGGER, "Differential inverse kinematics failed to converge to a solution!"); + return false; + case DifferentialInverseKinematicsStatus::kStuck: + // TODO: In this result joint velocities are still generated, they are + // just not guaranteed to follow the Cartesian target velocity. Is it + // acceptable to still use them? It may be the only way to get unstuck. + RCLCPP_WARN(LOGGER, "Differential inverse kinematics is stuck, likely due to constraints.!"); + return false; + } + } + + // Store result in solution + + return true; +} + +bool DiffIKPlugin::getPositionFK(const std::vector& link_names, const std::vector& joint_angles, + std::vector& poses) const +{ + poses.resize(link_names.size()); + if (joint_angles.size() != dimension_) + { + RCLCPP_ERROR(getLogger(), "Joint angles vector must have size: %ld", dimension_); + return false; + } + + // TODO Implement FK + RCLCPP_ERROR(getLogger(), "Forward kinematics not implemented"); + + return false; +} +} // namespace diff_ik_plugin