Differences

This shows you the differences between two versions of the page.

Link to this comparison view

Both sides previous revision Previous revision
Next revision
Previous revision
wiki:tutorials:collision_avoidance [2019/11/18 13:28]
simon [Collision Avoidance]
wiki:tutorials:collision_avoidance [2020/10/22 08:16] (current)
simon [Collision Avoidance]
Line 19: Line 19:
 The collision avoidance is implemented with a set of constraints that repel robot links from close objects in the environment. 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 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 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:
 <code yaml> <code yaml>
 collision_avoidance:​ collision_avoidance:​
-  ​distance_thresholds+  ​external_collision_avoidance
-    ​default+    ​distance_thresholds# external thresholds are per joint, they therefore count for all directly controlled links 
-      ​max_weight_distance: 0.0 +      ​default: 
-      ​low_weight_distance: 0.01 +        hard_threshold: 0.0 # at this distance in [m] that can not be surpassed 
-      ​zero_weight_distance: 0.05 +        ​soft_threshold: 0.1 # robot links are pushed to that distance, but it can be overpowered with high weight constraints 
-    ​base_link+      ​override: 
-      ​max_weight_distance: 0.04 +        zero: &zero 
-      ​low_weight_distance: 0.06 +          hard_threshold:​ 0.0 
-      ​zero_weight_distance: 0.09+          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
 </​code>​ </​code>​
-At ''​max_weight_distance''​(in meter), the repelling force reaches its maximum ​and will be stronger than any other constraint+Collision avoidance is split into external collision avoidance and self collision avoidance. 
-At ''​zero_weight_distance'', ​the collision avoidance becomes active and will be enforced, if there are no conflicting constraints+Both of them share the ''​distance_thresholds''​ entries. It should always contain a default with a ''​hard_threshold''​ and a ''​soft_threshold''​.  
-''​low_weight_distance'' ​defines how fast the force reaches its maximum+The ''​hard_threshold'' ​is a distance thresholdthat Giskard is not allowed to cross
-Most constraints are able to overpower the collision avoidance up to a certain point.  +A Value of ''​0.0'' ​m is generally a good idea.  
-Howeverthis exact point is difficult to calculate.  +Be careful, when setting it to a higher value, because if the robots comes inside of thresholdit is difficult to get out again
-It depends on how many constraints are pushing against ​the collision avoidance.+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.
  
-If set in the config filegiskard publishes markers ​that visualize ​the collision avoidance:+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.
  
-{{:​wiki:​tutorials:​collision_example.png?400|}}+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.
  
-Each of these string represent the distance to close object.  +To save runtime, ​self collision matrix is calculated, when a robot is first loaded into giskard, and saved in the ''​data''​ folder in the giskardpy directory
-The robot links are pushed away in the direction of that string+When calculating the self collision matrix, Giskard puts the robot into random configurations,​ to determine link pairs that are never or always ​in collision
-Green and yellow strings only indicate close objectswhen the strings because red, the collision ​avoidance is active.+Because this is a random processit might miss certain pairs. 
 +For that reasonthe 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!
  
-===External Collision Avoidance=== +You can set a parameter ​in the config file to visualize the collisions.
-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:+
 <code yaml> <code yaml>
-external_collision_avoidance+plugins
-  ​number_of_repellereach movable joint gets pushed away from the 5 objects closest to it+  ​CPIMarker: # contact visualization,​ slows planning down a little bit 
 +    enabled: True
 </​code>​ </​code>​
-===Self Collision Avoidance=== + 
-Giskard calculates a self collision matrix the first time it sees a new urdf.  +If it is set, giskard publishes markers ​that visualize ​the collision ​avoidance: 
-This is a list of link pairs that may collide. + 
-You can overwrite ​the collision ​matrix using the config file. +{{:wiki:​tutorials:​collision_example.png?​400|}} 
-<code yaml> + 
-self_collision_avoidance# each pair of links in the collision matrix get push away from each other +Each of these string represent ​the distance to a close object. ​ 
-  ​ignore:​ [] # list pairs of links that should be removed from the self collision avoidance +Red means that the object is closer than 5cmyellow indicates a distance of 10cm and green a distance of 20cm. 
-  add: # list pairs of links that should be added to the self collision matrix + 
-    - [base_linkhead_mount_kinect_ir_link] ​ +===CollisionEntry=== 
-</​code>​ +When interacting with the environment, it might be necessary to override the settings from the config file, e.g. when touching objects
-Each pair in this matrix gets repelled from each other. +For this reason, collision entries ​can be send with each goal.
-===Smarter than the collision avoidance=== +
-The collision avoidance works fine most of the timebut if it doesn'​t you can add exceptions+
-When constructing a move goalyou can also set collision entries.+
 <​code>​ <​code>​
-  giskard_msgs/​CollisionEntry[] collisions +# definitions of possible collision types to use for value 
-    uint8 AVOID_COLLISION=0 +uint8 AVOID_COLLISION=0 
-    uint8 ALLOW_COLLISION=1 +uint8 ALLOW_COLLISION=1 
-    uint8 AVOID_ALL_COLLISIONS=2 +uint8 AVOID_ALL_COLLISIONS=2 
-    uint8 ALLOW_ALL_COLLISIONS=3 +uint8 ALLOW_ALL_COLLISIONS=3 
-    string ALL= # this means empty string +string ALL= #this means empty string
-    uint8 type +
-    float64 min_dist +
-    string[] robot_links +
-    string body_b +
-    string[] link_bs +
-</​code>​+
  
-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. +# use one of the above constants ​as collision ​type 
 +uint8 type
  
-A message to achieve that example would need ''​type=1'',​ ''​robot_links=[r_gripper_l_finger_link]'',​ ''​body_b=cup'',​ ''​link_bs=[CollisionEntry.ALL]''​. +# enforced minimum distance between the closest points on both surfaces 
-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.+float64 min_dist
  
-Collision entries are parsed in sequence.  +# link of the controlled robotif left empty all links will be selected 
-If your 1. entries allows all collisions and your 2. entry avoids allthen the avoid all will overwrite the allow all. +string[] robot_links
-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+# name of body Bi.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 
 +</​code>​
  
-''​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''​+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: Here are some other useful examples:
-  - turn off collision avoidance: ''​type=1'',​ ''​robot_links=[CollisionEntry.ALL]'',​ ''​body_b=CollisionEntry.ALL'',​ ''​link_bs=[CollisionEntry.ALL]''​ + 
-  ​- turn off self collision avoidance: ​''​type=1''​''​robot_links=[CollisionEntry.ALL]'',​ ''​body_b=<robot name in urdf>''​''​link_bs=[CollisionEntry.ALL]''​+  * 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.