This tutorials shows you how to send joint goals to the robot in python using the action interface.

The Code

import actionlib
import rospy
from giskard_msgs.msg import MoveAction
from giskard_msgs.msg import MoveCmd, MoveResult, CartesianConstraint, CollisionEntry
from giskard_msgs.msg import MoveGoal
 
 
def execute_cartesian_goal():
    # Creates the SimpleActionClient, passing the type of the action
    # (MoveAction) to the constructor.
    client = actionlib.SimpleActionClient("/giskardpy/command", MoveAction)
 
    # Waits until the action server has started up and started
    # listening for goals.
    print('waiting for giskard')
    client.wait_for_server()
    print('connected to giskard')
 
    # Creates a goal to send to the action server.
    action_goal = MoveGoal()
    action_goal.type = MoveGoal.PLAN_AND_EXECUTE
 
    goal = MoveCmd()
 
    cartesian_goal = CartesianConstraint()
 
    cartesian_goal.type = cartesian_goal.POSE_6D
 
    # specify the kinematic chain
    cartesian_goal.root_link = 'odom_combined'
    cartesian_goal.tip_link = 'l_gripper_tool_frame'
 
    cartesian_goal.goal.header.frame_id = 'l_gripper_tool_frame'
    cartesian_goal.goal.pose.position.y = -0.1
    cartesian_goal.goal.pose.orientation.w = 1
 
    goal.cartesian_constraints.append(cartesian_goal)
 
    # allow all collision
    ce = CollisionEntry()
    ce.robot_links = [CollisionEntry.ALL]
    ce.body_b = CollisionEntry.ALL
    ce.link_bs = [CollisionEntry.ALL]
    ce.type = CollisionEntry.ALLOW_COLLISION
    goal.collisions.append(ce)
 
    action_goal.cmd_seq.append(goal)
 
    # Sends the goal to the action server.
    client.send_goal(action_goal)
 
    # Waits for the server to finish performing the action.
    client.wait_for_result()
 
    result = client.get_result()  # type: MoveResult
    if result.error_code == MoveResult.SUCCESS:
        print('giskard returned success')
    else:
        print('something went wrong')
 
 
if __name__ == '__main__':
    try:
        # Initializes a rospy node so that the SimpleActionClient can
        # publish and subscribe over ROS.
        rospy.init_node('joint_space_client')
        execute_cartesian_goal()
    except rospy.ROSInterruptException:
        print("program interrupted before completion")

The Code explained

cartesian_goal = CartesianConstraint()
 
cartesian_goal.type = cartesian_goal.POSE_6D

In the first part we decide on which type of cartesian goal we want to use. In contrast to the previous tutorial, we have three options: TRANSLATION_3D, ROTATION_3D, POSE_6D. With the first one, only a position is enforced and Giskard chooses whatever rotation is the closest or most convenient according to the other constraints. The second one only sets a rotation goal and the last one sets a goal for both at the same time. In this example we set a POSE_6D goal.

# specify the kinematic chain
cartesian_goal.root_link = 'odom_combined'
cartesian_goal.tip_link = 'l_gripper_tool_frame'
 
cartesian_goal.goal.header.frame_id = 'l_gripper_tool_frame'
cartesian_goal.goal.pose.position.y = -0.1
cartesian_goal.goal.pose.orientation.w = 1

In the next part, the kinematic chain and the goal pose is specified. You can choose any two links of the robot. The goal pose is transformed into the root link frame and Giskard will then use only the joints between both links to place the tip into the goal pose. In most cases you probably want the root_link to be the base link of the robot. In this case we choose torso_lift_link. This link is positioned after the torso joint, as a result, the robot will not use its slow torso to achieve the goal. With TRANSLATION_3D and ROTATION_3D, the rotation or translation are ignored respectively.

goal.cartesian_constraints.append(cartesian_goal)

At the end we append the cartesian goal to the MoveCmd. It would be equivalent to have a TRANSLATION_3D and a ROTATION_3D with the same chain and goal pose. In fact, a cartesian goal gets translated into such two goals internally.

Running The Code

First we have to launch giskard and the PR2 simulation so open a terminal and enter the following commands.

roslaunch iai_pr2_sim ros_control_sim_with_base.launch
roslaunch giskardpy giskardpy_pr2.launch

Now, create the file action_client.py and paste the code from above inside it. We can run the code by entering the following command into the terminal.

python action_client.py