add grouping animation

robot/abstract_robot.py

@@ -8,9 +8,10 @@
 class AbstractRobot(MovableObject, ABC):
 
     # An abstract robot    
-    def __init__(self, limits_low, limits_high, **kwargs):
+    def __init__(self, limits_low, limits_high, collision_eps, **kwargs):
         self.limits_low = limits_low
         self.limits_high = limits_high
+        self.collision_eps = collision_eps
         self.config_dim = len(self.limits_low)
         super(AbstractRobot, self).__init__(**kwargs)
 

robot/grouping.py

@@ -10,7 +10,7 @@ class RobotGroup(AbstractRobot):
     Grouping multiple robots together into a meta-robot
     '''
 
-    def __init__(self, robots, grouping_mask_fn=None, **kwargs):
+    def __init__(self, robots, grouping_mask_fn=None, collision_eps=None, **kwargs):
         '''
         grouping mask function aims to assign the collision mask to each robot
         the argument to the function is an instance of the robot group
@@ -19,10 +19,12 @@ def __init__(self, robots, grouping_mask_fn=None, **kwargs):
         assert np.all([isinstance(robot, IndividualRobot) for robot in robots])
         self.robots = robots
         self.grouping_mask_fn = grouping_mask_fn
-        self.collision_eps = min([robot.collision_eps for robot in self.robots])
+        if collision_eps is None:
+            collision_eps = min([robot.collision_eps for robot in self.robots])
         limits_low, limits_high = self._get_limits()
         super(RobotGroup, self).__init__(limits_low=limits_low, 
-                                         limits_high=limits_high, **kwargs)
+                                         limits_high=limits_high, 
+                                         collision_eps=collision_eps, **kwargs)
 
     def _get_limits(self):
         all_limits_low = []
@@ -55,7 +57,7 @@ def set_config(self, config, item_ids=None):
 
     def no_collision(self):
         p.performCollisionDetection()
-        if np.all([len(p.getContactPoints(item_id)) == 0] for item_id in self.item_ids):
+        if np.all([len(p.getContactPoints(item_id)) == 0 for item_id in self.item_ids]):
             self.collision_check_count += 1
             return True
         else:

robot/individual_robot.py

@@ -6,12 +6,11 @@
 
 class IndividualRobot(AbstractRobot, ABC):
     # An individual robot
-    def __init__(self, base_position, base_orientation, urdf_file, collision_eps, **kwargs):
+    def __init__(self, base_position, base_orientation, urdf_file, **kwargs):
         # for loading the pybullet
         self.base_position = base_position
         self.base_orientation = base_orientation
 
-        self.collision_eps = collision_eps
         self.urdf_file = urdf_file
 
         joints, limits_low, limits_high = self._get_joints_and_limits(self.urdf_file)