Adicionar validacao de bounds em add_goal para garantir que todas as poses alvo estejam dentro do workspace

cliport/tasks/task.py

@@ -505,8 +505,39 @@ class Task():
         - params ([(zone_target, zone_size)])): has to be [(zone_target, zone_size)] if the metric is `zone` where obj_pts is a dictionary that maps object ID to points.
         - step_max_reward (float): the maximum reward of matching all the objects with all the target poses.
         """
+        # Validate that all target poses are within workspace bounds
+        validated_targ_poses = []
+        for i, targ_pose in enumerate(targ_poses):
+            if isinstance(targ_pose, tuple) and len(targ_pose) == 2:
+                pos, rot = targ_pose
+                if isinstance(pos, (list, tuple, np.ndarray)) and len(pos) >= 3:
+                    x, y, z = float(pos[0]), float(pos[1]), float(pos[2])
+                    
+                    # Check if position needs clamping
+                    x_original, y_original, z_original = x, y, z
+                    x = np.clip(x, self.bounds[0, 0], self.bounds[0, 1])
+                    y = np.clip(y, self.bounds[1, 0], self.bounds[1, 1])
+                    z = np.clip(z, self.bounds[2, 0], self.bounds[2, 1])
+                    
+                    # Warn if position was adjusted
+                    if (x != x_original or y != y_original or z != z_original):
+                        print(f"Warning: Target pose {i} was clamped to workspace bounds: "
+                              f"({x_original:.3f}, {y_original:.3f}, {z_original:.3f}) -> "
+                              f"({x:.3f}, {y:.3f}, {z:.3f})")
+                    
+                    # Create validated pose (preserve original format if possible)
+                    if isinstance(pos, np.ndarray):
+                        validated_pos = np.array([x, y, z], dtype=pos.dtype)
+                    else:
+                        validated_pos = np.array([x, y, z])
+                    validated_targ_poses.append((validated_pos, rot))
+                else:
+                    validated_targ_poses.append(targ_pose)
+            else:
+                validated_targ_poses.append(targ_pose)
+        
         objs = self.zip_obj_ids(objs, symmetries)
-        self.goals.append((objs, matches, targ_poses, replace, rotations,
+        self.goals.append((objs, matches, validated_targ_poses, replace, rotations,
                            metric, params, step_max_reward))
         if language_goal is not None:
             if type(language_goal) is str: