The World

Giskard uses Pybullet to keep track of a world model. The world model can be viewed by starting Giskard like this

roslaunch giskardpy giskardpy_pr2.launch gui:=true

The robot is always part of the world, but other objects can be added.

Objects are added through a ROS service. All objects are represented as URDF. You can also make Giskard listen to joint states topic for each object in the world. Giskard will update the joint state of that object with every message. This can be used to keep track of other robots, or to change the state of movable parts of your environment.

Collision Avoidance

The world model is primarily used for collision avoidance. The collision avoidance is implemented with a set of constraints that repel robot links from close objects in the environment. The closer the robot link is to the object, the stronger the force pushing it away. Collision avoidance is active by default. It can be influenced by two means: with the config file and with collision entries, when sending a goal.

Config File

Here is an example of the collision part of PR2's config file:

collision_avoidance:
  external_collision_avoidance:
    distance_thresholds: # external thresholds are per joint, they therefore count for all directly controlled links
      default:
        hard_threshold: 0.0 # at this distance in [m] that can not be surpassed
        soft_threshold: 0.1 # robot links are pushed to that distance, but it can be overpowered with high weight constraints
      override:
        zero: &zero
          hard_threshold: 0.0
          soft_threshold: 0.0
        5cm: &5cm
          hard_threshold: 0.0
          soft_threshold: 0.05
        25mm: &25mm
          hard_threshold: 0.0
          soft_threshold: 0.025
        odom_z_joint: # this affects all links between this and the next controlled joint
          hard_threshold: 0.05 # at this distance in [cm] that can not be surpassed
          soft_threshold: 0.1 # robot links are pushed to that distance, but it can be overpowered with high weight constraints
        r_forearm_roll_joint: *25mm
        l_forearm_roll_joint: *25mm
        r_elbow_flex_joint: *5cm
        l_elbow_flex_joint: *5cm
        r_wrist_roll_joint: *5cm
        l_wrist_roll_joint: *5cm
        r_wrist_flex_joint: *5cm
        l_wrist_flex_joint: *5cm
    # each controlled joint gets pushed away from its X nearest objects
    # to reduce the runtime, you can set different values for joints that control end effectors
    number_of_repeller:
      default: 1
      end_effector_joints: 4
  self_collision_avoidance:
    distance_thresholds: # thresholds for self collision avoidance are set for each link pair
      default: &default # you can set variables and reuse them
        hard_threshold: 0.0 # at this distance in [m] that can not be surpassed
        soft_threshold: 0.05 # robot links are pushed to that distance, but it can be overpowered with high weight constraints
      override:
        # examples, that don't have an affect:
        base_link: *default  # this would count for all pairs containing 'base_link', if not specified otherwise
        base_link, r_upper_arm_link: *default # you can set thresholds for specific pairs
    ignore: [] # list pairs of links that should be removed from the self collision avoidance
#    ignore: #example
#      - [base_link, l_upper_arm_link]
    add: [] # list pairs of links that should be added to the self collision matrix
    number_of_repeller: 1 # each movable joint gets pushed away from the X objects closest to it

Collision avoidance is split into external collision avoidance and self collision avoidance. Both of them share the distance_thresholds entries. It should always contain a default with a hard_threshold and a soft_threshold. The hard_threshold is a distance threshold, that Giskard is not allowed to cross. A Value of 0.0 m is generally a good idea. Be careful, when setting it to a higher value, because if the robots comes inside of threshold, it is difficult to get out again. This could for example happen, if an object is spawned inside of the robot. If that happens, Giskard will return a HARD_CONSTRAINTS_VIOLATED error code.

The soft_threshold is a keep that Giskard will try to keep, if possible, but it is allowed to get closer, depending on the other goals. Read the Weights section for more information. It might be the case, that you want to specify different thresholds for different links. For that case, you can specify overrides. In the PR2 example, the default is a soft_threshold of 10cm and for some links an override with 5cm is set. When setting overrides for the self collision avoidance, you can also set the thresholds for specific link pairs.

