As already mentionied, you can combine multiple controller messages in the controllerlist action message and Giskard tries to satisfy every controller at the same time. So, let's see what happens when we combine Joint space and Cartesian space commands.

The Code

In the source folder of the giskard_tutorials package, create joint_and_cartesian_space_commands.cpp and add the following code.

#include <giskard_msgs/ControllerListAction.h> 
#include <actionlib/client/simple_action_client.h>
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "joint_and_cartesian_controller");
  actionlib::SimpleActionClient<giskard_msgs::ControllerListAction> client("/qp_controller/command", true);
  client.waitForServer();
 
  giskard_msgs::ControllerListGoal goal;
 
 
  // create the cartesian space controller
  giskard_msgs::Controller cartesian_space_controller;
 
  cartesian_space_controller.type = giskard_msgs::Controller::TRANSLATION_3D;
  cartesian_space_controller.root_link = "base_link";
  cartesian_space_controller.tip_link = "l_gripper_tool_frame";
  cartesian_space_controller.p_gain = 1;
  cartesian_space_controller.weight = 1;
  cartesian_space_controller.enable_error_threshold = true;
  cartesian_space_controller.threshold_value = 1;
  cartesian_space_controller.goal_pose.header.frame_id = "base_link";
 
  cartesian_space_controller.goal_pose.pose.position.x = 0.0;
  cartesian_space_controller.goal_pose.pose.position.y = 0.5;
  cartesian_space_controller.goal_pose.pose.position.z = 1.3;
 
 
  // create the joint space controller
  giskard_msgs::Controller joint_space_controller;
 
  joint_space_controller.type = giskard_msgs::Controller::JOINT;
  joint_space_controller.root_link = "torso_lift_link";
  joint_space_controller.tip_link = "l_gripper_palm_link";
  joint_space_controller.p_gain = 1;
  joint_space_controller.weight = 1;
  joint_space_controller.enable_error_threshold = false;
  joint_space_controller.threshold_value = 0.1;
  joint_space_controller.goal_pose.header.frame_id = "base_link";
 
  joint_space_controller.goal_state.name = {"l_shoulder_pan_joint", "l_upper_arm_roll_joint", "l_shoulder_lift_joint", "l_forearm_roll_joint", "l_elbow_flex_joint", "l_wrist_flex_joint", "l_wrist_roll_joint"};
 
  joint_space_controller.goal_state.position = {0.5, 1, 0.2, 0, -0.8, -1, 0};
 
 
  // add both controllers to the ActionList message
  goal.controllers.push_back(joint_space_controller);
  goal.controllers.push_back(cartesian_space_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

Thsi time we create two controller messages and add them to the controller list action. As mentioned above Giskard takes every controller in the contoller list action into account when calculating the motion. The weight value in the controller message determines how much Giskard weighs the controller.

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

Include the headers and create the action client as in the previous tutorials.

  // create the cartesian space controller
  giskard_msgs::Controller cartesian_space_controller;
 
  cartesian_space_controller.type = giskard_msgs::Controller::TRANSLATION_3D;
  cartesian_space_controller.root_link = "base_link";
  cartesian_space_controller.tip_link = "l_gripper_tool_frame";
  cartesian_space_controller.p_gain = 1;
  cartesian_space_controller.weight = 1;
  cartesian_space_controller.enable_error_threshold = true;
  cartesian_space_controller.threshold_value = 1;
  cartesian_space_controller.goal_pose.header.frame_id = "base_link";
 
  cartesian_space_controller.goal_pose.pose.position.x = 0.0;
  cartesian_space_controller.goal_pose.pose.position.y = 0.5;
  cartesian_space_controller.goal_pose.pose.position.z = 1.3;

This should look familar from the previous tutorials as well. Here we create the cartesian space controller.

  // create the joint space controller
  giskard_msgs::Controller joint_space_controller;
 
  joint_space_controller.type = giskard_msgs::Controller::JOINT;
  joint_space_controller.root_link = "torso_lift_link";
  joint_space_controller.tip_link = "l_gripper_palm_link";
  joint_space_controller.p_gain = 1;
  joint_space_controller.weight = 1;
  joint_space_controller.enable_error_threshold = false;
  joint_space_controller.threshold_value = 0.1;
  joint_space_controller.goal_pose.header.frame_id = "base_link";
 
  joint_space_controller.goal_state.name = {"l_shoulder_pan_joint", "l_upper_arm_roll_joint", "l_shoulder_lift_joint", "l_forearm_roll_joint", "l_elbow_flex_joint", "l_wrist_flex_joint", "l_wrist_roll_joint"};
 
  joint_space_controller.goal_state.position = {0.5, 1, 0.2, 0, -0.8, -1, 0};

In this step we create the joint space controller just like in the Moving a single arm in joint space tutorial.

//add both controllers to the ActionList message
  goal.controllers.push_back(joint_space_controller);
  goal.controllers.push_back(cartesian_space_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;
}

And finally we add both controllers to controller list action message.

Building The Node

Add these two lines to the CMakeLists.txt

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

Running The Node

Launch the simulation and set up the environment

roslaunch giskard_pr2 pr2_qp_controller.launch

Run the node

rosrun giskard_tutorials joint_and_cartesian_action_client

The robot should start moving now.

Movement Graphs

This section shows how weights for the controllers in the controller list action influence the motion of the robot. But first, let's have a look at the movement graphs when both controller have a weight of 1. You can see that neither the desired joint states nor the goal position are achieved completely. The reason for that is that giskard tries to satisfy the goals of both controllers at the same time.

Let's see what happens when we change the weight of the joint controller to 0.5. In this case the position goal has a higher weight and therefore the robot moves the tip of the gripper closer to the goal position. The joint states on the other hand become less important and differ more from the goal states than in in previous example.