From 41400e042ba79f3263f36aa32d0afa1afd8c114e Mon Sep 17 00:00:00 2001 From: ibrahiminfinite Date: Tue, 8 Aug 2023 21:34:40 +0530 Subject: [PATCH] Review updates --- doc/examples/realtime_servo/CMakeLists.txt | 2 +- .../config/panda_simulated_config.yaml | 2 +- .../realtime_servo_tutorial.rst | 132 ++++++++++++------ .../src/pose_tracking_tutorial.cpp | 75 ++++++++-- 4 files changed, 160 insertions(+), 51 deletions(-) diff --git a/doc/examples/realtime_servo/CMakeLists.txt b/doc/examples/realtime_servo/CMakeLists.txt index 175f5314f8..a33cf18998 100644 --- a/doc/examples/realtime_servo/CMakeLists.txt +++ b/doc/examples/realtime_servo/CMakeLists.txt @@ -4,7 +4,7 @@ ament_target_dependencies(pose_tracking_tutorial ${THIS_PACKAGE_INCLUDE_DEPENDS install( TARGETS - pose_tracking_tutorial + pose_tracking_tutorial DESTINATION lib/${PROJECT_NAME} ) diff --git a/doc/examples/realtime_servo/config/panda_simulated_config.yaml b/doc/examples/realtime_servo/config/panda_simulated_config.yaml index 36263b37a0..35e8eedd25 100644 --- a/doc/examples/realtime_servo/config/panda_simulated_config.yaml +++ b/doc/examples/realtime_servo/config/panda_simulated_config.yaml @@ -46,7 +46,7 @@ ee_frame: panda_hand # The name of the end effector link, used to return the EE lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. -leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) +leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity. ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/doc/examples/realtime_servo/realtime_servo_tutorial.rst b/doc/examples/realtime_servo/realtime_servo_tutorial.rst index 654650f0b1..186b7239ee 100644 --- a/doc/examples/realtime_servo/realtime_servo_tutorial.rst +++ b/doc/examples/realtime_servo/realtime_servo_tutorial.rst @@ -3,11 +3,18 @@ Realtime Servo MoveIt Servo facilitates realtime control of your robot arm. -Servo accepts any of the following types of commands: + +.. raw:: html + +
+ +
+ +MoveIt Servo accepts any of the following types of commands: 1. Individual joint velocities. - 2. The desired velocity of end-effector. - 3. The desired pose of end-effector. + 2. The desired velocity of end effector. + 3. The desired pose of end effector. This enables teleoperation via a wide range of input schemes, or for other autonomous software to control the robot - in visual servoing or closed loop position control for instance. @@ -20,29 +27,81 @@ If you haven't already done so, make sure you've completed the steps in :doc:`Ge Design overview --------------- -Moveit Servo consists of two main parts, the core implementation ``Servo`` which provides a C++ interface, and the ``ServoNode`` which -wraps the C++ interface and provides a ROS interface. - -The configuration of Servo is done through ROS parameters specified in `servo_parameters.yaml `_ +Moveit Servo consists of two main parts: The core implementation ``Servo`` which provides a C++ interface, and the ``ServoNode`` which +wraps the C++ interface and provides a ROS interface.The configuration of Servo is done through ROS parameters specified in :moveit_codedir:`servo_parameters.yaml ` In addition to the servoing capability, MoveIt Servo has some convenient features such as: - Checking for singularities - Checking for collisions + - Motion smoothing + - Joint position and velocity limits enforced + +Singularity checking and collision checking are safety features that scale down the velocities when approaching singularities or collisions (self collision or collision with other objects). +The collision checking and smoothing are optional features that can be disabled using the ``check_collisions`` parameter and the ``use_smoothing`` parameters respectively. + +The inverse kinematics is handled through either the inverse Jacobain or the robot's IK solver if one was provided. + + +Inverse Kinematics in Servo +^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Inverse Kinematics may be handled internally by MoveIt Servo via inverse Jacobian calculations. However, you may also use an IK plugin. +To configure an IK plugin for use in MoveIt Servo, your robot config package must define one in a :code:`kinematics.yaml` file, such as the one +in the :moveit_resources_codedir:`Panda config package `. +Several IK plugins are available :moveit_codedir:`within MoveIt `, as well as `externally `_. +:code:`bio_ik/BioIKKinematicsPlugin` is the most common choice. + +Once your :code:`kinematics.yaml` file has been populated, include it with the ROS parameters passed to the servo node in your launch file: + +.. code-block:: python + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + servo_node = Node( + package="moveit_servo", + executable="servo_node", + parameters=[ + servo_params, + low_pass_filter_coeff, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, # here is where kinematics plugin parameters are passed + ], + ) + + +The above excerpt is taken from :moveit_codedir:`servo_example.launch.py ` in MoveIt. +In the above example, the :code:`kinematics.yaml` file is taken from the :moveit_resources_codedir:`moveit_resources ` repository in the workspace, specifically :code:`moveit_resources/panda_moveit_config/config/kinematics.yaml`. +The actual ROS parameter names that get passed by loading the yaml file are of the form :code:`robot_description_kinematics..`, e.g. :code:`robot_description_kinematics.panda_arm.kinematics_solver`. + +Since :code:`moveit_servo` does not allow undeclared parameters found in the :code:`kinematics.yaml` file to be set on the Servo node, custom solver parameters need to be declared from inside your plugin code. + +For example, :code:`bio_ik` defines a :code:`getROSParam()` function in `bio_ik/src/kinematics_plugin.cpp `_ that declares parameters if they're not found on the Servo Node. + +Setup on a New Robot +-------------------- + +The bare minimum requirements for running MoveIt Servo with your robot include: + 1. A valid URDF and SRDF of the robot. + 2. A controller that can accept joint positions or velocities. + 3. Joint encoders that provide rapid and accurate joint position feedback. + +Because the kinematics are handled by the core parts of MoveIt, it is recommended that you have a valid config package for your robot and you can run the demo launch file included with it. -The collision checking is an optional feature that can be disabled using the ``check_collisions`` parameter. Using the C++ API ------------------ -The C++ interface allows the user to create and utilize an instance of Servo within their C++ programs. -This can be beneficial when there is a performance requirement to avoid the overhead of ROS communication infrastucture, -or when the output generated by Servo needs to be fed into some other controller that does not have a ROS interface. +This can be beneficial when there is a performance requirement to avoid the overhead of ROS communication infrastucture, or when the output generated by Servo needs to be fed into some other controller that does not have a ROS interface. -When using Servo with the C++ interface the three input command types are ``JointJogCommand``, ``TwistCommand`` and ``PoseCommand``. +When using MoveIt Servo with the C++ interface the three input command types are ``JointJogCommand``, ``TwistCommand`` and ``PoseCommand``. The output from Servo when using the C++ interface is ``KinematicState``, a struct containing joint names, positions, velocities and accelerations. -As given by the definitions in `datatypes `_ header file. +As given by the definitions in :moveit_codedir:`datatypes ` header file. -The first step is to create a Servo instance. +The first step is to create a ``Servo`` instance. .. code-block:: c++ @@ -50,27 +109,19 @@ The first step is to create a Servo instance. #include #include -.. code-block:: c++ - // The node to be used by Servo. rclcpp::Node::SharedPtr node = std::make_shared("servo_tutorial"); -.. code-block:: c++ - // Get the Servo parameters. const std::string param_namespace = "moveit_servo"; const std::shared_ptr servo_param_listener = std::make_shared(node, param_namespace); const servo::Params servo_params = servo_param_listener->get_params(); -.. code-block:: c++ - // Create the planning scene monitor. const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor = createPlanningSceneMonitor(node, servo_params); -.. code-block:: c++ - // Create a Servo instance. Servo servo = Servo(node, servo_param_listener, planning_scene_monitor); @@ -129,16 +180,16 @@ Using the PoseCommand The ``next_joint_state`` result can then be used for further steps in the control pipeline. -The status of Servo resulting from the last command can be obtained by: +The status of MoveIt Servo resulting from the last command can be obtained by: .. code-block:: c++ StatusCode status = servo.getStatus(); -The satus can be used as input for various decisions. +The user can use status for higher-level decision making. -See `moveit_servo/demos `_ for complete examples of using the C++ interface. -The demos can be launched using the launch files found in `moveit_servo/launch `_. +See :moveit_codedir:`moveit_servo/demos ` for complete examples of using the C++ interface. +The demos can be launched using the launch files found in :moveit_codedir:`moveit_servo/launch `. - ``ros2 launch moveit_servo demo_joint_jog.launch.py`` - ``ros2 launch moveit_servo demo_twist.launch.py`` @@ -148,31 +199,32 @@ The demos can be launched using the launch files found in `moveit_servo/launch < Using the ROS API ----------------- -To use Servo through the ROS interface, it must be launched as a ``Node`` or ``Component`` along with the required parameters as seen `here `_. +To use MoveIt Servo through the ROS interface, it must be launched as a ``Node`` or ``Component`` along with the required parameters as seen :moveit_codedir:`here `. -When using Servo with the ROS interface the commands are ROS messages of the following types published to respective topics specified by the Servo parameters. +When using MoveIt Servo with the ROS interface the commands are ROS messages of the following types published to respective topics specified by the Servo parameters. - 1. ``control_msgs::msg::JointJog`` on the topic specified by ``joint_command_in_topic`` parameter. - 2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by ``cartesian_command_in_topic`` parameter. - 3. ``geometry_msgs::msg::PoseStamped`` on the topic specified by ``pose_command_in_topic`` parameter. + 1. ``control_msgs::msg::JointJog`` on the topic specified by the ``joint_command_in_topic`` parameter. + 2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by the ``cartesian_command_in_topic`` parameter. For now, the twist message must be in the planning frame of the robot. (This will be updated soon.) + 3. ``geometry_msgs::msg::PoseStamped`` on the topic specified by the ``pose_command_in_topic`` parameter. -The output from Servo can either be ``trajectory_msgs::msg::JointTrajectory`` or ``std_msgs::msg::Float64MultiArray`` +Twist and Pose commands require that the ``header.frame_id`` is always specified. +The output from ``ServoNode`` (the ROS interface) can either be ``trajectory_msgs::msg::JointTrajectory`` or ``std_msgs::msg::Float64MultiArray`` selected using the *command_out_type* parameter, and published on the topic specified by *command_out_topic* parameter. -The command type for Servo can be selected using the ``ServoCommandType`` service, see definition `ServoCommandType `_. +The command type can be selected using the ``ServoCommandType`` service, see definition :moveit_msgs_codedir:`ServoCommandType `. -From cli : ``ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"`` +From cli : ``ros2 service call //switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"`` Programmatically: .. code-block:: c++ - swith_input_client = node->create_client("servo_node/switch_command_type"); + switch_input_client = node->create_client("//switch_command_type"); auto request = std::make_shared(); request->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST; - if (swith_input_client->wait_for_service(std::chrono::seconds(1))) + if (switch_input_client->wait_for_service(std::chrono::seconds(1))) { - auto result = swith_input_client->async_send_request(request); + auto result = switch_input_client->async_send_request(request); if (result.get()->success) { RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Twist"); @@ -183,9 +235,9 @@ Programmatically: } } -Similarly, Servo can be paused using the pause service of type ``std_msgs::srv::SetBool``. +Similarly, servoing can be paused using the pause service ``/pause_servo`` of type ``std_msgs::srv::SetBool``. -When using the ROS interface, the status of Servo is available on the topic ``/servo_node/status``, see definition `ServoStatus `_. +When using the ROS interface, the status of Servo is available on the topic ``//status``, see definition :moveit_msgs_codedir:`ServoStatus `. Launch ROS interface demo: ``ros2 launch moveit_servo demo_ros_api.launch.py``. @@ -193,4 +245,4 @@ Once the demo is running, the robot can be teleoperated through the keyboard. Launch the keyboard demo: ``ros2 run moveit_servo servo_keyboard_input``. -An example of using the pose commands in the context of servoing to open a door can be seen in this `example `_. +An example of using the pose commands in the context of servoing to open a door can be seen in this :codedir:`example `. diff --git a/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp b/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp index 8a40f17ca1..a32bf45cb4 100644 --- a/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp +++ b/doc/examples/realtime_servo/src/pose_tracking_tutorial.cpp @@ -1,4 +1,44 @@ - +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, 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. + *********************************************************************/ + +/* Title : pose_tracking_tutorial.cpp + * Project : moveit2_tutorials + * Created : 08/07/2023 + * Author : V Mohammed Ibrahim + * + * Description : Example of using pose tracking via the ROS API in a door opening scenario. + */ #include #include @@ -9,6 +49,9 @@ #include #include +/** + * \brief Handles the simulation of the collision object representing the door. + */ class Door { public: @@ -82,6 +125,9 @@ class Door shape_msgs::msg::SolidPrimitive door_primitive_; }; +/** + * \brief Generates the path to follow when opening the door. + */ std::vector getPath() { const double start_angle = M_PI / 2 + (M_PI / 8); @@ -99,6 +145,9 @@ std::vector getPath() return traj; } +/** + * \brief Creates an Rviz marker message to represent a waypoint in the path. + */ visualization_msgs::msg::Marker getMarker(int id, const Eigen::Vector3d& position, const std::string& frame) { visualization_msgs::msg::Marker marker; @@ -125,6 +174,9 @@ visualization_msgs::msg::Marker getMarker(int id, const Eigen::Vector3d& positio return marker; } +/** + * \brief Generates a PoseStamped message with the given position and orientation. + */ geometry_msgs::msg::PoseStamped getPose(const Eigen::Vector3d& position, const Eigen::Quaterniond& rotation) { geometry_msgs::msg::PoseStamped target_pose; @@ -194,29 +246,34 @@ int main(int argc, char* argv[]) const double publish_period = 0.15; rclcpp::WallRate rate(1 / publish_period); - size_t i = path.size() - 1; - while (i > 0) + // The path needs to be reversed since the last point in the path is where we want to start. + std::reverse(path.begin(), path.end()); + + for (auto& waypoint : path) { - auto target_pose = getPose(path[i], Eigen::Quaterniond(ee_pose.rotation())); + auto target_pose = getPose(waypoint, Eigen::Quaterniond(ee_pose.rotation())); target_pose.header.stamp = node->now(); pose_publisher->publish(target_pose); rate.sleep(); - i--; } + // Simulated door opening + double door_angle = M_PI / 2; const double step = 0.01745329; // 1 degree in radian - i = 0; - while (i < path.size()) + + // Reverse again to so that we follow that path in reverse order. + std::reverse(path.begin(), path.end()); + for (auto& waypoint : path) { ee_pose.rotate(Eigen::AngleAxisd(-step, Eigen::Vector3d::UnitZ())); - auto target_pose = getPose(path[i], Eigen::Quaterniond(ee_pose.rotation())); + auto target_pose = getPose(waypoint, Eigen::Quaterniond(ee_pose.rotation())); target_pose.header.stamp = node->now(); pose_publisher->publish(target_pose); rate.sleep(); + door.rotateDoor(door_angle); door_angle += step; - i++; } executor->cancel();