├── .gitignore ├── LICENSE ├── MoveIt! Tutorial.md ├── README.md └── assets ├── 1557726877702.png ├── 1557726886051.png ├── 1557726910810.png ├── 1557727013881.png ├── 1557727232441.png ├── 1557727472789.png ├── 1557734257255.png ├── 1563791835579.png ├── COFFEE BUTTON ヾ(°∇°^).png ├── MoveIt_Launchfile_Structure.png ├── Overview.0012.jpg └── moveit_logo.png /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | *.no_toc 3 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2019, methylDragon 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /MoveIt! Tutorial.md: -------------------------------------------------------------------------------- 1 | # MoveIt! Crash Course 2 | 3 | Author: methylDragon 4 | Some personal notes regarding the use of the MoveIt! package 5 | Just following through tutorials and filling in gaps or other handy information along the way 6 | I'll be adapting it from the MoveIt! Tutorials: 7 | 8 | ------ 9 | 10 | ## Pre-Requisites 11 | 12 | - A system with Ubuntu 18.04 installed 13 | - I'll assume we're using ROS Melodic 14 | - ROS knowledge 15 | - Catkin knowledge 16 | - Linux 17 | - Python 3 and/or C++ 18 | 19 | 20 | 21 | ## Table Of Contents 22 | 23 | 1. [Introduction](#1) 24 | 2. [Setup and Basic Concepts](#2) 25 | 2.1 [Installing MoveIt!](#2.1) 26 | 2.2 [Basic Concepts](#2.2) 27 | 2.3 [MoveIt! In RViz](#2.3) 28 | 2.4 [Launchfile Structure](#2.4) 29 | 2.5 [Command Line Interface](#2.5) 30 | 3. [C++ Move Group Interface](#3) 31 | 3.1 [Import Dependencies](#3.1) 32 | 3.2 [Define Planning Groups](#3.2) 33 | 3.3 [Obtain Basic Information](#3.3) 34 | 3.4 [Setting Start State](#3.4) 35 | 3.5 [Assign and Plan for End Effector Pose Goal](#3.5) 36 | 3.6 [Move to Goal (Execute Plan)](#3.6) 37 | 3.7 [Assign and Plan for Joint-Space Pose Goal](#3.7) 38 | 3.8 [Assign and Plan for Cartesian Paths](#3.8) 39 | 3.9 [Planning Constraints](#3.9) 40 | 3.10 [Collision Objects](#3.10) 41 | 3.11 [Setting Tolerances](#3.11) 42 | 3.12 [Visualisation Tools](#3.12) 43 | 4. [Python Move Group Interface](#4) 44 | 4.1 [Import Dependencies](#4.1) 45 | 4.2 [Define Planning Groups](#4.2) 46 | 4.3 [Obtain Basic Information](#4.3) 47 | 4.4 [Setting Start State](#4.4) 48 | 4.5 [Assign and Plan for End Effector Pose Goal](#4.5) 49 | 4.6 [Move to Goal (Execute Plan)](#4.6) 50 | 4.7 [Assign and Plan for Joint-Space Pose Goal](#4.7) 51 | 4.8 [Assign and Plan for Cartesian Paths](#4.8) 52 | 4.9 [Planning Constraints](#4.9) 53 | 4.10 [Collision Objects](#4.10) 54 | 4.11 [Visualisation Tools](#4.11) 55 | 56 | 57 | 58 | 59 | ## 1. Introduction 60 | 61 | The MoveIt! project is a ROS package that helps with motion planning for robotic manipulators. As such, it is especially useful for working with and planning for arms! 62 | 63 | Features: 64 | 65 | > - Motion Planning 66 | > - Generate high-degree of freedom trajectories through cluttered environments and avoid local minimums 67 | > - Manipulation 68 | > - Analyze and interact with your environment with grasp generation 69 | > - Inverse Kinematics 70 | > - Solve for joint positions for a given pose, even in over-actuated arms 71 | > - Control 72 | > - Execute time-parameterized joint trajectories to low level hardware controllers through common interfaces 73 | > - 3D Perception 74 | > - Connect to depth sensors and point clouds with Octomaps 75 | > - Collision Checking 76 | > - Avoid obstacles using geometric primitives, meshes, or point cloud data 77 | > 78 | > 79 | 80 | This tutorial will only cover the basics for MoveIt!, up to and including the move_group interface. It won't cover the implementations of that interface, or any other deeper APIs, such as the pick and place pipeline, or time parametrisation. 81 | 82 | 83 | 84 | ## 2. Setup and Basic Concepts 85 | 86 | ### 2.1 Installing MoveIt! 87 | [go to top](#top) 88 | 89 | 90 | ```shell 91 | $ sudo apt install ros-melodic-moveit 92 | ``` 93 | 94 | 95 | 96 | ### 2.2 Basic Concepts 97 | [go to top](#top) 98 | 99 | 100 | Source: 101 | 102 | #### **System Architecture** 103 | 104 | ![img](assets/Overview.0012.jpg) 105 | 106 | #### **Move Group** 107 | 108 | > ### The move_group node 109 | > 110 | > The figure above shows the high-level system architecture for the primary node provided by MoveIt called [move_group](http://docs.ros.org/melodic/api/moveit_ros_move_group/html/move__group_8cpp.html). This node serves as an integrator: pulling all the individual components together to provide a set of ROS actions and services for users to use. 111 | 112 | #### **User Interfaces** 113 | 114 | - C++ API 115 | - [move_group_interface]() Package 116 | - Python API 117 | - [moveit_commander]() Package 118 | - GUI 119 | - Motion Planning plugin on RViz 120 | 121 | #### **Configuration** 122 | 123 | - URDF 124 | - From robot_description parameter 125 | - SRDF 126 | - From robot_description_semantic parameter 127 | - Other parameters 128 | - Joint limits, kinematics, motion planning, perception, etc. 129 | 130 | #### **Robot Interfaces** 131 | 132 | - Joint state 133 | - Keeps track of the state of each joint; represented on the `/joint_states` topic 134 | - You need a joint_state_publisher node to publish these 135 | - Transform 136 | - TF transforms 137 | - You need a robot_state_publisher to handle these 138 | - Controllers 139 | - ROS action interface. A server on the robot needs to service the action via the `FollowJointTrajectoryAction` interface 140 | - MoveIt will create the Client only 141 | - Planning Scene 142 | - The planning scene represents the world and the current state of the robot. (Carried objects by the robot are assumed to be rigidly attached to the robot.) 143 | - Extensibilty 144 | - Pick and place, kinematics, and motion planners are implemented as plugins. 145 | - Plugins are configurable using .yaml parameters. 146 | 147 | #### **Motion Planning** 148 | 149 | > ### **The Motion Planning Plugin** 150 | > 151 | > MoveIt works with motion planners through a *plugin* interface. This allows MoveIt to communicate with and use different motion planners from multiple libraries, making MoveIt easily extensible. The interface to the motion planners is through a ROS Action or service (offered by the *move_group* node). The default motion planners for move_group are configured using OMPL and the MoveIt interface to OMPL by the MoveIt Setup Assistant. 152 | > 153 | > ### **The Motion Plan Request** 154 | > 155 | > The motion plan request clearly specifies what you would like the motion planner to do. Typically, you will be asking the motion planner to move an arm to a different location (in joint space) or the end-effector to a new pose. Collisions are checked for by default (including self-collisions). You can *attach* an object to the end-effector (or any part of the robot), e.g. if the robot picks up an object. This allows the motion planner to account for the motion of the object while planning paths. You can also specify constraints for the motion planner to check - the inbuilt constraints provided by MoveIt are *kinematic constraints*: 156 | > 157 | > - Position constraints - restrict the position of a link to lie within a region of space 158 | > - Orientation constraints - restrict the orientation of a link to lie within specified roll, pitch or yaw limits 159 | > - Visibility constraints - restrict a point on a link to lie within the visibility cone for a particular sensor 160 | > - Joint constraints - restrict a joint to lie between two values 161 | > - User-specified constraints - you can also specify your own constraints with a user-defined callback. 162 | > 163 | > ### **The Motion Plan Result** 164 | > 165 | > The move*group node will generate a desired trajectory in response to your motion plan request. This trajectory will move the arm (or any group of joints) to the desired location. Note that the result coming out of move_group is a trajectory and not just a path - _move_group* will use the desired maximum velocities and accelerations (if specified) to generate a trajectory that obeys velocity and acceleration constraints at the joint level. 166 | > 167 | > 168 | 169 | There's also a motion planning pipeline! There's pre-processing (correction of the robot before planning, or specification of the workspace), and post-processing (refinement of the motion plan, time parameterisation for the plan, for example.) 170 | 171 | 172 | 173 | I've covered a couple of basics, but there's a lot more to cover! Check out the [link]()! 174 | 175 | 176 | 177 | ### 2.3 MoveIt! In RViz 178 | [go to top](#top) 179 | 180 | 181 | Source: 182 | 183 | #### **Run the Demo Launch File** 184 | 185 | ```shell 186 | $ roslaunch panda_moveit_config demo.launch rviz_tutorial:=true 187 | ``` 188 | 189 | It should open up an empty Rviz world. Click `Add` and select the `MotionPlanning` display type to add the controller. 190 | 191 | 192 | 193 | #### **Configure the MotionPlanning interface** 194 | 195 | > - Once you have the Motion Planning Plugin loaded, we can configure it. In the “Global Options” tab of the “Displays” subwindow, set the **Fixed Frame** field to `/panda_link0` 196 | > - Now, you can start configuring the Plugin for your robot (the Panda in this case). Click on “MotionPlanning” within “Displays”. 197 | > - Make sure the **Robot Description** field is set to `robot_description`. 198 | > - Make sure the **Planning Scene Topic** field is set to `/planning_scene`. Click on topic name to expose topic-name drop-down. 199 | > - In **Planning Request**, change the **Planning Group** to `panda_arm`. 200 | > - In **Planned Path**, change the **Trajectory Topic** to `/move_group/display_planned_path`. 201 | 202 | 203 | 204 | #### **Interact with the Arm!** 205 | 206 | Now you can play with the arm by selecting the `Interact` tool and moving the arm where you want it to go. 207 | 208 | You'll be able to change both the start and ending state for the robot arm by moving the end manipulator around using the interact tool! 209 | 210 | - **Green** is the **start** state 211 | - **You won't be able to see the manipulatable start state by default!** 212 | - Enable it by checking `Query Start state` in the `Planning Request` tab 213 | - **Orange** is the **end** state 214 | - **Collisions** will be shown in **red** 215 | - You might also want to uncheck the `Show Robot Visual` checkbox in the `Scene Robot` tab to only leave the starting and ending state visuals 216 | 217 | ![1557726910810](assets/1557726910810.png) 218 | 219 | 220 | 221 | #### **Configure The Planners** 222 | 223 | The MotionPlanning plugin has a few nifty configuration settings that you can use. But the first more important step is to choose the planner to use. 224 | 225 | > **The default planner is pretty so-so.** I personally prefer to use **RRTConnect**. But this sort of thing is not a one-size fits all thing. 226 | > 227 | > ![1557726877702](assets/1557726877702.png) 228 | > 229 | > You can see the difference haha.. 230 | > 231 | > ![1557727232441](assets/1557727232441.png) 232 | 233 | You can then swap over to the `Planning` tab and configure a bunch of other parameters as well! 234 | 235 | ![1557727013881](assets/1557727013881.png) 236 | 237 | - **Use Cartesian Path** 238 | - The planner will only cause the manipulator to move in a striaght line 239 | - **Use Collision-Aware IK** 240 | - The planner will plan with collision avoidance in mind 241 | - Without this enabled the planner will be okay with paths that result in collisions (even self-collisions) occuring! 242 | 243 | 244 | 245 | #### **Planning** 246 | 247 | Now you can click on the shiny `Plan` button and watch the planned path play out in an animation. 248 | 249 | You can also enable the `Show Trail` checkbox in the `Planned Path` tab to show the trails. 250 | 251 | > If you enable the Slider, you can introspect the trajectory waypoints too! 252 | > 253 | > ![1557727472789](assets/1557727472789.png) 254 | 255 | > Do remember if you change the position of the end effector to **replan the path!** 256 | 257 | 258 | 259 | ### 2.4 Launchfile Structure 260 | [go to top](#top) 261 | 262 | 263 | ![MoveIt Launchfile Structure](assets/MoveIt_Launchfile_Structure.png) 264 | 265 | It's fairly complicated because there are a whole bunch of nodes and configuration files flying around. 266 | 267 | - **bringup.launch** 268 | - Brings up everything 269 | - In the demo it's called demo.launch 270 | - **planning_context.launch** 271 | - Defines the arm and all the relevant configurations for the planner to work with 272 | - Most of the configurations loaded here are found in the `/config` directory 273 | - **move_group.launch** 274 | - Sets up planners, executors, and sensor managers 275 | - Also sets up the action server that will take in and execute goals 276 | - Calls a bunch of other launch.xml files (which act as easier ways to call any defined launch files) 277 | - **moveit_rviz.launch** 278 | - Starts RViz 279 | - **TF and other miscellaneous nodes** 280 | - Helps to define the TF tree (especially important for a simulated robot with no true TF publishers) 281 | - Also helps keep track of the joint states 282 | - And does some relaying for relevant topics 283 | - Specifically joint_states to joint_states_desired 284 | 285 | 286 | 287 | ### 2.5 Command Line Interface 288 | [go to top](#top) 289 | 290 | 291 | ```shell 292 | # Start the command line interface 293 | $ rosrun moveit_commander moveit_commander_cmdline.py 294 | ``` 295 | 296 | Now you can connect and run commands! 297 | 298 | ```shell 299 | # Initialise group for commanding 300 | $ use 301 | 302 | # Get current state of group 303 | $ current 304 | 305 | # Record state (in this case as the variable c) 306 | $ rec c 307 | 308 | # Play back state 309 | $ goal = c 310 | $ go goal 311 | 312 | # Play back state but with modified joint value 313 | $ goal = c 314 | $ goal[0] = 0.2 315 | $ go goal 316 | 317 | # Plan then execute instead 318 | $ goal[0] = 0.2 319 | $ goal[1] = 0.2 320 | $ plan goal 321 | $ execute 322 | ``` 323 | 324 | 325 | 326 | ## 3. C++ Move Group Interface 327 | 328 | Source: 329 | 330 | Code Source: 331 | 332 | Of course, all this planning is useless without some programmatic way to call it for even more powerful interfacing with ROS! So let's get to it! 333 | 334 | > 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 interchangably. 335 | 336 | ### 3.1 Import Dependencies 337 | [go to top](#top) 338 | 339 | 340 | ```c++ 341 | #include 342 | #include 343 | 344 | #include 345 | #include 346 | 347 | #include 348 | #include 349 | 350 | #include 351 | ``` 352 | 353 | 354 | 355 | ### 3.2 Define Planning Groups 356 | [go to top](#top) 357 | 358 | 359 | ```c++ 360 | static const std::string PLANNING_GROUP = "panda_arm"; 361 | 362 | // Define Interfaces 363 | moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP); 364 | moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 365 | 366 | // Get Joint Model Group 367 | const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); 368 | ``` 369 | 370 | - The `MoveGroupInterface` is the object that you want to plan and control 371 | - The `PlanningSceneInterface` allows you to add and remove collision objects in the world 372 | 373 | 374 | 375 | ### 3.3 Obtain Basic Information 376 | [go to top](#top) 377 | 378 | 379 | ```c++ 380 | // Get robot reference frame 381 | ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str()); 382 | 383 | // Get end-effector link 384 | ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str()); 385 | 386 | // Get all groups in the robot 387 | ROS_INFO_NAMED("tutorial", "Available Planning Groups:"); 388 | std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), 389 | std::ostream_iterator(std::cout, ", ")); 390 | ``` 391 | 392 | 393 | 394 | ### 3.4 Setting Start State 395 | [go to top](#top) 396 | 397 | 398 | ```c++ 399 | // Create start state object 400 | robot_state::RobotState start_state(*move_group.getCurrentState()); 401 | geometry_msgs::Pose start_pose2; 402 | 403 | // Create starting pose 404 | start_pose2.orientation.w = 1.0; 405 | start_pose2.position.x = 0.55; 406 | start_pose2.position.y = -0.05; 407 | start_pose2.position.z = 0.8; 408 | 409 | // Calculate and set start state object 410 | start_state.setFromIK(joint_model_group, start_pose2); 411 | 412 | // Assign start state to move_group 413 | move_group.setStartState(start_state); 414 | ``` 415 | 416 | 417 | 418 | ### 3.5 Assign and Plan for End Effector Pose Goal 419 | [go to top](#top) 420 | 421 | 422 | ```c++ 423 | // Create Goal Pose 424 | geometry_msgs::Pose target_pose1; 425 | target_pose1.orientation.w = 1.0; 426 | target_pose1.position.x = 0.28; 427 | target_pose1.position.y = -0.2; 428 | target_pose1.position.z = 0.5; 429 | 430 | // Set Goal Pose 431 | move_group.setPoseTarget(target_pose1); 432 | 433 | // Create Planner 434 | moveit::planning_interface::MoveGroupInterface::Plan my_plan; 435 | 436 | // Check if Successful 437 | // (Note this DOES NOT move the robot! Just plans for the movement) 438 | bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); 439 | 440 | ROS_INFO_NAMED("tutorial", "Visualizing plan (pose goal) %s", success ? "" : "FAILED"); 441 | ``` 442 | 443 | > Compute a motion plan that takes the group declared in the constructor from the current state to the specified target. No execution is performed. The resulting plan is stored in *plan*. 444 | 445 | 446 | 447 | ### 3.6 Move to Goal (Execute Plan) 448 | [go to top](#top) 449 | 450 | 451 | ```c++ 452 | move_group.move(); 453 | ``` 454 | 455 | 456 | 457 | ### 3.7 Assign and Plan for Joint-Space Pose Goal 458 | [go to top](#top) 459 | 460 | 461 | ```c++ 462 | // Get current state object 463 | // This contains all the position, velocity, and acceleration data 464 | moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); 465 | 466 | // Get current state as a vector 467 | std::vector joint_group_positions; 468 | current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); 469 | 470 | // Edit the vector 471 | joint_group_positions[0] = -1.0; // radians 472 | 473 | // Set new target 474 | move_group.setJointValueTarget(joint_group_positions); 475 | 476 | // Plan and Check if Successful 477 | bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); 478 | 479 | ROS_INFO_NAMED("tutorial", "Visualizing plan (pose goal) %s", success ? "" : "FAILED"); 480 | ``` 481 | 482 | 483 | 484 | ### 3.8 Assign and Plan for Cartesian Paths 485 | [go to top](#top) 486 | 487 | 488 | Waypoint navigation! 489 | 490 | ```c++ 491 | // Create waypoints 492 | std::vector waypoints; 493 | waypoints.push_back(start_pose2); 494 | 495 | geometry_msgs::Pose target_pose3 = start_pose2; 496 | 497 | target_pose3.position.z -= 0.2; 498 | waypoints.push_back(target_pose3); // down 499 | 500 | target_pose3.position.y -= 0.2; 501 | waypoints.push_back(target_pose3); // right 502 | 503 | target_pose3.position.z += 0.2; 504 | target_pose3.position.y += 0.2; 505 | target_pose3.position.x -= 0.2; 506 | waypoints.push_back(target_pose3); // up and left 507 | 508 | // Reduce robot speed 509 | move_group.setMaxVelocityScalingFactor(0.1); 510 | 511 | // Set resolution 512 | moveit_msgs::RobotTrajectory trajectory; 513 | // No jump thresholding. THIS IS DANGEROUS ON A REAL ROBOT 514 | const double jump_threshold = 0.0; 515 | const double eef_step = 0.01; // 1cm resolution 516 | 517 | // Compute path 518 | double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); 519 | ROS_INFO_NAMED("tutorial", "Visualizing plan (Cartesian path) (%.2f%% acheived)", fraction * 100.0); 520 | ``` 521 | 522 | > Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. 523 | > 524 | > 525 | 526 | 527 | 528 | ### 3.9 Planning Constraints 529 | [go to top](#top) 530 | 531 | 532 | This particular constraint limits the end-effector to a single orientation. So the arm has to hold that orientation all the way. 533 | 534 | ```c++ 535 | // Create constraint 536 | moveit_msgs::OrientationConstraint ocm; 537 | ocm.link_name = "panda_link7"; 538 | ocm.header.frame_id = "panda_link0"; 539 | ocm.orientation.w = 1.0; 540 | ocm.absolute_x_axis_tolerance = 0.1; 541 | ocm.absolute_y_axis_tolerance = 0.1; 542 | ocm.absolute_z_axis_tolerance = 0.1; 543 | ocm.weight = 1.0; 544 | 545 | // Populate and set constraint 546 | moveit_msgs::Constraints test_constraints; 547 | test_constraints.orientation_constraints.push_back(ocm); 548 | move_group.setPathConstraints(test_constraints); 549 | 550 | // Give planner more time to plan 551 | move_group.setPlanningTime(10.0); 552 | 553 | // Then execute the new plan! 554 | move_group.setPoseTarget(target_pose1); 555 | success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); 556 | ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); 557 | 558 | // Visualise! 559 | visual_tools.deleteAllMarkers(); 560 | visual_tools.publishAxisLabeled(start_pose2, "start"); 561 | visual_tools.publishAxisLabeled(target_pose1, "goal"); 562 | visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE); 563 | visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); 564 | visual_tools.trigger(); 565 | visual_tools.prompt("next step"); 566 | ``` 567 | 568 | > Remember to clear the path constraint when you're done with it 569 | > 570 | > ```c++ 571 | > move_group.clearPathConstraints(); 572 | > ``` 573 | 574 | 575 | 576 | ### 3.10 Collision Objects 577 | [go to top](#top) 578 | 579 | 580 | ![1563791835579](assets/1563791835579.png) 581 | 582 | #### **Creating Collision Objects** 583 | 584 | > **Note:** Redeclaring collision objects under the same ID **replaces** the originally defined object! This is pretty handy! 585 | 586 | ```c++ 587 | // Create collision object 588 | moveit_msgs::CollisionObject collision_object; 589 | collision_object.header.frame_id = move_group.getPlanningFrame(); 590 | collision_object.id = "box1"; 591 | 592 | // Define 3D box 593 | shape_msgs::SolidPrimitive primitive; 594 | primitive.type = primitive.BOX; 595 | primitive.dimensions.resize(3); 596 | primitive.dimensions[0] = 0.4; 597 | primitive.dimensions[1] = 0.1; 598 | primitive.dimensions[2] = 0.4; 599 | 600 | geometry_msgs::Pose box_pose; 601 | box_pose.orientation.w = 1.0; 602 | box_pose.position.x = 0.4; 603 | box_pose.position.y = -0.2; 604 | box_pose.position.z = 1.0; 605 | 606 | // Populate the collision object data 607 | collision_object.primitives.push_back(primitive); 608 | collision_object.primitive_poses.push_back(box_pose); 609 | collision_object.operation = collision_object.ADD; 610 | 611 | std::vector collision_objects; 612 | collision_objects.push_back(collision_object); 613 | 614 | // Add collision object to the world 615 | ROS_INFO_NAMED("tutorial", "Add an object into the world"); 616 | planning_scene_interface.addCollisionObjects(collision_objects); 617 | ``` 618 | 619 | **Now you can plan a trajectory around the object!** Just plan normally, the arm will consider the object in place. 620 | 621 | #### **Attaching and Detaching Collision Objects** 622 | 623 | ```c++ 624 | // Attach 625 | move_group.attachObject(collision_object.id); 626 | 627 | // Detach 628 | move_group.detachObject(collision_object.id); 629 | ``` 630 | 631 | #### **Removing Collision Objects** 632 | 633 | ```c++ 634 | std::vector object_ids; 635 | object_ids.push_back(collision_object.id); 636 | planning_scene_interface.removeCollisionObjects(object_ids); 637 | ``` 638 | 639 | 640 | 641 | ### 3.11 Setting Tolerances 642 | [go to top](#top) 643 | 644 | 645 | > Note that this is for **planning not execution**! 646 | 647 | ```c++ 648 | move_group.setGoalJointTolerance(); 649 | move_group.setGoalOrientationTolerance(); 650 | move_group.setGoalPositionTolerance(); 651 | move_group.setGoalTolerance(); // This sets all tolerances 652 | 653 | move_group.setPlanningTime(); 654 | ``` 655 | 656 | 657 | 658 | ### 3.12 Visualisation Tools 659 | [go to top](#top) 660 | 661 | 662 | RViz has a lot of cool programmatic visualisation tools you can use too! 663 | 664 | ```c++ 665 | // Setup RViz Visualisation tools 666 | namespace rvt = rviz_visual_tools; // This is just an alias for the namespace 667 | moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); 668 | 669 | visual_tools.deleteAllMarkers(); // Reset marker 670 | visual_tools.loadRemoteControl(); // Initialise programmatic control 671 | 672 | // Publishing to visual_tools causes objects to appear in RViz 673 | // In this case we configure a text placeholder to display text above the arm 674 | Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); 675 | text_pose.translation().z() = 1.75; 676 | visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE); 677 | 678 | // Trigger causes the text to be displayed 679 | // It's like a publish call! 680 | visual_tools.trigger(); 681 | 682 | // Prompt for input 683 | visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); 684 | 685 | // Visualise a plan 686 | ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line"); 687 | visual_tools.publishAxisLabeled(target_pose1, "pose1"); 688 | visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE); 689 | visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); 690 | visual_tools.trigger(); 691 | ``` 692 | 693 | ![1557734257255](assets/1557734257255.png) 694 | 695 | 696 | 697 | ## 4. Python Move Group Interface 698 | 699 | Source: 700 | 701 | Code Source: 702 | 703 | ### 4.1 Import Dependencies 704 | [go to top](#top) 705 | 706 | 707 | ```python 708 | import sys 709 | import copy 710 | import rospy 711 | import moveit_commander 712 | import moveit_msgs.msg 713 | import geometry_msgs.msg 714 | from math import pi 715 | from std_msgs.msg import String 716 | from moveit_commander.conversions import pose_to_list 717 | from moveit_msgs.msg import RobotState, Constraints, OrientationConstraint 718 | ``` 719 | 720 | 721 | 722 | ### 4.2 Define Planning Groups 723 | [go to top](#top) 724 | 725 | 726 | ```python 727 | # Init node 728 | moveit_commander.roscpp_initialize(sys.argv) 729 | rospy.init_node('move_group_python_interface_tutorial', anonymous=True) 730 | 731 | # Define Interfaces 732 | robot = moveit_commander.RobotCommander() 733 | scene = moveit_commander.PlanningSceneInterface() 734 | 735 | # Define Move Group 736 | group_name = "panda_arm" 737 | move_group = moveit_commander.MoveGroupCommander(group_name) 738 | ``` 739 | 740 | - The `RobotCommander` is the object that contains information about the robot's state and kinematic model 741 | - The `PlanningSceneInterface` allows you to add and remove collision objects in the world 742 | - The `MoveGroupCommander` is the object that you use to control and plan for the robot 743 | 744 | 745 | 746 | ### 4.3 Obtain Basic Information 747 | [go to top](#top) 748 | 749 | 750 | ```python 751 | # Get reference frame 752 | planning_frame = move_group.get_planning_frame() 753 | 754 | # Get end-effector link 755 | eef_link = move_group.get_end_effector_link() 756 | 757 | # Get all groups in robot 758 | group_names = robot.get_group_names() 759 | 760 | # Get entire state 761 | robot.get_current_state() 762 | ``` 763 | 764 | 765 | 766 | ### 4.4 Setting Start State 767 | [go to top](#top) 768 | 769 | 770 | ```python 771 | # Create joint state object 772 | joint_state = JointState() 773 | joint_state.header = Header() 774 | joint_state.header.stamp = rospy.Time.now() 775 | joint_state.name = ['joint_a', 'joint_b'] 776 | joint_state.position = [0.17, 0.34] 777 | 778 | # Assign it to a robot state 779 | moveit_robot_state = RobotState() 780 | moveit_robot_state.joint_state = joint_state 781 | 782 | # Assign robot state to move_group 783 | move_group.set_start_state(moveit_robot_state) 784 | ``` 785 | 786 | 787 | 788 | ### 4.5 Assign and Plan for End Effector Pose Goal 789 | [go to top](#top) 790 | 791 | 792 | ```python 793 | # Create Goal Pose 794 | pose_goal = geometry_msgs.msg.Pose() 795 | pose_goal.orientation.w = 1.0 796 | pose_goal.position.x = 0.4 797 | pose_goal.position.y = 0.1 798 | pose_goal.position.z = 0.4 799 | 800 | # Set Goal Pose 801 | move_group.set_pose_target(pose_goal) 802 | 803 | # Obtain plan 804 | plan = move_group.plan() 805 | 806 | # Stop group (to prevent any residual movement) 807 | move_group.stop() 808 | 809 | # Clear targets 810 | move_group.clear_pose_targets() 811 | ``` 812 | 813 | 814 | 815 | ### 4.6 Move to Goal (Execute Plan) 816 | [go to top](#top) 817 | 818 | 819 | ```python 820 | # Execute a previously computed plan 821 | move_group.execute(plan, wait=True) 822 | 823 | # Plan, then execute the plan 824 | move_group.go(wait=True) 825 | move_group.go(joint_goal, wait=True) # Or for joint-space 826 | 827 | # Stop group (to prevent any residual movement) 828 | move_group.stop() 829 | ``` 830 | 831 | 832 | 833 | ### 4.7 Assign and Plan for Joint-Space Pose Goal 834 | [go to top](#top) 835 | 836 | 837 | ```python 838 | # Get current joint values 839 | joint_goal = move_group.get_current_joint_values() 840 | 841 | # Adjust some values 842 | joint_goal[0] = 0 843 | joint_goal[1] = -pi/4 844 | joint_goal[2] = 0 845 | joint_goal[3] = -pi/2 846 | joint_goal[4] = 0 847 | joint_goal[5] = pi/3 848 | joint_goal[6] = 0 849 | 850 | # Obtain plan 851 | plan = move_group.plan(joint_goal) 852 | ``` 853 | 854 | 855 | 856 | ### 4.8 Assign and Plan for Cartesian Paths 857 | [go to top](#top) 858 | 859 | 860 | Waypoint navigation! 861 | 862 | ```python 863 | # Create waypoints 864 | waypoints = [] 865 | 866 | wpose = move_group.get_current_pose().pose 867 | wpose.position.z -= scale * 0.1 # First move up (z) 868 | wpose.position.y += scale * 0.2 # and sideways (y) 869 | waypoints.append(copy.deepcopy(wpose)) 870 | 871 | wpose.position.x += scale * 0.1 # Second move forward/backwards in (x) 872 | waypoints.append(copy.deepcopy(wpose)) 873 | 874 | wpose.position.y -= scale * 0.1 # Third move sideways (y) 875 | waypoints.append(copy.deepcopy(wpose)) 876 | 877 | # Obtain plan 878 | (plan, fraction) = move_group.compute_cartesian_path(waypoints, # waypoints to follow 879 | 0.01, # eef_step 880 | 0.0) # jump_threshold 881 | 882 | # (DO NOT SET JUMP THRESHOLD TO 0 ON REAL ROBOT) 883 | ``` 884 | 885 | > Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. 886 | > 887 | > 888 | 889 | 890 | 891 | ### 4.9 Planning Constraints 892 | [go to top](#top) 893 | 894 | 895 | This particular constraint limits the end-effector to a single orientation. So the arm has to hold that orientation all the way. 896 | 897 | ```python 898 | # Create constraint 899 | orientation_constraint = OrientationConstraint() 900 | orientation_constraint.header = pose.header 901 | orientation_constraint.link_name = self.arm.get_end_effector_link() 902 | orientation_constraint.orientation = pose.pose.orientation 903 | orientation_constraint.absolute_x_axis_tolerance = 0.4 904 | orientation_constraint.absolute_y_axis_tolerance = 0.4 905 | orientation_constraint.absolute_z_axis_tolerance = 0.4 906 | orientation_constraint.weight = 1 907 | 908 | # Populate and set constraint 909 | upright_constraints = Constraints() 910 | upright_constraints.name = "upright" 911 | 912 | upright_constraints.orientation_constraints.append(orientation_constraint) 913 | 914 | # Enable constraints 915 | move_group.set_path_constraints(upright_constraints) 916 | 917 | # Disable constraints 918 | move_group.set_path_constraints(None) 919 | ``` 920 | 921 | 922 | 923 | ### 4.10 Collision Objects 924 | [go to top](#top) 925 | 926 | 927 | ![1563791835579](assets/1563791835579.png) 928 | 929 | #### **Creating Collision Objects** 930 | 931 | > **Note:** Redeclaring collision objects under the same ID **replaces** the originally defined object! This is pretty handy! 932 | 933 | ```python 934 | # Create collision object 935 | box_pose = geometry_msgs.msg.PoseStamped() 936 | box_pose.header.frame_id = "panda_leftfinger" 937 | box_pose.pose.orientation.w = 1.0 938 | box_pose.pose.position.z = 0.07 # slightly above the end effector 939 | box_name = "box" 940 | 941 | # Add collision object to the world 942 | scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1)) 943 | 944 | # Check if changes are reflected using the following tests! 945 | 946 | # Test if the box is attached 947 | attached_objects = scene.get_attached_objects([box_name]) 948 | is_attached = len(attached_objects.keys()) > 0 949 | 950 | # Test if the box is in the scene. 951 | # Note that attaching the box will remove it from known_objects 952 | is_known = box_name in scene.get_known_object_names() 953 | ``` 954 | 955 | **Now you can plan a trajectory around the object!** Just plan normally, the arm will consider the object in place. 956 | 957 | #### **Attaching and Detaching Collision Objects** 958 | 959 | ```python 960 | # Attach 961 | grasping_group = 'hand' 962 | touch_links = robot.get_link_names(group=grasping_group) 963 | scene.attach_box(eef_link, box_name, touch_links=touch_links) 964 | 965 | # Detach 966 | scene.remove_attached_object(eef_link, name=box_name) 967 | ``` 968 | 969 | #### **Removing Collision Objects** 970 | 971 | ```python 972 | scene.remove_world_object(box_name) 973 | ``` 974 | 975 | 976 | 977 | ### 4.11 Visualisation Tools 978 | [go to top](#top) 979 | 980 | 981 | RViz has a lot of cool programmatic visualisation tools you can use too! 982 | 983 | > Do note that the plan() method automatically publishes the visualisation for the robot already. 984 | 985 | ```python 986 | # Setup display trajectory publisher 987 | display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', 988 | moveit_msgs.msg.DisplayTrajectory, 989 | queue_size=20) 990 | 991 | # Create trajectory message 992 | display_trajectory = moveit_msgs.msg.DisplayTrajectory() 993 | display_trajectory.trajectory_start = robot.get_current_state() 994 | display_trajectory.trajectory.append(plan) 995 | 996 | # Publish 997 | display_trajectory_publisher.publish(display_trajectory); 998 | ``` 999 | 1000 | 1001 | 1002 | 1003 | ``` 1004 | . . 1005 | . |\-^-/| . 1006 | /| } O.=.O { |\ 1007 | ``` 1008 | 1009 | ​ 1010 | 1011 | ------ 1012 | 1013 | [![Yeah! Buy the DRAGON a COFFEE!](./assets/COFFEE%20BUTTON%20%E3%83%BE(%C2%B0%E2%88%87%C2%B0%5E).png)](https://www.buymeacoffee.com/methylDragon) -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MoveIt! Tutorial 2 | 3 | ![Brand](assets/moveit_logo.png) 4 | 5 | 6 | 7 | ## Introduction 8 | 9 | The MoveIt! project is a ROS package that helps with motion planning for robotic manipulators. As such, it is especially useful for working with and planning for arms! 10 | 11 | Features: 12 | 13 | > - Motion Planning 14 | > - Generate high-degree of freedom trajectories through cluttered environments and avoid local minimums 15 | > - Manipulation 16 | > - Analyze and interact with your environment with grasp generation 17 | > - Inverse Kinematics 18 | > - Solve for joint positions for a given pose, even in over-actuated arms 19 | > - Control 20 | > - Execute time-parameterized joint trajectories to low level hardware controllers through common interfaces 21 | > - 3D Perception 22 | > - Connect to depth sensors and point clouds with Octomaps 23 | > - Collision Checking 24 | > - Avoid obstacles using geometric primitives, meshes, or point cloud data 25 | > 26 | > 27 | 28 | This tutorial will only cover the basics for MoveIt!, up to and including the move_group interface. It won't cover the implementations of that interface, or any other deeper APIs, such as the pick and place pipeline, or time parametrisation. 29 | 30 | 31 | 32 | ## Pre-Requisites 33 | 34 | Knowledge of the following will be presumed: 35 | 36 | - Python 37 | - C++ 38 | - Robot Operating System (ROS) 39 | 40 | 41 | 42 | ## Support my efforts! 43 | 44 | [![Yeah! Buy the DRAGON a COFFEE!](./assets/COFFEE%20BUTTON%20%E3%83%BE(%C2%B0%E2%88%87%C2%B0%5E).png)](https://www.buymeacoffee.com/methylDragon) 45 | 46 | [Or leave a tip! ヾ(°∇°*)](https://www.paypal.me/methylDragon) 47 | 48 | 49 | 50 | ## Credits 51 | 52 | All credits and sources are listed inside the tutorials and references themselves. 53 | 54 | 55 | 56 | ``` 57 | . . 58 | . |\-^-/| . 59 | /| } O.=.O { |\ 60 | ``` -------------------------------------------------------------------------------- /assets/1557726877702.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/1557726877702.png -------------------------------------------------------------------------------- /assets/1557726886051.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/1557726886051.png -------------------------------------------------------------------------------- /assets/1557726910810.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/1557726910810.png -------------------------------------------------------------------------------- /assets/1557727013881.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/1557727013881.png -------------------------------------------------------------------------------- /assets/1557727232441.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/1557727232441.png -------------------------------------------------------------------------------- /assets/1557727472789.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/1557727472789.png -------------------------------------------------------------------------------- /assets/1557734257255.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/1557734257255.png -------------------------------------------------------------------------------- /assets/1563791835579.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/1563791835579.png -------------------------------------------------------------------------------- /assets/COFFEE BUTTON ヾ(°∇°^).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/COFFEE BUTTON ヾ(°∇°^).png -------------------------------------------------------------------------------- /assets/MoveIt_Launchfile_Structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/MoveIt_Launchfile_Structure.png -------------------------------------------------------------------------------- /assets/Overview.0012.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/Overview.0012.jpg -------------------------------------------------------------------------------- /assets/moveit_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/methylDragon/moveit-tutorial/152a609d3f42f6d882204ac6afe3f5d381296773/assets/moveit_logo.png --------------------------------------------------------------------------------