The Action Server Interface

Giskardpy offers and requires certain interfaces, which are visualized in the following picture, with PR2 as an example.

Required Interfaces

Giskardpy generates joint trajectories and thus requires an action server of type control_msgs/FollowJointTrajectoryActionGoal, that executes them. However, sometimes a robot does not offer an interface that executes trajectories for all its joint. For example, the PR2 has separate interfaces for the arms, base, head and gripper. If you want to control all of these together, you have to use the goal splitter, which is also part of the Giskardpy package. It offers a single action servers of type control_msgs/FollowJointTrajectoryActionGoal for Giskard and when it receives a goal, it gets split into different goals for each action server of the robot. For example, if you want Giskard to plan for the base as well, you have to use the goal splitter to merge the action server for the arms and the base.

Offered Interfaces

Giskard offeres an action server to plan and execute goals and a service interface to modify its beliefstate.

Service Interface

The Service interface is offered over /giskard/update_world and of type giskard_msgs/UpdateWorld which looks like this:

# Request

# Some constants to fill the field operation
uint8 UNDEFINED=0

# Add a new body to the world. If a body with the same name is already present, 
# an error will be thrown.
uint8 ADD=1

# Remove a body from the world. If no body with that name exists, an error will
# be thrown.
uint8 REMOVE=2

# Alter the properties of an existing world body. If no body with that name exists,
# an error will be thrown.
uint8 ALTER=3

# Remove all known bodies from the world. The robot will remain. This operation ignores
# the world body specified in the body field of the request.
uint8 REMOVE_ALL=4

# detach object from robot and spawn it into the world.
# everything but the name in body is being ignored
uint8 DETACH=5

# Type of operation that shall be performed on the world representation
uint8 operation

# Description of the body that shall be added to/removed from/altered within the world.
giskard_msgs/WorldBody body
  uint8 UNDEFINED=0
  uint8 PRIMITIVE_BODY=1
  uint8 MESH_BODY=2
  uint8 URDF_BODY=3
  uint8 type
  string name
  shape_msgs/SolidPrimitive shape
    uint8 BOX=1
    uint8 SPHERE=2
    uint8 CYLINDER=3
    uint8 CONE=4
    uint8 BOX_X=0
    uint8 BOX_Y=1
    uint8 BOX_Z=2
    uint8 SPHERE_RADIUS=0
    uint8 CYLINDER_HEIGHT=0
    uint8 CYLINDER_RADIUS=1
    uint8 CONE_HEIGHT=0
    uint8 CONE_RADIUS=1
    uint8 type
    float64[] dimensions
  string mesh # path to mesh
  string urdf
  string joint_state_topic # used in combination with urdf, giskard will listen to this topic for joint state updates.

# Flag that specifies whether the body shall be rigidly attached to an existing 
# link of the robot
bool rigidly_attached

# Pose of the body in the world. TF will be used to resolve the header information.
# Note: If the body shall be rigidly attached to the robot, then the frame_id from 
# the header of this PoseStampled must be the name of a link of the robot. 
geometry_msgs/PoseStamped pose
---
# Response

# constants to use as error codes
uint8 SUCCESS=0
uint8 MISSING_BODY_ERROR=1
uint8 DUPLICATE_BODY_ERROR=2
uint8 CORRUPT_SHAPE_ERROR=3
uint8 CORRUPT_MESH_ERROR=4
uint8 CORRUPT_URDF_ERROR=5
uint8 TF_ERROR=6
uint8 MISSING_LINK_ERROR=7
uint8 INVALID_OPERATION=8
uint8 UNSUPPORTED_OPTIONS=9

# errors that occured during the service
uint8 error_codes

# optional additional error message
string error_msg

All objects added to the world are used for collision avoidance. The interface is pretty straight forward for the most part. It can be used to add primitives, meshes or urdfs. Internally everything gets turned into a urdf. If you are adding a urdf, for example for a second robot, you can also specify a joint states topic, which giskard will use to automatically update the joint state. If you combine ADD with rigidly_attached=True, then the object will be attached to the robot at the frame_id used in pose. If an object with that name already exists, then it will be removed from the world and attached to the robot. In that case, body and the pose in pose will be ignored.

