Giskards Behavior Tree

Giskard uses py_trees to structure its program flow. Read their documentation to learn about behavior trees in general. From now on, it is assumed, that the reader understands behavior trees. Giskard's behavior tree looks something like this, with minor differences depending on the commit: The wait for goal branch is the idle mode of Giskard, where it waits for goals. In that loop, plugins publish tf frames of the environment if enabled, listens to the robots joint states to update the godmap, and processes services calls that modify the pybullet environment. Once a goal has been received, Giskard will process the goal to generate a trajectory in process move goal, send it to the robot in move robot and finally return a result to the client.

The planning branch has 5 different layers. The top most layer reads the ros messages, hands them to planning I and calls some debugging tools, if enabled.

The planning I branch parses the ROS message in update constraints ( plugin_update_constraints.py). This is the place where constraints, including your custom ones, are parsed.

The planning II branch includes a guard which checks if the goal has been canceled. Otherwise it just calls some visualization plugins, until planning III returns success or failure.

The planning III phase runs planning IIII and calls some post processing plugins as well as the visualization plugins again, after planning IIII has succeeded.

The planning IIII branch is a custom composite (https://github.com/SemRoCo/giskardpy/blob/devel/src/giskardpy/plugin.py), which executes its children in a separate thread. This is important to allow Giskard to check the action server state and publish visualizations, without slowing down the planning, or being blocked by it. All of its children will return the running state and thus block planning III, until one of the children returns either success or failure. The children implement a closed loop controller, which is executed in a simple kinematic simulation. The most important plugin is the controller. Here, the a quadratic program is solved to compute the next velocity command. The kin sim plugin just integrates these commands. Other plugins are responsible to check if the planning should be interrupted, either because the goal has been reached or a failure has occurred. Once this happens, planning IIII stops, planning III does its post processing and final visualizations, planning runs it debugging plugins, and move robot sends a trajectory to the robot.