[WIP] Add core modules for ML, perception, and planning#1
Conversation
…nner, and configs Co-authored-by: ansh1113 <200216682+ansh1113@users.noreply.github.com>
Co-authored-by: ansh1113 <200216682+ansh1113@users.noreply.github.com>
Co-authored-by: ansh1113 <200216682+ansh1113@users.noreply.github.com>
|
@copilot fix and error and add all the missing files from all the phases |
There was a problem hiding this comment.
Pull request overview
This WIP pull request adds comprehensive core modules for ML/perception/planning capabilities to the humanoid motion planning system. The PR introduces substantial new functionality including stability analysis, object detection, workspace analysis, goal prediction, and enhanced motion planning with Drake integration.
Key Changes:
- Complete stability analysis module with CoM tracking, ZMP calculation, and dynamic stability metrics
- Perception module with object detection interfaces, environment state management, and workspace analysis
- Enhanced motion planner with Drake optimization framework integration and ZMP constraints
- Comprehensive test suites covering unit tests, integration tests, and performance benchmarks
- Production infrastructure including Docker support, CI/CD pipeline, and pre-commit hooks
- Complete configuration system with YAML files for robot and perception parameters
- End-to-end demo showcasing the full pipeline from perception to trajectory execution
Reviewed changes
Copilot reviewed 15 out of 16 changed files in this pull request and generated 23 comments.
Show a summary per file
| File | Description |
|---|---|
src/humanoid_planner/stability_analysis.py |
New module implementing stability analysis with CoM computation, ZMP verification, capture point calculation, and trajectory stability checking |
src/humanoid_planner/perception.py |
New module providing object detection interfaces, environment state estimation, workspace analysis, and goal prediction for reaching tasks |
src/humanoid_planner/motion_planner.py |
Enhanced with Drake optimization, ZMP constraint integration, IK solving, trajectory metrics computation, and execution capabilities |
src/humanoid_planner/__init__.py |
Updated exports to include new stability analysis and perception modules |
tests/test_stability_analysis.py |
Comprehensive unit tests for stability analyzer and CoM tracker with 13 test cases |
tests/test_perception.py |
Unit tests covering object detection, environment state, workspace analysis, and goal prediction with 15 test cases |
tests/test_integration.py |
Integration tests for end-to-end scenarios, multi-module integration, and performance benchmarks |
config/robot_config.yaml |
Complete robot configuration defining joint limits, mass properties, dimensions, and model paths |
config/perception_config.yaml |
Perception system configuration including camera settings, detection parameters, and visualization options |
examples/complete_demo.py |
End-to-end demo script demonstrating the full pipeline from environment setup to visualization |
docker-compose.yml |
Docker compose configuration with services for development, Jupyter, and main application |
Dockerfile |
Container definition with all dependencies and development tools |
.github/workflows/ci.yml |
CI/CD pipeline with testing, building, Docker image creation, and documentation generation |
.pre-commit-config.yaml |
Code quality hooks including Black, flake8, isort, mypy, and pydocstyle |
CONTRIBUTING.md |
Comprehensive contribution guidelines with setup instructions, style guide, and PR process |
requirements.txt |
Updated with dataclasses backport dependency |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
|
|
||
| result = minimize(cost, q_init, method='SLSQP', bounds=bounds) | ||
|
|
||
| if result.success and cost(result.x) < 0.01: # 10cm tolerance |
There was a problem hiding this comment.
The comment states "10cm tolerance" but the actual threshold used is 0.01, which would be 1cm squared (since it's comparing squared distances). Either the comment should be corrected to "1cm tolerance" or the threshold should be changed to 0.01 (100 square cm, which would be 10cm).
| x = np.sum(link_lengths[:len(q)] * np.cos(q[:len(link_lengths)])) | ||
| y = np.sum(link_lengths[:len(q)] * np.sin(q[:len(link_lengths)])) |
There was a problem hiding this comment.
Potential array indexing bug: The slicing q[:len(link_lengths)] inside np.cos() and np.sin() on lines 212-213 could be problematic when len(q) > len(link_lengths). If q has more elements than link_lengths, this will only use the first len(link_lengths) elements of q, but then tries to sum link_lengths (which is shorter) element-wise. This should be: link_lengths * np.cos(q[:len(link_lengths)]) to ensure proper broadcasting.
| x = np.sum(link_lengths[:len(q)] * np.cos(q[:len(link_lengths)])) | |
| y = np.sum(link_lengths[:len(q)] * np.sin(q[:len(link_lengths)])) | |
| x = np.sum(link_lengths * np.cos(q[:len(link_lengths)])) | |
| y = np.sum(link_lengths * np.sin(q[:len(link_lengths)])) |
| - ./data:/app/data | ||
| - ./results:/app/results | ||
| ports: | ||
| - "8888:8888" |
There was a problem hiding this comment.
The docker-compose.yml file defines two services ('humanoid-planner' and 'jupyter') that both expose port 8888, which will cause a port conflict when both services are started simultaneously. Either use different ports for each service or ensure only one is running at a time by using profiles or separate compose files.
| - "8888:8888" | |
| - "8889:8888" |
| if approach_direction is None: | ||
| approach_direction = np.array([0, 0, -1]) | ||
|
|
||
| approach_direction = approach_direction / np.linalg.norm(approach_direction) |
There was a problem hiding this comment.
Potential division by zero: If the approach_direction passed in has zero norm (or is very close to zero), the normalization on line 476 will fail or produce invalid results. Consider adding a check to handle this edge case, perhaps by falling back to a default direction or raising a descriptive error.
| approach_direction = approach_direction / np.linalg.norm(approach_direction) | |
| norm = np.linalg.norm(approach_direction) | |
| if norm < 1e-8: | |
| # Fallback to default direction if zero or near-zero vector is provided | |
| approach_direction = np.array([0, 0, -1]) | |
| norm = np.linalg.norm(approach_direction) | |
| approach_direction = approach_direction / norm |
| for radius in range(max_search_radius): | ||
| # Check all cells at this radius | ||
| for dx in range(-radius, radius + 1): | ||
| for dy in range(-radius, radius + 1): | ||
| for dz in range(-radius, radius + 1): | ||
| if abs(dx) == radius or abs(dy) == radius or abs(dz) == radius: | ||
| idx = grid_idx + np.array([dx, dy, dz]) | ||
|
|
||
| if (idx >= 0).all() and (idx < self.reachability_map.shape).all(): | ||
| if self.reachability_map[tuple(idx)] >= threshold: | ||
| # Convert back to world coordinates | ||
| pos = min_bounds + idx * self.resolution | ||
| return pos |
There was a problem hiding this comment.
The find_nearest_reachable_position method uses a brute-force O(n³) search over a cube that could be inefficient for large search spaces. Consider using a more efficient spatial search algorithm like scipy's KDTree or implementing a smarter search pattern (e.g., breadth-first search from the target point).
|
|
||
| start_time = time.time() | ||
|
|
||
| q_solution = ik.solve_ik(target_pos, q_init=q_init, max_iter=100) |
There was a problem hiding this comment.
Variable q_solution is not used.
| q_solution = ik.solve_ik(target_pos, q_init=q_init, max_iter=100) | |
| ik.solve_ik(target_pos, q_init=q_init, max_iter=100) |
| ObjectDetector, EnvironmentState, GoalPredictor, | ||
| WorkspaceAnalyzer, ObjectType, | ||
| TrajectoryOptimizer, ZMPConstraint, | ||
| SupportPolygonCalculator, StabilityAnalyzer, | ||
| ForwardKinematics | ||
| ) | ||
|
|
||
|
|
There was a problem hiding this comment.
Import of 'EnvironmentState' is not used.
Import of 'WorkspaceAnalyzer' is not used.
Import of 'ForwardKinematics' is not used.
| ObjectDetector, EnvironmentState, GoalPredictor, | |
| WorkspaceAnalyzer, ObjectType, | |
| TrajectoryOptimizer, ZMPConstraint, | |
| SupportPolygonCalculator, StabilityAnalyzer, | |
| ForwardKinematics | |
| ) | |
| ObjectDetector, GoalPredictor, | |
| ObjectType, | |
| TrajectoryOptimizer, ZMPConstraint, | |
| SupportPolygonCalculator, StabilityAnalyzer, | |
| ) |
| """ | ||
|
|
||
| import numpy as np | ||
| from typing import List, Tuple, Optional, Dict |
There was a problem hiding this comment.
Import of 'Dict' is not used.
| from typing import List, Tuple, Optional, Dict | |
| from typing import List, Tuple, Optional |
| import numpy as np | ||
| from typing import Tuple, Optional, List | ||
| from .zmp_constraint import ZMPConstraint | ||
| from .support_polygon import SupportPolygonCalculator |
There was a problem hiding this comment.
Import of 'SupportPolygonCalculator' is not used.
| from .support_polygon import SupportPolygonCalculator |
| self.plant.SetPositions(self.plant_context, q) | ||
| com = self.plant.CalcCenterOfMassPosition(self.plant_context) | ||
| return com | ||
| except: |
There was a problem hiding this comment.
Except block directly handles BaseException.
| except: | |
| except Exception: |
Comprehensive Humanoid Motion Planning Implementation Plan
Phase 1: Core Implementation (Priority)
Phase 2: ML/Perception Components
Phase 3: Configuration & Integration
Phase 4: Testing & Documentation
Phase 5: Professional Infrastructure
Phase 6: Examples & Demos
Original prompt
✨ Let Copilot coding agent set things up for you — coding agent works faster and does higher quality work when set up for your repo.