motion_planning_api: updating goals and pics (#23)

doc/motion_planning_api/launch/motion_planning_api_tutorial.launch

@@ -4,7 +4,7 @@
   <arg unless="$(arg debug)" name="launch_prefix" value="" />
   <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
 
-  <include file="$(find pr2_moveit_config)/launch/planning_context.launch">
+  <include file="$(find panda_moveit_config)/launch/planning_context.launch">
     <arg name="load_robot_description" value="true"/>
   </include>
 
@@ -13,17 +13,17 @@
   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
     <param name="/use_gui" value="true"/>
   </node>
-  
+
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
 
-  <include file="$(find pr2_moveit_config)/launch/moveit_rviz.launch">
+  <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch">
     <arg name="config" value="true"/>
   </include>
 
   <node name="motion_planning_api_tutorial" pkg="moveit_tutorials" type="motion_planning_api_tutorial" respawn="false" launch-prefix="$(arg launch_prefix)" output="screen">
-    <rosparam command="load" file="$(find pr2_moveit_config)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
     <param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/>
-    <rosparam command="load" file="$(find pr2_moveit_config)/config/ompl_planning.yaml"/>
+    <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/>
   </node>
 
 

doc/motion_planning_api/motion_planning_api_tutorial.rst

@@ -6,43 +6,50 @@ 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
-^^^^^^^^^^^^^^^
+---------------
 The entire code can be seen :codedir:`here in the moveit_tutorials github project<motion_planning_api>`.
 
-Compiling the code
-^^^^^^^^^^^^^^^^^^
-Follow the `instructions for compiling code from source <http://moveit.ros.org/install/source/>`_.
+.. tutorial-formatter:: ./src/motion_planning_api_tutorial.cpp
 
 The launch file
-^^^^^^^^^^^^^^^
+---------------
 The entire launch file is :codedir:`here <motion_planning_api/launch/motion_planning_api_tutorial.launch>` on github. All the code in this tutorial can be compiled and run from the moveit_tutorials package.
 
 Running the code
-^^^^^^^^^^^^^^^^
+----------------
+Make sure you have sourced the setup files::
+
+ source ~/ws_moveit/devel/setup.bash
 
 Roslaunch the launch file to run the code directly from moveit_tutorials::
 
  roslaunch moveit_tutorials motion_planning_api_tutorial.launch
 
 Expected Output
-^^^^^^^^^^^^^^^
-
+---------------
 In Rviz, we should be able to see four trajectories being replayed eventually:
 
- 1. The robot moves its right arm to the pose goal in front of it,
+ 1. The robot moves its arm to the first pose goal,
+
     |A|
 
- 2. The robot moves its right arm to the joint goal to the side,
- 3. The robot moves its right arm back to the original pose goal in front of it,
- 4. The robot moves its right arm to a new pose goal while maintaining the end-effector level.
+ 2. The robot moves its arm to the joint goal,
+
     |B|
 
-.. |A| image:: motion_planning_api_tutorial_robot_move_arm_front.png
-.. |B| image:: motion_planning_api_tutorial_robot_move_right.png
+ 3. The robot moves its arm back to the original pose goal,
+ 4. The robot moves its arm to a new pose goal while maintaining the end-effector level.
+
+    |C|
+
+.. |A| image:: motion_planning_api_tutorial_robot_move_arm_1st.png
+               :width: 200px
+.. |B| image:: motion_planning_api_tutorial_robot_move_arm_2nd.png
+               :width: 200px
+.. |C| image:: motion_planning_api_tutorial_robot_move_arm_3rd.png
+               :width: 200px

doc/motion_planning_api/src/motion_planning_api_tutorial.cpp

@@ -32,7 +32,7 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
-/* Author: Sachin Chitta */
+/* Author: Sachin Chitta, Michael Lautman */
 
 #include <pluginlib/class_loader.h>
 #include <ros/ros.h>
@@ -60,13 +60,11 @@ int main(int argc, char** argv)
   // Setting up to start using a planner is pretty easy. Planners are
   // setup as plugins in MoveIt! and you can use the ROS pluginlib
   // interface to load any planner that you want to use. Before we
-  // can load the planner, we need two objects, a RobotModel
-  // and a PlanningScene.
-  // We will start by instantiating a
-  // `RobotModelLoader`_
-  // object, which will look up
-  // the robot description on the ROS parameter server and construct a
-  // :moveit_core:`RobotModel` for us to use.
+  // can load the planner, we need two objects, a RobotModel and a
+  // PlanningScene. We will start by instantiating a `RobotModelLoader`_
+  // object, which will look up the robot description on the ROS
+  // parameter server and construct a :moveit_core:`RobotModel` for us
+  // to use.
   //
   // .. _RobotModelLoader: http://docs.ros.org/indigo/api/moveit_ros_planning/html/classrobot__model__loader_1_1RobotModelLoader.html
   robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
@@ -120,15 +118,15 @@ int main(int argc, char** argv)
 
   // Pose Goal
   // ^^^^^^^^^
-  // We will now create a motion plan request for the right arm of the PR2
+  // We will now create a motion plan request for the arm of the Panda
   // specifying the desired pose of the end-effector as input.
   planning_interface::MotionPlanRequest req;
   planning_interface::MotionPlanResponse res;
   geometry_msgs::PoseStamped pose;
-  pose.header.frame_id = "torso_lift_link";
-  pose.pose.position.x = 0.75;
+  pose.header.frame_id = "panda_link0";
+  pose.pose.position.x = 0.3;
   pose.pose.position.y = 0.0;
-  pose.pose.position.z = 0.0;
+  pose.pose.position.z = 0.75;
   pose.pose.orientation.w = 1.0;
 
   // A tolerance of 0.01 m is specified in position
@@ -142,9 +140,9 @@ int main(int argc, char** argv)
   // package.
   //
   // .. _kinematic_constraints: http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132
-  req.group_name = "right_arm";
+  req.group_name = "panda_arm";
   moveit_msgs::Constraints pose_goal =
-      kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle);
+      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
   req.goal_constraints.push_back(pose_goal);
 
   // We now construct a planning context that encapsulate the scene,
