#################################### ## note.0= [[Industrial/Tutorials/Create a URDF for an Industrial Robot|URDFs for Industrial Robots]] ## note.1= [[http://moveit.ros.org/install/|MoveIt Installation]] ## title = Create a MoveIt Package for an Industrial Robot ## description = Walks through the steps of creating a !MoveIt package for an industrial robot. The !MoveIt package provides collision-aware path planning for the robot. ## level= IntermediateCategory ## keywords= moveit, industrial, manipulator #################################### <> <> This tutorial is based on the [[http://docs.ros.org/kinetic/api/moveit_tutorials/html|MoveIt tutorials]]. It may be instructive to review those tutorials first. This tutorial walks through filling out the MoveIt Setup Assistant wizard and creating additional configuration files required for communicating with the robot interface nodes. The Setup Assistant generates a moveit package for the robot described by the URDF. '''The Setup Assistant should be re-executed whenever the URDF changes, to ensure that all changes are migrated to the other moveit configuration files.''' Once you have completed this tutorial you will have a package that enables interactive motion planning for your robot in simulation and on actual hardware. == Run MoveIt Setup Assistant == These steps provide additional clarification to the [[http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html|MoveIt Setup Assistant tutorial]]. It may be helpful to refer to both tutorials. 1. Launch the setup assistant: {{{ $ roslaunch moveit_setup_assistant setup_assistant.launch }}} 1. Create a '''new''' package and select the path to your robot's URDF file * it is okay to reference URDF files in previously-created `robot_description` or `_arm_navigation` packages * you can directly import XACRO files in addition to URDF files 1. Calculate Self-Collision matrix: * Set Sampling Density fairly high (~80,000?). For most robots, this doesn't take long. * press "Regenerate Default Collision Matrix" button * review (select) the various collision-pairs to see if they make sense 1. Add required Virtual Joint: * !MoveIt requires at least one virtual joint, connecting the robot to the world * Add Virtual Joint: * `name = 'FixedBase'` (arbitrary) * `child = 'base_link'` (should match the URDF root link) * `parent = 'world'` (arbitrary, until robot is placed in an environment) * `type = 'fixed' 1. Add Planning Groups: 1. Add manipulator (arm) group * Add Group, `name = 'manipulator'`, `kinematic solver = 'KDLKinematicsPlugin'` * Add Kin. Chain * this method is generally preferred over Add Joints or Add Links * assign `base_link` and `tip_link` (e.g. `link_6`, `tool0`, or your robot's equivalent)<
> 1. Add end-effector group '''[OPTIONAL]'''<
> ~-''If you define an End-Effector, !MoveIt can use this in grasp-planning and visualization methods.<
>If your robot doesn't have an end-effector, just leave this blank.''-~ * Add Group, `name = 'gripper'`, `kinematic solver = 'none'` * Add Links * assign end-effector links 1. Add Robot Poses '''[OPTIONAL]''': * Add one or more default poses, to reference in later planning code (all-zeros, home, etc.) * The `MoveIt!` button will cycle the virtual robot through all defined poses 1. Add an End Effector '''[OPTIONAL]''': * `name = 'gripper'` (arbitrary) * `group = 'gripper'` (should match group created above) * `parent = 'flange'` (last arm link) * `parent group = 'manipulator'` 1. Generate Configuration Files: * specify a path to create the new moveit package for your robot * recommended: `.../_moveit_config`<
> ~-''where matches the robot name defined in your URDF''-~ * press "Generate Package" button * check for the "Generated Successfully" message! ~-''You may see a warning that an End-Effector has not been defined. It is okay to continue, if this is the desired configuration. !MoveIt will work just fine with robots that have no end-effector (just a bare tool flange).''-~ * if the robot's URDF file is located in a rosbuild package, the Setup Assistant will place an absolute file path to the URDF in the `.setup_assistant` file. This is not portable, but can be fixed with a manual edit to that file: {{{ package = relative_path = path/relative/to/package/root/.../robot.urdf }}} === Test MoveIt Configuration (dummy robot) === At this point, !MoveIt has enough information to do planning and collision checking, but not enough to talk to a physical robot. It may be helpful to verify the !MoveIt configuration at this point before continuing on with the tutorial. This section describes the use of the `demo.launch` file created by the moveit setup assistant to provide a dummy joint-state-publisher to stand in for the robot connection. It roughly parallels the [[http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/ros_visualization/visualization_tutorial.html|MoveIt! RViz Plugin Tutorial]]. 1. Run the demo launch file {{{ $ roslaunch _moveit_config/launch/demo.launch }}} * set `Displays->Global Options->Fixed Frame` to `base_link` (or your robot's equivalent) * uncheck `Displays->Motion Planning->Planning Request->Query Start State` * check `Query Goal State` in the same section * select `File -> Save Config` to save the changes to the package `moveit.rviz` config file 1. Select "Interact" and move the end-effector to a new goal * !MoveIt should compute IK and follow the end-effector marker 1. Select Motion Planning -> Planning tab -> Plan * planning-scene robot should move from start->goal as plan is calculated 1. Exit RViz and Ctrl-C the demo.launch window == Update Configuration Files == This section explains how to update various configuration/launch files to provide additional robot-specific data that allows !MoveIt to communicate with the robot interface node. See also the !MoveIt [[http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/controller_configuration.html|Controllers Configuration Tutorial]] for more information. 1. Create the `controllers.yaml` file (~-`_moveit_config/config/controllers.yaml`-~): {{{ controller_list: - name: "" action_ns: joint_trajectory_action type: FollowJointTrajectory joints: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6] }}} * !MoveIt requires a "Controller Manager" node, but single-arm industrial robots typically don't need this capability. We use the `moveit_simple_controller_manager` plugin to provide the minimum required functionality.<
> ~-''Note: this requires that you have the `ros-groovy-moveit-plugins` package installed.''-~ * to me, the "`name`" and "`ns`" parameters seem backwards. !MoveIt will send the desired trajectory to on the topic "name/ns". A non-empty "`name`" parameter may be required if multiple robots are being controlled, to provide separate namespaces. * the joint-list should match the joint names specified in the robot's URDF file. Order is not important. * this controller manager also interfaces to a `GripperCommandAction` server, if available. See [[http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/pr2_tutorials/planning/src/doc/controller_configuration.html#grippercommand-controller-interface|GripperCommand Controller Interface]] for more details 1. '''[OPTIONAL]''' Create the `joint_names.yaml` file (~-`_moveit_config/config/joint_names.yaml`-~): * ''~-this file is only required if your robot uses non-standard joint names-~'' ~-"standard" joint names are: "joint_1", "joint_2", ..., "joint_6"-~ * ''~-The order of this list should match that expected by your robot's socket interface. Typically Base -> Tip.<
>'' See [[industrial_robot_client/joint_naming|here]] for more details.-~ ~-{{{ controller_joint_names: [joint_1, joint_2, ... joint_N] }}}-~ 1. Fill in the blank controller_manager launch file (~-_moveit_config/launch/_moveit_controller_manager.launch-~): ~-{{{ }}}-~ * you'll need to provide the path to your robot's moveit_config 1. Create the Planning & Execution launch file: * create ~-_moveit_config/launch/moveit_planning_execution.launch-~ using [[attachment:moveit_planning_execution.launch.txt|this]] template * as usual, update robot-specific sections * if you created a `joint_names.yaml` file, uncomment the following line: ~-{{{ }}}-~ === Test MoveIt Configuration (simulated robot) === Before connecting to a real robot, you may want to verify the last set of configuration changes using a simulated robot node. This section describes using the `industrial_robot_simulator` package to test all parts of the !MoveIt planning/execution chain. 1. Verify the `industrial_robot_simulator` package is installed: * `rospack find industrial_robot_simulator` * if necessary, install the package: * from binary packages: `apt-get install industrial_core` * from [[https://github.com/ros-industrial/industrial_core|source]] 1. Launch the planning environment: {{{ $ roslaunch _moveit_config moveit_planning_execution.launch }}} 1. use "Interact" mode to move the end-effector to a new goal state: 1. "Motion Planning" -> "Plan and Execute" to send trajectory to the sim robot * you should see the planning scene update twice: 1. ~-show computed trajectory plan (quick)-~ 1. ~-show simulated robot "executing" the trajectory (slower)-~ * you can also confirm/monitor the simulated robot operation at the command line: {{{ $ rostopic echo joint_states (broadcast by simulator) $ rostopic echo joint_path_command (inputs to simulator) }}} == Connect to a Real Robot == {{{#!wiki warning '''Respect Robot Safety Practices''' WARNING: industrial robots are very dangerous and can seriously injure or kill. the ROS-Industrial software is provided under the terms of the BSD and Apache 2.0 licenses (i.e. as-is and with no warranty). When operating an industrial robot under ROS-Industrial control, make certain that no one is within the robot workspace and the e-stop is under operator control. }}} === Required Topics/Services === !MoveIt interfaces with the robot through the [[http://docs.ros.org/api/control_msgs/html/action/FollowJointTrajectory.html|FollowJointTrajectory]] action. If desired, your robot can use the `joint_trajectory_action` in the `industrial_robot_client` package to manage the action interface. If so, your robot node is only required to subscribe/publish a few basic topic streams: * publish `feedback_states` (`control_msgs/FollowJointTrajectoryFeedback`) * subscribe `joint_path_command` ([[http://docs.ros.org/api/trajectory_msgs/html/msg/JointTrajectory.html|trajectory_msgs/JointTrajectory]]) The same robot-interface nodes are often used for all different robot models for a given manufacturer. It may be helpful to combine these 3 nodes into a single `robot_interface.launch` file that can be called from model-specific !MoveIt packages. To implement control of a real platform, update the launch file created in the previous step with the commands required to launch the robot interfaces described above. === Verify Joint Position Scaling === Take this opportunity to verify that the ROS robot model correctly mirrors the physical robot: 1. Start the planning environment from the command line: {{{ $ roslaunch _moveit_config moveit_planning_execution.launch sim:=false robot_ip:= }}} 1. Move the robot around manually (using a teach-pendant or similar method). 1. Verify that the joint positions in RViz match the physical robot configuration. 1. Repeat for every joint and orientation. '''NOTE: Mistakes made during this verification step can result in dangerous collisions''' === Plan and Execute on a Real Robot === Experiment with using the !MoveIt planning environment to command trajectories with the real robot. Be certain that an E-stop is close by whenever commanding robot motion. ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE