GcsTrajectoryOptimization Bug

regions.pickle

tmp.py

import numpy as np
import pickle
from pydrake.all import GcsTrajectoryOptimization, GraphOfConvexSetsOptions, Point, CommonSolverOption, GurobiSolver, MosekSolver

start = np.array([2.72861773, -1.63614893, -2.24210693, -1.09417266, -2.47138911, -1.08448094, -2.02794058, -1.38])
goal = np.array([2.74726035, -0.8637625, -1.89093302, -0.78100625, -2.54191325, -1.20940114, -0.99160079, -1.04])

with open("regions.pickle", "rb") as f:
	regions = pickle.load(f)

order = 1
continuity = 0

gcs = GcsTrajectoryOptimization(8)

main_graph = gcs.AddRegions(regions, order, h_min=0.1, h_max=100, name="")
start_graph = gcs.AddRegions([Point(start)], 0)
goal_graph = gcs.AddRegions([Point(goal)], 0)
gcs.AddEdges(start_graph, main_graph)
gcs.AddEdges(main_graph, goal_graph)

options = GraphOfConvexSetsOptions()
options.preprocessing = False
options.solver_options.SetOption(CommonSolverOption.kPrintToConsole, 1)
options.solver_options.SetOption(MosekSolver.id(), "MSK_IPAR_INTPNT_BASIS", 0)

# Uncommenting either of the below options fixes it
# options.solver_options.SetOption(MosekSolver.id(), "MSK_IPAR_INTPNT_SOLVE_FORM", 1)
# options.solver_options.SetOption(MosekSolver.id(), "MSK_DPAR_INTPNT_CO_TOL_REL_GAP", 1e-3)

traj, result = gcs.SolvePath(start_graph, goal_graph, options)

print("\n\n\nNo objective cost result:", result.is_success(), "\n\n\n")

gcs.AddPathLengthCost()

traj, result = gcs.SolvePath(start_graph, goal_graph, options)

print("\n\n\nPath length cost result:", result.is_success(), "\n\n\n")