The robot's zero configuration was falsely detected as in collision with environment:
- Moved ground plane from z=-0.01 to z=-0.05 to clear robot base
- Moved table further away (x=0.6) and lower (z=0.3) to avoid arm collisions
- Updated box_obstacle position to [0.4, 0.3, 0.5] with smaller size
- Applied same fixes to both
planning_scene_op.pyandplanner_ompl_with_collision_op.py
Updated motion_commander.py with more reachable target configurations:
- Home position now starts at
[0, 0, 0, -1.57, 0, 1.57, 0](arm centered, easy to plan from) - Simplified pick/place sequence with smaller joint movements
- All poses verified to be within Panda joint limits
MuJoCo simulator sends 9 joint values (7 arm + 2 gripper fingers), but planner expects 7:
motion_commander.py: Truncate joint positions to first 7 arm joints before planningtrajectory_executor.py: Truncate joint positions to first 7 arm joints for interpolation
Scene updates were being processed redundantly on every tick:
planner_ompl_with_collision_op.py: Track scene version to skip already-processed updatescollision_check_op.py: Track scene version to skip already-processed updates
Replaced all emoji characters with ASCII text for Windows compatibility:
ik_op.py: Replaced checkmark/cross emojis withSUCCESS/FAILEDplanner_ompl_with_collision_op.py: Replaced checkmark/cross emojis withSUCCESS/FAILEDcollision_check_op.py: Replaced status emojis withCOLLISION/CLEARdemo_node.py: Replaced emojis with[OK]/[FAIL]/[COLLISION]/[CLEAR]motion_commander.py: Removed all emoji characterscollision_lib.py: Replaced checkmark emoji with[OK]
Fixed scene update handling in collision_check_op.py and planner_ompl_with_collision_op.py:
- Now supports both full scene broadcasts (
{"world_objects": [...]}) and single object commands ({"action": "add", "object": {...}}) - Full scene sync clears and rebuilds all obstacles from
planning_scene_op.pybroadcasts - Single object commands continue to work for incremental updates