From 1b3e04e700318cf95b86183c2d6e5f009ae64ea6 Mon Sep 17 00:00:00 2001 From: Wyatt Rees Date: Thu, 7 Jul 2022 16:23:43 -0600 Subject: [PATCH 1/7] Adding kinematics cost function tutorial --- CMakeLists.txt | 1 + doc/examples/examples.rst | 1 + .../kinematics_cost_function/CMakeLists.txt | 13 + .../kinematics_cost_function_tutorial.rst | 54 ++++ ...inematics_cost_function_tutorial.launch.py | 21 ++ .../src/kinematics_cost_function_tutorial.cpp | 257 ++++++++++++++++++ 6 files changed, 347 insertions(+) create mode 100644 doc/examples/kinematics_cost_function/CMakeLists.txt create mode 100644 doc/examples/kinematics_cost_function/kinematics_cost_function_tutorial.rst create mode 100644 doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py create mode 100644 doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 829905f9ef..43743fe586 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,7 @@ add_subdirectory(doc/examples/moveit_cpp) add_subdirectory(doc/examples/planning_scene_ros_api) add_subdirectory(doc/examples/planning_scene) add_subdirectory(doc/examples/realtime_servo) +add_subdirectory(doc/examples/kinematics_cost_function) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) add_subdirectory(doc/examples/robot_model_and_robot_state) add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor) diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index 01c76ca958..009a698289 100644 --- a/doc/examples/examples.rst +++ b/doc/examples/examples.rst @@ -40,6 +40,7 @@ Building more complex applications with MoveIt often requires developers to dig moveit_cpp/moveitcpp_tutorial bullet_collision_checker/bullet_collision_checker mobile_base_arm/mobile_base_arm_tutorial + kinematics_cost_function/kinematics_cost_function_tutorial Integration with a New Robot ---------------------------- diff --git a/doc/examples/kinematics_cost_function/CMakeLists.txt b/doc/examples/kinematics_cost_function/CMakeLists.txt new file mode 100644 index 0000000000..53ba3080bf --- /dev/null +++ b/doc/examples/kinematics_cost_function/CMakeLists.txt @@ -0,0 +1,13 @@ +add_executable(kinematics_cost_function_tutorial + src/kinematics_cost_function_tutorial.cpp) +target_include_directories(kinematics_cost_function_tutorial + PUBLIC include) +ament_target_dependencies(kinematics_cost_function_tutorial + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install(TARGETS kinematics_cost_function_tutorial + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/examples/kinematics_cost_function/kinematics_cost_function_tutorial.rst b/doc/examples/kinematics_cost_function/kinematics_cost_function_tutorial.rst new file mode 100644 index 0000000000..2fbf1ba318 --- /dev/null +++ b/doc/examples/kinematics_cost_function/kinematics_cost_function_tutorial.rst @@ -0,0 +1,54 @@ +Kinematics Cost Functions +================================== + +When querying for IK solutions for a goal pose or for a cartesian path goal, we can specify *cost functions* that will be used +to evaluate the fitness of solutions. + +Getting Started +--------------- +If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. + +This tutorial also requires the use of `bio_ik `_ as the Inverse Kinematics plugin. First, clone the "ros2" branch of this repository into your workspace's **src** directory: :: + + git clone -b ros2 https://github.com/PickNikRobotics/bio_ik.git + +Then, change your kinematics plugin for the Panda robot by copying the following into moveit_resources/panda_moveit_config/config/kinematics.yaml within your workspace: :: + + panda_arm: + kinematics_solver: bio_ik/BioIKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 1 + mode: gd_c # use the gradient descent solver + +After making these changes, rebuild your workspace: :: + + colcon build --mixin release + +Running the Code +---------------- +Open two shells. In the first shell, start RViz and wait for everything to finish loading: :: + + ros2 launch moveit2_tutorials move_group.launch.py + +In the second shell, run the tutorial launch file: :: + + ros2 launch moveit2_tutorials kinematics_cost_function_tutorial.launch.py + +To progress through each demo step either press the **Next** button in the **RvizVisualToolsGui** panel at the bottom of the screen or select **Key Tool** in the **Tools** panel at the top of the screen and then press **0** on your keyboard while RViz is focused. + +Expected Output +--------------- +In RViz, we should be able to see the following: + 1. The robot moves its arm to the pose goal to its right. The L2 norm of the joint movement with and without the cost function specified is logged in the tutorial terminal. + 2. The robot moves its arm via a straight cartesian movement to the pose goal at its left. + +The Entire Code +--------------- +The entire code can be seen :codedir:`here in the MoveIt GitHub project`. Next, we step through the code piece by piece to explain its functionality. + +.. tutorial-formatter:: ./src/kinematics_cost_function_tutorial.cpp + +The Launch File +--------------- +The entire launch file is :codedir:`here` on GitHub. All the code in this tutorial can be run from the **moveit2_tutorials** package that you have as part of your MoveIt setup. diff --git a/doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py b/doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py new file mode 100644 index 0000000000..52c21e0715 --- /dev/null +++ b/doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs() + + move_group_demo = Node( + name="kinematics_cost_fn_tutorial", + package="moveit2_tutorials", + executable="kinematics_cost_function_tutorial", + output="screen", + parameters=[ + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + ) + + return LaunchDescription([move_group_demo]) diff --git a/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp b/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp new file mode 100644 index 0000000000..22159d39bf --- /dev/null +++ b/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp @@ -0,0 +1,257 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, 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 SRI International 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: Wyatt Rees */ + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +// All source files that use ROS logging should define a file-specific +// static const rclcpp::Logger named LOGGER, located at the top of the file +// and inside the namespace with the narrowest scope (if there is one) +static const rclcpp::Logger LOGGER = rclcpp::get_logger("kinematics_cost_fn_demo"); + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto node = rclcpp::Node::make_shared("kinematics_cost_fn_tutorial", node_options); + + // We spin up a SingleThreadedExecutor for the current state monitor to get information + // about the robot's state. + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread([&executor]() { executor.spin(); }).detach(); + + // BEGIN_TUTORIAL + // + // Setup + // ^^^^^ + // + // MoveIt operates on sets of joints called "planning groups" and stores them in an object called + // the ``JointModelGroup``. Throughout MoveIt, the terms "planning group" and "joint model group" + // are used interchangeably. + static const std::string PLANNING_GROUP = "panda_arm"; + + // The + // :moveit_codedir:`MoveGroupInterface` + // class can be easily set up using just the name of the planning group you would like to control and plan for. + moveit::planning_interface::MoveGroupInterface move_group(node, PLANNING_GROUP); + + // Raw pointers are frequently used to refer to the planning group for improved performance. + const moveit::core::JointModelGroup* joint_model_group = + move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); + + // Visualization + // ^^^^^^^^^^^^^ + namespace rvt = rviz_visual_tools; + moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "move_group_tutorial", + move_group.getRobotModel()); + + visual_tools.deleteAllMarkers(); + visual_tools.loadRemoteControl(); + + Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); + text_pose.translation().z() = 1.0; + visual_tools.publishText(text_pose, "KinematicsCostFn_Demo", rvt::WHITE, rvt::XLARGE); + + visual_tools.trigger(); + + // Start the demo + // ^^^^^^^^^^^^^^^^^^^^^^^^^ + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + + // Computing IK solutions with cost functions + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // + // When computing IK solutions for goal poses, we can specify a cost function that will be used to + // evaluate the "fitness" for a particular solution. At the time of writing this tutorial, this is + // only supported by the bio_ik kinematics plugin. + // + // + // To start, we'll create two pointers that references the current robot's state. + // RobotState is the object that contains all the current position/velocity/acceleration data. + // By making two copies, we can test the difference between IK calls that do/don't include cost functions + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10); + moveit::core::RobotStatePtr copy_state = move_group.getCurrentState(10); + // We store the starting joint positions so we can evaluate performance later. + std::vector start_joint_positions; + current_state->copyJointGroupPositions(joint_model_group, start_joint_positions); + + // Set a target pose that we would like to solve IK for + geometry_msgs::msg::Pose target_pose; + target_pose.orientation.w = 1.0; + target_pose.position.x = 0.28; + target_pose.position.y = -0.2; + target_pose.position.z = 0.5; + + moveit::core::GroupStateValidityCallbackFn callback_fn; + + // Cost functions usually require one to accept approximate IK solutions + kinematics::KinematicsQueryOptions opts; + opts.return_approximate_solution = true; + + // Our cost function will optimize for minimal joint movement. Note that this is not the cost + // function that we pass directly to the IK call, but a helper function that we can also use + // to evaluate the solution. + const auto compute_l2_norm = [](std::vector solution, std::vector start) { + double sum = 0.0; + for (size_t ji = 0; ji < solution.size(); ji++) + { + double d = solution[ji] - start[ji]; + sum += d * d; + } + return sum; + }; + + // The weight of the cost function often needs tuning. A tradeoff exists between the accuracy of the + // solution pose and the extent to which the IK solver obeys our cost function. + const double weight = 0.0005; + const auto cost_fn = [&weight, &compute_l2_norm](const geometry_msgs::msg::Pose& /*goal_pose*/, + const moveit::core::RobotState& solution_state, + moveit::core::JointModelGroup* jmg, + const std::vector& seed_state) { + std::vector proposed_joint_positions; + solution_state.copyJointGroupPositions(jmg, proposed_joint_positions); + double cost = compute_l2_norm(proposed_joint_positions, seed_state); + return weight * cost; + }; + + // Now, we request an IK solution twice for the same pose. Once with a cost function, and once without. + current_state->setFromIK(joint_model_group, target_pose, 0.0, callback_fn, opts, cost_fn); + copy_state->setFromIK(joint_model_group, target_pose, 0.0, callback_fn, opts); + + std::vector solution; + current_state->copyJointGroupPositions(joint_model_group, solution); + + std::vector solution_no_cost_fn; + copy_state->copyJointGroupPositions(joint_model_group, solution_no_cost_fn); + + // Now we can use our helper function from earlier to evaluate the effectiveness of the cost function. + double l2_solution = compute_l2_norm(solution, start_joint_positions); + RCLCPP_INFO_STREAM(LOGGER, "L2 norm of the solution WITH a cost function is " << l2_solution); + l2_solution = compute_l2_norm(solution_no_cost_fn, start_joint_positions); + RCLCPP_INFO_STREAM(LOGGER, "L2 norm of the solution WITHOUT a cost function is " << l2_solution); + + // If we're happy with the solution, we can set it as a joint value target. + move_group.setJointValueTarget(solution); + + // We lower the allowed maximum velocity and acceleration to 5% of their maximum. + // The default values are 10% (0.1). + // Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config + // or set explicit factors in your code if you need your robot to move faster. + move_group.setMaxVelocityScalingFactor(0.05); + move_group.setMaxAccelerationScalingFactor(0.05); + + moveit::planning_interface::MoveGroupInterface::Plan my_plan; + bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); + RCLCPP_INFO(LOGGER, "Visualizing plan 1 (joint space goal with cost function) %s", success ? "" : "FAILED"); + + // Visualize the plan in RViz: + visual_tools.deleteAllMarkers(); + visual_tools.publishAxisLabeled(target_pose, "goal_pose"); + visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.trigger(); + + // Uncomment if you would like to execute the motion + /* + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to execute the motion"); + move_group.execute(my_plan); + */ + + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue."); + + // We can also specify cost functions when computing robot trajectories that must follow a cartesian path. + // First, let's change the target pose for the end of the path, so that if the previous motion plan was executed, + // we still have somewhere to move. + target_pose.position.y += 0.4; + target_pose.position.z -= 0.1; + target_pose.orientation.w = 0; + target_pose.orientation.x = -1.0; + Eigen::Isometry3d target; + tf2::fromMsg(target_pose, target); + + auto start_state = move_group.getCurrentState(10.0); + std::vector traj; + moveit::core::MaxEEFStep max_eef_step(0.01, 0.1); + // Here, were effectively disabling the jump threshold for joints. This is not recommended on real hardware. + moveit::core::JumpThreshold jump_thresh(0.0); + + // The trajectory, traj, passed to computeCartesianPath will contain several waypoints toward + // the goal pose, target. For each of these waypoints, the IK solver is queried with the given cost function. + const auto frac = moveit::core::CartesianInterpolator::computeCartesianPath( + start_state.get(), joint_model_group, traj, joint_model_group->getLinkModel("panda_link8"), target, true, + max_eef_step, jump_thresh, callback_fn, opts, cost_fn); + + RCLCPP_INFO(LOGGER, "Computed %f percent of cartesian path.", frac.value * 100.0); + + // Once we've computed the points in our Cartesian path, we need to add timestamps to each point for execution. + robot_trajectory::RobotTrajectory rt(start_state->getRobotModel(), PLANNING_GROUP); + for (const moveit::core::RobotStatePtr& traj_state : traj) + rt.addSuffixWayPoint(traj_state, 0.0); + trajectory_processing::TimeOptimalTrajectoryGeneration time_param; + time_param.computeTimeStamps(rt, 1.0); + + moveit_msgs::msg::RobotTrajectory rt_msg; + rt.getRobotTrajectoryMsg(rt_msg); + + visual_tools.deleteAllMarkers(); + visual_tools.publishTrajectoryLine(rt, joint_model_group); + visual_tools.publishText(text_pose, "Cartesian_Path_Goal", rvt::WHITE, rvt::XLARGE); + visual_tools.publishAxisLabeled(target_pose, "cartesian_goal_pose"); + visual_tools.trigger(); + + // Once we've computed the timestamps, we can pass the trajectory to move_group to execute it. + move_group.execute(rt_msg); + visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to end the demo."); + + // END_TUTORIAL + visual_tools.deleteAllMarkers(); + visual_tools.trigger(); + + rclcpp::shutdown(); + return 0; +} From c0e67fff33aa5c75b145094715c950878615a969 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 21 Apr 2023 12:30:32 +0200 Subject: [PATCH 2/7] Address compilor errors --- .../src/kinematics_cost_function_tutorial.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp b/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp index 22159d39bf..b9a637c35e 100644 --- a/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp +++ b/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp @@ -33,7 +33,6 @@ *********************************************************************/ /* Author: Wyatt Rees */ - #include #include #include @@ -151,7 +150,7 @@ int main(int argc, char** argv) const double weight = 0.0005; const auto cost_fn = [&weight, &compute_l2_norm](const geometry_msgs::msg::Pose& /*goal_pose*/, const moveit::core::RobotState& solution_state, - moveit::core::JointModelGroup* jmg, + moveit::core::JointModelGroup const* jmg, const std::vector& seed_state) { std::vector proposed_joint_positions; solution_state.copyJointGroupPositions(jmg, proposed_joint_positions); @@ -160,8 +159,8 @@ int main(int argc, char** argv) }; // Now, we request an IK solution twice for the same pose. Once with a cost function, and once without. - current_state->setFromIK(joint_model_group, target_pose, 0.0, callback_fn, opts, cost_fn); - copy_state->setFromIK(joint_model_group, target_pose, 0.0, callback_fn, opts); + current_state->setFromIK(joint_model_group, target_pose, 0.0 /* timeout */, callback_fn, opts, cost_fn); + copy_state->setFromIK(joint_model_group, target_pose, 0.0 /* timeout */, callback_fn, opts); std::vector solution; current_state->copyJointGroupPositions(joint_model_group, solution); @@ -193,7 +192,7 @@ int main(int argc, char** argv) visual_tools.deleteAllMarkers(); visual_tools.publishAxisLabeled(target_pose, "goal_pose"); visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); + visual_tools.publishTrajectoryLine(my_plan.trajectory, joint_model_group); visual_tools.trigger(); // Uncomment if you would like to execute the motion From 7e362ac57e904860a25fe3fd26724d80330f51f8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 24 Apr 2023 09:15:22 +0200 Subject: [PATCH 3/7] Update doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../launch/kinematics_cost_function_tutorial.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py b/doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py index 52c21e0715..2a8678366d 100644 --- a/doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py +++ b/doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py @@ -15,6 +15,7 @@ def generate_launch_description(): moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, + moveit_config.joint_limits, ], ) From acb4b513e985ad1fbaff5bfb597d975d753e87c8 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 24 Apr 2023 18:36:43 +0200 Subject: [PATCH 4/7] Update doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> --- .../src/kinematics_cost_function_tutorial.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp b/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp index b9a637c35e..67e8a58b45 100644 --- a/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp +++ b/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp @@ -216,7 +216,7 @@ int main(int argc, char** argv) auto start_state = move_group.getCurrentState(10.0); std::vector traj; moveit::core::MaxEEFStep max_eef_step(0.01, 0.1); - // Here, were effectively disabling the jump threshold for joints. This is not recommended on real hardware. + // Here, we're effectively disabling the jump threshold for joints. This is not recommended on real hardware. moveit::core::JumpThreshold jump_thresh(0.0); // The trajectory, traj, passed to computeCartesianPath will contain several waypoints toward From 7f8e5f3c4bf9116bc941b95c700fb54a9397bc23 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 24 Apr 2023 18:50:07 +0200 Subject: [PATCH 5/7] Move to how to guides --- CMakeLists.txt | 2 +- doc/examples/examples.rst | 1 - doc/how_to_guides/how_to_guides.rst | 1 + .../kinematics_cost_function/CMakeLists.txt | 0 .../kinematics_cost_function_tutorial.rst | 0 .../launch/kinematics_cost_function_tutorial.launch.py | 0 .../src/kinematics_cost_function_tutorial.cpp | 0 7 files changed, 2 insertions(+), 2 deletions(-) rename doc/{examples => how_to_guides}/kinematics_cost_function/CMakeLists.txt (100%) rename doc/{examples => how_to_guides}/kinematics_cost_function/kinematics_cost_function_tutorial.rst (100%) rename doc/{examples => how_to_guides}/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py (100%) rename doc/{examples => how_to_guides}/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 27f9e0f73e..a14778b70a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,8 +63,8 @@ add_subdirectory(doc/examples/pilz_industrial_motion_planner) add_subdirectory(doc/examples/planning_scene_ros_api) add_subdirectory(doc/examples/planning_scene) add_subdirectory(doc/examples/realtime_servo) -add_subdirectory(doc/examples/kinematics_cost_function) add_subdirectory(doc/examples/robot_model_and_robot_state) +add_subdirectory(doc/how_to_guides/kinematics_cost_function) add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) add_subdirectory(doc/how_to_guides/parallel_planning) add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor) diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index 5e7cdfb8e3..293dea1d05 100644 --- a/doc/examples/examples.rst +++ b/doc/examples/examples.rst @@ -38,7 +38,6 @@ Building more complex applications with MoveIt often requires developers to dig moveit_cpp/moveitcpp_tutorial bullet_collision_checker/bullet_collision_checker mobile_base_arm/mobile_base_arm_tutorial - kinematics_cost_function/kinematics_cost_function_tutorial Using MoveIt Directly Through the Python API -------------------------------------------- diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index 98a15e747a..333a163e7b 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -10,6 +10,7 @@ These how-to guides will help you quickly solve specific problems using MoveIt. how_to_generate_api_doxygen_locally how_to_setup_docker_containers_in_ubuntu how_to_write_doxygen + kinematics_cost_function/kinematics_cost_function_tutorial using_ompl_constrained_planning/ompl_constrained_planning parallel_planning/parallel_planning_tutorial controller_teleoperation/controller_teleoperation diff --git a/doc/examples/kinematics_cost_function/CMakeLists.txt b/doc/how_to_guides/kinematics_cost_function/CMakeLists.txt similarity index 100% rename from doc/examples/kinematics_cost_function/CMakeLists.txt rename to doc/how_to_guides/kinematics_cost_function/CMakeLists.txt diff --git a/doc/examples/kinematics_cost_function/kinematics_cost_function_tutorial.rst b/doc/how_to_guides/kinematics_cost_function/kinematics_cost_function_tutorial.rst similarity index 100% rename from doc/examples/kinematics_cost_function/kinematics_cost_function_tutorial.rst rename to doc/how_to_guides/kinematics_cost_function/kinematics_cost_function_tutorial.rst diff --git a/doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py b/doc/how_to_guides/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py similarity index 100% rename from doc/examples/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py rename to doc/how_to_guides/kinematics_cost_function/launch/kinematics_cost_function_tutorial.launch.py diff --git a/doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp b/doc/how_to_guides/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp similarity index 100% rename from doc/examples/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp rename to doc/how_to_guides/kinematics_cost_function/src/kinematics_cost_function_tutorial.cpp From da48a7b25963771447f5a9526f14cea69e0a1de1 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 24 Apr 2023 18:51:06 +0200 Subject: [PATCH 6/7] Update links --- .../kinematics_cost_function_tutorial.rst | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/doc/how_to_guides/kinematics_cost_function/kinematics_cost_function_tutorial.rst b/doc/how_to_guides/kinematics_cost_function/kinematics_cost_function_tutorial.rst index 2fbf1ba318..0b34783f98 100644 --- a/doc/how_to_guides/kinematics_cost_function/kinematics_cost_function_tutorial.rst +++ b/doc/how_to_guides/kinematics_cost_function/kinematics_cost_function_tutorial.rst @@ -45,10 +45,10 @@ In RViz, we should be able to see the following: The Entire Code --------------- -The entire code can be seen :codedir:`here in the MoveIt GitHub project`. Next, we step through the code piece by piece to explain its functionality. +The entire code can be seen :codedir:`here in the MoveIt GitHub project`. Next, we step through the code piece by piece to explain its functionality. .. tutorial-formatter:: ./src/kinematics_cost_function_tutorial.cpp The Launch File --------------- -The entire launch file is :codedir:`here` on GitHub. All the code in this tutorial can be run from the **moveit2_tutorials** package that you have as part of your MoveIt setup. +The entire launch file is :codedir:`here` on GitHub. All the code in this tutorial can be run from the **moveit2_tutorials** package that you have as part of your MoveIt setup. From 1bfc7ecf815931d91b0db6713d7081559bf285ff Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Mon, 24 Apr 2023 18:54:25 +0200 Subject: [PATCH 7/7] Cleanup --- CMakeLists.txt | 1 - doc/how_to_guides/how_to_guides.rst | 5 +---- 2 files changed, 1 insertion(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b2b818b4c..3733ad3a47 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,7 +63,6 @@ add_subdirectory(doc/examples/planning_scene) add_subdirectory(doc/examples/realtime_servo) add_subdirectory(doc/examples/robot_model_and_robot_state) add_subdirectory(doc/how_to_guides/kinematics_cost_function) -add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning) add_subdirectory(doc/how_to_guides/parallel_planning) add_subdirectory(doc/how_to_guides/persistent_scenes_and_states) add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner) diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index d36a4732b9..5927f22d5f 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -11,6 +11,7 @@ Configuring and Using MoveIt .. toctree:: :maxdepth: 1 + kinematics_cost_function/kinematics_cost_function_tutorial moveit_configuration/moveit_configuration_tutorial pilz_industrial_motion_planner/pilz_industrial_motion_planner using_ompl_constrained_planning/ompl_constrained_planning @@ -28,7 +29,3 @@ Developing and Documenting MoveIt how_to_generate_api_doxygen_locally how_to_setup_docker_containers_in_ubuntu how_to_write_doxygen - kinematics_cost_function/kinematics_cost_function_tutorial - using_ompl_constrained_planning/ompl_constrained_planning - parallel_planning/parallel_planning_tutorial - controller_teleoperation/controller_teleoperation