Moving a single arm in cartesian space works very similar to moving a single arm in joint space. So, let's get started.

The Code

Head over to the giskard_tutorials package we created in the last tutorial and create a new file called cartesian_action_client.cpp. Then paste the following code inside it.

#include <giskard_msgs/ControllerListAction.h> 
#include <actionlib/client/simple_action_client.h>
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "cartesian_space_action_client");
  actionlib::SimpleActionClient<giskard_msgs::ControllerListAction> client("/qp_controller/command", true);
  client.waitForServer();
 
  giskard_msgs::ControllerListGoal goal;
  giskard_msgs::Controller controller;
 
  controller.type = giskard_msgs::Controller::TRANSLATION_3D;
  controller.root_link = "torso_lift_link";
  controller.tip_link = "l_gripper_tool_frame";
  controller.p_gain = 1;
  controller.weight = 1;
  controller.enable_error_threshold = true;
  controller.threshold_value = 0.1;
  controller.goal_pose.header.frame_id = "base_link";
 
  controller.goal_pose.pose.position.x = 0.3;
  controller.goal_pose.pose.position.y = 0.5;
  controller.goal_pose.pose.position.z = 1;
 
  goal.controllers.push_back(controller);
 
  client.sendGoal(goal);
  client.waitForResult(ros::Duration(5.0));
  if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    printf("Success \n");
  printf("Current State: %s\n", client.getState().toString().c_str());
  return 0;
}

The Code Explained

The code above should look very familiar if you read the joint space tutorial. But let's go through it, to see what is different.

#include <giskard_msgs/ControllerListAction.h> 
#include <actionlib/client/simple_action_client.h>
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "cartesian_space_action_client");
  actionlib::SimpleActionClient<giskard_msgs::ControllerListAction> client("/qp_controller/command", true);
  client.waitForServer();
 
  giskard_msgs::ControllerListGoal goal;
  giskard_msgs::Controller controller;

Include the headers and create the action client and messages.

  controller.type = giskard_msgs::Controller::TRANSLATION_3D;
  controller.root_link = "torso_lift_link";
  controller.tip_link = "l_gripper_tool_frame";
  controller.p_gain = 1;
  controller.weight = 1;
  controller.enable_error_threshold = true;
  controller.threshold_value = 0.1;
  controller.goal_pose.header.frame_id = "base_link";
 
  controller.goal_pose.pose.position.x = 0.3;
  controller.goal_pose.pose.position.y = 0.5;
  controller.goal_pose.pose.position.z = 1;
 
  goal.controllers.push_back(controller);

Instead of filling the goal state field of the controller message, we fill the goal position field this time. Because this is a position controller, the goal pose defines the goal position of the tip_link. In order to let the action server know that we are sending a position controller this time, we set the type to giskard_msgs::Controller::TRANSLATION_3D.

  client.sendGoal(goal);
  client.waitForResult(ros::Duration(5.0));
  if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    printf("Success \n");
  printf("Current State: %s\n", client.getState().toString().c_str());
  return 0;
}

Wait for the result.

Building The Node

Just like in the previous tutorial, add these two lines the the CmakeLists.txt

add_executable(cartesian_action_client src/cartesian_action_client.cpp)
target_link_libraries(cartesian_action_client ${catkin_LIBRARIES})

And run catkin build.

Running The Node

Launch the simulation and set up the environment

roslaunch giskard_pr2 pr2_qp_controller.launch

Run the node

rosrun giskard_tutorials cartesian_action_client

The robot should start moving now.

Movement Graphs

The following graph shows the distance of the tip link to the goal position. The red dashed line represents the time when the controllerList action was send to the action server. So, the robots starts to move about 0.5 seconds after the goal was send. The velocity of the tip link is constant at first because every joint of the robot is moving as fast as possible already. When the tip link gets closer to the goal position the velocity is decreased.