The next entry, that external and self collision have in common is the number_of_repeller. This number dictates how many collisions are considered for each link or link pair for external and self collision respectively. For the external collision avoidance you can further set a different number for end effectors, which are link that don't have a child link. Since the robot is usually interacting with the environment using its end effectors, it might make sense to choose a higher number here. A higher number results in a more stable collision avoidance, when many objects are close, but it slows down the planning.

To save runtime, a self collision matrix is calculated, when a robot is first loaded into giskard, and saved in the data folder in the giskardpy directory. When calculating the self collision matrix, Giskard puts the robot into random configurations, to determine link pairs that are never or always in collision. Because this is a random process, it might miss certain pairs. For that reason, the user has the option to use the ignore and add entries in the config file to modify the collision matrix. If you change these entries, makes sure to delete the self collision matrix in the data folder, to trigger the computation of a new one, that takes the changes into account!

You can set a parameter in the config file to visualize the collisions.

plugins:
  CPIMarker: # contact visualization, slows planning down a little bit
    enabled: True

If it is set, giskard publishes markers that visualize the collision avoidance:

Each of these string represent the distance to a close object. Red means that the object is closer than 5cm, yellow indicates a distance of 10cm and green a distance of 20cm.

CollisionEntry

When interacting with the environment, it might be necessary to override the settings from the config file, e.g. when touching objects. For this reason, collision entries can be send with each goal.

# definitions of possible collision types to use for value
uint8 AVOID_COLLISION=0
uint8 ALLOW_COLLISION=1
uint8 AVOID_ALL_COLLISIONS=2
uint8 ALLOW_ALL_COLLISIONS=3
string ALL= #this means empty string

# use one of the above constants as collision type
uint8 type

# enforced minimum distance between the closest points on both surfaces
float64 min_dist

# link of the controlled robot, if left empty all links will be selected
string[] robot_links

# name of body B, i.e. the second body in the described collision, can also be the controlled robot
string body_b
# optional: link of the particular link on body B
## note: only works if body B is a multibody with a link with that name
# note: if left empty, the entire body B is selected
string[] link_bs

Per default, Giskard will try to avoid collisions with the environment. So you can assume, that there is always a collision entries, that tells Giskard to avoid all collisions. However, sometimes you might want to add exceptions. For example, when a cup is modeled as a cylinder and the robot should reach inside with a finger, you have to allow that collision. This is the scenario where this interface comes into play. Collision entries are always defined between the robot and one object of the environment. To talk about self collision, you can set the environment object, to the robots name. For both, a link list can be set, if you want to be more specific.

A message to allow the collision between the PR2's fingers and a cup might look like this: type=1, robot_links=[r_gripper_l_finger_link, r_gripper_r_finger_link], body_b=cup, link_bs=[CollisionEntry.ALL]. Robot links contains only names of the controlled robot, as they are in the urdf. Body b is the name you gave the object, when it got added to the world. Link bs is a list of links for body b. If you are dealing with primitive objects, this can be empty. But other objects can also be urdf, where you would list the link names from the urdf again. A list with an empty string will select all links.

Collision entries are parsed in sequence, and the 0. entry is an avoid all, that gets added by Giskard. If your 1. entries allows all collisions and your 2. entry avoids all, then the avoid all will overwrite the allow all. This means, that any entries before an avoid all or allow all, are being ignored. The min_dist entry is only used in combination with AVOID_ALL_COLLISIONS. If you do that, the soft_threshold for all links and link combinations gets overwritten with min_dist. This might be useful, if you want to drive around, without interacting with the environment.

Here are some other useful examples:

  • turn off self collision avoidance: type=CollisionEntry.ALLOW_COLLISION, robot_links=[CollisionEntry.ALL], body_b=<robot name in urdf>, link_bs=[CollisionEntry.ALL]
  • AVOID_ALL_COLLISIONS and ALLOW_ALL_COLLISIONS are equivalent to robot_links=[CollisionEntry.ALL], body_b=CollisionEntry.ALL and link_bs=[CollisionEntry.ALL] with type being equal to AVOID_COLLISION and ALLOW_COLLISION, respectively.