@@ -181,15 +179,12 @@ int main(int argc, char** argv)
   /* First, set the state in the planning scene to the final state of the last plan */
   robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
   planning_scene->setCurrentState(response.trajectory_start);
-  const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("right_arm");
+  const robot_state::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("panda_arm");
   robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
 
   // Now, setup a joint space goal
   robot_state::RobotState goal_state(robot_model);
-  std::vector<double> joint_values(7, 0.0);
-  joint_values[0] = -2.0;
-  joint_values[3] = -0.2;
-  joint_values[5] = -0.15;
+  std::vector<double> joint_values = {-1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0};
   goal_state.setJointGroupPositions(joint_model_group, joint_values);
   moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
   req.goal_constraints.clear();
@@ -232,11 +227,13 @@ int main(int argc, char** argv)
   // ^^^^^^^^^^^^^^^^^^^^^^^
   // Let's add a new pose goal again. This time we will also add a path constraint to the motion.
   /* Let's create a new pose goal */
-  pose.pose.position.x = 0.65;
-  pose.pose.position.y = -0.2;
-  pose.pose.position.z = -0.1;
+
+  pose.pose.position.x = 0.32;
+  pose.pose.position.y = -0.25;
+  pose.pose.position.z = 0.65;
+  pose.pose.orientation.w = 1.0;
   moveit_msgs::Constraints pose_goal_2 =
-      kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose, tolerance_pose, tolerance_angle);
+      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
   /* First, set the state in the planning scene to the final state of the last plan */
   robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
   /* Now, let's try to move to this new pose goal*/
@@ -246,9 +243,9 @@ int main(int argc, char** argv)
   /* But, let's impose a path constraint on the motion.
      Here, we are asking for the end-effector to stay level*/
   geometry_msgs::QuaternionStamped quaternion;
-  quaternion.header.frame_id = "torso_lift_link";
+  quaternion.header.frame_id = "panda_link0";
   quaternion.quaternion.w = 1.0;
-  req.path_constraints = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", quaternion);
+  req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion);
 
   // Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
   // (the workspace of the robot)