Skip to content

Commit

Permalink
Kinetic panda prereqs (#13)
Browse files Browse the repository at this point in the history
* working move_group_interface tutorial

* splitting out prerequisites

* adding link to prerequisite tutorial where aproporiate

* fixing spelling errors and clarifying instructions

* fixup for prerequisites
  • Loading branch information
mlautman authored and davetcoleman committed May 15, 2018
1 parent 9a93b0a commit fa134db
Show file tree
Hide file tree
Showing 19 changed files with 232 additions and 177 deletions.
10 changes: 5 additions & 5 deletions _themes/sphinx_rtd_theme/layout.html
Expand Up @@ -86,7 +86,7 @@

gtag('config', 'UA-108532843-1');
</script>

</head>

<body class="wy-body-for-nav" role="document">
Expand Down Expand Up @@ -156,10 +156,10 @@
{% include "breadcrumbs.html" %}
<div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
<div itemprop="articleBody">
<div class="admonition note">
<p class="first admonition-title">Code Used in this Tutorial Available</p>
<p class="last">Code can be found at <a href = "https://github.com/ros-planning/moveit_tutorials">moveit_tutorials repository</a> in doc folder. Use kinetic-devel branch.</p>
</div>
<!-- <div class="admonition note"> -->
<!-- <p class="first admonition-title">Code Used in this Tutorial Available</p> -->
<!-- <p class="last">Code can be found at <a href = "https://github.com/ros-planning/moveit_tutorials">moveit_tutorials repository</a> in doc folder. Use kinetic-devel branch.</p> -->
<!-- </div> -->
{% block body %}{% endblock %}
{% if display_github %}
<div class="admonition note">
Expand Down
71 changes: 38 additions & 33 deletions doc/collision_contact/collision_contact_tutorial.rst
Expand Up @@ -4,6 +4,11 @@ Collision Contact Tutorial
This section walks you through C++ code which allows you to see collision contact points between the robot, itself, and the world as you move and interact with the robot’s arm in Rviz.


Prerequisites
-------------
If you haven't already done so, make sure you've completed the steps in `Prerequisites
<../prerequisites/prerequisites.html>`_.

Code
----

Expand All @@ -18,23 +23,23 @@ The **InteractiveRobot** class uses the **IMarker** class which maintains an int
Code Description
^^^^^^^^^^^^^^^^

We will walk through the code in the order that it is run in the program, starting with the **main()** function in **collision_contact_tutorial.cpp**:
We will walk through the code in the order that it is run in the program, starting with the **main()** function in **collision_contact_tutorial.cpp**:
::

int main(int argc, char **argv)
{
ros::init (argc, argv, "acorn_play");
ros::NodeHandle nh;

InteractiveRobot robot;

After the standard ROS initialization we create an instance of the **InteractiveRobot** class.
After the standard ROS initialization we create an instance of the **InteractiveRobot** class.
Next we implement a planning scene: ::

g_planning_scene = new planning_scene::PlanningScene();
g_planning_scene->configure(robot.robotModel());

Among other things the **PlanningScene** maintains collision information for the robot and the world.
Among other things the **PlanningScene** maintains collision information for the robot and the world.
We have to tell the **PlanningScene** about the world geometry: ::

Eigen::Affine3d world_cube_pose;
Expand All @@ -46,20 +51,20 @@ We have to tell the **PlanningScene** about the world geometry: ::
**getWorldGeometry()** gets the size and pose of the cube from the **InteractiveRobot** class. **g_world_cube_shape** is a shared pointer to a new box shape describing the cube. ::

g_marker_array_publisher = new ros::Publisher(nh.advertise<visualization_msgs::MarkerArray>("interactive_robot_marray",100));

The **g_marker_array_publisher** is used to publish collision contact points for display in Rviz.
::

robot.setUserCallback(userCallback);
ros::spin();

Here we set the callback, which will be called when the interactive markers are manipulated, and then enter the ros::spin() infinite loop.
The rest of the main function is just cleanup: ::

delete g_planning_scene;
delete g_marker_array_publisher;;
ros::shutdown();

ros::shutdown();
return 0;
}

Expand Down Expand Up @@ -91,13 +96,13 @@ Then actually run the collision check: ::

g_planning_scene->checkCollision(c_req, c_res, *robot.robotState());

This checks for collisions between the "right_arm" and the world, as well as between the "right_arm" and the rest of the robot.
This checks for collisions between the "right_arm" and the world, as well as between the "right_arm" and the rest of the robot.
If a collision occurred (c_res.collision is true) then we display the collision points: ::

if (c_res.collision)
{
ROS_INFO("COLLIDING contact_point_count=%d",(int)c_res.contact_count);

if (c_res.contact_count > 0)
{
std_msgs::ColorRGBA color;
Expand All @@ -118,19 +123,19 @@ And finally we publish the markers to Rviz: ::

publishMarkers(markers);
}
If no collision occurred we erase any collision contact point markers that we may have placed there last time the callback was called:

If no collision occurred we erase any collision contact point markers that we may have placed there last time the callback was called:
::

else
{
ROS_INFO("Not colliding");

// delete the old collision point markers
visualization_msgs::MarkerArray empty_marker_array;
publishMarkers(empty_marker_array);
}

The **publishMarkers()** function deletes any old markers and then adds new ones: ::

void publishMarkers(visualization_msgs::MarkerArray& markers)
Expand All @@ -143,10 +148,10 @@ The **publishMarkers()** function deletes any old markers and then adds new ones

g_marker_array_publisher->publish(g_collision_points);
}

// move new markers into g_collision_points
std::swap(g_collision_points.markers, markers.markers);

// draw new markers (if there are any)
if (g_collision_points.markers.size())
g_marker_array_publisher->publish(g_collision_points);
Expand All @@ -163,45 +168,45 @@ Running
Launch file
^^^^^^^^^^^

A launch file is located here. It loads the URDF and SRDF parameters for the PR2 robot, launches Rviz, and runs the collision_contact_tutorial program described above. If moveit_tutorials is in your ROS_PACKAGE_PATH then launch it by typing:
A launch file is located here. It loads the URDF and SRDF parameters for the PR2 robot, launches Rviz, and runs the collision_contact_tutorial program described above. If moveit_tutorials is in your ROS_PACKAGE_PATH then launch it by typing:
::

roslaunch moveit_tutorials collision_contact_tutorial.launch

Rviz setup
^^^^^^^^^^

When Rviz starts up you will have to add some displays to see the objects your code is publishing. This is done in the "Displays" panel in rviz.
When Rviz starts up you will have to add some displays to see the objects your code is publishing. This is done in the "Displays" panel in rviz.

* Under GlobalOptions set FixedFrame to /base_footprint.
* Cick Add and (under moveit_ros_visualization) add a RobotState display.
* Cick Add and (under moveit_ros_visualization) add a RobotState display.

* Set the RobotState::RobotDescription to robot_description

* Set the RobotState::RobotStateTopic to interactive_robot_state

* Set the RobotState::RobotAlpha to 0.3 (to make the robot transparent and see the collision points)
* Click Add and (under Rviz) add a Marker display.

* Click Add and (under Rviz) add a Marker display.

* Set the Marker::MarkerTopic to interactive_robot_markers
* Click Add and (under Rviz) add a InteractiveMarkers display.

* Click Add and (under Rviz) add a InteractiveMarkers display.

* Set the Marker::UpdateTopic to interactive_robot_imarkers/update
* Click Add and (under Rviz) add a MarkerArray display.

* Click Add and (under Rviz) add a MarkerArray display.

* Set the Marker::UpdateTopic to interactive_robot_marray.

You should now see the PR2 robot with 2 interactive markers which you can drag around.
You should now see the PR2 robot with 2 interactive markers which you can drag around.

.. image:: collision_contact_tutorial_screen.png

Interacting
^^^^^^^^^^^

In Rviz you will see two sets of Red/Green/Blue interactive marker arrows. Drag these around with the mouse.
Move the right arm so it is in contact with the left arm. You will see magenta spheres marking the contact points.
If you do not see the magenta spheres be sure that you added the MarkerArray display with interactive_robot_marray topic as described above. Also be sure to set RobotAlpha to 0.3 (or some other value less than 1) so the robot is transparent and the spheres can be seen.
Move the right arm so it is in contact with the yellow cube (you may also move the yellow cube). You will see magenta spheres marking the contact points.
In Rviz you will see two sets of Red/Green/Blue interactive marker arrows. Drag these around with the mouse.
Move the right arm so it is in contact with the left arm. You will see magenta spheres marking the contact points.
If you do not see the magenta spheres be sure that you added the MarkerArray display with interactive_robot_marray topic as described above. Also be sure to set RobotAlpha to 0.3 (or some other value less than 1) so the robot is transparent and the spheres can be seen.
Move the right arm so it is in contact with the yellow cube (you may also move the yellow cube). You will see magenta spheres marking the contact points.
2 changes: 1 addition & 1 deletion doc/ikfast/ikfast_tutorial.rst
Expand Up @@ -15,7 +15,7 @@ MoveIt! IKFast is a tool that generates a IKFast kinematics plugin for MoveIt us
This tutorial will step you through setting up your robot to utilize the power of IKFast. MoveIt! IKFast is tested on ROS Groovy with Catkin using OpenRAVE 0.8 with a 6dof and 7dof robot arm manipulator.
While it works in theory, currently the IKFast plugin generator tool does not work with >7 degree of freedom arms.

Pre-requisites
Prerequisites
---------------

You should have already created a MoveIt! configuration package for your robot, by using the Setup Assistant.
Expand Down
@@ -1,24 +1,29 @@
Joystick Control Teleoperation
==========================================

Prerequisites
-------------
If you haven't already done so, make sure you've completed the steps in `Prerequisites
<../prerequisites/prerequisites.html>`_.

Run
---

Startup regular MoveIt! planning node with Rviz (for example demo.launch)

Make sure you have the dependencies installed::

sudo apt-get install ros-indigo-joy
sudo apt-get install ros-kinetic-joy

In the Motion Planning plugin of Rviz, enable "Allow External Comm." checkbox in the "Planning" tab. Enable the 'Query Goal State' robot display in the MoveIt! Motion Planning Plugins's 'Planning Request' section.

Now launch the joystick control launch file specific to your robot. If you are missing this file, first re-run the MoveIt! Setup Assistant using the latest version of the Setup Assistant::

roslaunch YOURROBOT_moveit_config joystick_control.launch
roslaunch ROBOT_moveit_config joystick_control.launch

The script defaults to using ``/dev/input/js0`` for your game controller port. To customize, you can also use, for example::

roslaunch YOURROBOT_moveit_config joystick_control.launch dev:=/dev/input/js1
roslaunch ROBOT_moveit_config joystick_control.launch dev:=/dev/input/js1

This script can read four types of joysticks:

Expand Down
5 changes: 5 additions & 0 deletions doc/kinematic_model/kinematic_model_tutorial.rst
Expand Up @@ -10,6 +10,11 @@ The :moveit_core:`RobotModel` and :moveit_core:`RobotState` classes are the core
you access to the kinematics. In this example, we will walk through
the process of using the classes for the right arm of the PR2.

Prerequisites
^^^^^^^^^^^^^
If you haven't already done so, make sure you've completed the steps in `Prerequisites
<../prerequisites/prerequisites.html>`_.

.. tutorial-formatter:: ./src/kinematic_model_tutorial.cpp

The entire code
Expand Down
Expand Up @@ -6,17 +6,13 @@ In this section, we will examine some of the parameters for configuring kinemati
The kinematics.yaml file
------------------------

The kinematics.yaml file generated by the MoveIt! Setup Assistant is the primary configuration file for kinematics for MoveIt!. You can see an entire example file for the PR2 robot in the `pr2_moveit_config Github project <https://github.com/davetcoleman/pr2_moveit_config/blob/master/config/kinematics.yaml>`_ ::
The kinematics.yaml file generated by the MoveIt! Setup Assistant is the primary configuration file for kinematics for MoveIt!. You can see an entire example file for the panda robot in the `panda_moveit_config Github project <https://github.com/PickNikRobotics/panda_moveit_config/blob/master/config/kinematics.yaml>`_ ::

right_arm:
kinematics_solver: pr2_arm_kinematics/PR2ArmKinematicsPlugin
kinematics_solver_search_resolution: 0.001
kinematics_solver_timeout: 0.05
kinematics_solver_attempts: 3
right_arm_and_torso:
panda_arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.05
kinematics_solver_attempts: 3

Parameters
^^^^^^^^^^
Expand Down
5 changes: 5 additions & 0 deletions doc/motion_planning_api/motion_planning_api_tutorial.rst
Expand Up @@ -5,6 +5,11 @@ In MoveIt!, the motion planners are loaded using a plugin infrastructure. This
allows MoveIt! to load motion planners at runtime. In this example, we will
run through the C++ code required to do this.

Prerequisites
^^^^^^^^^^^^^
If you haven't already done so, make sure you've completed the steps in `Prerequisites
<../prerequisites/prerequisites.html>`_.

.. tutorial-formatter:: ./src/motion_planning_api_tutorial.cpp

The entire code
Expand Down
Expand Up @@ -9,6 +9,11 @@ stages. The pre and post-processing stages, called planning request adapters, ca
be configured by name from the ROS parameter server. In this tutorial, we will
run you through the C++ code to instantiate and call such a planning pipeline.

Prerequisites
^^^^^^^^^^^^^
If you haven't already done so, make sure you've completed the steps in `Prerequisites
<../prerequisites/prerequisites.html>`_.

.. tutorial-formatter:: ./src/motion_planning_pipeline_tutorial.cpp

The entire code
Expand Down
47 changes: 12 additions & 35 deletions doc/move_group_interface/move_group_interface_tutorial.rst
@@ -1,40 +1,18 @@
Move Group Interface Tutorial
==================================

In MoveIt!, the primary user interface is through the :move_group_interface:`MoveGroup` class. It provides easy to use functionality for most operations that a user may want to carry out, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot. This interface communicates over ROS topics, services, and actions to the `MoveGroup Node <http://docs.ros.org/indigo/api/moveit_ros_move_group/html/annotated.html>`_.
In MoveIt!, the simplest user interface is through the :move_group_interface:`MoveGroup` class. It provides easy to use functionality for most operations that a user may want to carry out, specifically setting joint or pose goals, creating motion plans, moving the robot, adding objects into the environment and attaching/detaching objects from the robot. This interface communicates over ROS topics, services, and actions to the `MoveGroup Node <http://docs.ros.org/indigo/api/moveit_ros_move_group/html/annotated.html>`_.

.. image:: move_group_interface_tutorial_start_screen.png

Watch the `YouTube video demo <https://youtu.be/4FSmZRQh37Q>`_

Create A Catkin Workspace
^^^^^^^^^^^^^^^^^^^^^^^^^
You do not need to build all of MoveIt! from source, but you do need to have a catkin workspace setup. If you do not have a workspace already setup, follow the "Prerequisites" section on the `MoveIt! source install page <http://moveit.ros.org/install/source/>`_ and be sure to then source the workspace as documented at the bottom of that page under "Source the Catkin Workspace."

Compiling the Example Code
^^^^^^^^^^^^^^^^^^^^^^^^^^
Prerequisites
^^^^^^^^^^^^^
If you haven't already done so, make sure you've completed the steps in `Prerequisites
<../prerequisites/prerequisites.html>`_.

Within your catkin workspace (``cd ~/ws_moveit/src``), download this tutorial::

git clone https://github.com/ros-planning/moveit_tutorials.git

Temporary PR2 on Kinetic Instructions
-------------------------------------

You will also need a **pr2_moveit_config** package to run this tutorial. Currently this is unreleased in ROS Kinetic but the following is a temporary workaround::

git clone https://github.com/PR2/pr2_common.git -b kinetic-devel
git clone https://github.com/davetcoleman/pr2_moveit_config.git

Install Dependencies and Build
--------------------------------------

Scans your catkin workspace for missing packages before compiling new code::

rosdep install --from-paths . --ignore-src --rosdistro kinetic
cd ~/ws_moveit
catkin config --extend /opt/ros/kinetic --cmake-args -DCMAKE_BUILD_TYPE=Release
catkin build

Start Rviz and MoveGroup node
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Expand All @@ -44,7 +22,7 @@ Make sure you have re-sourced the setup files::

Start Rviz and wait for everything to finish loading::

roslaunch pr2_moveit_config demo.launch
roslaunch panda_moveit_config demo.launch

Running the demo
^^^^^^^^^^^^^^^^
Expand All @@ -60,18 +38,17 @@ Expected Output

Watch the `YouTube video demo <https://youtu.be/4FSmZRQh37Q>`_ for expected output. In Rviz, we should be able to see the following:

1. The robot moves its right arm to the pose goal to its right front.
2. The robot moves its right arm to the joint goal at its right side.
3. The robot moves its right arm back to a new pose goal while maintaining the end-effector level.
4. The robot moves its right arm along the desired cartesian path (a triangle up+forward, left, down+back).
5. A box object is added into the environment to the right of the right arm.
1. The robot moves its arm to the pose goal to its front.
2. The robot moves its arm to the joint goal at its side.
3. The robot moves its arm back to a new pose goal while maintaining the end-effector level.
4. The robot moves its arm along the desired cartesian path (a triangle up+forward, left, down+back).
5. A box object is added into the environment to the right of the arm.
|B|

6. The robot moves its right arm to the pose goal, avoiding collision with the box.
6. The robot moves its arm to the pose goal, avoiding collision with the box.
7. The object is attached to the wrist (its color will change to purple/orange/green).
8. The object is detached from the wrist (its color will change back to green).
9. The object is removed from the environment.
10. The robot moves both arms to two different pose goals at the same time

.. |B| image:: ./move_group_interface_tutorial_robot_with_box.png

Expand Down

0 comments on commit fa134db

Please sign in to comment.