Significantly improved convergence for mesh and cuboid, new ESDF collision.

src/curobo/rollout/arm_base.py

@@ -40,7 +40,7 @@ from curobo.types.base import TensorDeviceType
 from curobo.types.robot import CSpaceConfig, RobotConfig
 from curobo.types.state import JointState
 from curobo.util.logger import log_error, log_info, log_warn
-from curobo.util.tensor_util import cat_sum
+from curobo.util.tensor_util import cat_sum, cat_sum_horizon
 
 
 @dataclass
@@ -104,10 +104,10 @@ class ArmCostConfig:
 
 @dataclass
 class ArmBaseConfig(RolloutConfig):
-    model_cfg: KinematicModelConfig
-    cost_cfg: ArmCostConfig
-    constraint_cfg: ArmCostConfig
-    convergence_cfg: ArmCostConfig
+    model_cfg: Optional[KinematicModelConfig] = None
+    cost_cfg: Optional[ArmCostConfig] = None
+    constraint_cfg: Optional[ArmCostConfig] = None
+    convergence_cfg: Optional[ArmCostConfig] = None
     world_coll_checker: Optional[WorldCollision] = None
 
     @staticmethod
@@ -322,7 +322,9 @@ class ArmBase(RolloutBase, ArmBaseConfig):
             self.null_convergence = DistCost(self.convergence_cfg.null_space_cfg)
 
         # set start state:
-        start_state = torch.randn((1, self.dynamics_model.d_state), **vars(self.tensor_args))
+        start_state = torch.randn(
+            (1, self.dynamics_model.d_state), **(self.tensor_args.as_torch_dict())
+        )
         self._start_state = JointState(
             position=start_state[:, : self.dynamics_model.d_dof],
             velocity=start_state[:, : self.dynamics_model.d_dof],
@@ -366,9 +368,11 @@ class ArmBase(RolloutBase, ArmBaseConfig):
                 )
                 cost_list.append(coll_cost)
         if return_list:
-
             return cost_list
-        cost = cat_sum(cost_list)
+        if self.sum_horizon:
+            cost = cat_sum_horizon(cost_list)
+        else:
+            cost = cat_sum(cost_list)
         return cost
 
     def constraint_fn(