Spaces:
Sleeping
Sleeping
Commit ·
cae9adc
1
Parent(s): 5779965
Corrigir geracao de espiral: usar coordenadas polares e adicionar exemplo validado
Browse files- cliport/generated_tasks/build_red_and_blue_pyramids_fixed.py +1 -0
- cliport/generated_tasks/build_red_pyramid_and_blue_row_fixed.py +1 -0
- cliport/generated_tasks/build_spiral_fixed.py +67 -0
- cliport/generated_tasks/sequence_ball_placement_in_colored_corners_fixed.py +1 -0
- gensim/agent.py +2 -0
- prompts/topdown_task_generation_prompt/cliport_prompt_code_split_template.txt +9 -0
- prompts/topdown_task_generation_prompt_simple/cliport_prompt_code_split_template.txt +9 -0
- prompts/vanilla_task_generation_prompt/cliport_prompt_code_split_template.txt +9 -0
cliport/generated_tasks/build_red_and_blue_pyramids_fixed.py
CHANGED
|
@@ -153,3 +153,4 @@ class BuildRedAndBluePyramids(Task):
|
|
| 153 |
)
|
| 154 |
|
| 155 |
|
|
|
|
|
|
| 153 |
)
|
| 154 |
|
| 155 |
|
| 156 |
+
|
cliport/generated_tasks/build_red_pyramid_and_blue_row_fixed.py
CHANGED
|
@@ -137,3 +137,4 @@ class BuildRedPyramidAndBlueRow(Task):
|
|
| 137 |
language_goal=language_goal
|
| 138 |
)
|
| 139 |
|
|
|
|
|
|
| 137 |
language_goal=language_goal
|
| 138 |
)
|
| 139 |
|
| 140 |
+
|
cliport/generated_tasks/build_spiral_fixed.py
ADDED
|
@@ -0,0 +1,67 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import numpy as np
|
| 2 |
+
import os
|
| 3 |
+
import pybullet as p
|
| 4 |
+
import random
|
| 5 |
+
from cliport.tasks import primitives
|
| 6 |
+
from cliport.tasks.grippers import Spatula
|
| 7 |
+
from cliport.tasks.task import Task
|
| 8 |
+
from cliport.utils import utils
|
| 9 |
+
|
| 10 |
+
|
| 11 |
+
class BuildSpiralWithColoredBlocks(Task):
|
| 12 |
+
"""Build a spiral with colored blocks starting from the center."""
|
| 13 |
+
|
| 14 |
+
def __init__(self):
|
| 15 |
+
super().__init__()
|
| 16 |
+
self.max_steps = 10
|
| 17 |
+
self.lang_template = "build a spiral with the {blocks}"
|
| 18 |
+
self.task_completed_desc = "done building spiral."
|
| 19 |
+
self.additional_reset()
|
| 20 |
+
|
| 21 |
+
def reset(self, env):
|
| 22 |
+
super().reset(env)
|
| 23 |
+
|
| 24 |
+
# Define block colors.
|
| 25 |
+
colors = [
|
| 26 |
+
utils.COLORS['red'], utils.COLORS['orange'], utils.COLORS['yellow'],
|
| 27 |
+
utils.COLORS['green'], utils.COLORS['blue'], utils.COLORS['purple']
|
| 28 |
+
]
|
| 29 |
+
|
| 30 |
+
# Add blocks.
|
| 31 |
+
block_size = (0.04, 0.04, 0.04)
|
| 32 |
+
block_urdf = 'stacking/block.urdf'
|
| 33 |
+
objs = []
|
| 34 |
+
for i in range(6):
|
| 35 |
+
block_pose = self.get_random_pose(env, block_size)
|
| 36 |
+
block_id = env.add_object(block_urdf, block_pose, color=colors[i])
|
| 37 |
+
objs.append(block_id)
|
| 38 |
+
|
| 39 |
+
# Define spiral placement using polar coordinates with increasing radius
|
| 40 |
+
# Spiral: radius increases, angle increases (one full rotation = 2*pi)
|
| 41 |
+
num_blocks = 6
|
| 42 |
+
spiral_center = (0.5, 0.0, 0.02) # Center of workspace, fixed pose
|
| 43 |
+
base_rotation = (0, 0, 0, 1)
|
| 44 |
+
base_pose = (spiral_center, base_rotation)
|
| 45 |
+
|
| 46 |
+
# Spiral parameters: radius increases from 0.02 to 0.12, angle goes from 0 to 2*pi
|
| 47 |
+
spiral_place_pos = []
|
| 48 |
+
for i in range(num_blocks):
|
| 49 |
+
# Angle increases from 0 to 2*pi (one full rotation)
|
| 50 |
+
angle = (i / num_blocks) * 2 * np.pi
|
| 51 |
+
# Radius increases linearly from 0.02 to 0.12
|
| 52 |
+
radius = 0.02 + (i / (num_blocks - 1)) * 0.10 if num_blocks > 1 else 0.02
|
| 53 |
+
# Convert to Cartesian coordinates relative to center
|
| 54 |
+
x = radius * np.cos(angle)
|
| 55 |
+
y = radius * np.sin(angle)
|
| 56 |
+
z = 0.02 # All blocks at same height for 2D spiral
|
| 57 |
+
spiral_place_pos.append((x, y, z))
|
| 58 |
+
|
| 59 |
+
# Apply base pose to all spiral positions
|
| 60 |
+
targs = [(utils.apply(base_pose, pos), base_pose[1]) for pos in spiral_place_pos]
|
| 61 |
+
|
| 62 |
+
# Goal: blocks are placed in a spiral pattern.
|
| 63 |
+
language_goal = self.lang_template.format(blocks="red, orange, yellow, green, blue and purple blocks")
|
| 64 |
+
self.add_goal(objs=objs, matches=np.eye(6), targ_poses=targs, replace=False,
|
| 65 |
+
rotations=True, metric='pose', params=None, step_max_reward=1 / 6,
|
| 66 |
+
symmetries=[np.pi/2]*6, language_goal=language_goal)
|
| 67 |
+
|
cliport/generated_tasks/sequence_ball_placement_in_colored_corners_fixed.py
CHANGED
|
@@ -73,3 +73,4 @@ class SequenceBallPlacementInColoredCorners(Task):
|
|
| 73 |
|
| 74 |
|
| 75 |
|
|
|
|
|
|
| 73 |
|
| 74 |
|
| 75 |
|
| 76 |
+
|
gensim/agent.py
CHANGED
|
@@ -180,6 +180,8 @@ class Agent:
|
|
| 180 |
'align-balls-in-colored-boxes': 'align_balls_in_colored_boxes.py',
|
| 181 |
'color-sorted-container-stack': 'color_sorted_container_stack.py',
|
| 182 |
'rainbow-stack': 'rainbow_stack.py',
|
|
|
|
|
|
|
| 183 |
}
|
| 184 |
|
| 185 |
# Normalize task name (handle both formats)
|
|
|
|
| 180 |
'align-balls-in-colored-boxes': 'align_balls_in_colored_boxes.py',
|
| 181 |
'color-sorted-container-stack': 'color_sorted_container_stack.py',
|
| 182 |
'rainbow-stack': 'rainbow_stack.py',
|
| 183 |
+
'build-spiral': 'build_spiral_fixed.py',
|
| 184 |
+
'build-spiral-with-colored-blocks': 'build_spiral_fixed.py',
|
| 185 |
}
|
| 186 |
|
| 187 |
# Normalize task name (handle both formats)
|
prompts/topdown_task_generation_prompt/cliport_prompt_code_split_template.txt
CHANGED
|
@@ -279,6 +279,15 @@ CRITICAL FOR PYRAMIDS, BRIDGES, AND MULTI-LAYER STRUCTURES:
|
|
| 279 |
- Split the reward across layers: `step_max_reward=1.0 / num_layers` for each layer
|
| 280 |
- Each layer should have its own `language_goal` describing that specific step (e.g., "build the bottom row", "build the middle row", "place the top block")
|
| 281 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 282 |
CRITICAL FOR MULTIPLE STRUCTURES (e.g., "two pyramids", "three towers", "multiple zones", "pyramid and row"):
|
| 283 |
- If the task requires MULTIPLE separate structures (e.g., "two pyramids", "one blue pyramid and one red pyramid", "pyramid and row"), you MUST create separate base poses for each structure
|
| 284 |
- **CRITICAL**: Each structure MUST have its own base pose positioned at a DIFFERENT location in the workspace. DO NOT reuse the same base_pose for different structures.
|
|
|
|
| 279 |
- Split the reward across layers: `step_max_reward=1.0 / num_layers` for each layer
|
| 280 |
- Each layer should have its own `language_goal` describing that specific step (e.g., "build the bottom row", "build the middle row", "place the top block")
|
| 281 |
|
| 282 |
+
CRITICAL FOR SPIRAL PATTERNS:
|
| 283 |
+
- For spiral patterns, you MUST use POLAR COORDINATES with increasing radius and varying angle
|
| 284 |
+
- DO NOT use rectangular patterns like `[(0, 0), (0.04, 0), (0.04, 0.04), ...]` - that creates a rectangle, NOT a spiral
|
| 285 |
+
- Use: `angle = (i / num_blocks) * 2 * np.pi` and `radius = start_radius + (i / (num_blocks - 1)) * radius_increase`
|
| 286 |
+
- Convert to Cartesian: `x = radius * np.cos(angle)`, `y = radius * np.sin(angle)`
|
| 287 |
+
- Use a FIXED center pose (e.g., `(0.5, 0.0, 0.02)`) within workspace bounds, NOT `get_random_pose()`
|
| 288 |
+
- Apply the center pose using `utils.apply()`: `targs = [(utils.apply(base_pose, (x, y, z)), base_pose[1]) for x, y, z in spiral_positions]`
|
| 289 |
+
- Example for 6 blocks: radius increases from 0.02 to 0.12, angle goes from 0 to 2*pi (one full rotation)
|
| 290 |
+
|
| 291 |
CRITICAL FOR MULTIPLE STRUCTURES (e.g., "two pyramids", "three towers", "multiple zones", "pyramid and row"):
|
| 292 |
- If the task requires MULTIPLE separate structures (e.g., "two pyramids", "one blue pyramid and one red pyramid", "pyramid and row"), you MUST create separate base poses for each structure
|
| 293 |
- **CRITICAL**: Each structure MUST have its own base pose positioned at a DIFFERENT location in the workspace. DO NOT reuse the same base_pose for different structures.
|
prompts/topdown_task_generation_prompt_simple/cliport_prompt_code_split_template.txt
CHANGED
|
@@ -277,6 +277,15 @@ CRITICAL FOR PYRAMIDS, BRIDGES, AND MULTI-LAYER STRUCTURES:
|
|
| 277 |
- Split the reward across layers: `step_max_reward=1.0 / num_layers` for each layer
|
| 278 |
- Each layer should have its own `language_goal` describing that specific step (e.g., "build the bottom row", "build the middle row", "place the top block")
|
| 279 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 280 |
CRITICAL FOR MULTIPLE STRUCTURES (e.g., "two pyramids", "three towers", "multiple zones", "pyramid and row"):
|
| 281 |
- If the task requires MULTIPLE separate structures (e.g., "two pyramids", "one blue pyramid and one red pyramid", "pyramid and row"), you MUST create separate base poses for each structure
|
| 282 |
- **CRITICAL**: Each structure MUST have its own base pose positioned at a DIFFERENT location in the workspace. DO NOT reuse the same base_pose for different structures.
|
|
|
|
| 277 |
- Split the reward across layers: `step_max_reward=1.0 / num_layers` for each layer
|
| 278 |
- Each layer should have its own `language_goal` describing that specific step (e.g., "build the bottom row", "build the middle row", "place the top block")
|
| 279 |
|
| 280 |
+
CRITICAL FOR SPIRAL PATTERNS:
|
| 281 |
+
- For spiral patterns, you MUST use POLAR COORDINATES with increasing radius and varying angle
|
| 282 |
+
- DO NOT use rectangular patterns like `[(0, 0), (0.04, 0), (0.04, 0.04), ...]` - that creates a rectangle, NOT a spiral
|
| 283 |
+
- Use: `angle = (i / num_blocks) * 2 * np.pi` and `radius = start_radius + (i / (num_blocks - 1)) * radius_increase`
|
| 284 |
+
- Convert to Cartesian: `x = radius * np.cos(angle)`, `y = radius * np.sin(angle)`
|
| 285 |
+
- Use a FIXED center pose (e.g., `(0.5, 0.0, 0.02)`) within workspace bounds, NOT `get_random_pose()`
|
| 286 |
+
- Apply the center pose using `utils.apply()`: `targs = [(utils.apply(base_pose, (x, y, z)), base_pose[1]) for x, y, z in spiral_positions]`
|
| 287 |
+
- Example for 6 blocks: radius increases from 0.02 to 0.12, angle goes from 0 to 2*pi (one full rotation)
|
| 288 |
+
|
| 289 |
CRITICAL FOR MULTIPLE STRUCTURES (e.g., "two pyramids", "three towers", "multiple zones", "pyramid and row"):
|
| 290 |
- If the task requires MULTIPLE separate structures (e.g., "two pyramids", "one blue pyramid and one red pyramid", "pyramid and row"), you MUST create separate base poses for each structure
|
| 291 |
- **CRITICAL**: Each structure MUST have its own base pose positioned at a DIFFERENT location in the workspace. DO NOT reuse the same base_pose for different structures.
|
prompts/vanilla_task_generation_prompt/cliport_prompt_code_split_template.txt
CHANGED
|
@@ -267,6 +267,15 @@ CRITICAL FOR PYRAMIDS, BRIDGES, AND MULTI-LAYER STRUCTURES:
|
|
| 267 |
- Split the reward across layers: `step_max_reward=1.0 / num_layers` for each layer
|
| 268 |
- Each layer should have its own `language_goal` describing that specific step (e.g., "build the bottom row", "build the middle row", "place the top block")
|
| 269 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 270 |
CRITICAL FOR MULTIPLE STRUCTURES (e.g., "two pyramids", "three towers", "multiple zones", "pyramid and row"):
|
| 271 |
- If the task requires MULTIPLE separate structures (e.g., "two pyramids", "one blue pyramid and one red pyramid", "pyramid and row"), you MUST create separate base poses for each structure
|
| 272 |
- **CRITICAL**: Each structure MUST have its own base pose positioned at a DIFFERENT location in the workspace. DO NOT reuse the same base_pose for different structures.
|
|
|
|
| 267 |
- Split the reward across layers: `step_max_reward=1.0 / num_layers` for each layer
|
| 268 |
- Each layer should have its own `language_goal` describing that specific step (e.g., "build the bottom row", "build the middle row", "place the top block")
|
| 269 |
|
| 270 |
+
CRITICAL FOR SPIRAL PATTERNS:
|
| 271 |
+
- For spiral patterns, you MUST use POLAR COORDINATES with increasing radius and varying angle
|
| 272 |
+
- DO NOT use rectangular patterns like `[(0, 0), (0.04, 0), (0.04, 0.04), ...]` - that creates a rectangle, NOT a spiral
|
| 273 |
+
- Use: `angle = (i / num_blocks) * 2 * np.pi` and `radius = start_radius + (i / (num_blocks - 1)) * radius_increase`
|
| 274 |
+
- Convert to Cartesian: `x = radius * np.cos(angle)`, `y = radius * np.sin(angle)`
|
| 275 |
+
- Use a FIXED center pose (e.g., `(0.5, 0.0, 0.02)`) within workspace bounds, NOT `get_random_pose()`
|
| 276 |
+
- Apply the center pose using `utils.apply()`: `targs = [(utils.apply(base_pose, (x, y, z)), base_pose[1]) for x, y, z in spiral_positions]`
|
| 277 |
+
- Example for 6 blocks: radius increases from 0.02 to 0.12, angle goes from 0 to 2*pi (one full rotation)
|
| 278 |
+
|
| 279 |
CRITICAL FOR MULTIPLE STRUCTURES (e.g., "two pyramids", "three towers", "multiple zones", "pyramid and row"):
|
| 280 |
- If the task requires MULTIPLE separate structures (e.g., "two pyramids", "one blue pyramid and one red pyramid", "pyramid and row"), you MUST create separate base poses for each structure
|
| 281 |
- **CRITICAL**: Each structure MUST have its own base pose positioned at a DIFFERENT location in the workspace. DO NOT reuse the same base_pose for different structures.
|