Action Server Interface

The action server listens to /giskard/command and accepts goals of type giskard_msgs/MoveAction:

# goal

# definitions of possible values to use as type
# the following types can be added to create new combinations. e.g. PLAN_AND_EXECUTE = PLAN_ONLY + EXECUTE
int64 UNDEFINED=0
int64 PLAN_ONLY=1 
int64 CHECK_REACHABILITY=2 # don't use on its own! Different planning parameters that speed up planning but don't create nice trajectories, don't use with execute!
int64 EXECUTE=4 # don't use on its own! Result gets send to the robot
int64 SKIP_FAILURES=8 # don't use on its own! If there are multiple move commands, failures are skipped.
int64 CUT_OFF_SHAKING=16 # don't use on its own! If endless shaking is detected, the size of the detection window is cut off from the trajectory and it is executed anyway

# predefined combinations
int64 PLAN_AND_CHECK_REACHABILITY=3
int64 PLAN_AND_EXECUTE=5
int64 PLAN_AND_SKIP_FAILURES=9
int64 PLAN_AND_EXECUTE_AND_SKIP_FAILURES=13
int64 PLAN_AND_EXECUTE_AND_CUT_OFF_SHAKING=21

# use one of the above constants to indicate the goal type
int64 type

# sequence of movement commands to execute
giskard_msgs/MoveCmd[] cmd_seq  
---
# result

# definition of possible error code values
int64 SUCCESS=0
int64 ERROR=1 # if no code fits

# solver exceptions
int64 QP_SOLVER_ERROR=10 # if no solver code fits
int64 MAX_NWSR_REACHED=11 # increasing NWSR in config file might fix this
int64 OUT_OF_JOINT_LIMITS=12
int64 HARD_CONSTRAINTS_VIOLATED=13 # conflicting hard constraints, probably because of collision avoidance

# world state exceptions
int64 WORLD_ERROR=20 # if no world error fits
int64 UNKNOWN_OBJECT=21

# error during motion problem building phase
int64 CONSTRAINT_ERROR=30 # if no constraint code fits
int64 UNKNOWN_CONSTRAINT=31
int64 CONSTRAINT_INITIALIZATION_ERROR=32
int64 INVALID_GOAL=33

# errors during planning
int64 PLANNING_ERROR=40 # if no planning code fits
int64 SHAKING=41 # Planning was stopped because the trajectory contains a shaky velocity profile. Detection parameters can be tuned in config file
int64 UNREACHABLE=42 # if reachability check fails

# errors during execution
int64 EXECUTION_ERROR=50 # if no execution code fits
int64 PREEMPTED=51 # goal got cancled via action server interface

# error codes and messages for each command
int64[] error_codes

string[] error_messages

# planned trajectory
trajectory_msgs/JointTrajectory trajectory
---
# feedback

# definitions of possible action phases
int64 UNDEFINED=0
int64 PLANNING=1
int64 EXECUTION=2

# cmd that is currenlty being planned
int64 cmd_id

# value between 0.0 and 1.0 to report progress 
# only published during phases EXECUTION
float64 progress

Each goal is composed of a list of giskard_msgs/MoveCmd, where each entry is a single goal. If it contains multiple items, Giskard will plan a trajectory that executes all of them in a sequence. The different items do not influence each other besides the fact that the end state of the previous motion will be the start state for the next. Each move command contains a list of giskard_msgs/Constraint, giskard_msgs/JointConstraint, giskard_msgs/CartesianConstraint and giskard_msgs/CollisionEntry. The first three are constraints that are used to specify the goal of your motion. Joint constraints and cartesian constraints are just special cases of constraints with a more convenient interface. Click on joint and cartesian constraints to find examples. giskard_msgs/Constraint can be used to call more advanced or your on constraints.

Move commands also contain a list of giskard_msgs/CollisionEntry, that can be used to modify the collision avoidance. Find more information here.

The result will contain an error code and message for each MoveCmd.