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. The strength only matters if there are conflicting goal and can be fine tuned in the config file

collision_avoidance:
  distance_thresholds:
    default:
      max_weight_distance: 0.0
      low_weight_distance: 0.01
      zero_weight_distance: 0.05
    base_link:
      max_weight_distance: 0.04
      low_weight_distance: 0.06
      zero_weight_distance: 0.09

At max_weight_distance(in meter), the repelling force reaches its maximum and will be stronger than any other constraint. At zero_weight_distance, the collision avoidance becomes active and will be enforced, if there are no conflicting constraints. low_weight_distance defines how fast the force reaches its maximum. Most constraints are able to overpower the collision avoidance up to a certain point. However, this exact point is difficult to calculate. It depends on how many constraints are pushing against the collision avoidance.

If set in the config file, giskard publishes markers that visualize the collision avoidance:

Each of these string represent the distance to a close object. The robot links are pushed away in the direction of that string. Green and yellow strings only indicate close objects, when the strings because red, the collision avoidance is active.

External Collision Avoidance

External collision avoidance works by repelling robot links from close objects in the environment. Since each repeller needs one constraints, the links are grouped to reduce that number of constraints needed. All links between two movable joints are considered a group. Giskard adds n repel constraints for each link group, this number can be set in the config file:

external_collision_avoidance:
  number_of_repeller: 5 # each movable joint gets pushed away from the 5 objects closest to it

Self Collision Avoidance

Giskard calculates a self collision matrix the first time it sees a new urdf. This is a list of link pairs that may collide. You can overwrite the collision matrix using the config file.

self_collision_avoidance: # each pair of links in the collision matrix get push away from each other
  ignore: [] # list pairs of links that should be removed from the self collision avoidance
  add: # list pairs of links that should be added to the self collision matrix
    - [base_link, head_mount_kinect_ir_link] 

Each pair in this matrix gets repelled from each other.

Smarter than the collision avoidance

The collision avoidance works fine most of the time, but if it doesn't you can add exceptions. When constructing a move goal, you can also set collision entries.

  giskard_msgs/CollisionEntry[] collisions
    uint8 AVOID_COLLISION=0
    uint8 ALLOW_COLLISION=1
    uint8 AVOID_ALL_COLLISIONS=2
    uint8 ALLOW_ALL_COLLISIONS=3
    string ALL= # this means empty string
    uint8 type
    float64 min_dist
    string[] robot_links
    string body_b
    string[] link_bs

Per default, Giskard will try to avoid collisions with the environment. 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.

A message to achieve that example would need type=1, robot_links=[r_gripper_l_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. You can also use this interface to overwrite the self collision avoidance by using the name of the robot as body b.

Collision entries are parsed in sequence. If your 1. entries allows all collisions and your 2. entry avoids all, then the avoid all will overwrite the allow all. So if you only want self collision avoidance, you first need an entry that allows all collisions and then add one that avoids self collision.

Don't use min_dist, it is only present for backward compatibility and will be removed eventually.

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 or ALLOW_COLLISION

Here are some other useful examples:

  1. turn off collision avoidance: type=1, robot_links=[CollisionEntry.ALL], body_b=CollisionEntry.ALL, link_bs=[CollisionEntry.ALL]
  2. turn off self collision avoidance: type=1, robot_links=[CollisionEntry.ALL], body_b=<robot name in urdf>, link_bs=[CollisionEntry.ALL]