Box Box Problem Reproduction

maze_new.py

# In[2]:


import numpy as np
import os
import pydot
from IPython.display import display, Image, SVG
from tqdm import tqdm
import matplotlib.pyplot as plt
import scipy, scipy.sparse
import networkx as nx
import time
import pickle


# In[3]:


from pydrake.all import (
    StartMeshcat,
    DiagramBuilder,
    RobotDiagramBuilder,
    AddMultibodyPlantSceneGraph,
    Parser,
    LoadModelDirectivesFromString,
    ProcessModelDirectives,
    MeshcatVisualizerParams,
    MeshcatVisualizer,
    Role,
    Simulator,
    SceneGraphCollisionChecker,
    IrisOptions,
    IrisFromCliqueCoverOptions,
    RandomGenerator,
    IrisInConfigurationSpaceFromCliqueCover,
    IrisInConfigurationSpace,
    CalcPairwiseIntersections,
    PartitionConvexSet,
    CheckIfSatisfiesConvexityRadius,
    Point,
    GcsTrajectoryOptimization,
    GraphOfConvexSetsOptions,
    CommonSolverOption,
    MosekSolver,
    ScsSolver,
    RigidTransform,
    RollPitchYaw,
    SolverOptions,
    KinematicTrajectoryOptimization,
    MinimumDistanceLowerBoundConstraint,
    Solve
)

meshcat = StartMeshcat()

scenario_data = """
directives:
- add_model:
    name: maze1
    file: package://se3maze/models/maze.sdf
- add_weld:
    parent: world
    child: maze1::wall1
    X_PC:
        translation: [0, 0.25, 0.15]
- add_model:
    name: box
    file: package://se3maze/models/box.sdf
- add_weld:
    parent: world
    child: box::base
"""


# In[6]:


def RepoDir():
#     return os.path.dirname(os.path.dirname(os.path.realpath(__file__)))
    return os.path.abspath("")

def build_env(meshcat, directives_file, robot_diagram=False):
    meshcat.Delete()
    
    if robot_diagram:
        builder = RobotDiagramBuilder()
        plant = builder.plant()
        scene_graph = builder.scene_graph()
    else:
        builder = DiagramBuilder()
        plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
    
    parser = Parser(plant)
    parser.package_map().Add("se3maze", RepoDir())
    
    directives = LoadModelDirectivesFromString(scenario_data)
    models = ProcessModelDirectives(directives, plant, parser)

    plant.Finalize()
    
    if not robot_diagram:
        meshcat_visual_params = MeshcatVisualizerParams()
        meshcat_visual_params.delete_on_initialization_event = False
        meshcat_visual_params.role = Role.kIllustration
        meshcat_visual_params.prefix = "visual"
        meshcat_visual = MeshcatVisualizer.AddToBuilder(
            builder, scene_graph, meshcat, meshcat_visual_params)

        meshcat_collision_params = MeshcatVisualizerParams()
        meshcat_collision_params.delete_on_initialization_event = False
        meshcat_collision_params.role = Role.kProximity
        meshcat_collision_params.prefix = "collision"
        meshcat_collision_params.visible_by_default = False
        meshcat_collision = MeshcatVisualizer.AddToBuilder(
            builder, scene_graph, meshcat, meshcat_collision_params)

    diagram = builder.Build()
    
    return diagram, plant, scene_graph



diagram, plant, scene_graph = build_env(meshcat, scenario_data, robot_diagram=True)

points_to_plan = [
    np.array([0.25, 0.25, 0.15, 0, 0, 0]),
    np.array([0.25, 2.25, 0.15, np.pi, np.pi, -np.pi]),
    np.array([0.25, 4.25, 0.15, 0, 0, 0]),
    np.array([2.25, 0.25, 0.15, np.pi, 0, 0]),
    np.array([2.25, 2.25, 0.15, 0, 0, np.pi]),
    np.array([2.25, 4.25, 0.15, 0, -np.pi, 0]),
    np.array([4.25, 0.25, 0.15, 0, 0, 0]),
    np.array([4.25, 2.25, 0.15, 0, np.pi, 0]),
    np.array([4.25, 4.25, 0.15, np.pi, np.pi, np.pi]),
]

context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)

for i in tqdm(range(len(points_to_plan))):
    for j in range(len(points_to_plan)):
        if i == j:
            continue
        
        trajopt = KinematicTrajectoryOptimization(6, 100)
        trajopt.AddPositionBounds(plant.GetPositionLowerLimits(), plant.GetPositionUpperLimits())
        trajopt.AddVelocityBounds(-np.ones(6), np.ones(6))
        trajopt.AddPathPositionConstraint(points_to_plan[i], points_to_plan[i], 0)
        trajopt.AddPathPositionConstraint(points_to_plan[j], points_to_plan[j], 1)
        trajopt.AddPathLengthCost()
        
        min_dist = 0.0
        penalty_dist = 0.1 # 10 cm
        collision_constraint = MinimumDistanceLowerBoundConstraint(plant, min_dist, plant_context, None, penalty_dist)
        for s in np.linspace(0, 1, num=200):
            trajopt.AddPathPositionConstraint(collision_constraint, s)

        result = Solve(trajopt.prog())

models.zip

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>se3maze</name>
  <version>0.0.0</version>
  <maintainer email="tcohn@mit.edu">Thomas Cohn</maintainer>
  <author>Thomas Cohn</author>
  <license>N/A</license>
</package>