Author: Jordi Pages < jordi.pages@pal-robotics.com >

Maintainer: Sara Cooper < sara.cooper@pal-robotics.com >

Support: ari-support@pal-robotics.com

Source: https://github.com/pal-robotics/ari_tutorials.git

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.

Planning in cartesian space

Purpose

This tutorial shows how to use MoveIt! in order to move the ARI's right arm to a desired pose in cartesian space. An example is given in C++.

Pre-requisites

First make sure that the tutorials are properly installed along with the ARI simulation, as shown in the Tutorials Installation Section.

Execution

First open two consoles and source the public simulation workspace as follows:

• $ cd ~/ari_public_ws
  $ source ./devel/setup.bash

Launching the simulation

In the first console launch the following simulation

• roslaunch ari_gazebo ari_gazebo.launch public_sim:=true

Gazebo will show up with ARI.

Launching the nodes

Now we are going to run an example that will bring ARI's left arm's end-effector frame, i.e. arm_left_4_link, to the following cartesian space configuration with respect to /base_footprint:

• x: -0.2
  y: 0.38
  z: 1.1
  Roll: -1.81
  Pitch: -0.65
  Yaw: -2.59

which corresponds to the following pose as shown in rviz:

In order to safely reach such a cartesian goal the node plan_left_arm_ik included in ari_moveit_tutorial package and has to be called as follows

• rosrun ari_moveit_tutorial plan_left_arm_ik -0.2 0.38 1.1 -1.81 -0.65 -2.59

An example of plan executed by the node is depicted in the following sequence of images:

Note that the final pose of /arm_left_4_link is the desired one:

Inspecting the code

The code to implement such a node able to plan in cartesian space is shown below. Note that the key parts of the code are:

• Choose a group of joints
• Choose a planner and define the reference frame, i.e. /base_footprint in this case

• Set desired pose of /arm_tool_link encoded in a geometry_msgs::PoseStamped

• Give time to find a plan
• Execute the plan if found

     1 // ROS headers
     2 #include <ros/ros.h>
     3 
     4 // MoveIt! headers
     5 #include <moveit/move_group_interface/move_group_interface.h>
     6 
     7 // Std C++ headers
     8 #include <string>
     9 #include <vector>
    10 #include <map>
    11 
    12 int main(int argc, char** argv)
    13 {
    14   ros::init(argc, argv, "plan_left_arm_ik");
    15 
    16   if ( argc < 4 )
    17   {
    18     ROS_INFO(" ");
    19     ROS_INFO("\tUsage:");
    20     ROS_INFO(" ");
    21     ROS_INFO("\trosrun ari_moveit_tutorial plan_left_arm_ik  x y z  r p y");
    22     ROS_INFO(" ");
    23     ROS_INFO("\twhere the list of arguments specify the target pose of /arm_left_4_link expressed in /base_footprint");
    24     ROS_INFO(" ");
    25     return EXIT_FAILURE;
    26   }
    27 
    28   
    29   geometry_msgs::PoseStamped goal_pose;
    30   goal_pose.header.frame_id = "base_footprint";
    31   goal_pose.pose.position.x = atof(argv[1]);
    32   goal_pose.pose.position.y = atof(argv[2]);
    33   goal_pose.pose.position.z = atof(argv[3]);
    34   goal_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(atof(argv[4]), atof(argv[5]), atof(argv[6]));
    35 
    36   ros::NodeHandle nh;
    37   ros::AsyncSpinner spinner(1);
    38   spinner.start();
    39 
    40   std::vector<std::string> arm_left_joint_names;
    41   //select group of joints
    42   moveit::planning_interface::MoveGroupInterface group_arm_left("arm_left");
    43   //choose your preferred planner
    44   group_arm_left.setPlannerId("SBLkConfigDefault");
    45   group_arm_left.setPoseReferenceFrame("base_footprint");
    46   group_arm_left.setPoseTarget(goal_pose);
    47 
    48   ROS_INFO_STREAM("Planning to move " <<
    49                   group_arm_left.getEndEffectorLink() << " to a target pose expressed in " <<
    50                   group_arm_left.getPlanningFrame());
    51 
    52   
    53   group_arm_left.setStartStateToCurrentState();
    54   group_arm_left.setMaxVelocityScalingFactor(1.0);
    55   
    56   //Current position
    57 
    58   ROS_INFO_STREAM(group_arm_left.getCurrentPose());
    59 
    60   moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    61   //set maximum time to find a plan
    62   group_arm_left.setPlanningTime(5.0);
    63   bool success = bool(group_arm_left.plan(my_plan));
    64 
    65   if ( !success )
    66     throw std::runtime_error("No plan found");
    67 
    68   ROS_INFO_STREAM("Plan found in " << my_plan.planning_time_ << " seconds");
    69 
    70   // Execute the plan
    71   ros::Time start = ros::Time::now();
    72 
    73   moveit::planning_interface::MoveItErrorCode e = group_arm_left.move();
    74   if (!bool(e))
    75     throw std::runtime_error("Error executing plan");
    76 
    77   ROS_INFO_STREAM("Motion duration: " << (ros::Time::now() - start).toSec());
    78 
    79   spinner.stop();
    80 
    81   return EXIT_SUCCESS;
    82 }
  

Note that when a plan is found and is executed with the following line of code

  73   moveit::planning_interface::MoveItErrorCode e = group_arm_left.move();

The required control commands are sent to the left arm controller through its Action interface:

• /arm_left_controller/follow_joint_trajectory/goal