Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

''Safe'' arm trajectory control

ROS Setup

The first thing we'll need to do is create a package for all the tutorials that we will work with. To do this we'll use the handy roscreate-pkg command where we want to create the package directory with a dependency on the actionlib and pr2_controller_msgs packages as shown below:

roscreate-pkg arm_navigation_tutorials actionlib pr2_controllers_msgs

After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace

roscd arm_navigation_tutorials

Also, make sure to set an environment variable called ROBOT to sim. E.g., in a bash shell,

 export ROBOT=sim

(Note: you need to do this only if you are running this tutorial on a simulator).

Running in simulation

To use the environment server, we first need to launch the simulator.

roscd pr2_gazebo
roslaunch pr2_empty_world.launch

Setup the sensor nodes

To get collision information about the world, you first need to setup the nodes that will take sensory information and convert it into a collision space representation. Fortunately, you can set this up using a single launch file.

roscd arm_navigation_pr2
roslaunch perception/laser-perception.launch

Setup the controller

Now, launch the controller.

roscd collision_free_arm_trajectory_controller
roslaunch right_arm_controller.launch

You should see the following line at the end of the node's output:

[ INFO] 116.845000000: Collision free arm trajectory controller started

Command the controller

The controller has a simple action interface so it can be commanded by creating an action client just like the client you would create to talk to a joint trajectory action. Here's simple code that will send a goal to the action:

   1 #include <ros/ros.h>
   2 #include <actionlib/client/simple_action_client.h>
   3 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
   4 
   5 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::JointTrajectoryAction> JointExecutorActionClient;
   6 
   7 void spinThread()
   8 {
   9   ros::spin();
  10 }
  11 
  12 int main(int argc, char** argv)
  13 {
  14   ros::init(argc, argv, "test_controller");
  15   boost::thread spin_thread(&spinThread);
  16   JointExecutorActionClient *traj_action_client_ = new JointExecutorActionClient("collision_free_arm_trajectory_action_right_arm");
  17 
  18   while(!traj_action_client_->waitForServer(ros::Duration(1.0))){
  19     ROS_INFO("Waiting for the joint_trajectory_action action server to come up");
  20     if(!ros::ok()) {
  21       exit(0);
  22     }
  23   }
  24 
  25   pr2_controllers_msgs::JointTrajectoryGoal goal;
  26   goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
  27   goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
  28   goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
  29   goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
  30   goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
  31   goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
  32   goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
  33 
  34   goal.trajectory.points.resize(1);
  35   for(unsigned int i=0; i < 7; i++)
  36     goal.trajectory.points[0].positions.push_back(0.0);
  37   goal.trajectory.points[0].positions[0] = -1.57/2.0;
  38   goal.trajectory.points[0].time_from_start = ros::Duration(0.0);
  39 
  40   traj_action_client_->sendGoal(goal);
  41   ROS_INFO("Sent goal");
  42 
  43   while(!traj_action_client_->getState().isDone() && ros::ok())
  44   {
  45     ros::Duration(0.1).sleep();
  46   }
  47   return 0;
  48 }

If the tilting laser sensor on the PR2 detects an obstacle in the way or if an obstacle appears in the tilting laser sensor data once the arm starts moving, the action will refuse to let the arm move.