Welcome to pypolycontain’s documentation!¶
pypolycontain is a python package for polytopic objects, operations, and polytope containment problems. It is written as part of a project for verification and control of hybrid systems.
Setup¶
Installation is now easy:
pip install pypolycontain
Or download, and:
python3 setup.py install
Dependencies:¶
- numpy (Use latest version)
Optional dependencies (for some features)¶
Examples¶
Getting Started¶
The simplest tasks are pypolycontain is defining polytopic objects, performing operations, and visualizing them. First, import the package alongside numpy.
[1]:
import numpy as np
import pypolycontain as pp
Objects¶
We define an H-polytope \(\mathbb{P}=\{x \in \mathbb{R}^2 | Hx \le h\}\). We give the following numericals for H and h:
\[\begin{split}H=\left( \begin{array}{cc} 1 & 1 \\ -1 & 1 \\ 0 & -1 \end{array}\right),
h= \left( \begin{array}{c} 1 \\ 1 \\ 0 \end{array}\right).\end{split}\]
[2]:
H=np.array([[1,1],[-1,1],[0,-1]])
h=np.array([1,1,0])
A=pp.H_polytope(H,h)
This a triangle as it is defined by intersection of 3 half-spaces in \(\mathbb{R}^2\). In order to visualizate the polytope, we call the following function. Note the brackets around visualize function - it takes in a list of polytopes as its primary argument.
[3]:
pp.visualize([A],title=r'$A$')
We define an AH-polytope as \(t+T\mathbb{P}\) with the following numbers. The transformation represents a rotation of \(30^\circ\) and translation in \(x\) direction.
[4]:
t=np.array([5,0]).reshape(2,1) # offset
theta=np.pi/6 # 30 degrees
T=np.array([[np.cos(theta),np.sin(theta)],[-np.sin(theta),np.cos(theta)]]) # Linear transformation
B=pp.AH_polytope(t,T,A)
pp.visualize([B],title=r'$B$')
We define a zonotope as \(\mathbb{Z}=x+G[-1,1]^{n_p}\), where \(n_p\) is the number of rows in \(p\).
[5]:
x=np.array([4,0]).reshape(2,1) # offset
G=np.array([[1,0,0.5],[0,0.5,-1]]).reshape(2,3)
C=pp.zonotope(x=x,G=G)
pp.visualize([C],title=r'$C$')
Visualization¶
The visualize function allows for visualizing multiple polytopic objects.
[6]:
pp.visualize([A,C,B],title=r'$A,B,C$')
You may have noticed the colors. Here are the default colors for various polytopic objects. Using color argument you can change the color.
| Object | Default Color |
|---|---|
| H-polytope | Red |
| Zonotope | Green |
| AH-polytope | Blue |
visualize has a set of options. While it only supports 2D plotting, it can take high dimensional polytopes alongside the argument tuple_of_projection_dimensions. Its default is [0,1], meaning the projection into the first and second axis is demonstrated.
You can also add a separate subplot environment to plot the polytopes. Take the following example:
[7]:
import matplotlib.pyplot as plt
fig,ax=plt.subplots()
fig.set_size_inches(6, 3)
pp.visualize([A,B,C],ax=ax,fig=fig)
ax.set_title(r'A triangle (red), rotated by 30 degrees (blue), and a zonotope (green)',FontSize=15)
ax.set_xlabel(r'$x$',FontSize=15)
ax.set_ylabel(r'$y$',FontSize=15)
ax.axis('equal')
[7]:
(-1.343301270189222,
6.209326673973662,
-1.6499999999999997,
1.6499999999999997)
Operations¶
pypolycontain supports a broad range of polytopic operations. The complete list of operations is here.
[8]:
D=pp.operations.minkowski_sum(A,B)
D.color=(0.9, 0.9, 0.1)
pp.visualize([D,A,B],title=r'$A$ (red),$B$ (blue), $D=B\oplus C$ (yellow)')
Let’s take the convex-hull of \(A\) and \(C\).
[9]:
E=pp.convex_hull(A,C)
E.color='purple'
pp.visualize([E,A,C],alpha=0.5,title=r'$A$ (red),$C$ (green), $E={ConvexHull}(A,C)$ (purple)')
Let’s take the intersection of \(B\) and \(D\).
[10]:
F=pp.intersection(D,E)
F.color='black'
pp.visualize([D,E,F],alpha=1,title=r'$D$ (yellow), $E$ (purple), $F=D \cap E$ (black)')
We can compute the bounding box of a polytopic object. For instance, let’s compute the bounding box of \(D\).
[11]:
G=pp.bounding_box(D)
pp.visualize([G,D],title=r'$D$ and its bounding box')
Or the following:
[12]:
mylist=[A,B,C]
pp.visualize([pp.bounding_box(p) for p in mylist]+mylist,title=r'$A,B,C$ and their bounding boxes')
Example 1: The Necessity Gap for Minkowski Sums¶
In this example, we look for an instance of the following problem. We are given two H-polytopes \(\mathbb{A}, \mathbb{B}\). We manually find the H-polytope form of \(C:=A \oplus B\). Then we check $ \mathbb{C} \subseteq `:nbsphinx-math:mathbb{A}` \oplus `:nbsphinx-math:mathbb{B}` \subseteq `:nbsphinx-math:mathbb{C}`$ using containment arguments. Let \(\mathbb{A}\) be a triangle.
[1]:
import numpy as np
import pypolycontain as pp
H=np.array([[1,1],[-1,1],[0,-1]])
h=np.array([[1,1,0]]).reshape(3,1)
A=pp.H_polytope(H,h)
pp.visualize([A],title=r'$\mathbb{A}$')
And let \(B\) be a tiny rectangle at the bottom of \(A\) as follows.
[2]:
e=1/3
H=np.array([[1,0],[-1,0],[0,-1],[0,1]])
h=np.array([[e,e,1,0]]).reshape(4,1)
B=pp.H_polytope(H,h,color='green')
pp.visualize([A,B],title=r'$\mathbb{A}$ (top) and $\mathbb{B}$ (Bottom)')
The H-polytope form of the Minkowski sum \(\mathbb{A} \oplus \mathbb{B}\) can be easily found. We call this H-polytope \(C_H\).
[3]:
H=np.array([[1,0],[-1,0],[0,-1],[1,1],[-1,1],[0,1]])
h=np.array([[1+e,1+e,1,1+e,1+e,1]]).reshape(6,1)
p_sum=pp.H_polytope(H,h,color='purple')
pp.visualize([p_sum],title=r"$\mathbb{A} \oplus \mathbb{B}$")
We can also call the AH-polytope form of \(A\oplus B\).
[4]:
C=pp.minkowski_sum(A,B)
pp.visualize([C],title=r"$\mathbb{A} \oplus \mathbb{B}$")
Now we run the following experiment. We find the largest
\[ \begin{align}\begin{aligned}\begin{split} \begin{array}{lll}
\alpha^*_N = & \max. & \alpha \\
& \text{subject to} & \alpha C \subseteq_N (A \oplus B)
\end{array}\end{split}\\where for :math:`N=-1` the condition is necessary and sufficient and for :math:`N=0` it is only sufficient.\end{aligned}\end{align} \]
What we expect is that for necessary and sufficient condition, we obtain the largest possible \(\alpha^*\), which is 1. However, as we drop necessity, we are going to observe conservatieness in the fact that
\[\alpha_i\le 1, i \ge 0.\]
Maximzing of \(\alpha\) with subset encoding: linear program¶
We import the mathematicalprogram module from pydrake. As the optimization solver, we import Gurobi bindings of pydrake, but other solvers may also be used - there are often slower.
[10]:
pp.necessity_gap_k(p_sum,C,[0,1,2])
==================================================
==================================================
Computing Necessity Gaps
==================================================
==================================================
k Theta.shape delta(X,Y,C)
0 (7, 9) 0.0
1 (7, 12) 0.18557078904567903
2 (7, 7) 0.2142857142857152
Example 2: Necessary and Sufficiency for Containment¶
We investigate zonotope in V-polytope containment. First, we import numpy and pypolycontain.
[1]:
import pypolycontain as pp
import numpy as np
np.random.seed(0)
Import mathematical program with Gurobi solver from pydrake. Warning: Here we assume you have Gurobi solver installed with Drake’s python bindings. Otherwise, replace Gurobi with a different solver. For instance, SCS solver comes with Drake by default.
[2]:
N=[2,3,4,6,8,10,12,14]
Table={}
for n in N:
X=pp.box(n)
Y= pp.V_polytope([np.random.normal(size=(n,1)) for i in range(5+n)])
Table[n]=pp.necessity_gap_k(X,Y)
===========================================================================
===========================================================================
Computing Necessity Gaps
===========================================================================
===========================================================================
k Theta.shape delta(X,Y,C) alpha Computation Time
---------------------------------------------------------------------------
0 (7, 5) 0.000 0.594 0.003
1 (7, 8) 0.000 0.594 0.004
2 (7, 11) 0.085 0.543 0.004
3 (7, 10) 0.087 0.542 0.004
4 (7, 7) 0.087 0.542 0.003
===========================================================================
===========================================================================
Computing Necessity Gaps
===========================================================================
===========================================================================
k Theta.shape delta(X,Y,C) alpha Computation Time
---------------------------------------------------------------------------
0 (8, 12) 0.000 0.636 0.007
1 (8, 16) 0.045 0.608 0.011
2 (8, 16) 0.075 0.588 0.012
3 (8, 15) 0.135 0.551 0.011
4 (8, 8) 0.220 0.497 0.005
===========================================================================
===========================================================================
Computing Necessity Gaps
===========================================================================
===========================================================================
k Theta.shape delta(X,Y,C) alpha Computation Time
---------------------------------------------------------------------------
0 (9, 21) 0.000 0.233 0.017
1 (9, 28) 0.040 0.224 0.024
2 (9, 27) 0.086 0.213 0.025
3 (9, 20) 0.110 0.208 0.016
4 (9, 9) 0.166 0.195 0.008
===========================================================================
===========================================================================
Computing Necessity Gaps
===========================================================================
===========================================================================
k Theta.shape delta(X,Y,C) alpha Computation Time
---------------------------------------------------------------------------
0 (11, 58) 0.000 0.114 0.095
1 (11, 58) 0.011 0.113 0.095
2 (11, 44) 0.044 0.109 0.067
3 (11, 28) 0.123 0.100 0.041
4 (11, 11) 0.163 0.095 0.017
===========================================================================
===========================================================================
Computing Necessity Gaps
===========================================================================
===========================================================================
k Theta.shape delta(X,Y,C) alpha Computation Time
---------------------------------------------------------------------------
0 (13, 131) 0.000 0.088 0.508
1 (13, 116) 0.008 0.087 0.422
2 (13, 72) 0.036 0.085 0.209
3 (13, 42) 0.061 0.082 0.107
4 (13, 13) 0.114 0.078 0.032
===========================================================================
===========================================================================
Computing Necessity Gaps
===========================================================================
===========================================================================
k Theta.shape delta(X,Y,C) alpha Computation Time
---------------------------------------------------------------------------
0 (15, 289) 0.000 0.051 2.864
1 (15, 214) 0.023 0.050 1.739
2 (15, 120) 0.075 0.047 0.717
3 (15, 56) 0.112 0.045 0.234
4 (15, 15) 0.199 0.041 0.056
===========================================================================
===========================================================================
Computing Necessity Gaps
===========================================================================
===========================================================================
k Theta.shape delta(X,Y,C) alpha Computation Time
---------------------------------------------------------------------------
0 (17, 585) 0.000 0.078 14.981
1 (17, 390) 0.032 0.075 7.130
2 (17, 192) 0.078 0.072 2.246
3 (17, 70) 0.128 0.068 0.444
4 (17, 17) 0.170 0.064 0.087
===========================================================================
===========================================================================
Computing Necessity Gaps
===========================================================================
===========================================================================
k Theta.shape delta(X,Y,C) alpha Computation Time
---------------------------------------------------------------------------
0 (19, 1041) 0.000 0.064 60.858
1 (19, 528) 0.049 0.060 16.423
2 (19, 248) 0.095 0.058 4.249
3 (19, 78) 0.141 0.055 0.693
4 (19, 19) 0.209 0.050 0.133
<matplotlib.figure.Figure at 0x7fd400f8c208>
<matplotlib.figure.Figure at 0x7fd400f83dd8>
<matplotlib.figure.Figure at 0x7fd3fed5ddd8>
<matplotlib.figure.Figure at 0x7fd3fecd1128>
<matplotlib.figure.Figure at 0x7fd3febcf208>
<matplotlib.figure.Figure at 0x7fd3feb26518>
<matplotlib.figure.Figure at 0x7fd3fea35f60>
<matplotlib.figure.Figure at 0x7fd3fec27a58>
[3]:
import matplotlib.pyplot as plt
a={}
text={4: '(Corollary 2)', 0:'(Theorem 1)' , 1:'', 2:'',3:''}
Color=['red','green','blue','orange','gray']
for j in [4,3,2,1,0]:
a[j]=plt.plot( N, [Table[n][j][0] for n in N],label=r'$k=%d$ %s'%(j,text[j]),color=Color[j])
a[j]=plt.plot( N, [Table[n][j][0] for n in N],'o',color=Color[j])
plt.legend(loc="upper left")
plt.grid('on')
plt.xlabel(r'$n$',FontSize=20)
plt.ylabel(r'# of rows in $\Theta_k$',FontSize=20)
[4]:
import matplotlib.pyplot as plt
a={}
for j in [4,3,2,1,0]:
a[j]=plt.plot( N, [Table[n][j][1] for n in N],label=r'$k=%d$ %s'%(j,text[j]),color=Color[j])
a[j]=plt.plot( N, [Table[n][j][1] for n in N],'o',color=Color[j])
plt.legend(loc="upper center")
plt.grid('on')
plt.xlabel(r'$n$',FontSize=20)
plt.ylabel(r'$\delta(\mathbb{X},\mathbb{Y},\mathbb{C}_k)$',FontSize=20)
[5]:
import matplotlib.pyplot as plt
a={}
for j in [4,3,2,1,0]:
a[j]=plt.plot( N, [Table[n][j][3] for n in N],label=r'$k=%d$ %s'%(j,text[j]),color=Color[j])
a[j]=plt.plot( N, [Table[n][j][3] for n in N],'o',color=Color[j])
plt.legend(loc="upper left")
plt.grid('on')
plt.xlabel(r'$n$',FontSize=20)
plt.ylabel(r'CPU time',FontSize=20)
plt.yscale("log")
[ ]:
Example 3: Necessary and Sufficiency for containment in V-polytopes¶
We investigate unitbox in V-polytope containment. First, we import numpy and pypolycontain.
[1]:
import pypolycontain as pp
import numpy as np
np.random.seed(0)
[2]:
n_range=[3,5,7,9]
Table={}
N_vertices_range=[1,2,4,6,8,10]
for n in n_range:
for N_vertices in N_vertices_range:
print(n,N_vertices)
X=pp.box(n)
Y= pp.V_polytope([np.random.normal(size=(n,1)) for i in range(N_vertices+n)])
Table[n,N_vertices+n]=pp.necessity_gap_k(X,Y,only_final=True)
3 1
3 2
3 4
3 6
3 8
3 10
5 1
5 2
5 4
5 6
5 8
5 10
7 1
7 2
7 4
7 6
7 8
7 10
9 1
9 2
9 4
9 6
9 8
9 10
[3]:
import matplotlib.pyplot as plt
a={}
text={n: '%d'%n for n in n_range}
Color={2:'cyan', 3:'orange', 5:'red',7:'green',9:'blue'}
N_v= N_vertices_range
for n in n_range:
a[n]=plt.plot( N_v, [Table[n,k+n][0] for k in N_v],label=r'$n=%s$'%(text[n]),color=Color[n])
a[n]=plt.plot( N_v, [Table[n,k+n][0] for k in N_v],'o',color=Color[n])
plt.legend(loc="upper left")
plt.grid('on')
plt.xlabel(r'$n_v$',FontSize=20)
plt.ylabel(r'# of rows in $\Theta^*$ (Theorem 1)',FontSize=15)
[5]:
import matplotlib.pyplot as plt
a={}
text={n: '%d'%n for n in n_range}
Color={2:'cyan', 3:'orange', 5:'red',7:'green',9:'blue'}
N_v= N_vertices_range
for n in n_range:
a[n]=plt.plot( N_v, [Table[n,k+n][1] for k in N_v],label=r'$n=%s$'%(text[n]),color=Color[n])
a[n]=plt.plot( N_v, [Table[n,k+n][1] for k in N_v],'o',color=Color[n])
plt.legend(loc="upper left")
plt.grid('on')
plt.xlabel(r'$n_v$',FontSize=20)
plt.ylabel(r'$\delta(\mathbb{X},\mathbb{Y},\mathbb{R}_+^{q_y})$ (Corollary 2)',FontSize=15)
[ ]:
Example 4: Zonotope Containment: Integral Zonotope Counter Example¶
[1]:
import numpy as np
import pypolycontain as pp
import time
import pydrake.solvers.mathematicalprogram as MP
# use Gurobi solver
import pydrake.solvers.gurobi as Gurobi_drake
gurobi_solver=Gurobi_drake.GurobiSolver()
np.random.seed(0)
[2]:
def alpha_necessary(Z_i,Z_o):
alpha_min=np.inf
V=pp.vcube(Z_i.G.shape[1])
B=np.dot(Z_i.G,V.T)
for i in range(V.shape[0]):
program=MP.MathematicalProgram()
zeta=program.NewContinuousVariables(Z_o.G.shape[1],"zeta")
alpha=program.NewContinuousVariables(1,"alpha")
Aeq=np.hstack(( Z_o.G, -B[:,i:i+1]))
program.AddLinearEqualityConstraint( Aeq=np.hstack(( Z_o.G, -B[:,i:i+1])), \
beq=np.zeros((Z_o.G.shape[0])),\
vars= np.hstack((zeta,alpha)) )
program.AddBoundingBoxConstraint(-1,1,zeta)
program.AddLinearCost(-np.eye(1),np.zeros((1)),alpha)
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
alpha=result.GetSolution(alpha)[0]
alpha_min=min(alpha,alpha_min)
else:
print("optimization failed")
return alpha_min
def alpha_sufficient(Z_i,Z_o):
program=MP.MathematicalProgram()
beta=program.NewContinuousVariables(1,"beta")
circumbody=pp.to_AH_polytope(Z_o)
parametric_circumbody=circumbody.copy()
parametric_circumbody.P.h=circumbody.P.h*beta
Theta,*_=pp.subset(program,Z_i,parametric_circumbody,k=-1)
program.AddLinearCost(np.eye(1),np.zeros((1)),beta)
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
alpha=1/result.GetSolution(beta)[0]
return alpha
else:
print("optimization failed")
[3]:
n=3
n_y=4
gap,i=0,0
threshold=0.1
while gap<threshold and i<500:
G=np.random.randint(-5,6,size=(n,n_y))
X=pp.zonotope(G=np.eye(n))
Y=pp.zonotope(G=G,color='red')
a_nec=alpha_necessary(X,Y)
a_suf=alpha_sufficient(X,Y)
gap=1-a_suf/a_nec
i+=1
if gap>threshold:
print("stopped at iteration: ",i)
print('G=',G)
print('gap=',gap)
print('a_suf=',a_suf,'a_nec=',a_nec)
else:
print('Could not find a counter-example')
stopped at iteration: 348
G= [[ 4 -5 -2 5]
[ 2 -3 4 -5]
[ 4 5 4 4]]
gap= 0.14614516357454488
a_suf= 5.3670875432457175 a_nec= 6.285714285714286
[4]:
pp.visualize([Y,a_suf*X],tuple_of_projection_dimensions=[1,2])
pp.visualize([Y,a_nec*X],tuple_of_projection_dimensions=[1,2])
Example 5: Zonotope Containment: Empirial Study of Necessity Gaps¶
[1]:
import numpy as np
import pypolycontain as pp
import time
import pydrake.solvers.mathematicalprogram as MP
# use Gurobi solver
import pydrake.solvers.gurobi as Gurobi_drake
gurobi_solver=Gurobi_drake.GurobiSolver()
np.random.seed(0)
[2]:
def alpha_necessary_old(Z_i,Z_o):
program=MP.MathematicalProgram()
zeta=program.NewContinuousVariables(Z_o.G.shape[1],2**Z_i.G.shape[1],"zeta")
beta=program.NewContinuousVariables(1,"beta")
V=pp.vcube(Z_i.G.shape[1])
for i in range(V.shape[0]):
program.AddLinearEqualityConstraint( Aeq=Z_o.G, beq=np.dot(Z_i.G,V.T)[:,i], vars= zeta[:,i] )
program.AddLinearConstraint( np.less_equal(zeta,beta*np.ones(zeta.shape),dtype='object').flatten() )
program.AddLinearConstraint( np.less_equal(-zeta,beta*np.ones(zeta.shape),dtype='object').flatten() )
program.AddLinearCost(np.eye(1),np.zeros((1)),beta)
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
alpha=1/result.GetSolution(beta)[0]
return alpha
else:
print("optimization failed")
def alpha_necessary_older(Z_i,Z_o):
alpha_min=np.inf
V=pp.vcube(Z_i.G.shape[1])
B=np.dot(Z_i.G,V.T)
for i in range(V.shape[0]):
program=MP.MathematicalProgram()
zeta=program.NewContinuousVariables(Z_o.G.shape[1],"zeta")
beta=program.NewContinuousVariables(1,"beta")
program.AddLinearEqualityConstraint( Aeq=Z_o.G, beq=B[:,i:i+1], vars= zeta )
# for j in range(zeta.shape[0]):
# program.AddLinearConstraint( zeta[j]-beta, -np.inf, 0 )
# program.AddLinearConstraint( -zeta[j]+ beta, -np.inf, 0 )
program.AddLinearConstraint( np.less_equal(zeta,beta*np.ones(zeta.shape),dtype='object').flatten() )
program.AddLinearConstraint( np.less_equal(-zeta,beta*np.ones(zeta.shape),dtype='object').flatten() )
program.AddLinearCost(np.eye(1),np.zeros((1)),beta)
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
alpha=1/result.GetSolution(beta)[0]
alpha_min=min(alpha,alpha_min)
else:
print("optimization failed")
return alpha_min
def alpha_necessary(Z_i,Z_o):
alpha_min=np.inf
V=pp.vcube(Z_i.G.shape[1])
B=np.dot(Z_i.G,V.T)
for i in range(V.shape[0]):
program=MP.MathematicalProgram()
zeta=program.NewContinuousVariables(Z_o.G.shape[1],"zeta")
alpha=program.NewContinuousVariables(1,"alpha")
Aeq=np.hstack(( Z_o.G, -B[:,i:i+1]))
program.AddLinearEqualityConstraint( Aeq=np.hstack(( Z_o.G, -B[:,i:i+1])), \
beq=np.zeros((Z_o.G.shape[0])),\
vars= np.hstack((zeta,alpha)) )
program.AddBoundingBoxConstraint(-1,1,zeta)
program.AddLinearCost(-np.eye(1),np.zeros((1)),alpha)
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
alpha=result.GetSolution(alpha)[0]
alpha_min=min(alpha,alpha_min)
else:
print("optimization failed")
return alpha_min
def alpha_sufficient(Z_i,Z_o):
program=MP.MathematicalProgram()
beta=program.NewContinuousVariables(1,"beta")
circumbody=pp.to_AH_polytope(Z_o)
parametric_circumbody=circumbody.copy()
parametric_circumbody.P.h=circumbody.P.h*beta
Theta,*_=pp.subset(program,Z_i,parametric_circumbody,k=-1)
program.AddLinearCost(np.eye(1),np.zeros((1)),beta)
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
alpha=1/result.GetSolution(beta)[0]
return alpha
else:
print("optimization failed")
[3]:
N,n_max=100,20
D=10 # Percent to report
k,j=0,0 # Counter
d,j={},0
Q_o={ 3:[5,10,15,20], 5:[10,15,20], 10:[15,20], 15:[20] }
for n in Q_o.keys():
print("="*20,"\n","n=",n)
for q_o in Q_o[n]:
d[n,q_o]=np.zeros(N)
for i in range(N):
j+=1
print(n,q_o,i)
# q_i=np.random.randint(n,q_o+1)
z_i=pp.zonotope(G=np.eye(n))
z_o=pp.zonotope(G=np.random.normal(size=(n,q_o)))
# x=time.time()
a_suf=alpha_sufficient(z_i,z_o)
# print("sufficient:",time.time()-x)
# x=time.time()
a_nec=alpha_necessary(z_i,z_o)
# print("necessary:",time.time()-x)
d[n,q_o][i]=1-a_suf/a_nec
# print("gap:",n,q_o,"=",d[n,q_o][i])
j+=1
print("Done")
====================
n= 3
3 5 0
3 5 1
3 5 2
3 5 3
3 5 4
3 5 5
3 5 6
3 5 7
3 5 8
3 5 9
3 5 10
3 5 11
3 5 12
3 5 13
3 5 14
3 5 15
3 5 16
3 5 17
3 5 18
3 5 19
3 5 20
3 5 21
3 5 22
3 5 23
3 5 24
3 5 25
3 5 26
3 5 27
3 5 28
3 5 29
3 5 30
3 5 31
3 5 32
3 5 33
3 5 34
3 5 35
3 5 36
3 5 37
3 5 38
3 5 39
3 5 40
3 5 41
3 5 42
3 5 43
3 5 44
3 5 45
3 5 46
3 5 47
3 5 48
3 5 49
3 5 50
3 5 51
3 5 52
3 5 53
3 5 54
3 5 55
3 5 56
3 5 57
3 5 58
3 5 59
3 5 60
3 5 61
3 5 62
3 5 63
3 5 64
3 5 65
3 5 66
3 5 67
3 5 68
3 5 69
3 5 70
3 5 71
3 5 72
3 5 73
3 5 74
3 5 75
3 5 76
3 5 77
3 5 78
3 5 79
3 5 80
3 5 81
3 5 82
3 5 83
3 5 84
3 5 85
3 5 86
3 5 87
3 5 88
3 5 89
3 5 90
3 5 91
3 5 92
3 5 93
3 5 94
3 5 95
3 5 96
3 5 97
3 5 98
3 5 99
3 10 0
3 10 1
3 10 2
3 10 3
3 10 4
3 10 5
3 10 6
3 10 7
3 10 8
3 10 9
3 10 10
3 10 11
3 10 12
3 10 13
3 10 14
3 10 15
3 10 16
3 10 17
3 10 18
3 10 19
3 10 20
3 10 21
3 10 22
3 10 23
3 10 24
3 10 25
3 10 26
3 10 27
3 10 28
3 10 29
3 10 30
3 10 31
3 10 32
3 10 33
3 10 34
3 10 35
3 10 36
3 10 37
3 10 38
3 10 39
3 10 40
3 10 41
3 10 42
3 10 43
3 10 44
3 10 45
3 10 46
3 10 47
3 10 48
3 10 49
3 10 50
3 10 51
3 10 52
3 10 53
3 10 54
3 10 55
3 10 56
3 10 57
3 10 58
3 10 59
3 10 60
3 10 61
3 10 62
3 10 63
3 10 64
3 10 65
3 10 66
3 10 67
3 10 68
3 10 69
3 10 70
3 10 71
3 10 72
3 10 73
3 10 74
3 10 75
3 10 76
3 10 77
3 10 78
3 10 79
3 10 80
3 10 81
3 10 82
3 10 83
3 10 84
3 10 85
3 10 86
3 10 87
3 10 88
3 10 89
3 10 90
3 10 91
3 10 92
3 10 93
3 10 94
3 10 95
3 10 96
3 10 97
3 10 98
3 10 99
3 15 0
3 15 1
3 15 2
3 15 3
3 15 4
3 15 5
3 15 6
3 15 7
3 15 8
3 15 9
3 15 10
3 15 11
3 15 12
3 15 13
3 15 14
3 15 15
3 15 16
3 15 17
3 15 18
3 15 19
3 15 20
3 15 21
3 15 22
3 15 23
3 15 24
3 15 25
3 15 26
3 15 27
3 15 28
3 15 29
3 15 30
3 15 31
3 15 32
3 15 33
3 15 34
3 15 35
3 15 36
3 15 37
3 15 38
3 15 39
3 15 40
3 15 41
3 15 42
3 15 43
3 15 44
3 15 45
3 15 46
3 15 47
3 15 48
3 15 49
3 15 50
3 15 51
3 15 52
3 15 53
3 15 54
3 15 55
3 15 56
3 15 57
3 15 58
3 15 59
3 15 60
3 15 61
3 15 62
3 15 63
3 15 64
3 15 65
3 15 66
3 15 67
3 15 68
3 15 69
3 15 70
3 15 71
3 15 72
3 15 73
3 15 74
3 15 75
3 15 76
3 15 77
3 15 78
3 15 79
3 15 80
3 15 81
3 15 82
3 15 83
3 15 84
3 15 85
3 15 86
3 15 87
3 15 88
3 15 89
3 15 90
3 15 91
3 15 92
3 15 93
3 15 94
3 15 95
3 15 96
3 15 97
3 15 98
3 15 99
3 20 0
3 20 1
3 20 2
3 20 3
3 20 4
3 20 5
3 20 6
3 20 7
3 20 8
3 20 9
3 20 10
3 20 11
3 20 12
3 20 13
3 20 14
3 20 15
3 20 16
3 20 17
3 20 18
3 20 19
3 20 20
3 20 21
3 20 22
3 20 23
3 20 24
3 20 25
3 20 26
3 20 27
3 20 28
3 20 29
3 20 30
3 20 31
3 20 32
3 20 33
3 20 34
3 20 35
3 20 36
3 20 37
3 20 38
3 20 39
3 20 40
3 20 41
3 20 42
3 20 43
3 20 44
3 20 45
3 20 46
3 20 47
3 20 48
3 20 49
3 20 50
3 20 51
3 20 52
3 20 53
3 20 54
3 20 55
3 20 56
3 20 57
3 20 58
3 20 59
3 20 60
3 20 61
3 20 62
3 20 63
3 20 64
3 20 65
3 20 66
3 20 67
3 20 68
3 20 69
3 20 70
3 20 71
3 20 72
3 20 73
3 20 74
3 20 75
3 20 76
3 20 77
3 20 78
3 20 79
3 20 80
3 20 81
3 20 82
3 20 83
3 20 84
3 20 85
3 20 86
3 20 87
3 20 88
3 20 89
3 20 90
3 20 91
3 20 92
3 20 93
3 20 94
3 20 95
3 20 96
3 20 97
3 20 98
3 20 99
====================
n= 5
5 10 0
5 10 1
5 10 2
5 10 3
5 10 4
5 10 5
5 10 6
5 10 7
5 10 8
5 10 9
5 10 10
5 10 11
5 10 12
5 10 13
5 10 14
5 10 15
5 10 16
5 10 17
5 10 18
5 10 19
5 10 20
5 10 21
5 10 22
5 10 23
5 10 24
5 10 25
5 10 26
5 10 27
5 10 28
5 10 29
5 10 30
5 10 31
5 10 32
5 10 33
5 10 34
5 10 35
5 10 36
5 10 37
5 10 38
5 10 39
5 10 40
5 10 41
5 10 42
5 10 43
5 10 44
5 10 45
5 10 46
5 10 47
5 10 48
5 10 49
5 10 50
5 10 51
5 10 52
5 10 53
5 10 54
5 10 55
5 10 56
5 10 57
5 10 58
5 10 59
5 10 60
5 10 61
5 10 62
5 10 63
5 10 64
5 10 65
5 10 66
5 10 67
5 10 68
5 10 69
5 10 70
5 10 71
5 10 72
5 10 73
5 10 74
5 10 75
5 10 76
5 10 77
5 10 78
5 10 79
5 10 80
5 10 81
5 10 82
5 10 83
5 10 84
5 10 85
5 10 86
5 10 87
5 10 88
5 10 89
5 10 90
5 10 91
5 10 92
5 10 93
5 10 94
5 10 95
5 10 96
5 10 97
5 10 98
5 10 99
5 15 0
5 15 1
5 15 2
5 15 3
5 15 4
5 15 5
5 15 6
5 15 7
5 15 8
5 15 9
5 15 10
5 15 11
5 15 12
5 15 13
5 15 14
5 15 15
5 15 16
5 15 17
5 15 18
5 15 19
5 15 20
5 15 21
5 15 22
5 15 23
5 15 24
5 15 25
5 15 26
5 15 27
5 15 28
5 15 29
5 15 30
5 15 31
5 15 32
5 15 33
5 15 34
5 15 35
5 15 36
5 15 37
5 15 38
5 15 39
5 15 40
5 15 41
5 15 42
5 15 43
5 15 44
5 15 45
5 15 46
5 15 47
5 15 48
5 15 49
5 15 50
5 15 51
5 15 52
5 15 53
5 15 54
5 15 55
5 15 56
5 15 57
5 15 58
5 15 59
5 15 60
5 15 61
5 15 62
5 15 63
5 15 64
5 15 65
5 15 66
5 15 67
5 15 68
5 15 69
5 15 70
5 15 71
5 15 72
5 15 73
5 15 74
5 15 75
5 15 76
5 15 77
5 15 78
5 15 79
5 15 80
5 15 81
5 15 82
5 15 83
5 15 84
5 15 85
5 15 86
5 15 87
5 15 88
5 15 89
5 15 90
5 15 91
5 15 92
5 15 93
5 15 94
5 15 95
5 15 96
5 15 97
5 15 98
5 15 99
5 20 0
5 20 1
5 20 2
5 20 3
5 20 4
5 20 5
5 20 6
5 20 7
5 20 8
5 20 9
5 20 10
5 20 11
5 20 12
5 20 13
5 20 14
5 20 15
5 20 16
5 20 17
5 20 18
5 20 19
5 20 20
5 20 21
5 20 22
5 20 23
5 20 24
5 20 25
5 20 26
5 20 27
5 20 28
5 20 29
5 20 30
5 20 31
5 20 32
5 20 33
5 20 34
5 20 35
5 20 36
5 20 37
5 20 38
5 20 39
5 20 40
5 20 41
5 20 42
5 20 43
5 20 44
5 20 45
5 20 46
5 20 47
5 20 48
5 20 49
5 20 50
5 20 51
5 20 52
5 20 53
5 20 54
5 20 55
5 20 56
5 20 57
5 20 58
5 20 59
5 20 60
5 20 61
5 20 62
5 20 63
5 20 64
5 20 65
5 20 66
5 20 67
5 20 68
5 20 69
5 20 70
5 20 71
5 20 72
5 20 73
5 20 74
5 20 75
5 20 76
5 20 77
5 20 78
5 20 79
5 20 80
5 20 81
5 20 82
5 20 83
5 20 84
5 20 85
5 20 86
5 20 87
5 20 88
5 20 89
5 20 90
5 20 91
5 20 92
5 20 93
5 20 94
5 20 95
5 20 96
5 20 97
5 20 98
5 20 99
====================
n= 10
10 15 0
10 15 1
10 15 2
10 15 3
10 15 4
10 15 5
10 15 6
10 15 7
10 15 8
10 15 9
10 15 10
10 15 11
10 15 12
10 15 13
10 15 14
10 15 15
10 15 16
10 15 17
10 15 18
10 15 19
10 15 20
10 15 21
10 15 22
10 15 23
10 15 24
10 15 25
10 15 26
10 15 27
10 15 28
10 15 29
10 15 30
10 15 31
10 15 32
10 15 33
10 15 34
10 15 35
10 15 36
10 15 37
10 15 38
10 15 39
10 15 40
10 15 41
10 15 42
10 15 43
10 15 44
10 15 45
10 15 46
10 15 47
10 15 48
10 15 49
10 15 50
10 15 51
10 15 52
10 15 53
10 15 54
10 15 55
10 15 56
10 15 57
10 15 58
10 15 59
10 15 60
10 15 61
10 15 62
10 15 63
10 15 64
10 15 65
10 15 66
10 15 67
10 15 68
10 15 69
10 15 70
10 15 71
10 15 72
10 15 73
10 15 74
10 15 75
10 15 76
10 15 77
10 15 78
10 15 79
10 15 80
10 15 81
10 15 82
10 15 83
10 15 84
10 15 85
10 15 86
10 15 87
10 15 88
10 15 89
10 15 90
10 15 91
10 15 92
10 15 93
10 15 94
10 15 95
10 15 96
10 15 97
10 15 98
10 15 99
10 20 0
10 20 1
10 20 2
10 20 3
10 20 4
10 20 5
10 20 6
10 20 7
10 20 8
10 20 9
10 20 10
10 20 11
10 20 12
10 20 13
10 20 14
10 20 15
10 20 16
10 20 17
10 20 18
10 20 19
10 20 20
10 20 21
10 20 22
10 20 23
10 20 24
10 20 25
10 20 26
10 20 27
10 20 28
10 20 29
10 20 30
10 20 31
10 20 32
10 20 33
10 20 34
10 20 35
10 20 36
10 20 37
10 20 38
10 20 39
10 20 40
10 20 41
10 20 42
10 20 43
10 20 44
10 20 45
10 20 46
10 20 47
10 20 48
10 20 49
10 20 50
10 20 51
10 20 52
10 20 53
10 20 54
10 20 55
10 20 56
10 20 57
10 20 58
10 20 59
10 20 60
10 20 61
10 20 62
10 20 63
10 20 64
10 20 65
10 20 66
10 20 67
10 20 68
10 20 69
10 20 70
10 20 71
10 20 72
10 20 73
10 20 74
10 20 75
10 20 76
10 20 77
10 20 78
10 20 79
10 20 80
10 20 81
10 20 82
10 20 83
10 20 84
10 20 85
10 20 86
10 20 87
10 20 88
10 20 89
10 20 90
10 20 91
10 20 92
10 20 93
10 20 94
10 20 95
10 20 96
10 20 97
10 20 98
10 20 99
====================
n= 15
15 20 0
15 20 1
15 20 2
15 20 3
15 20 4
15 20 5
15 20 6
15 20 7
15 20 8
15 20 9
15 20 10
15 20 11
15 20 12
15 20 13
15 20 14
15 20 15
15 20 16
15 20 17
15 20 18
15 20 19
15 20 20
15 20 21
15 20 22
15 20 23
15 20 24
15 20 25
15 20 26
15 20 27
15 20 28
15 20 29
15 20 30
15 20 31
15 20 32
15 20 33
15 20 34
15 20 35
15 20 36
15 20 37
15 20 38
15 20 39
15 20 40
15 20 41
15 20 42
15 20 43
15 20 44
15 20 45
15 20 46
15 20 47
15 20 48
15 20 49
15 20 50
15 20 51
15 20 52
15 20 53
15 20 54
15 20 55
15 20 56
15 20 57
15 20 58
15 20 59
15 20 60
15 20 61
15 20 62
15 20 63
15 20 64
15 20 65
15 20 66
15 20 67
15 20 68
15 20 69
15 20 70
15 20 71
15 20 72
15 20 73
15 20 74
15 20 75
15 20 76
15 20 77
15 20 78
15 20 79
15 20 80
15 20 81
15 20 82
15 20 83
15 20 84
15 20 85
15 20 86
15 20 87
15 20 88
15 20 89
15 20 90
15 20 91
15 20 92
15 20 93
15 20 94
15 20 95
15 20 96
15 20 97
15 20 98
15 20 99
Done
[4]:
for key in d.keys():
print(key[0],' & ',key[1],' & ',np.sum(d[key]<0.001), ' & ', np.sum(d[key]<0.01), ' & ',\
np.sum(d[key]<0.05), ' & ', np.sum(d[key]<0.05), np.round(np.max(d[key]),3) , ' \\\ \\hline')
3 & 5 & 97 & 97 & 100 & 0.039 \\ \hline
3 & 10 & 89 & 94 & 99 & 0.051 \\ \hline
3 & 15 & 94 & 96 & 100 & 0.041 \\ \hline
3 & 20 & 94 & 98 & 100 & 0.024 \\ \hline
5 & 10 & 82 & 86 & 96 & 0.086 \\ \hline
5 & 15 & 77 & 81 & 98 & 0.077 \\ \hline
5 & 20 & 60 & 68 & 92 & 0.104 \\ \hline
10 & 15 & 66 & 76 & 96 & 0.095 \\ \hline
10 & 20 & 29 & 41 & 70 & 0.177 \\ \hline
15 & 20 & 58 & 71 & 94 & 0.1 \\ \hline
[ ]:
Example 7: AH-polytopes of Zonotopes¶
[1]:
import numpy as np
import pypolycontain as pp
np.random.seed(0)
[2]:
D={}
for n in [2,3,5,7,10]:
print("\n",n)
X1=pp.zonotope( x=np.zeros((n,1)), G=np.random.normal(size=(n,2*n)) )
# X2=pp.zonotope( x=np.ones((n,1))*n, G=np.random.normal(size=(n,n)) )
# X1.color='red'
# X2.color='purple'
# X=pp.convex_hull(X1,X2)
# X.color='yellow'
X=pp.to_AH_polytope(X1)
Y0=pp.ray_shooting_hyperplanes(X,N=n*15,tol=2)
print(n,'\t',Y0.H.shape[0])
B=pp.bounding_box(X)
# pp.visualize([B,Y0,X],alpha=0.5)
#print(pp.Hausdorff_distance(X,Y0,directed=True,k=0))
D[n,'box']=B.D
Y=pp.boxing_order_reduction(X1,1)
D[n,'hyperplanes']=Y0.H.shape[0]
try:
Y1,D[n,'inner']=pp.inner_optimization(X,Y0,k=-1,iterations=10)
except:
Y1,D[n,'inner']=pp.inner_optimization(X,Y0,k=-1,method='SDP')
Y2,D[n,'outer']=pp.outer_optimization(X,Y0)
D[n,'ray']=pp.Hausdorff_distance(X,Y0,directed=True,k=-1)/B.D
D[n,'opt']=pp.Hausdorff_distance(X,Y2,directed=True,k=-1)/B.D
D[n,'order']=pp.Hausdorff_distance(X,Y,directed=True,k=-1)/B.D
D[n,'hausinner']=pp.Hausdorff_distance(Y1,X,directed=True,k=-1)/B.D
2
2 9
Using Positive Orthant
********************
Alternating Convex Program for Determinent Maximization
This is often faster than SDP solving. If not, use SDP
0 det= 1.0
1 det= 0.6212127404975762
converged
success
determinent= 0.6212127404975762
Using Positive Orthant
success
determinent= 1.0000140945119869
3
3 25
Using Positive Orthant
********************
Alternating Convex Program for Determinent Maximization
This is often faster than SDP solving. If not, use SDP
0 det= 1.0
1 det= 0.2550747700813517
2 det= 0.2586155658675187
converged
success
determinent= 0.25879513215545863
Using Positive Orthant
success
determinent= 1.0000036510825714
5
5 118
Using Positive Orthant
********************
Alternating Convex Program for Determinent Maximization
This is often faster than SDP solving. If not, use SDP
0 det= 1.0
1 det= 0.025538770097233963
2 det= 0.026585852932575427
3 det= 0.022051016959032187
4 det= 0.02749466736237516
5 det= 0.022051016959032187
6 det= 0.02749466736237516
7 det= 0.022051016959032187
8 det= 0.02749466736237516
9 det= 0.022051016959032187
Error of convergence
Using Positive Orthant
success
determinent= 0.022447368854947834
Using Positive Orthant
success
determinent= 1.0461038895954713
7
7 210
Using Positive Orthant
********************
Alternating Convex Program for Determinent Maximization
This is often faster than SDP solving. If not, use SDP
0 det= 1.0
1 det= 0.000361246282276939
2 det= -5.5351050062610405e-52
3 det= 0.0
4 det= 0.0
5 det= 0.0
6
/home/sadra/Dropbox (MIT)/pypolycontain/pypolycontain/projection.py:282: RuntimeWarning: invalid value encountered in double_scalars
r=np.linalg.det(G_0)/det
det= 0.0
7 det= 0.0
8 det= 0.0
9 det= 0.0
Error of convergence
Using Positive Orthant
[2021-01-20 22:57:14.690] [console] [info] SCS returns code 2, with message "SCS solved inaccurate".
success
determinent= 0.0005543392161256746
Using Positive Orthant
success
determinent= 1.1845853240250332
10
10 300
Using Positive Orthant
********************
Alternating Convex Program for Determinent Maximization
This is often faster than SDP solving. If not, use SDP
0 det= 1.0
1 det= 2.9622052028931224e-06
2 det= 2.8259181404534783e-08
3 det= 1.3898487685099477e-12
4 det= 0.0
5 det= 0.0
6 det= 0.0
7 det= 0.0
8 det= 0.0
9 det= 0.0
Error of convergence
Using Positive Orthant
[2021-01-20 22:59:02.433] [console] [info] SCS returns code 2, with message "SCS solved inaccurate".
success
determinent= 2.482021723913747e-06
Using Positive Orthant
[2021-01-20 23:00:16.872] [console] [info] SCS returns code 2, with message "SCS solved inaccurate".
success
determinent= 1.9042742960096186
[3]:
for n in [2,3,5,7,10]:
print(r' %d & %d & %0.03f & %0.03f & %0.003f & %0.03f & %0.03f & %0.03f \\ \hline'\
%(n, D[n,'hyperplanes'], D[n,'inner'], D[n,'hausinner'],\
D[n,'outer'], D[n,'ray'],D[n,'opt'],D[n,'order'] ))
2 & 9 & 0.621 & 0.101 & 1.000 & 0.105 & 0.105 & 0.254 \\ \hline
3 & 25 & 0.259 & 0.140 & 1.000 & 0.111 & 0.111 & 0.196 \\ \hline
5 & 118 & 0.022 & 0.188 & 0.956 & 0.197 & 0.195 & 0.322 \\ \hline
7 & 210 & 0.001 & 0.240 & 0.844 & 0.287 & 0.310 & 0.359 \\ \hline
10 & 300 & 0.000 & 0.275 & 0.525 & 0.448 & 0.470 & 0.362 \\ \hline
[4]:
# Y1=pp.inner_optimization(X,Y0,k=-1)
# Y2=pp.outer_optimization(X,Y0)
[5]:
Y2.color,Y1.color,Y0.color='red','blue','green'
X.color,Y.color='black','yellow'
pp.visualize([Y2,Y0,X,Y1],alpha=0.9)
projection on 0 and 1 dimensions
projection on 0 and 1 dimensions
projection on 0 and 1 dimensions
projection on 0 and 1 dimensions
[6]:
# print('Ray Shooting',pp.Hausdorff_distance(X,Y,directed=True,k=-1))
# print('Optimization',pp.Hausdorff_distance(X,Y2,directed=True,k=-1))
# print('Order Reduction',pp.Hausdorff_distance(X,Y,directed=True,k=-1))
Example 8: Feasible Set of an MPC problem¶
[1]:
import numpy as np
import scipy.linalg as spa
import pypolycontain as pp
import pydrake.solvers.mathematicalprogram as MP
import pydrake.solvers.gurobi as Gurobi_drake
# use Gurobi solver
global gurobi_solver, license
gurobi_solver=Gurobi_drake.GurobiSolver()
license = gurobi_solver.AcquireLicense()
import pydrake.solvers.scs as SCS
scs_solver=SCS.ScsSolver()
np.random.seed(0)
[2]:
# Triangular stack
def triangular_stack(A,B):
q=B.shape[1]-A.shape[1]
if q>=0:
return np.vstack((np.hstack((A,np.zeros((A.shape[0],q)))),B))
else:
return np.vstack((A,np.hstack((B,np.zeros((B.shape[0],-q))))))
Dynamics of the system¶
We have:
\[ \begin{align}\begin{aligned} x^+ = Ax + Bu\\Then we have:\end{aligned}\end{align} \]
\[x_t = A^t x_0 + (A^{t-1}B, \cdots, AB, B) [u_0, \cdots, u_{t-1}, u_{t-1}]\]
[3]:
# Dynamics
delta=0.01
T=100
A=np.array([[1,delta],[-delta*2,1]])
B=np.array([[1/2*delta**2,delta]]).reshape(2,1)
phi={}
phi[1,'u']=B
phi[0]=np.eye(2)
for t in range(2,T+1):
phi[t,'u']= np.hstack(( np.dot(A,phi[t-1,'u']) , B ))
for t in range(1,T+1):
At=np.linalg.matrix_power(A,t)
phi[t]=np.hstack(( At, phi[t,'u'] ))
[4]:
H=np.array([[-1,0],\
[0,-1],\
[1,1]]).reshape(3,2)
h=np.array([1,1,1]).reshape(3,1)
# H=np.array([[-1,0.5],\
# [0,-1],\
# [1,0.5],
# [0,1]]).reshape(4,2)
# h=np.array([1,1,1,1]).reshape(4,1)
my_constraint=pp.H_polytope(H,h)
pp.visualize([my_constraint],figsize=(3,3))
[5]:
my_H=H
for t in range(1,T+1):
Hphi=np.dot(H,phi[t])
my_H=triangular_stack(my_H,Hphi)
my_h=np.vstack([h]*(T+1))
u_max=1
H_control= np.hstack(( np.zeros((2*(T),2)),pp.unitbox(T).H_polytope.H ))
h_control= np.hstack(( pp.unitbox(T).H_polytope.h * u_max )).reshape((T)*2,1)
final_H=np.vstack(( my_H, H_control))
final_h=np.vstack(( my_h, h_control))
my_set=pp.H_polytope( final_H, final_h)
my_set.color='blue'
[6]:
h_control.shape
[6]:
(200, 1)
[7]:
Pi=np.zeros(( 2,T+2 ))
Pi[0,0],Pi[1,1]=1,1
X=pp.affine_map(T=Pi,P=my_set)
V=pp.to_V(X,10**4)
[8]:
X.color='yellow'
pp.visualize([my_constraint,X],figsize=(5,5),title=r'T=%d, $\delta$=%0.02f, red: $\mathbb{X}$, yellow: $\mathbb{X}_0$ %(T,\delta)')
[9]:
from cdd import Polyhedron,Matrix,RepType
V.shape
[9]:
(225, 2)
[10]:
p_mat=Matrix(np.hstack ((np.ones((V.shape[0],1)),V )) )
p_mat.rep_type = RepType.GENERATOR
poly=Polyhedron(p_mat)
ineq=np.array(poly.get_inequalities())
h=np.atleast_2d(ineq[:,0]).T
H=-ineq[:,1:]
Y=pp.H_polytope(H,h,color='yellow')
pp.visualize([my_constraint,Y],figsize=(5,5))
H.shape
[10]:
(225, 2)
[11]:
S0=pp.ray_shooting_hyperplanes(X,N=3,H_y=None,tol=0.01)
[12]:
print(S0.H.shape)
S0.color='green'
pp.visualize([my_constraint,S0,Y],figsize=(7,7),alpha=0.9,title=\
'T=%d, $\delta=%0.02f$, red: $\mathbb{X}$, yellow: $\mathbb{X}_{0}$ (%d Hyperplanes), \n green: $\mathbb{X}_{out}$, outer approximation - %d Hyperplanes \n Hyperplane normals chosen randomly'\
%(T,delta,H.shape[0],S0.H.shape[0]))
(6, 2)
With Necessary and Sufficient Encoding (Lossless)
[13]:
if T<4:
inner,volume=pp.inner_optimization(X,X=S0,N=100,k=0)
inner.color='blue'
pp.visualize([S0,my_constraint,Y,inner],figsize=(6,6),alpha=1,title=\
'T=%d, $\delta=%0.02f$, red: $\mathbb{X}$, yellow: $\mathbb{X}_0$ (%d Hyperplanes), \n blue: $\mathbb{X}_0$, inner approximation - %d Hyperplanes)'\
%(T,delta,H.shape[0],inner.H.shape[0]))
[14]:
if T<6:
inner,volume=pp.inner_optimization(X,X=S0,N=100,k=1,iterations=5)
inner.color='blue'
pp.visualize([my_constraint,Y,inner],figsize=(7,7),alpha=0.9,title=\
'T=%d, $\delta=%0.02f$, red: $\mathbb{X}$, yellow: $\mathbb{X}_0$ (%d Hyperplanes), \n blue: $\mathbb{X}_{in}$, inner approx. - %d Hyperplanes, Volume=$%0.02f V_o$ \n Subset Encoding with Theorem 1'\
%(T,delta,H.shape[0],inner.H.shape[0],volume))
With Only Sufficient Encoding (Conservative but Efficient)¶
[15]:
inner,volume=pp.inner_optimization(X,X=S0,N=100,k=-1,approach='SDP')
inner.color='blue'
pp.visualize([my_constraint,Y,inner],figsize=(7,7),alpha=0.9,title=\
'T=%d, $\delta=%0.02f$, red: $\mathbb{X}$, yellow: $\mathbb{X}_0$ (%d Hyperplanes), \n blue: $\mathbb{X}_{in}$, inner approx. - %d Hyperplanes, Volume=$%0.02fV_o$ \n Subset Encoding with Corollary 2'\
%(T,delta,H.shape[0],inner.H.shape[0],volume))
Using Positive Orthant
[2021-01-18 19:33:19.110] [console] [info] SCS returns code 2, with message "SCS solved inaccurate".
success
determinent= 0.4620654718750543
[16]:
pp.Hausdorff_distance(inner,X,directed=True,k=-1)
[16]:
0.4325240802019477
[17]:
X
[17]:
AH_polytope from R^102 to R^2
Example 9: Robust Controlled Invariance or Constrained Quadratic Regulator¶
The following theorem encodes the conditions for robust controlled invariance using zonotope containment. As a byproduct, we also obtain polytopic control laws and polytopic Lyapunov functions for linear discrete-time systems. ### Theorem Consider a discrete-time disturbed linear system
\begin{equation}
x^+ \in Ax + Bu \oplus \mathbb{W},
\end{equation}
where \(x \in \mathbb{R}^n\) is the state, \(u \in \mathbb{R}^{m}\) is the control input, and \(\mathbb{W}=\langle W \rangle \in \mathbb{R}^n\), is the zonotope disturbance and \(W \in \mathbb{R}^{n \times n_w}\). If there exist \(\phi \in \mathbb{R}^{n \times q}, \theta \in \mathbb{R}^{m \times q}\), where \(q\) is user-defined, such that the following properties hold:
\[\exists E \in \mathbb{R}^{n \times n_w}, \exists \alpha \in [0,1),\]
\[(A \phi + B \theta, W) = (E,\phi),~~ \langle E \rangle \subseteq \alpha
\langle W \rangle.\]
\[ \begin{align}\begin{aligned} \mathcal{X} := \frac{1}{1-\alpha} \langle \phi \rangle, \mathcal{U} := \frac{1}{1-\alpha} \langle \theta \rangle.\\Then, :math:`\mathcal{X}` is a robust control invariant set under the control law :math:`\pi: \mathcal{X} \rightarrow \mathcal{U}`, where $ :nbsphinx-math:`\mathcal{X}` := :nbsphinx-math:`\frac{1}{1-\alpha}` :nbsphinx-math:`\langle `:nbsphinx-math:`\phi `:nbsphinx-math:`\rangle`, :nbsphinx-math:`\mathcal{U}` := :nbsphinx-math:`\frac{1}{1-\alpha}` :nbsphinx-math:`\langle `:nbsphinx-math:`\theta `:nbsphinx-math:`\rangle`$ and\end{aligned}\end{align} \]
\[ \begin{align}\begin{aligned}\begin{split} \pi(x)=\theta~ \underset{\zeta, x=\phi \zeta}{\arg \min}~ \| \zeta \|_\infty. \\\end{split}\\Moroever, :math:`V : \mathbb{R}^n \rightarrow \mathbb{R}_+` serves as a Lyapunov function for the undisturbed closed-loop system :math:`Ax+B\pi(x)`, where:\end{aligned}\end{align} \]
\[\begin{split}\begin{array}{lll}
V(x)=& \min & \| \zeta \|_\infty \\
& \text{subject to} & x= \phi \zeta.
\end{array}\end{split}\]
Proof¶
Given \(x= \frac{1}{1-\alpha} \phi \zeta, \|\zeta\|_\infty \le 1\), the control policy chooses \(u=\frac{1}{1-\alpha} \theta \zeta\). Therefore, \(x^+ \in \frac{1}{1-\alpha} \langle A\phi+B\theta \rangle \oplus \mathbb{W}\). We need to show that
\[ \begin{align}\begin{aligned} \frac{1}{1-\alpha} \langle A\phi+B\theta \rangle \oplus \mathbb{W} \in \frac{1}{1-\alpha} \langle \phi \rangle.\\From above we have the following:\end{aligned}\end{align} \]
\[ \begin{align}\begin{aligned} \label{eq_rci_proof_main}
\langle A\phi + B \theta \rangle \oplus \mathbb{W} \subseteq \alpha \mathbb{W} \oplus \langle \phi \rangle\\In order to derive the proof, we divide both sides by :math:`(1-\alpha)` and then take :math:`\frac{\alpha}{1-\alpha}\mathbb{W}` out from both sides. Note that this is a valid operation by the virtue of properties of support functions. A quick inspection also reveals that the Lyapunov function level sets are scalar multiplications of :math:`\mathcal{X}` and we have :math:`V(A x + B\pi(x)) \subseteq V(x) \langle \phi \rangle`.\end{aligned}\end{align} \]
Theorem above provides a framework to compute constrained robust control invariant sets via subset encodings. Note that \(\pi(x)\) is given as a linear program hence its explicit form is, in general, piecewise linear \cite{bemporad2000piecewise}. Given polyhedral constraints \(\mathbb{X}\) and \(\mathbb{U}\) on state and control, respectively, we can compute the constrained Quadratic Regulator (QR) with the following quadratic program:
\begin{equation}
\label{eq_quadratic_rci}
\begin{array}{ll}
\min & \text{tr}{(\phi \phi' Q)} + \text{tr}{(\theta \theta' R)} \\
\text{subject to} & (A \phi + B \theta, W) = (E,\phi),\\
& \langle \phi \rangle \subseteq (1-\alpha) \mathbb{X}, \langle \theta \rangle \subseteq (1-\alpha) \mathbb{U},
\\
&\langle E \rangle \subseteq \alpha \langle W \rangle, \\
& 0 \le \alpha \le 1,
\end{array}
\end{equation}
where \(Q,R\) are appropriately sized positive definite matrix penalizing the zonotope generators of states and controls, respectively.
Example:¶
Consider a system where \(A=I+A_c\delta, B=[\frac{1}{2}\delta^2,\delta]\), where \(A_c=[(0,1),(0,1)]\) and \(\delta=0.01\), representing a double integrator with negative damping. The constraint sets are \(\mathbb{X}=\mathbb{B}_1\) and \(\mathbb{U}=20\mathbb{B}_\infty\), and \(W=([(1,-1),(1,1)])\times \frac{\delta}{4}\). We solved with $Q=I, R=100 $. The smallest \(q\) that makes the program feasible is \(13\) - we deliberately selected a disturbance set such that no linear feedback policy \(u=Kx\) for any \(K\) solves this problem. The sets and sample undisturbed trajectories are shown.
[1]:
import numpy as np
import pypolycontain as pp
import pydrake.solvers.mathematicalprogram as MP
import pydrake.solvers.gurobi as Gurobi_drake
# use Gurobi solver
global gurobi_solver, license
gurobi_solver=Gurobi_drake.GurobiSolver()
license = gurobi_solver.AcquireLicense()
np.random.seed(0)
[2]:
n=2
A=np.eye(n)+np.random.randint(-5,5,size=(n,n))*1/10
A=np.eye(n)+np.random.randint(-5,5,size=(n,n))*1/10
dt=0.01
A=np.array([[1,dt],[0,1+1*dt]])
B=np.array([[dt**2/2,dt]]).reshape(2,1)
W=pp.zonotope(x=np.zeros((n,1)),G=np.array([[1,-1],[1,1]])*0.25*dt)
# X=pp.zonotope(x=np.zeros((n,1)),G=np.array([[1,0],[-0.5,1]]) ,color='cyan')
H=np.array([[-1,1],\
[1,-1],\
[-1,-1],[1,1]]).reshape(4,2)
h=np.array([1,1,1,1]).reshape(4,1)
X=pp.H_polytope(H,h,color='blue')
U=pp.zonotope(x=np.zeros((1,1)),G=np.eye(1)*20)
# Cost
Q=np.eye(n)
R=np.eye(1)*10000
pp.visualize([X])
[3]:
prog=MP.MathematicalProgram()
n,m=A.shape[0],B.shape[1]
q=13
program = MP.MathematicalProgram()
phi=program.NewContinuousVariables(n,q,'phi')
theta=program.NewContinuousVariables(m,q,'theta')
alpha=program.NewContinuousVariables(1,'alpha')
program.AddBoundingBoxConstraint(0,1,alpha)
program.AddQuadraticCost(np.trace( np.linalg.multi_dot([phi.T,Q,phi]) ))
program.AddQuadraticCost(np.trace( np.linalg.multi_dot([theta.T,R,theta]) ))
K=np.hstack(( (np.dot(A,phi) + np.dot(B,theta))[:,n:] , W.G ))
program.AddLinearConstraint ( np.equal(K, phi, dtype='object').flatten() )
inbody=pp.zonotope(x=np.zeros((n,1)), G=(np.dot(A,phi)+np.dot(B,theta))[:,0:n])
_W=pp.to_AH_polytope(W)
_W.P.h=_W.P.h*alpha
_X=pp.to_AH_polytope(X)
_X.P.h=_X.P.h*(1-alpha)
_U=pp.to_AH_polytope(U)
_U.P.h=_U.P.h*(1-alpha)
pp.subset(program, inbody,circumbody=_W)
pp.subset(program, pp.zonotope(x=np.zeros((2,1)),G=phi),circumbody=_X)
pp.subset(program, pp.zonotope(x=np.zeros((1,1)),G=theta),circumbody=_U)
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
print("sucsess")
alpha_n=result.GetSolution(alpha)
phi_n= result.GetSolution(phi)
theta_n= result.GetSolution(theta)
Omega=pp.zonotope(x=np.zeros((2,1)),G=phi_n/(1-alpha_n),color='red')
pp.visualize([X,Omega],title='Robust control invariant set $\mathcal{X}$ (red) \n \
inside safe set $\mathbb{X}$ (blue)',figsize=(6,6),a=0.02)
print("alpha was",alpha_n[0])
else:
print("failure")
sucsess
alpha was 0.9366234521326345
/home/sadra/Dropbox (MIT)/pypolycontain/pypolycontain/conversions.py:164: UserWarning: Zonotope Vertex Enumeration: The number of generators 13 is very large. Resorting to ray shooting
warnings.warn('Zonotope Vertex Enumeration: \
[4]:
def control(phi,theta,x):
program = MP.MathematicalProgram()
n,q=phi.shape
assert n,q==theta.shape
zeta=program.NewContinuousVariables(q,1,'zeta')
zeta_inf=program.NewContinuousVariables(1,1,'zeta_phi')
program.AddLinearConstraint( np.equal(x, np.dot(phi,zeta), dtype='object').flatten() )
program.AddLinearConstraint( np.greater_equal( zeta_inf, zeta, dtype='object').flatten() )
program.AddLinearConstraint( np.greater_equal( zeta_inf, -zeta, dtype='object').flatten() )
program.AddLinearCost( np.array([1]),np.array([0]), zeta_inf )
result=gurobi_solver.Solve(program,None,None)
if result.is_success():
# print("sucsess")
zeta_n=result.GetSolution(zeta)
# print( result.GetSolution(zeta_inf)[0] )
return np.dot(theta,zeta_n).reshape(1,1),result.GetSolution(zeta_inf)[0]
import matplotlib.pyplot as plt
fig,ax=plt.subplots()
# pp.visualize([pp.zonotope(G=V[t]*phi_n,color=(1-0.8*V[t]/V[0],0,0)) \
# for t in range(T)],ax=ax,fig=fig,\
# title=r'Sample Trajectory and Lyapunov Function Level Sets',a=0.1)
pp.visualize([pp.zonotope(G=p/15*phi_n/(1-alpha_n),color=(1*p/15,0,0)) \
for p in range(15,1,-1)],ax=ax,fig=fig,\
title='Sample Undisturbed Trajectories and \n Lyapunov Function Level Sets',a=0.02)
x0=pp.vcube(q)
fig.set_size_inches(6,6)
N=101
for i in range(N):
x={}
u={}
V={}
T=40
zeta=2*(np.random.random(size=(q,1))-0.5)
zeta=x0[int(i*len(x0)/N),:].reshape(q,1)
x[0]=np.dot(phi_n/(1-alpha_n), zeta/max(np.abs(zeta)))
for t in range(T):
u[t],V[t]=control(phi_n,theta_n,x[t])
x[t+1]=np.dot(A,x[t])+np.dot(B,u[t])
# ax.plot([x[t][0,0] for t in range(T+1)],[x[t][1,0] for t in range(T+1)],'*',color='cyan')
ax.plot([x[t][0,0] for t in range(1)],[x[t][1,0] for t in range(1)],'*',color='black',MarkerSize=1)
ax.plot([x[t][0,0] for t in range(T+1)],[x[t][1,0] for t in range(T+1)],'--',color='black',Linewidth=1)
[10]:
K=np.dot( theta_n, np.linalg.pinv(phi_n) )
[16]:
A_cl=A+B@K
np.linalg.eigvals(A_cl)
[16]:
array([0.98768713, 0.90900118])
[18]:
K@phi_n-theta_n
[18]:
array([[ 0.19712037, -0.19475627, 0.09975514, -0.10199905, 0.01056548,
-0.02595605, -0.04187009, -0.02414759, -0.06270881, 0.05449374,
-0.13783213, 0.1248632 , -0.20471651]])
[ ]:
Example 10 A: Inverted Pendulum with Wall¶
[1]:
import numpy as np
import scipy.linalg as spa
import pypolycontain as pp
import pydrake.solvers.mathematicalprogram as MP
import pydrake.solvers.gurobi as Gurobi_drake
# use Gurobi solver
global gurobi_solver, license
gurobi_solver=Gurobi_drake.GurobiSolver()
license = gurobi_solver.AcquireLicense()
import pypolycontain as pp
import pypolycontain.pwa_control as pwa
import matplotlib.pyplot as plt
Dynamcis and matrices¶
The system is constrained to \(|\theta| \le 0.12\), \(|\dot{\theta}| \le 1\), \(|u| \le 4\), and the wall is situated at \(\theta=0.1\). The problem is to identify a set of states \(\mathcal{X} \in \mathbb{R}^2\) and the associated control law \(\mu: [-0.12,0.12] \times [-1,1] \rightarrow [-4,4]\) such that all states in \(\mathcal{X}\) are steered toward origin in finite time, while respecting the constraints. It is desired that \(\mathcal{X}\) is as large as
possible. The dynamical system is described as a hybrid system with two modes associated with contact-free" andcontact”. The piecewise affine dynamics is given as:
\begin{equation*}
A_1=
\left(
\begin{array}{cc}
1 & 0.01 \\
0.1 & 1
\end{array}
\right),
A_2=
\left(
\begin{array}{cc}
1 & 0.01 \\
-9.9 & 1
\end{array}
\right),
\end{equation*}\begin{equation*}
B_1=B_2=
\left(
\begin{array}{c}
0 \\ 0.01
\end{array}
\right),
c_1=
\left(
\begin{array}{c}
0 \\ 0
\end{array}
\right) ,
c_2=
\left(
\begin{array}{c}
0 \\ 1
\end{array}
\right),
\end{equation*}
where mode 1 and 2 correspond to contact-free \(\theta \le 0.1\) and contact dynamics \(\theta >0.1\), respectively.
[2]:
A=np.array([[1,0.01],[0.1,1]])
B=np.array([0,0.01]).reshape(2,1)
c=np.array([0,0]).reshape(2,1)
C=pp.unitbox(N=3).H_polytope
C.h=np.array([0.1,1,4,0.1,1,4]).reshape(6,1)
S1=pwa.affine_system(A,B,c,name='free',XU=C)
# X=pp.zonotope(G=np.array([[0.1,0],[0,1]]))
# U=pp.zonotope(G=np.ones((1,1))*4)
# W=pp.zonotope(G=np.array([[0.1,0],[0,1]]))
# Omega=rci_old(A, B, X, U , W, q=5,eta=0.001)
import pickle
(H,h)=pickle.load(open('example_inverted_pendulum_H.pkl','rb'))
Omega=pp.H_polytope(H, h)
A=np.array([[1,0.01],[-9.9,1]])
B=np.array([0,0.01]).reshape(2,1)
c=np.array([0,1]).reshape(2,1)
C=pp.unitbox(N=3).H_polytope
C.h=np.array([0.12,1,4,-0.1,1,4]).reshape(6,1)
S2=pwa.affine_system(A,B,c,name='contact',XU=C)
myS=pwa.pwa_system()
myS.add_mode(S1)
myS.add_mode(S2)
A Polytopic Trajectory¶
[3]:
T=50
goal=0.0001*pp.unitbox(2).H_polytope
x0=np.array([0,0.75]).reshape(2,1)
F,FH,_,_,_=pwa.extend(myS,x0,T,[goal],H_rep=False,color='blue')
disjunctive subset with 1 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
[4]:
fig,ax=plt.subplots()
pp.visualize(F,fig=fig,ax=ax,a=0.01,alpha=0.9)
ax.set_xlabel(r'$\theta$',FontSize=30)
ax.set_ylabel(r'$\dot{\theta}$',FontSize=30)
ax.set_title('A Polytopic Trajectory (Blue)',FontSize=30)
ax.axvline(x=0.1,LineWidth=1,linestyle=':',color='black')
[4]:
<matplotlib.lines.Line2D at 0x7f5e0d9d5fa0>
My first branch: connect polytopic trajectories¶
[5]:
T=18
x0=np.array([0.075,0]).reshape(2,1)
F2,_,_,_,_=pwa.extend(myS,x0,T,F,H_rep=False,color='red')
fig,ax=plt.subplots()
pp.visualize(F+F2,fig=fig,ax=ax,a=0.01,alpha=0.9)
ax.set_xlabel(r'$\theta$',FontSize=30)
ax.set_ylabel(r'$\dot{\theta}$',FontSize=30)
ax.set_title('A Branch Added (red)',FontSize=30)
ax.axvline(x=0.1,LineWidth=1,linestyle=':',color='black')
disjunctive subset with 50 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
[5]:
<matplotlib.lines.Line2D at 0x7f5e0d9b0c10>
Building A Tree¶
[6]:
def sampler():
L=np.array([0.12,1])
return np.random.uniform(-L,L).reshape(2,1)
T=10
list_of_H_polytopes=[Omega]
list_of_nodes=[Omega]
stop_sampling=False
sample=lambda :sampler()
branch=0
trajectory={}
i=0
while branch<30 and i<500:
i+=1
print("i:",i, "branch:", branch)
while not stop_sampling:
x0=sample()
flag=pwa.in_the_tree(x0,list_of_H_polytopes)
stop_sampling=not flag
try:
print("sample:",x0.T)
x,u,mu=pwa.point_trajectory(myS,x0,T=60,goal=Omega,Q=np.eye(2)*1)
Y,YY,xx,mumu,G=pwa.extend(myS,x0,T,list_of_nodes)
trajectory[branch]=(x,u,mu,xx,mumu,G)
# Y,YY=extend(x0,T,[Omega])
list_of_nodes.extend(Y)
list_of_H_polytopes.extend(YY)
branch+=1
except:
print('failed to extend')
stop_sampling=False
i: 1 branch: 0
sample: [[ 0.10542656 -0.55298934]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 1 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 2 branch: 1
sample: [[ 0.10888614 -0.30973276]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 11 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 3 branch: 2
sample: [[0.04962442 0.8912329 ]]
trajectory optimization failed
failed to extend
i: 4 branch: 2
sample: [[0.11944372 0.46074421]]
trajectory optimization failed
failed to extend
i: 5 branch: 2
sample: [[0.11556939 0.30248857]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 21 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 6 branch: 3
sample: [[ 0.11342511 -0.9978576 ]]
trajectory optimization failed
failed to extend
i: 7 branch: 3
sample: [[-0.0514583 -0.82882893]]
trajectory optimization failed
failed to extend
i: 8 branch: 3
sample: [[-0.11426517 -0.33292296]]
trajectory optimization failed
failed to extend
i: 9 branch: 3
sample: [[0.03129503 0.98847687]]
trajectory optimization failed
failed to extend
i: 10 branch: 3
sample: [[0.07053641 0.91341162]]
trajectory optimization failed
failed to extend
i: 11 branch: 3
sample: [[-0.09691844 -0.73970566]]
trajectory optimization failed
failed to extend
i: 12 branch: 3
sample: [[0.08270974 0.60170748]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 31 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 13 branch: 4
sample: [[0.03241493 0.87782005]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 41 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization failed
failed to extend
i: 14 branch: 4
sample: [[0.11976544 0.9696734 ]]
trajectory optimization failed
failed to extend
i: 15 branch: 4
sample: [[0.03416291 0.85523679]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 41 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 16 branch: 5
sample: [[-0.09993941 -0.48358568]]
trajectory optimization failed
failed to extend
i: 17 branch: 5
sample: [[-0.03921922 -0.94129096]]
trajectory optimization failed
failed to extend
i: 18 branch: 5
sample: [[0.05732121 0.80574223]]
trajectory optimization failed
failed to extend
i: 19 branch: 5
sample: [[-0.09042575 -0.75508228]]
trajectory optimization failed
failed to extend
i: 20 branch: 5
sample: [[ 0.10517515 -0.99817834]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 51 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 21 branch: 6
sample: [[0.05843089 0.72455771]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 61 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 22 branch: 7
sample: [[-0.11273845 -0.33203535]]
trajectory optimization failed
failed to extend
i: 23 branch: 7
sample: [[0.10017557 0.46108371]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 71 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 24 branch: 8
sample: [[-0.11400837 -0.26732704]]
trajectory optimization failed
failed to extend
i: 25 branch: 8
sample: [[0.07123008 0.83086621]]
trajectory optimization failed
failed to extend
i: 26 branch: 8
sample: [[-0.05622478 -0.91579199]]
trajectory optimization failed
failed to extend
i: 27 branch: 8
sample: [[-0.08161407 -0.66828099]]
trajectory optimization failed
failed to extend
i: 28 branch: 8
sample: [[-0.01766078 0.99402729]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 81 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 29 branch: 9
sample: [[-0.10880149 -0.66612543]]
trajectory optimization failed
failed to extend
i: 30 branch: 9
sample: [[0.05412946 0.98390406]]
trajectory optimization failed
failed to extend
i: 31 branch: 9
sample: [[0.06385508 0.63591318]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 91 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 32 branch: 10
sample: [[-0.1037607 -0.52974012]]
trajectory optimization failed
failed to extend
i: 33 branch: 10
sample: [[0.11996755 0.73008329]]
trajectory optimization failed
failed to extend
i: 34 branch: 10
sample: [[0.07064777 0.9047168 ]]
trajectory optimization failed
failed to extend
i: 35 branch: 10
sample: [[0.10722065 0.27392227]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 101 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 36 branch: 11
sample: [[0.0762188 0.88192287]]
trajectory optimization failed
failed to extend
i: 37 branch: 11
sample: [[0.09602561 0.97671866]]
trajectory optimization failed
failed to extend
i: 38 branch: 11
sample: [[-0.00778613 -0.97076523]]
trajectory optimization failed
failed to extend
i: 39 branch: 11
sample: [[0.06989215 0.84638053]]
trajectory optimization failed
failed to extend
i: 40 branch: 11
sample: [[-0.00542398 -0.98156167]]
trajectory optimization failed
failed to extend
i: 41 branch: 11
sample: [[-0.07845084 -0.66595505]]
trajectory optimization failed
failed to extend
i: 42 branch: 11
sample: [[0.10540004 0.11215525]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 111 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 43 branch: 12
sample: [[-0.05507639 -0.98282724]]
trajectory optimization failed
failed to extend
i: 44 branch: 12
sample: [[0.1131282 0.69070458]]
trajectory optimization failed
failed to extend
i: 45 branch: 12
sample: [[0.1157123 0.62149352]]
trajectory optimization failed
failed to extend
i: 46 branch: 12
sample: [[0.08352203 0.74713948]]
trajectory optimization failed
failed to extend
i: 47 branch: 12
sample: [[-0.06600508 -0.91675399]]
trajectory optimization failed
failed to extend
i: 48 branch: 12
sample: [[-0.07450641 -0.79898525]]
trajectory optimization failed
failed to extend
i: 49 branch: 12
sample: [[0.09373615 0.41904856]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 121 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 50 branch: 13
sample: [[-0.09261711 -0.64209763]]
trajectory optimization failed
failed to extend
i: 51 branch: 13
sample: [[0.00202059 0.98907483]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 131 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 52 branch: 14
sample: [[ 0.10949283 -0.97492373]]
trajectory optimization failed
failed to extend
i: 53 branch: 14
sample: [[-0.094824 -0.93463611]]
trajectory optimization failed
failed to extend
i: 54 branch: 14
sample: [[-0.1145375 -0.61817211]]
trajectory optimization failed
failed to extend
i: 55 branch: 14
sample: [[0.10029011 0.17202706]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 141 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 56 branch: 15
sample: [[0.119883 0.30390691]]
trajectory optimization failed
failed to extend
i: 57 branch: 15
sample: [[0.09039157 0.70584242]]
trajectory optimization failed
failed to extend
i: 58 branch: 15
sample: [[-0.10055656 -0.45173284]]
trajectory optimization failed
failed to extend
i: 59 branch: 15
sample: [[-0.11918982 -0.62078994]]
trajectory optimization failed
failed to extend
i: 60 branch: 15
sample: [[0.07910053 0.93461887]]
trajectory optimization failed
failed to extend
i: 61 branch: 15
sample: [[-0.11295766 -0.72702994]]
trajectory optimization failed
failed to extend
i: 62 branch: 15
sample: [[-0.04311219 -0.879585 ]]
trajectory optimization failed
failed to extend
i: 63 branch: 15
sample: [[0.09745691 0.59807837]]
trajectory optimization failed
failed to extend
i: 64 branch: 15
sample: [[-0.11971791 -0.72088293]]
trajectory optimization failed
failed to extend
i: 65 branch: 15
sample: [[ 0.11997876 -0.75016584]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 151 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 66 branch: 16
sample: [[-0.10072723 -0.53663274]]
trajectory optimization failed
failed to extend
i: 67 branch: 16
sample: [[0.07372111 0.48690014]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 161 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 68 branch: 17
sample: [[-0.09742895 -0.68910216]]
trajectory optimization failed
failed to extend
i: 69 branch: 17
sample: [[ 0.10978858 -0.97477297]]
trajectory optimization failed
failed to extend
i: 70 branch: 17
sample: [[0.08784748 0.86011972]]
trajectory optimization failed
failed to extend
i: 71 branch: 17
sample: [[-0.10812794 -0.63344248]]
trajectory optimization failed
failed to extend
i: 72 branch: 17
sample: [[-0.08252939 -0.90586513]]
trajectory optimization failed
failed to extend
i: 73 branch: 17
sample: [[0.0986582 0.12825889]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 171 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 74 branch: 18
sample: [[-0.05490038 -0.99814712]]
trajectory optimization failed
failed to extend
i: 75 branch: 18
sample: [[0.05556785 0.68262905]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 181 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 76 branch: 19
sample: [[0.06807875 0.89568415]]
trajectory optimization failed
failed to extend
i: 77 branch: 19
sample: [[0.05531242 0.86727866]]
trajectory optimization failed
failed to extend
i: 78 branch: 19
sample: [[-0.11299076 -0.9992515 ]]
trajectory optimization failed
failed to extend
i: 79 branch: 19
sample: [[0.06371035 0.79727799]]
trajectory optimization failed
failed to extend
i: 80 branch: 19
sample: [[-0.07329166 -0.88997109]]
trajectory optimization failed
failed to extend
i: 81 branch: 19
sample: [[-0.05844484 -0.94525344]]
trajectory optimization failed
failed to extend
i: 82 branch: 19
sample: [[-0.11646581 -0.24880708]]
trajectory optimization failed
failed to extend
i: 83 branch: 19
sample: [[-0.11459362 -0.98743668]]
trajectory optimization failed
failed to extend
i: 84 branch: 19
sample: [[-0.04249693 -0.98195373]]
trajectory optimization failed
failed to extend
i: 85 branch: 19
sample: [[-0.09369075 -0.79880513]]
trajectory optimization failed
failed to extend
i: 86 branch: 19
sample: [[0.07351154 0.88919075]]
trajectory optimization failed
failed to extend
i: 87 branch: 19
sample: [[0.11844392 0.84475193]]
trajectory optimization failed
failed to extend
i: 88 branch: 19
sample: [[ 0.1075672 -0.97479447]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 191 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 89 branch: 20
sample: [[-0.09716836 -0.63476201]]
trajectory optimization failed
failed to extend
i: 90 branch: 20
sample: [[0.11223008 0.76840897]]
trajectory optimization failed
failed to extend
i: 91 branch: 20
sample: [[-0.05363204 -0.82216387]]
trajectory optimization failed
failed to extend
i: 92 branch: 20
sample: [[-0.03307455 -0.97855596]]
trajectory optimization failed
failed to extend
i: 93 branch: 20
sample: [[-0.0726759 -0.80057389]]
trajectory optimization failed
failed to extend
i: 94 branch: 20
sample: [[0.09045347 0.72447944]]
trajectory optimization failed
failed to extend
i: 95 branch: 20
sample: [[-0.11913411 -0.49558518]]
trajectory optimization failed
failed to extend
i: 96 branch: 20
sample: [[ 0.11285666 -0.76232236]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 201 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 97 branch: 21
sample: [[0.04618188 0.69295343]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 211 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 98 branch: 22
sample: [[-0.10281498 -0.47360099]]
trajectory optimization failed
failed to extend
i: 99 branch: 22
sample: [[0.09303129 0.88988476]]
trajectory optimization failed
failed to extend
i: 100 branch: 22
sample: [[0.05195334 0.97869148]]
trajectory optimization failed
failed to extend
i: 101 branch: 22
sample: [[-0.11549521 -0.59828554]]
trajectory optimization failed
failed to extend
i: 102 branch: 22
sample: [[-0.09222452 -0.68035124]]
trajectory optimization failed
failed to extend
i: 103 branch: 22
sample: [[-0.02108991 -0.90921155]]
trajectory optimization failed
failed to extend
i: 104 branch: 22
sample: [[-0.09139065 -0.70872524]]
trajectory optimization failed
failed to extend
i: 105 branch: 22
sample: [[-0.11557154 -0.57088983]]
trajectory optimization failed
failed to extend
i: 106 branch: 22
sample: [[-0.10389996 -0.77777705]]
trajectory optimization failed
failed to extend
i: 107 branch: 22
sample: [[0.04219405 0.95253164]]
trajectory optimization failed
failed to extend
i: 108 branch: 22
sample: [[0.05713966 0.90791624]]
trajectory optimization failed
failed to extend
i: 109 branch: 22
sample: [[-0.11445288 -0.54972784]]
trajectory optimization failed
failed to extend
i: 110 branch: 22
sample: [[-0.11479177 -0.68865383]]
trajectory optimization failed
failed to extend
i: 111 branch: 22
sample: [[0.09199979 0.96043823]]
trajectory optimization failed
failed to extend
i: 112 branch: 22
sample: [[-0.0900344 -0.52432922]]
trajectory optimization failed
failed to extend
i: 113 branch: 22
sample: [[-0.11839136 -0.66452104]]
trajectory optimization failed
failed to extend
i: 114 branch: 22
sample: [[0.10145316 0.01844955]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 221 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 115 branch: 23
sample: [[0.11519801 0.5527809 ]]
trajectory optimization failed
failed to extend
i: 116 branch: 23
sample: [[-0.09476713 -0.75144806]]
trajectory optimization failed
failed to extend
i: 117 branch: 23
sample: [[-0.116486 -0.61691578]]
trajectory optimization failed
failed to extend
i: 118 branch: 23
sample: [[-0.03967126 -0.91984188]]
trajectory optimization failed
failed to extend
i: 119 branch: 23
sample: [[-0.10520639 -0.99716386]]
trajectory optimization failed
failed to extend
i: 120 branch: 23
sample: [[-0.0975656 -0.96632449]]
trajectory optimization failed
failed to extend
i: 121 branch: 23
sample: [[0.1059718 0.6713763]]
trajectory optimization failed
failed to extend
i: 122 branch: 23
sample: [[0.0770198 0.86236426]]
trajectory optimization failed
failed to extend
i: 123 branch: 23
sample: [[0.04769507 0.83743442]]
trajectory optimization failed
failed to extend
i: 124 branch: 23
sample: [[-0.09895076 -0.91685958]]
trajectory optimization failed
failed to extend
i: 125 branch: 23
sample: [[0.08365723 0.67195752]]
trajectory optimization failed
failed to extend
i: 126 branch: 23
sample: [[-0.09410883 -0.63503449]]
trajectory optimization failed
failed to extend
i: 127 branch: 23
sample: [[0.03041085 0.89581229]]
trajectory optimization failed
failed to extend
i: 128 branch: 23
sample: [[0.09290769 0.75428636]]
trajectory optimization failed
failed to extend
i: 129 branch: 23
sample: [[0.11541756 0.71036372]]
trajectory optimization failed
failed to extend
i: 130 branch: 23
sample: [[-0.11666035 -0.4263072 ]]
trajectory optimization failed
failed to extend
i: 131 branch: 23
sample: [[-0.07586798 -0.8010288 ]]
trajectory optimization failed
failed to extend
i: 132 branch: 23
sample: [[-0.06302459 -0.93942184]]
trajectory optimization failed
failed to extend
i: 133 branch: 23
sample: [[-0.03744275 -0.98845198]]
trajectory optimization failed
failed to extend
i: 134 branch: 23
sample: [[-0.10331746 -0.49486501]]
trajectory optimization failed
failed to extend
i: 135 branch: 23
sample: [[-0.08332503 -0.9335536 ]]
trajectory optimization failed
failed to extend
i: 136 branch: 23
sample: [[-0.11979001 -0.51324068]]
trajectory optimization failed
failed to extend
i: 137 branch: 23
sample: [[0.09828601 0.77593971]]
trajectory optimization failed
failed to extend
i: 138 branch: 23
sample: [[-0.1190515 -0.63536466]]
trajectory optimization failed
failed to extend
i: 139 branch: 23
sample: [[0.08561348 0.89530739]]
trajectory optimization failed
failed to extend
i: 140 branch: 23
sample: [[0.08990427 0.6543293 ]]
trajectory optimization failed
failed to extend
i: 141 branch: 23
sample: [[-0.11375931 -0.67623574]]
trajectory optimization failed
failed to extend
i: 142 branch: 23
sample: [[0.06447839 0.80680644]]
trajectory optimization failed
failed to extend
i: 143 branch: 23
sample: [[-0.11706791 -0.29151241]]
trajectory optimization failed
failed to extend
i: 144 branch: 23
sample: [[0.06244764 0.94688836]]
trajectory optimization failed
failed to extend
i: 145 branch: 23
sample: [[-0.0656401 -0.80218197]]
trajectory optimization failed
failed to extend
i: 146 branch: 23
sample: [[0.1172896 0.5142567]]
trajectory optimization failed
failed to extend
i: 147 branch: 23
sample: [[0.01952175 0.95785342]]
trajectory optimization failed
failed to extend
i: 148 branch: 23
sample: [[0.11951248 0.77102965]]
trajectory optimization failed
failed to extend
i: 149 branch: 23
sample: [[-0.07902062 -0.997121 ]]
trajectory optimization failed
failed to extend
i: 150 branch: 23
sample: [[-0.10713063 -0.5854988 ]]
trajectory optimization failed
failed to extend
i: 151 branch: 23
sample: [[-5.26669264e-04 -9.92880225e-01]]
trajectory optimization failed
failed to extend
i: 152 branch: 23
sample: [[0.01955107 0.94259535]]
trajectory optimization failed
failed to extend
i: 153 branch: 23
sample: [[0.1083106 0.79825266]]
trajectory optimization failed
failed to extend
i: 154 branch: 23
sample: [[-0.1142475 -0.27784218]]
trajectory optimization failed
failed to extend
i: 155 branch: 23
sample: [[ 0.01747008 -0.98915037]]
trajectory optimization failed
failed to extend
i: 156 branch: 23
sample: [[ 0.01463403 -0.989829 ]]
trajectory optimization failed
failed to extend
i: 157 branch: 23
sample: [[ 0.01121045 -0.95196231]]
trajectory optimization failed
failed to extend
i: 158 branch: 23
sample: [[-0.11399645 -0.24316862]]
trajectory optimization failed
failed to extend
i: 159 branch: 23
sample: [[-0.08400244 -0.80551144]]
trajectory optimization failed
failed to extend
i: 160 branch: 23
sample: [[-0.04576952 -0.90709062]]
trajectory optimization failed
failed to extend
i: 161 branch: 23
sample: [[0.11042078 0.73507701]]
trajectory optimization failed
failed to extend
i: 162 branch: 23
sample: [[-0.11978232 -0.30934088]]
trajectory optimization failed
failed to extend
i: 163 branch: 23
sample: [[0.04913017 0.82625968]]
trajectory optimization failed
failed to extend
i: 164 branch: 23
sample: [[0.06182258 0.97994973]]
trajectory optimization failed
failed to extend
i: 165 branch: 23
sample: [[-0.11836861 -0.52588475]]
trajectory optimization failed
failed to extend
i: 166 branch: 23
sample: [[-0.10011933 -0.42866992]]
trajectory optimization failed
failed to extend
i: 167 branch: 23
sample: [[-0.05341407 -0.80681778]]
trajectory optimization failed
failed to extend
i: 168 branch: 23
sample: [[-0.06105005 -0.92647779]]
trajectory optimization failed
failed to extend
i: 169 branch: 23
sample: [[0.03860915 0.95174515]]
trajectory optimization failed
failed to extend
i: 170 branch: 23
sample: [[-0.10916027 -0.78827898]]
trajectory optimization failed
failed to extend
i: 171 branch: 23
sample: [[-0.01329018 -0.90924753]]
trajectory optimization failed
failed to extend
i: 172 branch: 23
sample: [[-0.03768557 -0.99155774]]
trajectory optimization failed
failed to extend
i: 173 branch: 23
sample: [[-0.08854178 -0.84765042]]
trajectory optimization failed
failed to extend
i: 174 branch: 23
sample: [[0.11511095 0.79042297]]
trajectory optimization failed
failed to extend
i: 175 branch: 23
sample: [[-0.11987101 -0.79526727]]
trajectory optimization failed
failed to extend
i: 176 branch: 23
sample: [[-0.02336568 0.97783889]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 231 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 177 branch: 24
sample: [[0.08330944 0.94861051]]
trajectory optimization failed
failed to extend
i: 178 branch: 24
sample: [[0.11891175 0.52206629]]
trajectory optimization failed
failed to extend
i: 179 branch: 24
sample: [[-0.02608382 -0.89138716]]
trajectory optimization failed
failed to extend
i: 180 branch: 24
sample: [[0.09053383 0.65516359]]
trajectory optimization failed
failed to extend
i: 181 branch: 24
sample: [[0.02392238 0.93564252]]
trajectory optimization failed
failed to extend
i: 182 branch: 24
sample: [[-0.10539442 -0.57380341]]
trajectory optimization failed
failed to extend
i: 183 branch: 24
sample: [[0.11165268 0.59624516]]
trajectory optimization failed
failed to extend
i: 184 branch: 24
sample: [[0.07066351 0.77592204]]
trajectory optimization failed
failed to extend
i: 185 branch: 24
sample: [[-0.11236572 -0.73822695]]
trajectory optimization failed
failed to extend
i: 186 branch: 24
sample: [[-0.0535247 -0.97983998]]
trajectory optimization failed
failed to extend
i: 187 branch: 24
sample: [[0.07621421 0.8918434 ]]
trajectory optimization failed
failed to extend
i: 188 branch: 24
sample: [[-0.1138111 -0.34605282]]
trajectory optimization failed
failed to extend
i: 189 branch: 24
sample: [[-0.11860737 -0.67921754]]
trajectory optimization failed
failed to extend
i: 190 branch: 24
sample: [[-0.10204481 -0.70503806]]
trajectory optimization failed
failed to extend
i: 191 branch: 24
sample: [[0.09760863 0.87916447]]
trajectory optimization failed
failed to extend
i: 192 branch: 24
sample: [[0.0373044 0.99720996]]
trajectory optimization failed
failed to extend
i: 193 branch: 24
sample: [[0.01514514 0.96877429]]
trajectory optimization failed
failed to extend
i: 194 branch: 24
sample: [[-0.11079819 -0.66655424]]
trajectory optimization failed
failed to extend
i: 195 branch: 24
sample: [[-0.09000169 -0.59673218]]
trajectory optimization failed
failed to extend
i: 196 branch: 24
sample: [[0.10829279 0.98884896]]
trajectory optimization failed
failed to extend
i: 197 branch: 24
sample: [[0.09788254 0.99122251]]
trajectory optimization failed
failed to extend
i: 198 branch: 24
sample: [[0.08156303 0.77256454]]
trajectory optimization failed
failed to extend
i: 199 branch: 24
sample: [[0.1075196 0.61426823]]
trajectory optimization failed
failed to extend
i: 200 branch: 24
sample: [[0.1041689 0.94345275]]
trajectory optimization failed
failed to extend
i: 201 branch: 24
sample: [[-0.05506382 -0.94099202]]
trajectory optimization failed
failed to extend
i: 202 branch: 24
sample: [[-0.04681217 -0.85614085]]
trajectory optimization failed
failed to extend
i: 203 branch: 24
sample: [[-0.01956403 -0.92757561]]
trajectory optimization failed
failed to extend
i: 204 branch: 24
sample: [[-0.02918418 -0.93756597]]
trajectory optimization failed
failed to extend
i: 205 branch: 24
sample: [[0.10814032 0.73624735]]
trajectory optimization failed
failed to extend
i: 206 branch: 24
sample: [[0.09226258 0.87004778]]
trajectory optimization failed
failed to extend
i: 207 branch: 24
sample: [[0.10740756 0.93316182]]
trajectory optimization failed
failed to extend
i: 208 branch: 24
sample: [[0.0607057 0.9205062]]
trajectory optimization failed
failed to extend
i: 209 branch: 24
sample: [[-0.03157327 0.97044173]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 241 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 210 branch: 25
sample: [[0.07534078 0.79461766]]
trajectory optimization failed
failed to extend
i: 211 branch: 25
sample: [[-0.11901667 -0.952616 ]]
trajectory optimization failed
failed to extend
i: 212 branch: 25
sample: [[-0.11387816 -0.35628757]]
trajectory optimization failed
failed to extend
i: 213 branch: 25
sample: [[-0.1125938 -0.77862679]]
trajectory optimization failed
failed to extend
i: 214 branch: 25
sample: [[-0.11203371 -0.51224808]]
trajectory optimization failed
failed to extend
i: 215 branch: 25
sample: [[-0.02856861 -0.90104146]]
trajectory optimization failed
failed to extend
i: 216 branch: 25
sample: [[ 0.11914767 -0.55388679]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 251 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 217 branch: 26
sample: [[-0.09438112 -0.92963963]]
trajectory optimization failed
failed to extend
i: 218 branch: 26
sample: [[0.1006489 0.65499996]]
trajectory optimization failed
failed to extend
i: 219 branch: 26
sample: [[0.11806497 0.35480288]]
trajectory optimization failed
failed to extend
i: 220 branch: 26
sample: [[0.07712432 0.93731028]]
trajectory optimization failed
failed to extend
i: 221 branch: 26
sample: [[0.04472826 0.90145605]]
trajectory optimization failed
failed to extend
i: 222 branch: 26
sample: [[-0.11945582 -0.54953325]]
trajectory optimization failed
failed to extend
i: 223 branch: 26
sample: [[-0.01550877 -0.92433913]]
trajectory optimization failed
failed to extend
i: 224 branch: 26
sample: [[-0.06992656 -0.93479326]]
trajectory optimization failed
failed to extend
i: 225 branch: 26
sample: [[-0.05287619 -0.93290166]]
trajectory optimization failed
failed to extend
i: 226 branch: 26
sample: [[0.06748575 0.89918167]]
trajectory optimization failed
failed to extend
i: 227 branch: 26
sample: [[-0.11280356 -0.53522797]]
trajectory optimization failed
failed to extend
i: 228 branch: 26
sample: [[0.09480422 0.88935928]]
trajectory optimization failed
failed to extend
i: 229 branch: 26
sample: [[-0.05949151 -0.99359816]]
trajectory optimization failed
failed to extend
i: 230 branch: 26
sample: [[0.08945728 0.69242101]]
trajectory optimization failed
failed to extend
i: 231 branch: 26
sample: [[-0.10827062 -0.50862895]]
trajectory optimization failed
failed to extend
i: 232 branch: 26
sample: [[-0.1179092 -0.32300559]]
trajectory optimization failed
failed to extend
i: 233 branch: 26
sample: [[-0.11008892 -0.65841583]]
trajectory optimization failed
failed to extend
i: 234 branch: 26
sample: [[-0.10097323 -0.66081476]]
trajectory optimization failed
failed to extend
i: 235 branch: 26
sample: [[0.0495561 0.84448446]]
trajectory optimization failed
failed to extend
i: 236 branch: 26
sample: [[ 0.11512514 -0.92638707]]
trajectory optimization failed
failed to extend
i: 237 branch: 26
sample: [[0.08411468 0.96270291]]
trajectory optimization failed
failed to extend
i: 238 branch: 26
sample: [[-0.06387215 -0.94957187]]
trajectory optimization failed
failed to extend
i: 239 branch: 26
sample: [[ 0.02170752 -0.97623865]]
trajectory optimization failed
failed to extend
i: 240 branch: 26
sample: [[-0.01130795 -0.91957216]]
trajectory optimization failed
failed to extend
i: 241 branch: 26
sample: [[0.08413214 0.72752457]]
trajectory optimization failed
failed to extend
i: 242 branch: 26
sample: [[0.08340569 0.85934195]]
trajectory optimization failed
failed to extend
i: 243 branch: 26
sample: [[-0.02154069 -0.92131271]]
trajectory optimization failed
failed to extend
i: 244 branch: 26
sample: [[-0.11508994 -0.79930048]]
trajectory optimization failed
failed to extend
i: 245 branch: 26
sample: [[-0.11337685 -0.95149941]]
trajectory optimization failed
failed to extend
i: 246 branch: 26
sample: [[-0.08266393 -0.81512658]]
trajectory optimization failed
failed to extend
i: 247 branch: 26
sample: [[-0.04554997 -0.92832374]]
trajectory optimization failed
failed to extend
i: 248 branch: 26
sample: [[-0.1184096 -0.49264027]]
trajectory optimization failed
failed to extend
i: 249 branch: 26
sample: [[-0.0656783 -0.7302753]]
trajectory optimization failed
failed to extend
i: 250 branch: 26
sample: [[-0.07323589 -0.70721566]]
trajectory optimization failed
failed to extend
i: 251 branch: 26
sample: [[0.11953674 0.834327 ]]
trajectory optimization failed
failed to extend
i: 252 branch: 26
sample: [[-0.07667365 -0.94419312]]
trajectory optimization failed
failed to extend
i: 253 branch: 26
sample: [[-0.09781552 -0.81818882]]
trajectory optimization failed
failed to extend
i: 254 branch: 26
sample: [[-0.09120859 -0.70681144]]
trajectory optimization failed
failed to extend
i: 255 branch: 26
sample: [[-0.11689087 -0.80549353]]
trajectory optimization failed
failed to extend
i: 256 branch: 26
sample: [[0.0327915 0.99739747]]
trajectory optimization failed
failed to extend
i: 257 branch: 26
sample: [[0.09927465 0.86122047]]
trajectory optimization failed
failed to extend
i: 258 branch: 26
sample: [[-0.03107307 -0.94938097]]
trajectory optimization failed
failed to extend
i: 259 branch: 26
sample: [[-0.11290975 -0.49664959]]
trajectory optimization failed
failed to extend
i: 260 branch: 26
sample: [[-0.08602234 -0.57824493]]
trajectory optimization failed
failed to extend
i: 261 branch: 26
sample: [[-0.04871199 -0.8507253 ]]
trajectory optimization failed
failed to extend
i: 262 branch: 26
sample: [[-0.06044826 -0.83126495]]
trajectory optimization failed
failed to extend
i: 263 branch: 26
sample: [[0.11550264 0.45768083]]
trajectory optimization failed
failed to extend
i: 264 branch: 26
sample: [[0.04496852 0.84313585]]
trajectory optimization failed
failed to extend
i: 265 branch: 26
sample: [[0.07098959 0.81861226]]
trajectory optimization failed
failed to extend
i: 266 branch: 26
sample: [[0.11687895 0.82667616]]
trajectory optimization failed
failed to extend
i: 267 branch: 26
sample: [[0.076322 0.73581608]]
trajectory optimization failed
failed to extend
i: 268 branch: 26
sample: [[0.03249864 0.99157988]]
trajectory optimization failed
failed to extend
i: 269 branch: 26
sample: [[0.09225535 0.77946209]]
trajectory optimization failed
failed to extend
i: 270 branch: 26
sample: [[0.0735279 0.81228519]]
trajectory optimization failed
failed to extend
i: 271 branch: 26
sample: [[-0.01658467 -0.99823294]]
trajectory optimization failed
failed to extend
i: 272 branch: 26
sample: [[0.01001264 0.98340294]]
trajectory optimization failed
failed to extend
i: 273 branch: 26
sample: [[-0.10674955 -0.98317521]]
trajectory optimization failed
failed to extend
i: 274 branch: 26
sample: [[0.07960999 0.69591881]]
trajectory optimization failed
failed to extend
i: 275 branch: 26
sample: [[-0.11253558 -0.67961845]]
trajectory optimization failed
failed to extend
i: 276 branch: 26
sample: [[0.10252298 0.59796186]]
trajectory optimization failed
failed to extend
i: 277 branch: 26
sample: [[0.11069726 0.97282636]]
trajectory optimization failed
failed to extend
i: 278 branch: 26
sample: [[0.06278815 0.97401905]]
trajectory optimization failed
failed to extend
i: 279 branch: 26
sample: [[0.08599511 0.97984579]]
trajectory optimization failed
failed to extend
i: 280 branch: 26
sample: [[0.11969587 0.62975832]]
trajectory optimization failed
failed to extend
i: 281 branch: 26
sample: [[ 0.0307069 -0.99628275]]
trajectory optimization failed
failed to extend
i: 282 branch: 26
sample: [[-0.06282738 -0.90517832]]
trajectory optimization failed
failed to extend
i: 283 branch: 26
sample: [[-0.07462478 -0.90657446]]
trajectory optimization failed
failed to extend
i: 284 branch: 26
sample: [[0.07302318 0.88369213]]
trajectory optimization failed
failed to extend
i: 285 branch: 26
sample: [[-0.09790072 -0.65505797]]
trajectory optimization failed
failed to extend
i: 286 branch: 26
sample: [[-0.10708323 -0.39129628]]
trajectory optimization failed
failed to extend
i: 287 branch: 26
sample: [[-0.05362518 -0.97159328]]
trajectory optimization failed
failed to extend
i: 288 branch: 26
sample: [[0.05631734 0.99238027]]
trajectory optimization failed
failed to extend
i: 289 branch: 26
sample: [[-0.08091441 -0.70674873]]
trajectory optimization failed
failed to extend
i: 290 branch: 26
sample: [[0.10048244 0.80351645]]
trajectory optimization failed
failed to extend
i: 291 branch: 26
sample: [[-0.04257106 -0.84315778]]
trajectory optimization failed
failed to extend
i: 292 branch: 26
sample: [[-0.05470322 -0.88794705]]
trajectory optimization failed
failed to extend
i: 293 branch: 26
sample: [[0.10979487 0.87662569]]
trajectory optimization failed
failed to extend
i: 294 branch: 26
sample: [[-0.08255297 -0.6700572 ]]
trajectory optimization failed
failed to extend
i: 295 branch: 26
sample: [[-0.10724033 -0.88915374]]
trajectory optimization failed
failed to extend
i: 296 branch: 26
sample: [[-0.07030092 -0.78475738]]
trajectory optimization failed
failed to extend
i: 297 branch: 26
sample: [[-0.10188582 -0.46694896]]
trajectory optimization failed
failed to extend
i: 298 branch: 26
sample: [[-0.08462912 -0.78439101]]
trajectory optimization failed
failed to extend
i: 299 branch: 26
sample: [[-0.11377039 -0.24312417]]
trajectory optimization failed
failed to extend
i: 300 branch: 26
sample: [[-0.02408323 -0.95924296]]
trajectory optimization failed
failed to extend
i: 301 branch: 26
sample: [[-0.08890393 -0.81669056]]
trajectory optimization failed
failed to extend
i: 302 branch: 26
sample: [[0.11922815 0.17899327]]
trajectory optimization failed
failed to extend
i: 303 branch: 26
sample: [[-0.02125307 -0.93495933]]
trajectory optimization failed
failed to extend
i: 304 branch: 26
sample: [[-0.09026955 -0.89874368]]
trajectory optimization failed
failed to extend
i: 305 branch: 26
sample: [[0.06488644 0.96288001]]
trajectory optimization failed
failed to extend
i: 306 branch: 26
sample: [[0.0898398 0.89868763]]
trajectory optimization failed
failed to extend
i: 307 branch: 26
sample: [[-0.11724937 -0.7659871 ]]
trajectory optimization failed
failed to extend
i: 308 branch: 26
sample: [[0.11085508 0.72340854]]
trajectory optimization failed
failed to extend
i: 309 branch: 26
sample: [[-0.11813973 -0.82106886]]
trajectory optimization failed
failed to extend
i: 310 branch: 26
sample: [[0.10216381 0.98900501]]
trajectory optimization failed
failed to extend
i: 311 branch: 26
sample: [[-0.09497647 -0.91159699]]
trajectory optimization failed
failed to extend
i: 312 branch: 26
sample: [[0.07351454 0.84306501]]
trajectory optimization failed
failed to extend
i: 313 branch: 26
sample: [[-0.09368238 -0.59741093]]
trajectory optimization failed
failed to extend
i: 314 branch: 26
sample: [[ 0.01468346 -0.97395686]]
trajectory optimization failed
failed to extend
i: 315 branch: 26
sample: [[-0.11269364 -0.61264492]]
trajectory optimization failed
failed to extend
i: 316 branch: 26
sample: [[0.1092473 0.54298218]]
trajectory optimization failed
failed to extend
i: 317 branch: 26
sample: [[-0.10122278 -0.79048655]]
trajectory optimization failed
failed to extend
i: 318 branch: 26
sample: [[-0.11062215 -0.70518922]]
trajectory optimization failed
failed to extend
i: 319 branch: 26
sample: [[-0.11765818 -0.13806741]]
trajectory optimization failed
failed to extend
i: 320 branch: 26
sample: [[-0.1000055 -0.54870954]]
trajectory optimization failed
failed to extend
i: 321 branch: 26
sample: [[-0.00747964 -0.95146833]]
trajectory optimization failed
failed to extend
i: 322 branch: 26
sample: [[-0.01919957 -0.90275687]]
trajectory optimization failed
failed to extend
i: 323 branch: 26
sample: [[0.09343584 0.79531782]]
trajectory optimization failed
failed to extend
i: 324 branch: 26
sample: [[0.01448828 0.9730944 ]]
trajectory optimization failed
failed to extend
i: 325 branch: 26
sample: [[-0.1187001 -0.98859457]]
trajectory optimization failed
failed to extend
i: 326 branch: 26
sample: [[ 0.1195023 -0.84965671]]
trajectory optimization failed
failed to extend
i: 327 branch: 26
sample: [[-0.04888129 -0.85225888]]
trajectory optimization failed
failed to extend
i: 328 branch: 26
sample: [[-0.05477403 -0.83647791]]
trajectory optimization failed
failed to extend
i: 329 branch: 26
sample: [[-0.11707048 -0.8169296 ]]
trajectory optimization failed
failed to extend
i: 330 branch: 26
sample: [[ 0.11583016 -0.88558805]]
trajectory optimization failed
failed to extend
i: 331 branch: 26
sample: [[-0.11863019 -0.49383275]]
trajectory optimization failed
failed to extend
i: 332 branch: 26
sample: [[-0.1178525 -0.57901801]]
trajectory optimization failed
failed to extend
i: 333 branch: 26
sample: [[0.07825867 0.70678026]]
trajectory optimization failed
failed to extend
i: 334 branch: 26
sample: [[-0.10956664 -0.98239293]]
trajectory optimization failed
failed to extend
i: 335 branch: 26
sample: [[-0.1072632 -0.45816856]]
trajectory optimization failed
failed to extend
i: 336 branch: 26
sample: [[0.04075735 0.93286676]]
trajectory optimization failed
failed to extend
i: 337 branch: 26
sample: [[-0.06171702 -0.97392117]]
trajectory optimization failed
failed to extend
i: 338 branch: 26
sample: [[0.1001124 0.81796657]]
trajectory optimization failed
failed to extend
i: 339 branch: 26
sample: [[-0.08446658 -0.79610797]]
trajectory optimization failed
failed to extend
i: 340 branch: 26
sample: [[0.04978616 0.83856417]]
trajectory optimization failed
failed to extend
i: 341 branch: 26
sample: [[-0.07163229 -0.85753173]]
trajectory optimization failed
failed to extend
i: 342 branch: 26
sample: [[-0.03339442 -0.96311868]]
trajectory optimization failed
failed to extend
i: 343 branch: 26
sample: [[-0.10941396 -0.97320807]]
trajectory optimization failed
failed to extend
i: 344 branch: 26
sample: [[0.1191136 0.51540555]]
trajectory optimization failed
failed to extend
i: 345 branch: 26
sample: [[0.07700679 0.88290313]]
trajectory optimization failed
failed to extend
i: 346 branch: 26
sample: [[0.06894353 0.98917045]]
trajectory optimization failed
failed to extend
i: 347 branch: 26
sample: [[0.05548939 0.95089213]]
trajectory optimization failed
failed to extend
i: 348 branch: 26
sample: [[-0.06061795 -0.87909282]]
trajectory optimization failed
failed to extend
i: 349 branch: 26
sample: [[-0.10883641 -0.68114592]]
trajectory optimization failed
failed to extend
i: 350 branch: 26
sample: [[0.09933405 0.7099691 ]]
trajectory optimization failed
failed to extend
i: 351 branch: 26
sample: [[0.11592052 0.58498774]]
trajectory optimization failed
failed to extend
i: 352 branch: 26
sample: [[-0.11580631 -0.98238866]]
trajectory optimization failed
failed to extend
i: 353 branch: 26
sample: [[-0.09259575 -0.98035424]]
trajectory optimization failed
failed to extend
i: 354 branch: 26
sample: [[0.09706573 0.18393693]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 261 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 355 branch: 27
sample: [[-0.10075499 -0.49535901]]
trajectory optimization failed
failed to extend
i: 356 branch: 27
sample: [[-0.04864415 -0.89596882]]
trajectory optimization failed
failed to extend
i: 357 branch: 27
sample: [[-0.09059536 -0.96640037]]
trajectory optimization failed
failed to extend
i: 358 branch: 27
sample: [[-0.0222104 -0.9442554]]
trajectory optimization failed
failed to extend
i: 359 branch: 27
sample: [[-0.08516276 -0.66345683]]
trajectory optimization failed
failed to extend
i: 360 branch: 27
sample: [[-0.11271743 -0.77177639]]
trajectory optimization failed
failed to extend
i: 361 branch: 27
sample: [[0.11194482 0.96930433]]
trajectory optimization failed
failed to extend
i: 362 branch: 27
sample: [[0.05943595 0.82249902]]
trajectory optimization failed
failed to extend
i: 363 branch: 27
sample: [[-0.07746362 -0.94331333]]
trajectory optimization failed
failed to extend
i: 364 branch: 27
sample: [[0.06966309 0.80546969]]
trajectory optimization failed
failed to extend
i: 365 branch: 27
sample: [[0.08586111 0.72386063]]
trajectory optimization failed
failed to extend
i: 366 branch: 27
sample: [[0.10753626 0.63148034]]
trajectory optimization failed
failed to extend
i: 367 branch: 27
sample: [[-0.08518635 -0.82424356]]
trajectory optimization failed
failed to extend
i: 368 branch: 27
sample: [[0.10627517 0.61617253]]
trajectory optimization failed
failed to extend
i: 369 branch: 27
sample: [[-0.07708343 -0.62738813]]
trajectory optimization failed
failed to extend
i: 370 branch: 27
sample: [[-0.09783602 -0.79227846]]
trajectory optimization failed
failed to extend
i: 371 branch: 27
sample: [[0.11490433 0.84507439]]
trajectory optimization failed
failed to extend
i: 372 branch: 27
sample: [[-0.09739002 -0.80115424]]
trajectory optimization failed
failed to extend
i: 373 branch: 27
sample: [[0.08337059 0.90294332]]
trajectory optimization failed
failed to extend
i: 374 branch: 27
sample: [[-0.08782184 -0.87448077]]
trajectory optimization failed
failed to extend
i: 375 branch: 27
sample: [[0.10817875 0.60802687]]
trajectory optimization failed
failed to extend
i: 376 branch: 27
sample: [[-0.11484914 -0.26825659]]
trajectory optimization failed
failed to extend
i: 377 branch: 27
sample: [[-0.10843113 -0.68637189]]
trajectory optimization failed
failed to extend
i: 378 branch: 27
sample: [[0.10602275 0.67223682]]
trajectory optimization failed
failed to extend
i: 379 branch: 27
sample: [[-0.07427844 -0.8584383 ]]
trajectory optimization failed
failed to extend
i: 380 branch: 27
sample: [[-0.08840411 -0.94402953]]
trajectory optimization failed
failed to extend
i: 381 branch: 27
sample: [[-0.05952032 -0.97794162]]
trajectory optimization failed
failed to extend
i: 382 branch: 27
sample: [[0.11730939 0.88277198]]
trajectory optimization failed
failed to extend
i: 383 branch: 27
sample: [[0.09471603 0.95430323]]
trajectory optimization failed
failed to extend
i: 384 branch: 27
sample: [[-0.09147162 -0.65862891]]
trajectory optimization failed
failed to extend
i: 385 branch: 27
sample: [[ 0.11613967 -0.96503147]]
trajectory optimization failed
failed to extend
i: 386 branch: 27
sample: [[0.11938437 0.52984141]]
trajectory optimization failed
failed to extend
i: 387 branch: 27
sample: [[-0.1018782 -0.88621676]]
trajectory optimization failed
failed to extend
i: 388 branch: 27
sample: [[-0.10850558 -0.99827765]]
trajectory optimization failed
failed to extend
i: 389 branch: 27
sample: [[0.07974761 0.94530905]]
trajectory optimization failed
failed to extend
i: 390 branch: 27
sample: [[-0.08915399 -0.74431949]]
trajectory optimization failed
failed to extend
i: 391 branch: 27
sample: [[ 0.11894476 -0.39165121]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 271 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 392 branch: 28
sample: [[ 0.00459794 -0.93536401]]
trajectory optimization failed
failed to extend
i: 393 branch: 28
sample: [[0.09888175 0.97055705]]
trajectory optimization failed
failed to extend
i: 394 branch: 28
sample: [[-0.10164411 -0.97641279]]
trajectory optimization failed
failed to extend
i: 395 branch: 28
sample: [[-0.10179124 -0.9247304 ]]
trajectory optimization failed
failed to extend
i: 396 branch: 28
sample: [[-0.11780222 -0.22410736]]
trajectory optimization failed
failed to extend
i: 397 branch: 28
sample: [[-0.05774827 -0.99280366]]
trajectory optimization failed
failed to extend
i: 398 branch: 28
sample: [[0.08882183 0.66038805]]
trajectory optimization failed
failed to extend
i: 399 branch: 28
sample: [[-0.06732029 -0.95555477]]
trajectory optimization failed
failed to extend
i: 400 branch: 28
sample: [[-0.11354243 -0.71829689]]
trajectory optimization failed
failed to extend
i: 401 branch: 28
sample: [[-0.06257937 -0.93707558]]
trajectory optimization failed
failed to extend
i: 402 branch: 28
sample: [[-0.05450246 -0.89652223]]
trajectory optimization failed
failed to extend
i: 403 branch: 28
sample: [[-0.05538075 -0.82323756]]
trajectory optimization failed
failed to extend
i: 404 branch: 28
sample: [[-0.07783645 -0.71230133]]
trajectory optimization failed
failed to extend
i: 405 branch: 28
sample: [[0.02565707 0.93095624]]
trajectory optimization failed
failed to extend
i: 406 branch: 28
sample: [[0.08867756 0.8104134 ]]
trajectory optimization failed
failed to extend
i: 407 branch: 28
sample: [[-0.07892605 -0.75818168]]
trajectory optimization failed
failed to extend
i: 408 branch: 28
sample: [[-0.08603175 -0.89923428]]
trajectory optimization failed
failed to extend
i: 409 branch: 28
sample: [[-0.11564813 -0.94661979]]
trajectory optimization failed
failed to extend
i: 410 branch: 28
sample: [[0.069885 0.99231708]]
trajectory optimization failed
failed to extend
i: 411 branch: 28
sample: [[-0.11077043 -0.80068732]]
trajectory optimization failed
failed to extend
i: 412 branch: 28
sample: [[0.09249957 0.9088614 ]]
trajectory optimization failed
failed to extend
i: 413 branch: 28
sample: [[-0.08103078 -0.93987681]]
trajectory optimization failed
failed to extend
i: 414 branch: 28
sample: [[-0.09308428 -0.5007687 ]]
trajectory optimization failed
failed to extend
i: 415 branch: 28
sample: [[-0.05024073 -0.97671807]]
trajectory optimization failed
failed to extend
i: 416 branch: 28
sample: [[ 0.01477186 -0.96087251]]
trajectory optimization failed
failed to extend
i: 417 branch: 28
sample: [[0.11710237 0.7861417 ]]
trajectory optimization failed
failed to extend
i: 418 branch: 28
sample: [[0.11655318 0.85910421]]
trajectory optimization failed
failed to extend
i: 419 branch: 28
sample: [[ 0.01646125 -0.9758973 ]]
trajectory optimization failed
failed to extend
i: 420 branch: 28
sample: [[-0.09229781 -0.9728997 ]]
trajectory optimization failed
failed to extend
i: 421 branch: 28
sample: [[0.03577311 0.98889505]]
trajectory optimization failed
failed to extend
i: 422 branch: 28
sample: [[0.07944525 0.75198507]]
trajectory optimization failed
failed to extend
i: 423 branch: 28
sample: [[-0.06784006 -0.9467128 ]]
trajectory optimization failed
failed to extend
i: 424 branch: 28
sample: [[-0.10911545 -0.99626568]]
trajectory optimization failed
failed to extend
i: 425 branch: 28
sample: [[0.08460172 0.93196681]]
trajectory optimization failed
failed to extend
i: 426 branch: 28
sample: [[-0.10271422 -0.72644062]]
trajectory optimization failed
failed to extend
i: 427 branch: 28
sample: [[-0.11840603 -0.55662055]]
trajectory optimization failed
failed to extend
i: 428 branch: 28
sample: [[-0.10872783 -0.9960047 ]]
trajectory optimization failed
failed to extend
i: 429 branch: 28
sample: [[0.03155351 0.93201089]]
trajectory optimization failed
failed to extend
i: 430 branch: 28
sample: [[-0.01214287 -0.98427966]]
trajectory optimization failed
failed to extend
i: 431 branch: 28
sample: [[0.03992491 0.91366306]]
trajectory optimization failed
failed to extend
i: 432 branch: 28
sample: [[0.10354817 0.85381944]]
trajectory optimization failed
failed to extend
i: 433 branch: 28
sample: [[-0.09083501 -0.71844739]]
trajectory optimization failed
failed to extend
i: 434 branch: 28
sample: [[-0.0939171 -0.59698057]]
trajectory optimization failed
failed to extend
i: 435 branch: 28
sample: [[0.11663324 0.49525048]]
trajectory optimization failed
failed to extend
i: 436 branch: 28
sample: [[-0.06905518 -0.97758949]]
trajectory optimization failed
failed to extend
i: 437 branch: 28
sample: [[-0.06747309 -0.86248013]]
trajectory optimization failed
failed to extend
i: 438 branch: 28
sample: [[-0.02214393 -0.89135098]]
trajectory optimization failed
failed to extend
i: 439 branch: 28
sample: [[0.07919412 0.93928312]]
trajectory optimization failed
failed to extend
i: 440 branch: 28
sample: [[-0.09848281 -0.47486367]]
trajectory optimization failed
failed to extend
i: 441 branch: 28
sample: [[0.08639556 0.84922762]]
trajectory optimization failed
failed to extend
i: 442 branch: 28
sample: [[0.07463766 0.84593372]]
trajectory optimization failed
failed to extend
i: 443 branch: 28
sample: [[-0.10373183 -0.4070522 ]]
trajectory optimization failed
failed to extend
i: 444 branch: 28
sample: [[-0.11403511 -0.97469343]]
trajectory optimization failed
failed to extend
i: 445 branch: 28
sample: [[-0.11935255 -0.98448437]]
trajectory optimization failed
failed to extend
i: 446 branch: 28
sample: [[0.05579821 0.93511201]]
trajectory optimization failed
failed to extend
i: 447 branch: 28
sample: [[-0.11299418 -0.24234242]]
trajectory optimization failed
failed to extend
i: 448 branch: 28
sample: [[0.05362654 0.83083263]]
trajectory optimization failed
failed to extend
i: 449 branch: 28
sample: [[0.08849769 0.73722348]]
trajectory optimization failed
failed to extend
i: 450 branch: 28
sample: [[0.09457432 0.7285717 ]]
trajectory optimization failed
failed to extend
i: 451 branch: 28
sample: [[-0.07839596 -0.6998724 ]]
trajectory optimization failed
failed to extend
i: 452 branch: 28
sample: [[-0.11727219 -0.79833731]]
trajectory optimization failed
failed to extend
i: 453 branch: 28
sample: [[-0.11439027 -0.61657391]]
trajectory optimization failed
failed to extend
i: 454 branch: 28
sample: [[0.11491962 0.59973619]]
trajectory optimization failed
failed to extend
i: 455 branch: 28
sample: [[0.05278105 0.92487049]]
trajectory optimization failed
failed to extend
i: 456 branch: 28
sample: [[-0.11917616 -0.57314137]]
trajectory optimization failed
failed to extend
i: 457 branch: 28
sample: [[-0.07458035 -0.90651721]]
trajectory optimization failed
failed to extend
i: 458 branch: 28
sample: [[-0.01031382 -0.91589637]]
trajectory optimization failed
failed to extend
i: 459 branch: 28
sample: [[-0.05495527 -0.95949428]]
trajectory optimization failed
failed to extend
i: 460 branch: 28
sample: [[0.06867291 0.88111717]]
trajectory optimization failed
failed to extend
i: 461 branch: 28
sample: [[0.11767318 0.47283222]]
trajectory optimization failed
failed to extend
i: 462 branch: 28
sample: [[0.0759584 0.80600018]]
trajectory optimization failed
failed to extend
i: 463 branch: 28
sample: [[-0.09599321 -0.51816534]]
trajectory optimization failed
failed to extend
i: 464 branch: 28
sample: [[-0.11454208 -0.42281983]]
trajectory optimization failed
failed to extend
i: 465 branch: 28
sample: [[ 0.00484536 -0.96872048]]
trajectory optimization failed
failed to extend
i: 466 branch: 28
sample: [[0.08723244 0.95238849]]
trajectory optimization failed
failed to extend
i: 467 branch: 28
sample: [[0.05543868 0.86709548]]
trajectory optimization failed
failed to extend
i: 468 branch: 28
sample: [[0.11915221 0.25874279]]
trajectory optimization failed
failed to extend
i: 469 branch: 28
sample: [[0.11730983 0.41175434]]
trajectory optimization failed
failed to extend
i: 470 branch: 28
sample: [[0.11437453 0.68202856]]
trajectory optimization failed
failed to extend
i: 471 branch: 28
sample: [[0.11950884 0.68770876]]
trajectory optimization failed
failed to extend
i: 472 branch: 28
sample: [[0.06687865 0.80239442]]
trajectory optimization failed
failed to extend
i: 473 branch: 28
sample: [[0.11788595 0.7664097 ]]
trajectory optimization failed
failed to extend
i: 474 branch: 28
sample: [[0.04279399 0.85787952]]
trajectory optimization failed
failed to extend
i: 475 branch: 28
sample: [[0.10626384 0.56316007]]
trajectory optimization failed
failed to extend
i: 476 branch: 28
sample: [[-0.06120604 -0.98892477]]
trajectory optimization failed
failed to extend
i: 477 branch: 28
sample: [[-0.08120654 -0.9010922 ]]
trajectory optimization failed
failed to extend
i: 478 branch: 28
sample: [[ 0.11625545 -0.90105413]]
trajectory optimization failed
failed to extend
i: 479 branch: 28
sample: [[0.11885055 0.21790328]]
trajectory optimization failed
failed to extend
i: 480 branch: 28
sample: [[-0.0952809 -0.75711553]]
trajectory optimization failed
failed to extend
i: 481 branch: 28
sample: [[-0.06992671 -0.84580261]]
trajectory optimization failed
failed to extend
i: 482 branch: 28
sample: [[-0.08811423 -0.69361452]]
trajectory optimization failed
failed to extend
i: 483 branch: 28
sample: [[-0.09194281 -0.92230693]]
trajectory optimization failed
failed to extend
i: 484 branch: 28
sample: [[-0.08419589 -0.73894079]]
trajectory optimization failed
failed to extend
i: 485 branch: 28
sample: [[-0.11087611 -0.50599407]]
trajectory optimization failed
failed to extend
i: 486 branch: 28
sample: [[ 0.11611513 -0.9636485 ]]
trajectory optimization failed
failed to extend
i: 487 branch: 28
sample: [[0.09463135 0.8283478 ]]
trajectory optimization failed
failed to extend
i: 488 branch: 28
sample: [[-0.10787818 -0.83745507]]
trajectory optimization failed
failed to extend
i: 489 branch: 28
sample: [[-0.10956505 -0.32582746]]
trajectory optimization failed
failed to extend
i: 490 branch: 28
sample: [[-0.11445527 -0.67957811]]
trajectory optimization failed
failed to extend
i: 491 branch: 28
sample: [[0.11636732 0.99586485]]
trajectory optimization failed
failed to extend
i: 492 branch: 28
sample: [[ 0.11706548 -0.93816133]]
trajectory optimization failed
failed to extend
i: 493 branch: 28
sample: [[-0.06880208 -0.89913645]]
trajectory optimization failed
failed to extend
i: 494 branch: 28
sample: [[-0.11549612 -0.99743544]]
trajectory optimization failed
failed to extend
i: 495 branch: 28
sample: [[-0.06299468 -0.83952864]]
trajectory optimization failed
failed to extend
i: 496 branch: 28
sample: [[-0.07592277 -0.86835874]]
trajectory optimization failed
failed to extend
i: 497 branch: 28
sample: [[0.11046238 0.87617637]]
trajectory optimization failed
failed to extend
i: 498 branch: 28
sample: [[-0.08766575 -0.96859446]]
trajectory optimization failed
failed to extend
i: 499 branch: 28
sample: [[-0.01450865 -0.99206583]]
trajectory optimization failed
failed to extend
i: 500 branch: 28
sample: [[-0.09328529 -0.92381507]]
trajectory optimization failed
failed to extend
i: 501 branch: 28
sample: [[0.05837175 0.77578232]]
trajectory optimization failed
failed to extend
i: 502 branch: 28
sample: [[0.11862068 0.84721378]]
trajectory optimization failed
failed to extend
i: 503 branch: 28
sample: [[0.11025691 0.76665954]]
trajectory optimization failed
failed to extend
i: 504 branch: 28
sample: [[-0.11527721 -0.85986523]]
trajectory optimization failed
failed to extend
i: 505 branch: 28
sample: [[0.09967157 0.9329729 ]]
trajectory optimization failed
failed to extend
i: 506 branch: 28
sample: [[0.07524161 0.81518591]]
trajectory optimization failed
failed to extend
i: 507 branch: 28
sample: [[ 0.03270065 -0.98641057]]
trajectory optimization failed
failed to extend
i: 508 branch: 28
sample: [[-0.05499123 -0.88878407]]
trajectory optimization failed
failed to extend
i: 509 branch: 28
sample: [[-0.11681197 -0.86016431]]
trajectory optimization failed
failed to extend
i: 510 branch: 28
sample: [[-0.08813732 -0.56779097]]
trajectory optimization failed
failed to extend
i: 511 branch: 28
sample: [[0.10879398 0.93649207]]
trajectory optimization failed
failed to extend
i: 512 branch: 28
sample: [[-0.00447362 -0.94741229]]
trajectory optimization failed
failed to extend
i: 513 branch: 28
sample: [[-0.08975061 -0.82639412]]
trajectory optimization failed
failed to extend
i: 514 branch: 28
sample: [[-0.07164252 -0.81070901]]
trajectory optimization failed
failed to extend
i: 515 branch: 28
sample: [[-0.08317413 -0.68413928]]
trajectory optimization failed
failed to extend
i: 516 branch: 28
sample: [[0.11233904 0.85485426]]
trajectory optimization failed
failed to extend
i: 517 branch: 28
sample: [[-0.10713771 -0.79213825]]
trajectory optimization failed
failed to extend
i: 518 branch: 28
sample: [[0.11580809 0.7835477 ]]
trajectory optimization failed
failed to extend
i: 519 branch: 28
sample: [[0.11878509 0.70791227]]
trajectory optimization failed
failed to extend
i: 520 branch: 28
sample: [[0.06085392 0.96982336]]
trajectory optimization failed
failed to extend
i: 521 branch: 28
sample: [[-1.19119525e-04 -9.58162771e-01]]
trajectory optimization failed
failed to extend
i: 522 branch: 28
sample: [[0.11638064 0.93093501]]
trajectory optimization failed
failed to extend
i: 523 branch: 28
sample: [[0.11984834 0.40541677]]
trajectory optimization failed
failed to extend
i: 524 branch: 28
sample: [[-0.08857024 -0.84401727]]
trajectory optimization failed
failed to extend
i: 525 branch: 28
sample: [[0.11613196 0.89386275]]
trajectory optimization failed
failed to extend
i: 526 branch: 28
sample: [[-0.11165678 -0.42128748]]
trajectory optimization failed
failed to extend
i: 527 branch: 28
sample: [[0.09372712 0.87101588]]
trajectory optimization failed
failed to extend
i: 528 branch: 28
sample: [[0.1133346 0.54643707]]
trajectory optimization failed
failed to extend
i: 529 branch: 28
sample: [[0.10378157 0.59290569]]
trajectory optimization failed
failed to extend
i: 530 branch: 28
sample: [[-0.02050762 -0.98782301]]
trajectory optimization failed
failed to extend
i: 531 branch: 28
sample: [[-0.0842461 -0.66384784]]
trajectory optimization failed
failed to extend
i: 532 branch: 28
sample: [[-0.06336627 -0.77242083]]
trajectory optimization failed
failed to extend
i: 533 branch: 28
sample: [[-0.10514786 -0.57450968]]
trajectory optimization failed
failed to extend
i: 534 branch: 28
sample: [[-0.0343557 -0.92965104]]
trajectory optimization failed
failed to extend
i: 535 branch: 28
sample: [[0.0881111 0.89979423]]
trajectory optimization failed
failed to extend
i: 536 branch: 28
sample: [[-0.10249509 -0.7074981 ]]
trajectory optimization failed
failed to extend
i: 537 branch: 28
sample: [[0.11926301 0.76050542]]
trajectory optimization failed
failed to extend
i: 538 branch: 28
sample: [[-0.11608018 -0.50809905]]
trajectory optimization failed
failed to extend
i: 539 branch: 28
sample: [[0.04217413 0.94924355]]
trajectory optimization failed
failed to extend
i: 540 branch: 28
sample: [[0.03923095 0.90113579]]
trajectory optimization failed
failed to extend
i: 541 branch: 28
sample: [[0.06469717 0.96444929]]
trajectory optimization failed
failed to extend
i: 542 branch: 28
sample: [[-0.01378172 -0.94977353]]
trajectory optimization failed
failed to extend
i: 543 branch: 28
sample: [[0.10726489 0.74327105]]
trajectory optimization failed
failed to extend
i: 544 branch: 28
sample: [[0.05491511 0.88854954]]
trajectory optimization failed
failed to extend
i: 545 branch: 28
sample: [[-0.05812318 -0.86146739]]
trajectory optimization failed
failed to extend
i: 546 branch: 28
sample: [[0.08193942 0.73418048]]
trajectory optimization failed
failed to extend
i: 547 branch: 28
sample: [[0.03174773 0.91162468]]
trajectory optimization failed
failed to extend
i: 548 branch: 28
sample: [[ 0.11392954 -0.99242943]]
trajectory optimization failed
failed to extend
i: 549 branch: 28
sample: [[ 0.11733017 -0.93381785]]
trajectory optimization failed
failed to extend
i: 550 branch: 28
sample: [[0.0854407 0.79776285]]
trajectory optimization failed
failed to extend
i: 551 branch: 28
sample: [[-0.05169542 -0.97680392]]
trajectory optimization failed
failed to extend
i: 552 branch: 28
sample: [[-0.06227907 -0.81094173]]
trajectory optimization failed
failed to extend
i: 553 branch: 28
sample: [[-0.10586805 -0.55584904]]
trajectory optimization failed
failed to extend
i: 554 branch: 28
sample: [[0.11523757 0.39087934]]
trajectory optimization failed
failed to extend
i: 555 branch: 28
sample: [[0.04273416 0.84892481]]
trajectory optimization failed
failed to extend
i: 556 branch: 28
sample: [[-0.01543762 -0.91864759]]
trajectory optimization failed
failed to extend
i: 557 branch: 28
sample: [[0.10118712 0.89619489]]
trajectory optimization failed
failed to extend
i: 558 branch: 28
sample: [[0.11949458 0.70662793]]
trajectory optimization failed
failed to extend
i: 559 branch: 28
sample: [[-0.11196097 -0.87144091]]
trajectory optimization failed
failed to extend
i: 560 branch: 28
sample: [[-0.11968185 -0.37092006]]
trajectory optimization failed
failed to extend
i: 561 branch: 28
sample: [[-0.08700147 -0.75629757]]
trajectory optimization failed
failed to extend
i: 562 branch: 28
sample: [[-0.03816768 -0.84545073]]
trajectory optimization failed
failed to extend
i: 563 branch: 28
sample: [[-0.09882155 -0.47135088]]
trajectory optimization failed
failed to extend
i: 564 branch: 28
sample: [[-0.11710719 -0.58825414]]
trajectory optimization failed
failed to extend
i: 565 branch: 28
sample: [[0.10132591 0.65518651]]
trajectory optimization failed
failed to extend
i: 566 branch: 28
sample: [[-0.05512199 -0.98997122]]
trajectory optimization failed
failed to extend
i: 567 branch: 28
sample: [[-0.11518078 -0.89807862]]
trajectory optimization failed
failed to extend
i: 568 branch: 28
sample: [[-0.11280745 -0.33592156]]
trajectory optimization failed
failed to extend
i: 569 branch: 28
sample: [[0.11625622 0.68379123]]
trajectory optimization failed
failed to extend
i: 570 branch: 28
sample: [[-0.08930641 -0.77083642]]
trajectory optimization failed
failed to extend
i: 571 branch: 28
sample: [[0.1169311 0.82102916]]
trajectory optimization failed
failed to extend
i: 572 branch: 28
sample: [[0.1084652 0.90819874]]
trajectory optimization failed
failed to extend
i: 573 branch: 28
sample: [[0.11500529 0.7321826 ]]
trajectory optimization failed
failed to extend
i: 574 branch: 28
sample: [[-0.09054098 -0.64309592]]
trajectory optimization failed
failed to extend
i: 575 branch: 28
sample: [[-0.09412913 -0.55277671]]
trajectory optimization failed
failed to extend
i: 576 branch: 28
sample: [[ 0.00667995 -0.97186214]]
trajectory optimization failed
failed to extend
i: 577 branch: 28
sample: [[0.05059005 0.8202738 ]]
trajectory optimization failed
failed to extend
i: 578 branch: 28
sample: [[0.09535955 0.95335972]]
trajectory optimization failed
failed to extend
i: 579 branch: 28
sample: [[-0.09150022 -0.73521934]]
trajectory optimization failed
failed to extend
i: 580 branch: 28
sample: [[-0.00642118 -0.93681425]]
trajectory optimization failed
failed to extend
i: 581 branch: 28
sample: [[0.09182075 0.85196851]]
trajectory optimization failed
failed to extend
i: 582 branch: 28
sample: [[-0.10833387 -0.84579984]]
trajectory optimization failed
failed to extend
i: 583 branch: 28
sample: [[-0.11793937 -0.55369172]]
trajectory optimization failed
failed to extend
i: 584 branch: 28
sample: [[0.1173567 0.92776239]]
trajectory optimization failed
failed to extend
i: 585 branch: 28
sample: [[-0.04526725 -0.98175409]]
trajectory optimization failed
failed to extend
i: 586 branch: 28
sample: [[0.10740923 0.72034108]]
trajectory optimization failed
failed to extend
i: 587 branch: 28
sample: [[0.0944379 0.81381017]]
trajectory optimization failed
failed to extend
i: 588 branch: 28
sample: [[-0.0354525 -0.86689581]]
trajectory optimization failed
failed to extend
i: 589 branch: 28
sample: [[-0.08123895 -0.78178754]]
trajectory optimization failed
failed to extend
i: 590 branch: 28
sample: [[-0.06805443 -0.75816197]]
trajectory optimization failed
failed to extend
i: 591 branch: 28
sample: [[0.09811329 0.6899196 ]]
trajectory optimization failed
failed to extend
i: 592 branch: 28
sample: [[0.11622649 0.88459085]]
trajectory optimization failed
failed to extend
i: 593 branch: 28
sample: [[0.06595099 0.98122377]]
trajectory optimization failed
failed to extend
i: 594 branch: 28
sample: [[-0.0966091 -0.63191789]]
trajectory optimization failed
failed to extend
i: 595 branch: 28
sample: [[0.11850869 0.43970344]]
trajectory optimization failed
failed to extend
i: 596 branch: 28
sample: [[0.11980597 0.90176095]]
trajectory optimization failed
failed to extend
i: 597 branch: 28
sample: [[0.11735501 0.45013522]]
trajectory optimization failed
failed to extend
i: 598 branch: 28
sample: [[0.03825818 0.889486 ]]
trajectory optimization failed
failed to extend
i: 599 branch: 28
sample: [[-0.11402661 -0.80078818]]
trajectory optimization failed
failed to extend
i: 600 branch: 28
sample: [[-0.0472895 -0.83282941]]
trajectory optimization failed
failed to extend
i: 601 branch: 28
sample: [[-0.06714785 -0.75975436]]
trajectory optimization failed
failed to extend
i: 602 branch: 28
sample: [[ 0.00559299 -0.97360823]]
trajectory optimization failed
failed to extend
i: 603 branch: 28
sample: [[-0.07377052 -0.90547284]]
trajectory optimization failed
failed to extend
i: 604 branch: 28
sample: [[0.06459767 0.98721155]]
trajectory optimization failed
failed to extend
i: 605 branch: 28
sample: [[-0.01579399 -0.93410694]]
trajectory optimization failed
failed to extend
i: 606 branch: 28
sample: [[-0.08646934 -0.56834679]]
trajectory optimization failed
failed to extend
i: 607 branch: 28
sample: [[-0.09088322 -0.84917844]]
trajectory optimization failed
failed to extend
i: 608 branch: 28
sample: [[0.10793856 0.90224687]]
trajectory optimization failed
failed to extend
i: 609 branch: 28
sample: [[-0.0824614 -0.66677197]]
trajectory optimization failed
failed to extend
i: 610 branch: 28
sample: [[-0.10566517 -0.84041184]]
trajectory optimization failed
failed to extend
i: 611 branch: 28
sample: [[-0.10353753 -0.76824754]]
trajectory optimization failed
failed to extend
i: 612 branch: 28
sample: [[-0.11734813 -0.67886298]]
trajectory optimization failed
failed to extend
i: 613 branch: 28
sample: [[-0.11094624 -0.59175098]]
trajectory optimization failed
failed to extend
i: 614 branch: 28
sample: [[0.09354433 0.26437874]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 281 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 615 branch: 29
sample: [[-0.11867765 -0.18319575]]
trajectory optimization failed
failed to extend
i: 616 branch: 29
sample: [[0.10970021 0.93585923]]
trajectory optimization failed
failed to extend
i: 617 branch: 29
sample: [[0.08660146 0.92518798]]
trajectory optimization failed
failed to extend
i: 618 branch: 29
sample: [[0.09910697 0.62947813]]
trajectory optimization failed
failed to extend
i: 619 branch: 29
sample: [[-0.0929712 -0.6102875]]
trajectory optimization failed
failed to extend
i: 620 branch: 29
sample: [[-0.03155233 -0.99910242]]
trajectory optimization failed
failed to extend
i: 621 branch: 29
sample: [[-0.11852482 -0.74211237]]
trajectory optimization failed
failed to extend
i: 622 branch: 29
sample: [[-0.04042835 -0.84775632]]
trajectory optimization failed
failed to extend
i: 623 branch: 29
sample: [[0.07116088 0.72892261]]
trajectory optimization failed
failed to extend
i: 624 branch: 29
sample: [[-0.06918643 -0.92791992]]
trajectory optimization failed
failed to extend
i: 625 branch: 29
sample: [[-0.10519167 -0.53333138]]
trajectory optimization failed
failed to extend
i: 626 branch: 29
sample: [[-0.07506354 -0.76972713]]
trajectory optimization failed
failed to extend
i: 627 branch: 29
sample: [[-0.11270886 -0.60283235]]
trajectory optimization failed
failed to extend
i: 628 branch: 29
sample: [[0.11539525 0.61956143]]
trajectory optimization failed
failed to extend
i: 629 branch: 29
sample: [[0.04630935 0.98531422]]
trajectory optimization failed
failed to extend
i: 630 branch: 29
sample: [[0.04914064 0.9479187 ]]
trajectory optimization failed
failed to extend
i: 631 branch: 29
sample: [[0.11567394 0.34616253]]
trajectory optimization failed
failed to extend
i: 632 branch: 29
sample: [[0.07296813 0.84875421]]
trajectory optimization failed
failed to extend
i: 633 branch: 29
sample: [[0.09633547 0.72366288]]
trajectory optimization failed
failed to extend
i: 634 branch: 29
sample: [[-0.10767406 -0.56020066]]
trajectory optimization failed
failed to extend
i: 635 branch: 29
sample: [[0.11380723 0.40131315]]
trajectory optimization failed
failed to extend
i: 636 branch: 29
sample: [[-0.03901659 -0.97500868]]
trajectory optimization failed
failed to extend
i: 637 branch: 29
sample: [[-0.11563291 -0.36726461]]
trajectory optimization failed
failed to extend
i: 638 branch: 29
sample: [[-0.10443205 -0.77514268]]
trajectory optimization failed
failed to extend
i: 639 branch: 29
sample: [[-0.11704807 -0.53376019]]
trajectory optimization failed
failed to extend
i: 640 branch: 29
sample: [[-0.1125637 -0.74676167]]
trajectory optimization failed
failed to extend
i: 641 branch: 29
sample: [[-0.01864082 -0.95127822]]
trajectory optimization failed
failed to extend
i: 642 branch: 29
sample: [[-0.11343316 -0.5685837 ]]
trajectory optimization failed
failed to extend
i: 643 branch: 29
sample: [[0.11797515 0.71713241]]
trajectory optimization failed
failed to extend
i: 644 branch: 29
sample: [[0.08696716 0.77830243]]
trajectory optimization failed
failed to extend
i: 645 branch: 29
sample: [[-0.1082594 -0.45784702]]
trajectory optimization failed
failed to extend
i: 646 branch: 29
sample: [[-0.01108604 -0.94174435]]
trajectory optimization failed
failed to extend
i: 647 branch: 29
sample: [[0.07553131 0.92817265]]
trajectory optimization failed
failed to extend
i: 648 branch: 29
sample: [[-0.11400913 -0.82012517]]
trajectory optimization failed
failed to extend
i: 649 branch: 29
sample: [[0.11789958 0.4055224 ]]
trajectory optimization failed
failed to extend
i: 650 branch: 29
sample: [[0.07319306 0.98376721]]
trajectory optimization failed
failed to extend
i: 651 branch: 29
sample: [[-0.02033994 -0.97716355]]
trajectory optimization failed
failed to extend
i: 652 branch: 29
sample: [[ 0.00824205 -0.98017632]]
trajectory optimization failed
failed to extend
i: 653 branch: 29
sample: [[-0.09765459 -0.7707448 ]]
trajectory optimization failed
failed to extend
i: 654 branch: 29
sample: [[-0.05369898 -0.93935421]]
trajectory optimization failed
failed to extend
i: 655 branch: 29
sample: [[0.11078069 0.90765311]]
trajectory optimization failed
failed to extend
i: 656 branch: 29
sample: [[-0.00884501 -0.97153987]]
trajectory optimization failed
failed to extend
i: 657 branch: 29
sample: [[0.08288967 0.94908298]]
trajectory optimization failed
failed to extend
i: 658 branch: 29
sample: [[0.11597008 0.34536507]]
trajectory optimization failed
failed to extend
i: 659 branch: 29
sample: [[-0.11562439 -0.6082448 ]]
trajectory optimization failed
failed to extend
i: 660 branch: 29
sample: [[-0.11128262 -0.98463681]]
trajectory optimization failed
failed to extend
i: 661 branch: 29
sample: [[-0.05000351 -0.93380378]]
trajectory optimization failed
failed to extend
i: 662 branch: 29
sample: [[-0.09851817 -0.7619897 ]]
trajectory optimization failed
failed to extend
i: 663 branch: 29
sample: [[-0.1042082 -0.83421604]]
trajectory optimization failed
failed to extend
i: 664 branch: 29
sample: [[-0.05415134 -0.92227879]]
trajectory optimization failed
failed to extend
i: 665 branch: 29
sample: [[-0.11621424 -0.90276084]]
trajectory optimization failed
failed to extend
i: 666 branch: 29
sample: [[-0.10818125 -0.5167839 ]]
trajectory optimization failed
failed to extend
i: 667 branch: 29
sample: [[0.11773837 0.30522953]]
trajectory optimization failed
failed to extend
i: 668 branch: 29
sample: [[0.03262721 0.96684418]]
trajectory optimization failed
failed to extend
i: 669 branch: 29
sample: [[ 0.11454749 -0.83222636]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 291 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
Visualization¶
[7]:
fig,ax=plt.subplots()
pp.visualize([Omega]+list_of_nodes,fig=fig,ax=ax,a=0.01,alpha=0.9)
ax.set_xlabel(r'$\theta$',FontSize=30)
ax.set_ylabel(r'$\dot{\theta}$',FontSize=30)
ax.set_title('%d Branches %d AH-polytopes'%(branch,len(list_of_nodes)),FontSize=30)
ax.axvline(x=0.1,LineWidth=1,linestyle=':',color='black')
/usr/lib/python3/dist-packages/matplotlib/colors.py:235: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray
c = np.array(c)
[7]:
<matplotlib.lines.Line2D at 0x7f5e0ceafa90>
We generate random points and see
[9]:
Trials=200
covered=0
false_positive=0
feasible=0
feasible_but_not_covered_by_N_10=0
for N in range(Trials):
x0=sample()
print(N)
try:
_,_,_=pwa.point_trajectory(myS,x0,T=50,goal=Omega,Q=np.eye(2)*100)
feasible+=1
covered+=pwa.in_the_tree(x0,list_of_H_polytopes)
try:
_,_,_=pwa.point_trajectory(myS,x0,T=10,goal=Omega,Q=np.eye(2)*100)
except:
feasible_but_not_covered_by_N_10+=1
except:
false_positive+=pwa.in_the_tree(x0,list_of_H_polytopes)
0
trajectory optimization succesfull
trajectory optimization succesfull
1
trajectory optimization failed
2
trajectory optimization failed
3
trajectory optimization succesfull
trajectory optimization succesfull
4
trajectory optimization failed
5
trajectory optimization succesfull
trajectory optimization succesfull
6
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
7
trajectory optimization succesfull
trajectory optimization succesfull
8
trajectory optimization succesfull
trajectory optimization succesfull
9
trajectory optimization succesfull
trajectory optimization succesfull
10
trajectory optimization succesfull
trajectory optimization succesfull
11
trajectory optimization succesfull
trajectory optimization succesfull
12
trajectory optimization failed
13
trajectory optimization failed
14
trajectory optimization succesfull
trajectory optimization succesfull
15
trajectory optimization failed
16
trajectory optimization succesfull
trajectory optimization succesfull
17
trajectory optimization succesfull
trajectory optimization succesfull
18
trajectory optimization succesfull
trajectory optimization succesfull
19
trajectory optimization failed
20
trajectory optimization failed
21
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
22
trajectory optimization failed
23
trajectory optimization failed
24
trajectory optimization succesfull
trajectory optimization succesfull
25
trajectory optimization failed
26
trajectory optimization succesfull
trajectory optimization succesfull
27
trajectory optimization succesfull
trajectory optimization succesfull
28
trajectory optimization succesfull
trajectory optimization succesfull
29
trajectory optimization succesfull
trajectory optimization succesfull
30
trajectory optimization succesfull
trajectory optimization succesfull
31
trajectory optimization succesfull
trajectory optimization succesfull
32
trajectory optimization succesfull
trajectory optimization succesfull
33
trajectory optimization failed
34
trajectory optimization failed
35
trajectory optimization failed
36
trajectory optimization succesfull
trajectory optimization succesfull
37
trajectory optimization succesfull
trajectory optimization succesfull
38
trajectory optimization succesfull
trajectory optimization succesfull
39
trajectory optimization succesfull
trajectory optimization succesfull
40
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
41
trajectory optimization succesfull
trajectory optimization succesfull
42
trajectory optimization failed
43
trajectory optimization succesfull
trajectory optimization succesfull
44
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
45
trajectory optimization succesfull
trajectory optimization succesfull
46
trajectory optimization succesfull
trajectory optimization succesfull
47
trajectory optimization failed
48
trajectory optimization succesfull
trajectory optimization succesfull
49
trajectory optimization succesfull
trajectory optimization succesfull
50
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
51
trajectory optimization succesfull
trajectory optimization succesfull
52
trajectory optimization succesfull
trajectory optimization succesfull
53
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
54
trajectory optimization failed
55
trajectory optimization succesfull
trajectory optimization succesfull
56
trajectory optimization succesfull
trajectory optimization succesfull
57
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
58
trajectory optimization succesfull
trajectory optimization succesfull
59
trajectory optimization failed
60
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
61
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
62
trajectory optimization failed
63
trajectory optimization failed
64
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
65
trajectory optimization failed
66
trajectory optimization succesfull
trajectory optimization succesfull
67
trajectory optimization succesfull
trajectory optimization succesfull
68
trajectory optimization failed
69
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
70
trajectory optimization failed
71
trajectory optimization failed
72
trajectory optimization failed
73
trajectory optimization failed
74
trajectory optimization succesfull
trajectory optimization succesfull
75
trajectory optimization failed
76
trajectory optimization failed
77
trajectory optimization failed
78
trajectory optimization succesfull
trajectory optimization succesfull
79
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
80
trajectory optimization succesfull
trajectory optimization succesfull
81
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
82
trajectory optimization succesfull
trajectory optimization succesfull
83
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
84
trajectory optimization failed
85
trajectory optimization failed
86
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
87
trajectory optimization succesfull
trajectory optimization succesfull
88
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
89
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
90
trajectory optimization succesfull
trajectory optimization succesfull
91
trajectory optimization succesfull
trajectory optimization succesfull
92
trajectory optimization succesfull
trajectory optimization succesfull
93
trajectory optimization succesfull
trajectory optimization succesfull
94
trajectory optimization succesfull
trajectory optimization succesfull
95
trajectory optimization failed
96
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
97
trajectory optimization failed
98
trajectory optimization succesfull
trajectory optimization succesfull
99
trajectory optimization succesfull
trajectory optimization succesfull
100
trajectory optimization failed
101
trajectory optimization succesfull
trajectory optimization succesfull
102
trajectory optimization succesfull
trajectory optimization succesfull
103
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
104
trajectory optimization succesfull
trajectory optimization succesfull
105
trajectory optimization succesfull
trajectory optimization succesfull
106
trajectory optimization succesfull
trajectory optimization succesfull
107
trajectory optimization succesfull
trajectory optimization succesfull
108
trajectory optimization succesfull
trajectory optimization succesfull
109
trajectory optimization succesfull
trajectory optimization succesfull
110
trajectory optimization failed
111
trajectory optimization succesfull
trajectory optimization succesfull
112
trajectory optimization succesfull
trajectory optimization succesfull
113
trajectory optimization succesfull
trajectory optimization succesfull
114
trajectory optimization succesfull
trajectory optimization succesfull
115
trajectory optimization succesfull
trajectory optimization succesfull
116
trajectory optimization failed
117
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
118
trajectory optimization succesfull
trajectory optimization succesfull
119
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
120
trajectory optimization succesfull
trajectory optimization succesfull
121
trajectory optimization succesfull
trajectory optimization succesfull
122
trajectory optimization succesfull
trajectory optimization succesfull
123
trajectory optimization failed
124
trajectory optimization succesfull
trajectory optimization succesfull
125
trajectory optimization failed
126
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
127
trajectory optimization succesfull
trajectory optimization succesfull
128
trajectory optimization succesfull
trajectory optimization succesfull
129
trajectory optimization succesfull
trajectory optimization succesfull
130
trajectory optimization failed
131
trajectory optimization succesfull
trajectory optimization succesfull
132
trajectory optimization succesfull
trajectory optimization succesfull
133
trajectory optimization failed
134
trajectory optimization failed
135
trajectory optimization succesfull
trajectory optimization succesfull
136
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
137
trajectory optimization succesfull
trajectory optimization succesfull
138
trajectory optimization failed
139
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
140
trajectory optimization succesfull
trajectory optimization succesfull
141
trajectory optimization failed
142
trajectory optimization failed
143
trajectory optimization succesfull
trajectory optimization succesfull
144
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
145
trajectory optimization failed
146
trajectory optimization succesfull
trajectory optimization succesfull
147
trajectory optimization succesfull
trajectory optimization succesfull
148
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
149
trajectory optimization succesfull
trajectory optimization succesfull
150
trajectory optimization succesfull
trajectory optimization succesfull
151
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
152
trajectory optimization failed
153
trajectory optimization succesfull
trajectory optimization succesfull
154
trajectory optimization succesfull
trajectory optimization succesfull
155
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
156
trajectory optimization failed
157
trajectory optimization succesfull
trajectory optimization succesfull
158
trajectory optimization succesfull
trajectory optimization succesfull
159
trajectory optimization failed
160
trajectory optimization succesfull
trajectory optimization succesfull
161
trajectory optimization succesfull
trajectory optimization succesfull
162
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
contact mode change detected
163
trajectory optimization succesfull
trajectory optimization succesfull
164
trajectory optimization failed
165
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
166
trajectory optimization succesfull
trajectory optimization succesfull
167
trajectory optimization succesfull
trajectory optimization succesfull
168
trajectory optimization failed
169
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
170
trajectory optimization failed
171
trajectory optimization succesfull
trajectory optimization succesfull
172
trajectory optimization succesfull
trajectory optimization succesfull
173
trajectory optimization failed
174
trajectory optimization succesfull
trajectory optimization succesfull
175
trajectory optimization succesfull
trajectory optimization succesfull
176
trajectory optimization failed
177
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
178
trajectory optimization succesfull
trajectory optimization succesfull
179
trajectory optimization succesfull
trajectory optimization succesfull
180
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
181
trajectory optimization succesfull
trajectory optimization succesfull
182
trajectory optimization succesfull
trajectory optimization succesfull
183
trajectory optimization failed
184
trajectory optimization succesfull
trajectory optimization succesfull
185
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
186
trajectory optimization failed
187
trajectory optimization succesfull
trajectory optimization succesfull
188
trajectory optimization failed
189
trajectory optimization succesfull
trajectory optimization succesfull
190
trajectory optimization succesfull
trajectory optimization succesfull
191
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
192
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
193
trajectory optimization succesfull
contact mode change detected
trajectory optimization succesfull
194
trajectory optimization succesfull
trajectory optimization succesfull
195
trajectory optimization succesfull
trajectory optimization succesfull
196
trajectory optimization failed
197
trajectory optimization succesfull
trajectory optimization succesfull
198
trajectory optimization succesfull
contact mode change detected
trajectory optimization failed
199
trajectory optimization failed
feasible: 142 covered: 142
covered by N=10: 132
infeasible: 58 false positive because of H-rep over-approximation: 25
[10]:
print("feasible: %d covered: %d"%(feasible,covered))
print("covered by N=10: %d"%(feasible - feasible_but_not_covered_by_N_10))
print("infeasible: %d false positive because of H-rep over-approximation: %d"%(Trials-feasible,false_positive))
feasible: 142 covered: 142
covered by N=10: 132
infeasible: 58 false positive because of H-rep over-approximation: 25
Example 10 A: Cart-Pole with Wall¶
[1]:
from __future__ import print_function
import sys
import threading
from time import sleep
try:
import thread
except ImportError:
import _thread as thread
import numpy as np
import scipy.linalg as spa
import pypolycontain as pp
import pydrake.solvers.mathematicalprogram as MP
import pydrake.solvers.gurobi as Gurobi_drake
# use Gurobi solver
global gurobi_solver, license
gurobi_solver=Gurobi_drake.GurobiSolver()
license = gurobi_solver.AcquireLicense()
import pypolycontain as pp
import pypolycontain.pwa_control as pwa
import matplotlib.pyplot as plt
[2]:
dt=0.05
g=10
m=1
M=10
l=1
a=0.1*l
K=200
u_max=100
# state= x, v, theta, w
A=np.array([[1,dt,0,0],[0,1,-m/M*g*dt,0],[0,0,1,dt],[0,0,g/l*(1-m/M)*dt,1]])
B=np.array([0,dt/M,0,-1/M/l*dt]).reshape(4,1)
c=np.array([0,0,0,0]).reshape(4,1)
C=pp.unitbox(N=5).H_polytope
# C.H[2,0]=1
# C.h=np.array([0.15,1,0.1, 1, u_max, 0.15,1,0.1, 1, u_max ]).reshape(10,1)
h_contact=np.zeros((1,5))
h_contact[0,0],h_contact[0,2]=1,1
C.H=np.vstack(( C.H , h_contact ))
C.h=np.array([0.15,1,0.1, 1, u_max, 0.15,1,0.1, 1, u_max , 0.1 ]).reshape(11,1)
S1=pwa.affine_system(A,B,c,name='free',XU=C)
A=np.array([[1,dt,0,0],[0,1,-m/M*g*dt,0],[0,0,1,dt],[-K*dt,0,g/l*(1-m/M)*dt-K*dt*l,1]])
B=np.array([0,dt/M,0,-1/M/l*dt]).reshape(4,1)
c=np.array([0,0,0,K*a*dt]).reshape(4,1)
C=pp.unitbox(N=5).H_polytope
C.H[2,0]=0
C.H[7,0]=-1
C.h=np.array([0.15,1,0.1, 1, u_max, 0.15,1,-0.1, 1, u_max ]).reshape(10,1)
S2=pwa.affine_system(A,B,c,name='contact',XU=C)
myS=pwa.pwa_system()
myS.add_mode(S1)
myS.add_mode(S2)
A Trajectory¶
[19]:
start=np.array([0.0, 0.3, 0, 0]).reshape(4,1)
goal=np.zeros((4,1))
T=40
# x,u,mu=point_trajectory(myS,start,T,goal,Q=np.eye(2)*100)
# try:
x,u,mu=pwa.point_trajectory(myS,start,T,goal,Q=np.eye(4)*100,R=np.eye(1))
mu[T,'free'],mu[T,'contact']=1,1
plt.plot( [x[t][0] for t in range(T+1)] , [x[t][2] for t in range(T+1)] )
plt.plot( [x[t][0] for t in range(T+1) if mu[t,'free']==0] , \
[x[t][2] for t in range(T) if mu[t,'free']==0],'o',\
color='red')
plt.plot( [x[t][0] for t in range(T+1) if mu[t,'free']==1] , \
[x[t][2] for t in range(T+1) if mu[t,'free']==1],'o',\
color='blue')
plt.plot([0.2,-0.05],[-0.1,0.15] ,LineWidth=2,linestyle=':',color='black')
plt.xlabel(r'$x$',FontSize=30)
plt.ylabel(r'$\theta$',FontSize=30)
plt.title(r'A Trajectory With Contact',FontSize=30)
xS=x
trajectory optimization succesfull
contact mode change detected
[4]:
# T=35
# x0=np.array([0.0, 0.3, 0, 0]).reshape(4,1)
# F,_,_,_,_=pwa.extend(myS,x0,T,[pp.zonotope(G=np.eye(4)*0.001)],H_rep=False,color='red')
# fig,ax=plt.subplots()
# pp.visualize(F,fig=fig,ax=ax,a=0.01,alpha=0.9)
# ax.set_xlabel(r'$x$',FontSize=30)
# ax.set_ylabel(r'${\theta}$',FontSize=30)
# ax.set_title('A Polytopic Trajectory',FontSize=30)
# ax.plot([0.2,-0.2],[-0.1,0.3],LineWidth=1,linestyle=':',color='black')
Building The Tree¶
[5]:
def sampler(i):
L=np.array([0.1,1,0.1,1])
if np.random.random()<0.3:
return xS[np.random.randint(0,T)].reshape(4,1)
else:
return np.random.uniform(-L,L).reshape(4,1)
[6]:
def quit_function(fn_name):
# print to stderr, unbuffered in Python 2.
print('{0} took too long'.format(fn_name), file=sys.stderr)
sys.stderr.flush() # Python 3 stderr is likely buffered.
thread.interrupt_main() # raises KeyboardInterrupt
def exit_after(s):
'''
use as decorator to exit process if
function takes longer than s seconds
'''
def outer(fn):
def inner(*args, **kwargs):
timer = threading.Timer(s, quit_function, args=[fn.__name__])
timer.start()
try:
result = fn(*args, **kwargs)
finally:
timer.cancel()
return result
return inner
return outer
# @exit_after(15)
# def extend(start,T,list_of_nodes):
# color=(np.random.random(),np.random.random(),np.random.random())
# x,u,mu,G,theta=pwa.polytopic_trajectory(myS, start, T, list_of_nodes)
# Z={t: pp.zonotope(x=x[t],G=G[t],color=color) for t in range(T+1)}
# funnel=[None]*T
# H_funnel=[None]*T
# contact_free=True
# for t in range(T):
# if mu[t,"contact"]==1:
# contact_free=False
# break
# if contact_free:
# print("it was contact free!")
# funnel[0]=Z[0]
# for t in range(T):
# funnel[0]=pp.convex_hull(funnel[0], Z[t+1])
# funnel[0].color=color
# H_funnel=[pp.ray_shooting_hyperplanes(funnel[0],N=500,tol=1e-5)]
# funnel=[funnel[0]]
# else:
# print("Oh it had contact")
# for t in range(T):
# funnel[t]=pp.convex_hull(Z[t], Z[t+1])
# funnel[t].color=color
# H_funnel[t]=pp.ray_shooting_hyperplanes(funnel[t],N=500,tol=1e-5)
# return funnel,H_funnel,x,mu,G
def extend(start,T,list_of_nodes):
color=(np.random.random(),np.random.random(),np.random.random())
x,u,mu,G,theta=pwa.polytopic_trajectory(myS, start, T, list_of_nodes)
Z={t: pp.zonotope(x=x[t],G=G[t],color=color) for t in range(T+1)}
funnel=[None]*T
H_funnel=[None]*T
for t in range(T):
funnel[t]=pp.convex_hull(Z[t], Z[t+1])
funnel[t].color=color
H_funnel[t]=pp.ray_shooting_hyperplanes(funnel[t],N=500,tol=1e-5)
return funnel,H_funnel,x,mu,G
[7]:
T_synthesis=6
import pickle
(H,h)=pickle.load(open('cart_pole_inner_H.pkl','rb'))
Omega=pp.H_polytope(H, h)
list_of_H_polytopes=[Omega]
list_of_nodes=[Omega]
list_of_all_nodes=[Omega]
stop_sampling=False
sample=lambda i:sampler(i)
branch=0
trajectory={}
i=0
while branch<70:
i+=1
print("i:",i, branch)
while not stop_sampling:
x0=sample(i)
flag=pwa.in_the_tree(x0,list_of_H_polytopes)
# flag=False
stop_sampling=not flag
try:
print("sample:",x0.T)
x,u,mu=pwa.point_trajectory(myS,x0,T=30,goal=Omega,Q=np.eye(4)*0,R=np.eye(1)*0)
Y,YY,xx,mumu,G=extend(x0,T_synthesis,list_of_nodes)
trajectory[branch]=(x,u,mu,xx,mumu,G)
# Y,YY=extend(x0,T,[Omega])
list_of_nodes.extend(Y[0:min(2,len(Y))])
list_of_all_nodes.extend(Y)
list_of_H_polytopes.extend(YY)
branch+=1
except:
print('failed to extend')
stop_sampling=False
i: 1 0
sample: [[ 0.07092196 -0.59653732 0.02403366 -0.92815275]]
trajectory optimization failed
failed to extend
i: 2 0
sample: [[-0.04455041 -0.80285259 -0.06563064 -0.65221602]]
trajectory optimization failed
failed to extend
i: 3 0
sample: [[-0.00175373 0.68670624 0.08270563 0.52030233]]
trajectory optimization failed
failed to extend
i: 4 0
sample: [[-0.07461141 0.2171928 0.04820837 0.62789857]]
trajectory optimization failed
failed to extend
i: 5 0
sample: [[-0.1184629 -0.06429157 0.09760964 -0.07905556]]
trajectory optimization succesfull
disjunctive subset with 1 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 6 1
sample: [[ 0.04189141 -0.35536249 -0.02701298 0.06767547]]
trajectory optimization failed
failed to extend
i: 7 1
sample: [[-0.06171182 -0.13708749 0.01975396 0.48510867]]
trajectory optimization failed
failed to extend
i: 8 1
sample: [[-0.0220033 -0.66111188 0.08397752 0.35339769]]
trajectory optimization failed
failed to extend
i: 9 1
sample: [[-0.09024137 -0.38744188 0.07415778 0.2720816 ]]
trajectory optimization succesfull
disjunctive subset with 3 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 10 2
sample: [[ 0.09827056 -0.20937733 0.06523765 0.78709992]]
trajectory optimization failed
failed to extend
i: 11 2
sample: [[-0.01826844 -0.4781344 -0.00378785 -0.50838721]]
trajectory optimization failed
failed to extend
i: 12 2
sample: [[ 0.01955323 0.88379209 0.03671841 -0.98834561]]
trajectory optimization succesfull
disjunctive subset with 5 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 13 3
sample: [[ 0.04765058 0.12686335 -0.08391053 0.07875539]]
trajectory optimization succesfull
disjunctive subset with 7 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 14 4
sample: [[-0.02937331 -0.68258894 -0.04970365 0.67194544]]
trajectory optimization succesfull
disjunctive subset with 9 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 15 5
sample: [[-0.08737905 -0.26316139 0.1 0. ]]
trajectory optimization succesfull
disjunctive subset with 11 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 16 6
sample: [[ 0.04703089 -0.74690143 -0.00131516 -0.89826832]]
trajectory optimization failed
failed to extend
i: 17 6
sample: [[ 0.02412139 -0.66372844 0.08396912 -0.93354448]]
trajectory optimization failed
failed to extend
i: 18 6
sample: [[ 0.01665016 -0.41173282 -0.0826961 0.25431202]]
trajectory optimization failed
failed to extend
i: 19 6
sample: [[-0.00472876 -0.89284884 -0.02561115 -0.13203002]]
trajectory optimization failed
failed to extend
i: 20 6
sample: [[ 0.09080488 -0.37471518 0.08407274 0.91830149]]
trajectory optimization failed
failed to extend
i: 21 6
sample: [[0.02135367 0.64276339 0.02323482 0.98071284]]
trajectory optimization failed
failed to extend
i: 22 6
sample: [[ 0.14144672 0.17106556 -0.06806507 0.08006522]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 13 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization failed
failed to extend
i: 23 6
sample: [[0.07432586 0.05345131 0.05567503 0.68125864]]
trajectory optimization failed
failed to extend
i: 24 6
sample: [[-0.06178934 -0.73497921 0.04149903 -0.17570209]]
trajectory optimization failed
failed to extend
i: 25 6
sample: [[ 0.08071432 -0.62901264 0.04010514 0.56910218]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 13 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 26 7
sample: [[-0.02379727 -0.96103018 -0.04356616 0.30716883]]
trajectory optimization failed
failed to extend
i: 27 7
sample: [[ 0.08968694 0.51767509 -0.02936862 0.99472241]]
trajectory optimization failed
failed to extend
i: 28 7
sample: [[ 0.14144672 0.17106556 -0.06806507 0.08006522]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 15 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 29 8
sample: [[ 0.03570476 -0.76015703 -0.04116574 0.9204247 ]]
trajectory optimization succesfull
disjunctive subset with 17 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 30 9
sample: [[ 0.0984568 -0.44568502 -0.01864433 -0.95153705]]
trajectory optimization failed
failed to extend
i: 31 9
sample: [[ 0.04394874 0.47664911 0.02151407 -0.65989881]]
trajectory optimization succesfull
disjunctive subset with 19 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 32 10
sample: [[ 0.00907565 -0.81834037 -0.07865214 -0.06234836]]
trajectory optimization failed
failed to extend
i: 33 10
sample: [[-0.01328473 -0.62805778 0.00933942 -0.38327653]]
trajectory optimization failed
failed to extend
i: 34 10
sample: [[ 0.00552846 0.01540688 0.03783645 -0.96973503]]
trajectory optimization failed
failed to extend
i: 35 10
sample: [[ 0.05135454 0.48208602 -0.05653436 0.41965071]]
trajectory optimization failed
failed to extend
i: 36 10
sample: [[ 0.02296823 -0.10305118 0.07317533 0.25273711]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 21 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization failed
failed to extend
i: 37 10
sample: [[ 0.07265863 0.91360392 0.01950141 -0.52291806]]
trajectory optimization failed
failed to extend
i: 38 10
sample: [[ 0.0055932 -0.36720513 0.02959299 -0.07428433]]
trajectory optimization failed
failed to extend
i: 39 10
sample: [[-0.0867264 -0.80867311 0.02490936 0.33273441]]
trajectory optimization failed
failed to extend
i: 40 10
sample: [[-0.12068487 0.10177087 0.0874491 -0.16861139]]
trajectory optimization succesfull
disjunctive subset with 21 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 41 11
sample: [[ 0.04644028 0.02443 0.03249664 -0.22259682]]
trajectory optimization succesfull
disjunctive subset with 23 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 42 12
sample: [[ 0.01676684 -0.07177561 -0.01154101 -0.44851723]]
trajectory optimization failed
failed to extend
i: 43 12
sample: [[-0.09178619 -0.2737355 -0.0963378 0.22374277]]
trajectory optimization failed
failed to extend
i: 44 12
sample: [[-0.07748028 0.20213818 -0.07826005 0.19950615]]
trajectory optimization succesfull
disjunctive subset with 25 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 45 13
sample: [[-0.01603236 -0.74683202 -0.02905065 0.79481412]]
trajectory optimization succesfull
disjunctive subset with 27 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 46 14
sample: [[ 0.07413221 0.28012647 -0.09617668 0.28795567]]
trajectory optimization failed
failed to extend
i: 47 14
sample: [[-0.04200412 0.75434735 -0.05455828 0.67658044]]
trajectory optimization failed
failed to extend
i: 48 14
sample: [[ 0.07667603 -0.0697507 0.05319639 0.91373322]]
trajectory optimization failed
failed to extend
i: 49 14
sample: [[ 0.05140881 0.47612444 0.06971521 -0.94180987]]
trajectory optimization failed
failed to extend
i: 50 14
sample: [[ 0.05776641 0.51787387 -0.01129198 -0.50408582]]
trajectory optimization succesfull
disjunctive subset with 29 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 51 15
sample: [[ 0.02462286 0.53806527 -0.09997749 -0.61099702]]
trajectory optimization failed
failed to extend
i: 52 15
sample: [[-0.00240471 -0.07778048 0.03985774 0.54123166]]
trajectory optimization failed
failed to extend
i: 53 15
sample: [[-0.02526334 0.25164471 -0.07894867 0.72283165]]
trajectory optimization failed
failed to extend
i: 54 15
sample: [[-0.08272859 -0.45091149 -0.05399309 -0.62575288]]
trajectory optimization failed
failed to extend
i: 55 15
sample: [[-0.00976635 0.61748038 0.04008105 -0.18949546]]
trajectory optimization failed
failed to extend
i: 56 15
sample: [[-0.019867 0.4306925 -0.09264762 0.75068128]]
trajectory optimization failed
failed to extend
i: 57 15
sample: [[-0.09467662 0.81649419 0.05978452 -0.09178825]]
trajectory optimization failed
failed to extend
i: 58 15
sample: [[-0.01977872 0.49470721 -0.0869959 0.161386 ]]
trajectory optimization failed
failed to extend
i: 59 15
sample: [[ 0.08528862 0.63992844 -0.03481541 -0.92577396]]
trajectory optimization failed
failed to extend
i: 60 15
sample: [[ 0.08823177 -0.64573604 -0.0927031 0.88321406]]
trajectory optimization succesfull
disjunctive subset with 31 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 61 16
sample: [[-0.03429745 -0.41304899 0.0983551 0.03059169]]
trajectory optimization succesfull
disjunctive subset with 33 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 62 17
sample: [[ 0.10990339 -0.58378142 0.00605398 0.68102308]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 35 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 63 18
sample: [[-0.05045927 0.82940525 -0.06509853 -0.72973341]]
trajectory optimization failed
failed to extend
i: 64 18
sample: [[-0.01718563 0.26871222 -0.00247143 -0.98716228]]
trajectory optimization failed
failed to extend
i: 65 18
sample: [[-0.0258166 -0.48068182 -0.07961621 0.84349026]]
trajectory optimization succesfull
disjunctive subset with 37 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 66 19
sample: [[ 0.00235748 0.17124773 -0.03348553 0.59854834]]
trajectory optimization failed
failed to extend
i: 67 19
sample: [[0. 0.3 0. 0. ]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 39 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 68 20
sample: [[ 0.0712171 0.19946893 0.01932429 -0.74658058]]
trajectory optimization failed
failed to extend
i: 69 20
sample: [[-0.06284721 0.72065942 0.08270362 0.59889398]]
trajectory optimization failed
failed to extend
i: 70 20
sample: [[-0.02149812 0.71239875 -0.05771485 -0.545748 ]]
trajectory optimization succesfull
disjunctive subset with 41 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 71 21
sample: [[ 0.02780012 -0.52888267 0.07235968 -0.02515692]]
trajectory optimization failed
failed to extend
i: 72 21
sample: [[ 0.13325462 -0.46702464 -0.02620719 0.64522348]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 43 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 73 22
sample: [[0.05404642 0.26898726 0.01984556 0.89514839]]
trajectory optimization failed
failed to extend
i: 74 22
sample: [[ 0.15 -0.05327204 -0.06406181 0.27717679]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 45 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 75 23
sample: [[-0.04961527 -0.94879868 -0.09692865 0.03098327]]
trajectory optimization failed
failed to extend
i: 76 23
sample: [[ 0.04623737 0.44326466 -0.0701796 -0.99712374]]
trajectory optimization failed
failed to extend
i: 77 23
sample: [[ 0.0514152 -0.35499562 0.01252189 -0.47739803]]
trajectory optimization failed
failed to extend
i: 78 23
sample: [[ 0.05857595 -0.26287843 0.00178987 -0.43506623]]
trajectory optimization failed
failed to extend
i: 79 23
sample: [[-0.11123082 -0.14464153 0.09953564 -0.03851986]]
trajectory optimization succesfull
disjunctive subset with 47 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 80 24
sample: [[ 0.05150856 0.13198531 -0.04715868 -0.82530186]]
trajectory optimization failed
failed to extend
i: 81 24
sample: [[-0.02869207 0.41022115 -0.09108175 -0.86534745]]
trajectory optimization failed
failed to extend
i: 82 24
sample: [[0.02588341 0.87673428 0.08014515 0.44977483]]
trajectory optimization failed
failed to extend
i: 83 24
sample: [[ 0.06008896 -0.97750652 0.00762267 0.11371082]]
trajectory optimization failed
failed to extend
i: 84 24
sample: [[0.06213295 0.60508619 0.03323916 0.94585444]]
trajectory optimization failed
failed to extend
i: 85 24
sample: [[ 0.07901416 0.3190023 -0.05607978 -0.68197711]]
trajectory optimization failed
failed to extend
i: 86 24
sample: [[-0.09989248 0.32786269 0.01101727 -0.61585297]]
trajectory optimization failed
failed to extend
i: 87 24
sample: [[ 0.02544899 -0.13634219 0.08552776 -0.52952547]]
trajectory optimization failed
failed to extend
i: 88 24
sample: [[ 0.0464551 0.73765028 -0.0909533 0.77659053]]
trajectory optimization failed
failed to extend
i: 89 24
sample: [[-0.08371935 0.50564243 0.06379192 0.08234664]]
trajectory optimization failed
failed to extend
i: 90 24
sample: [[-0.04798332 0.67691745 0.08109942 0.05254026]]
trajectory optimization failed
failed to extend
i: 91 24
sample: [[-0.06397589 -0.06471143 0.01959213 -0.98140171]]
trajectory optimization failed
failed to extend
i: 92 24
sample: [[-0.06145156 0.08406143 -0.03054052 0.43140906]]
trajectory optimization failed
failed to extend
i: 93 24
sample: [[-0.0289153 0.85098853 -0.0641302 -0.96238959]]
trajectory optimization failed
failed to extend
i: 94 24
sample: [[-0.05379955 0.65019765 -0.01470024 0.07183687]]
trajectory optimization failed
failed to extend
i: 95 24
sample: [[-0.02369907 0.20606668 0.03032847 0.22085901]]
trajectory optimization failed
failed to extend
i: 96 24
sample: [[ 0.03639075 -0.23563316 -0.06840165 -0.77064918]]
trajectory optimization failed
failed to extend
i: 97 24
sample: [[ 0.06476284 0.80368437 -0.09906864 0.83413155]]
trajectory optimization failed
failed to extend
i: 98 24
sample: [[ 0.01632822 -0.56620098 -0.00637445 -0.28971288]]
trajectory optimization failed
failed to extend
i: 99 24
sample: [[0.04649513 0.23160443 0.088883 0.01598486]]
trajectory optimization failed
failed to extend
i: 100 24
sample: [[-0.07482097 -0.28379378 0.02330357 0.02211277]]
trajectory optimization failed
failed to extend
i: 101 24
sample: [[ 0.05675928 0.46930394 0.03375755 -0.4329494 ]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 49 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization failed
failed to extend
i: 102 24
sample: [[ 0.04352629 0.56098391 -0.02595056 0.97248543]]
trajectory optimization failed
failed to extend
i: 103 24
sample: [[ 0.06818137 0.43701376 -0.07525526 -0.60500547]]
trajectory optimization failed
failed to extend
i: 104 24
sample: [[-0.06263305 -0.3484801 0.05392155 0.43424809]]
trajectory optimization succesfull
disjunctive subset with 49 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 105 25
sample: [[-0.06563477 0.77100095 -0.05781272 -0.48728506]]
trajectory optimization succesfull
disjunctive subset with 51 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 106 26
sample: [[ 0.02233645 -0.68344571 -0.05137252 0.24766719]]
trajectory optimization failed
failed to extend
i: 107 26
sample: [[-0.04061161 -0.44533818 -0.05974366 -0.28025268]]
trajectory optimization failed
failed to extend
i: 108 26
sample: [[-0.02756519 -0.75424048 0.08292019 0.56000718]]
trajectory optimization failed
failed to extend
i: 109 26
sample: [[0.04985528 0.90265489 0.06279174 0.6614397 ]]
trajectory optimization failed
failed to extend
i: 110 26
sample: [[-0.00533974 -0.35761135 0.05803652 -0.33749759]]
trajectory optimization failed
failed to extend
i: 111 26
sample: [[ 0.08713555 -0.8682606 0.04207914 0.59679747]]
trajectory optimization failed
failed to extend
i: 112 26
sample: [[-0.00676543 0.28777923 0.09022221 -0.67316798]]
trajectory optimization succesfull
disjunctive subset with 53 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 113 27
sample: [[-0.047885 0.87510894 0.03695949 0.73227677]]
trajectory optimization failed
failed to extend
i: 114 27
sample: [[ 0.04202246 -0.82222279 0.0106626 0.48657339]]
trajectory optimization succesfull
disjunctive subset with 55 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 115 28
sample: [[ 0.02053908 -0.4013855 -0.01349976 -0.35320333]]
trajectory optimization failed
failed to extend
i: 116 28
sample: [[ 0.08191758 0.62469255 -0.09683578 0.46210912]]
trajectory optimization failed
failed to extend
i: 117 28
sample: [[-0.04027374 -0.8178368 0.05996386 0.89610994]]
trajectory optimization failed
failed to extend
i: 118 28
sample: [[-0.059876 0.87813735 0.07605255 -0.70810346]]
trajectory optimization succesfull
disjunctive subset with 57 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization failed
failed to extend
i: 119 28
sample: [[0.02143395 0.84517888 0.09543636 0.83164838]]
trajectory optimization failed
failed to extend
i: 120 28
sample: [[-0.01789527 0.26970837 0.03140287 0.72761535]]
trajectory optimization failed
failed to extend
i: 121 28
sample: [[ 0.03261582 0.55175701 -0.07254256 0.73854917]]
trajectory optimization failed
failed to extend
i: 122 28
sample: [[-0.00844664 -0.97798306 -0.02099427 0.86661906]]
trajectory optimization succesfull
disjunctive subset with 57 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 123 29
sample: [[ 0.06191852 0.74817023 0.08058931 -0.91651823]]
trajectory optimization failed
failed to extend
i: 124 29
sample: [[-0.07767024 -0.37602147 -0.07962935 0.07927237]]
trajectory optimization failed
failed to extend
i: 125 29
sample: [[ 0.02570874 -0.40349184 -0.0732031 -0.87656675]]
trajectory optimization failed
failed to extend
i: 126 29
sample: [[ 0.01802319 0.15110971 -0.09817656 -0.59381852]]
trajectory optimization failed
failed to extend
i: 127 29
sample: [[0.03156843 0.26903015 0.07819726 0.88673761]]
trajectory optimization failed
failed to extend
i: 128 29
sample: [[ 0.09239758 -0.6938193 -0.05976875 -0.48591894]]
trajectory optimization failed
failed to extend
i: 129 29
sample: [[ 0.04473467 -0.63356901 0.0943834 -0.03595307]]
trajectory optimization failed
failed to extend
i: 130 29
sample: [[ 0.05419786 -0.18608745 -0.00607648 0.6651075 ]]
trajectory optimization failed
failed to extend
i: 131 29
sample: [[-0.02058002 0.35597916 -0.03658191 0.60380268]]
trajectory optimization failed
failed to extend
i: 132 29
sample: [[ 0.01393014 0.24961657 0.05131025 -0.01787821]]
trajectory optimization failed
failed to extend
i: 133 29
sample: [[-0.08889561 -0.69941574 0.02030239 -0.10263736]]
trajectory optimization failed
failed to extend
i: 134 29
sample: [[ 0.06760356 -0.01940272 -0.03292747 0.2904384 ]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 59 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 135 30
sample: [[ 0.05828504 0.40177371 -0.05733871 0.41251978]]
trajectory optimization failed
failed to extend
i: 136 30
sample: [[ 0.054629 -0.29705037 -0.04565059 -0.6913565 ]]
trajectory optimization failed
failed to extend
i: 137 30
sample: [[-0.06081958 -0.42937518 -0.09388029 -0.93278461]]
trajectory optimization failed
failed to extend
i: 138 30
sample: [[-0.06109009 0.95466383 0.07052019 -0.64991149]]
trajectory optimization failed
failed to extend
i: 139 30
sample: [[-0.01010631 -0.32518883 0.02305753 -0.03592446]]
trajectory optimization failed
failed to extend
i: 140 30
sample: [[ 0.07227396 0.31062658 -0.08177143 -0.84952357]]
trajectory optimization failed
failed to extend
i: 141 30
sample: [[-0.01707005 -0.76414806 0.09849948 -0.62440274]]
trajectory optimization failed
failed to extend
i: 142 30
sample: [[-0.04150676 -0.9673504 -0.09512723 -0.91541395]]
trajectory optimization failed
failed to extend
i: 143 30
sample: [[-0.08875072 0.42482053 0.05039131 0.06714219]]
trajectory optimization failed
failed to extend
i: 144 30
sample: [[0.07674845 0.47520597 0.02486182 0.90097778]]
trajectory optimization failed
failed to extend
i: 145 30
sample: [[ 0.03932956 0.19174279 0.04988282 -0.15768541]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 61 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization failed
failed to extend
i: 146 30
sample: [[ 0.04467397 -0.98112781 -0.06343184 0.58948513]]
trajectory optimization failed
failed to extend
i: 147 30
sample: [[-0.09962287 -0.99151512 -0.09434909 -0.19116853]]
trajectory optimization failed
failed to extend
i: 148 30
sample: [[ 0.07159601 -0.59979542 0.00683255 -0.07958812]]
trajectory optimization failed
failed to extend
i: 149 30
sample: [[-0.01443247 -0.70501814 -0.07173743 -0.27622378]]
trajectory optimization failed
failed to extend
i: 150 30
sample: [[-0.00351669 0.62689003 0.02823304 -0.94659113]]
trajectory optimization failed
failed to extend
i: 151 30
sample: [[-0.01445202 -0.30217408 0.04399427 -0.98211864]]
trajectory optimization failed
failed to extend
i: 152 30
sample: [[ 0.04507848 0.95949814 0.06850492 -0.69282802]]
trajectory optimization failed
failed to extend
i: 153 30
sample: [[ 0.08688398 -0.70452371 0.01108972 0.16526423]]
trajectory optimization failed
failed to extend
i: 154 30
sample: [[-0.0683059 -0.27774081 -0.01442854 0.9125927 ]]
trajectory optimization failed
failed to extend
i: 155 30
sample: [[-0.01905096 -0.45482523 0.00322325 -0.92189892]]
trajectory optimization failed
failed to extend
i: 156 30
sample: [[-0.0780474 0.67451048 -0.08344917 -0.00507423]]
trajectory optimization failed
failed to extend
i: 157 30
sample: [[-0.01488368 0.98306062 -0.0085464 -0.38935418]]
trajectory optimization failed
failed to extend
i: 158 30
sample: [[-0.06188489 -0.8565825 -0.08192026 -0.21111035]]
trajectory optimization failed
failed to extend
i: 159 30
sample: [[ 0.08222608 -0.13527246 -0.09254104 0.09187442]]
trajectory optimization succesfull
disjunctive subset with 61 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 160 31
sample: [[-0.08244811 0.05866685 -0.06724124 0.67767815]]
trajectory optimization failed
failed to extend
i: 161 31
sample: [[-0.05974905 -0.69792248 -0.04481181 0.9051109 ]]
trajectory optimization succesfull
disjunctive subset with 63 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 162 32
sample: [[ 0.07614615 -0.09753781 0.05946358 -0.58570426]]
trajectory optimization failed
failed to extend
i: 163 32
sample: [[ 0.01438528 -0.28240302 -0.00114205 -0.41499681]]
trajectory optimization failed
failed to extend
i: 164 32
sample: [[ 1.11789873e-04 -2.18904715e-01 4.21581335e-02 5.82832796e-01]]
trajectory optimization failed
failed to extend
i: 165 32
sample: [[-0.09197404 0.53725212 -0.0040968 0.44301479]]
trajectory optimization failed
failed to extend
i: 166 32
sample: [[-0.07222098 -0.30316139 0.1 0. ]]
trajectory optimization succesfull
disjunctive subset with 65 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 167 33
sample: [[-0.05810891 -0.73302472 0.09808496 -0.66245375]]
trajectory optimization failed
failed to extend
i: 168 33
sample: [[-0.07193637 -0.55684848 0.03571807 0.67696513]]
trajectory optimization succesfull
disjunctive subset with 67 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 169 34
sample: [[ 0.04680861 -0.31945335 -0.09825182 -0.92264048]]
trajectory optimization failed
failed to extend
i: 170 34
sample: [[-0.02881649 0.07889355 -0.05926565 0.99635704]]
trajectory optimization failed
failed to extend
i: 171 34
sample: [[ 0.08234165 0.78802419 -0.04645239 0.39078289]]
trajectory optimization failed
failed to extend
i: 172 34
sample: [[0.04988778 0.57058343 0.02346226 0.64738644]]
trajectory optimization failed
failed to extend
i: 173 34
sample: [[-0.03430321 -0.85802557 0.03728418 -0.8967637 ]]
trajectory optimization failed
failed to extend
i: 174 34
sample: [[-0.03631208 0.26479975 -0.00395851 -0.58594441]]
trajectory optimization failed
failed to extend
i: 175 34
sample: [[-0.09938967 -0.45394131 0.02774247 0.58921441]]
trajectory optimization succesfull
disjunctive subset with 69 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 176 35
sample: [[ 0.04713487 0.2426849 0.04390249 -0.71988321]]
trajectory optimization failed
failed to extend
i: 177 35
sample: [[ 0.01705009 0.72942999 0.03969216 -0.04190986]]
trajectory optimization failed
failed to extend
i: 178 35
sample: [[ 0.02794716 -0.65097481 -0.03493531 -0.48168842]]
trajectory optimization failed
failed to extend
i: 179 35
sample: [[-0.0881003 -0.52242751 -0.07578948 -0.00219152]]
trajectory optimization failed
failed to extend
i: 180 35
sample: [[ 0.02331504 0.62436788 -0.0413258 -0.03551037]]
trajectory optimization failed
failed to extend
i: 181 35
sample: [[ 0.04772723 0.53409983 -0.04155053 0.50696822]]
trajectory optimization failed
failed to extend
i: 182 35
sample: [[-0.06654743 -0.80671215 0.04767537 0.62729459]]
trajectory optimization succesfull
disjunctive subset with 71 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 183 36
sample: [[ 0.03139127 0.14651705 0.02949441 -0.16848907]]
trajectory optimization succesfull
disjunctive subset with 73 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 184 37
sample: [[-0.07473091 0.63354635 -0.05944502 -0.20212066]]
trajectory optimization succesfull
disjunctive subset with 75 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 185 38
sample: [[ 0.01153236 -0.35776794 -0.02149723 -0.55745335]]
trajectory optimization failed
failed to extend
i: 186 38
sample: [[-0.01355705 -0.39669413 -0.05789962 -0.18334571]]
trajectory optimization failed
failed to extend
i: 187 38
sample: [[-0.00100542 -0.62079734 0.08360667 0.87547319]]
trajectory optimization failed
failed to extend
i: 188 38
sample: [[-0.02251786 -0.02595951 0.09840114 0.35901665]]
trajectory optimization failed
failed to extend
i: 189 38
sample: [[-0.05065023 0.27842952 0.09025002 -0.81396159]]
trajectory optimization failed
failed to extend
i: 190 38
sample: [[ 0.09024993 -0.52336028 -0.01634938 0.58496304]]
trajectory optimization succesfull
disjunctive subset with 77 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization failed
failed to extend
i: 191 38
sample: [[0.07666686 0.52513962 0.08707021 0.21466044]]
trajectory optimization failed
failed to extend
i: 192 38
sample: [[ 0.08897518 0.08306421 0.04277125 -0.35646052]]
trajectory optimization failed
failed to extend
i: 193 38
sample: [[ 0.07764212 -0.4356994 0.01751897 0.76657393]]
trajectory optimization failed
failed to extend
i: 194 38
sample: [[ 0.08881535 -0.73978489 0.09881488 0.06939521]]
trajectory optimization failed
failed to extend
i: 195 38
sample: [[ 0.00083902 -0.10772013 -0.06359676 -0.71999932]]
trajectory optimization failed
failed to extend
i: 196 38
sample: [[-0.07425103 0.15781823 0.0784626 0.05483793]]
trajectory optimization failed
failed to extend
i: 197 38
sample: [[-0.00914199 -0.35683253 0.00973317 -0.98322503]]
trajectory optimization failed
failed to extend
i: 198 38
sample: [[0.09178368 0.16650362 0.05635 0.72682631]]
trajectory optimization failed
failed to extend
i: 199 38
sample: [[ 0.05828638 0.23072397 -0.02932422 0.84091689]]
trajectory optimization failed
failed to extend
i: 200 38
sample: [[-0.00669021 -0.80374474 -0.07716136 0.29866628]]
trajectory optimization failed
failed to extend
i: 201 38
sample: [[ 0.06410957 0.72142719 -0.08681601 0.56703559]]
trajectory optimization failed
failed to extend
i: 202 38
sample: [[0.08934956 0.79861136 0.00342453 0.518488 ]]
trajectory optimization failed
failed to extend
i: 203 38
sample: [[0.07078724 0.89506676 0.06005243 0.28057887]]
trajectory optimization failed
failed to extend
i: 204 38
sample: [[-0.02844994 0.20709989 0.07499602 0.49551318]]
trajectory optimization failed
failed to extend
i: 205 38
sample: [[-0.01601578 -0.9005484 -0.01356497 0.20759423]]
trajectory optimization failed
failed to extend
i: 206 38
sample: [[ 0.0071674 -0.18744024 0.07904523 0.40583531]]
trajectory optimization failed
failed to extend
i: 207 38
sample: [[-0.0810644 -0.93689977 0.0801051 -0.17515365]]
trajectory optimization failed
failed to extend
i: 208 38
sample: [[-0.07338959 -0.81086014 0.03167163 0.80943831]]
trajectory optimization succesfull
disjunctive subset with 77 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 209 39
sample: [[ 0.09723301 -0.90613117 0.08976721 0.61535905]]
trajectory optimization failed
failed to extend
i: 210 39
sample: [[-0.06814437 -0.77685986 -0.06971948 0.4732762 ]]
trajectory optimization failed
failed to extend
i: 211 39
sample: [[-0.01948987 -0.47175195 0.01022356 -0.32405685]]
trajectory optimization failed
failed to extend
i: 212 39
sample: [[-0.04775118 0.80971425 0.01846158 0.27244217]]
trajectory optimization failed
failed to extend
i: 213 39
sample: [[-0.01755718 -0.10551126 -0.04181833 -0.86646261]]
trajectory optimization failed
failed to extend
i: 214 39
sample: [[ 0.01274661 0.95893098 -0.01291461 -0.53607485]]
trajectory optimization failed
failed to extend
i: 215 39
sample: [[ 0.03580425 0.82921925 -0.06208864 -0.28611014]]
trajectory optimization failed
failed to extend
i: 216 39
sample: [[-0.05625523 -0.27094477 0.06747283 0.65072549]]
trajectory optimization failed
failed to extend
i: 217 39
sample: [[ 0.04248025 0.51812305 0.0496979 -0.5868769 ]]
trajectory optimization succesfull
disjunctive subset with 79 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 218 40
sample: [[ 5.14107511e-03 5.42879716e-04 -8.87376241e-02 -8.98695880e-01]]
trajectory optimization failed
failed to extend
i: 219 40
sample: [[-0.01789988 -0.22538574 -0.07850801 0.12239444]]
trajectory optimization failed
failed to extend
i: 220 40
sample: [[ 0.06476063 -0.47030973 0.07598017 0.03953691]]
trajectory optimization failed
failed to extend
i: 221 40
sample: [[-0.06096338 -0.2296721 0.02264281 -0.93112307]]
trajectory optimization failed
failed to extend
i: 222 40
sample: [[-0.09478599 0.28268763 0.00662681 0.11752689]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 81 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 223 41
sample: [[-0.05771913 -0.1284344 -0.03415103 -0.11490144]]
trajectory optimization failed
failed to extend
i: 224 41
sample: [[ 0.09087502 -0.78246565 0.00700003 0.4870216 ]]
trajectory optimization succesfull
disjunctive subset with 83 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 225 42
sample: [[ 0.06025675 -0.35856428 0.084411 -0.46973473]]
trajectory optimization failed
failed to extend
i: 226 42
sample: [[-0.07246301 -0.15164158 0.03217092 -0.54696348]]
trajectory optimization failed
failed to extend
i: 227 42
sample: [[ 0.0125011 -0.8619332 -0.08375816 0.53994711]]
trajectory optimization failed
failed to extend
i: 228 42
sample: [[ 0.02610028 0.65665698 -0.00471077 0.76247251]]
trajectory optimization failed
failed to extend
i: 229 42
sample: [[ 0.06100476 0.53276033 -0.04044191 -0.64151827]]
trajectory optimization succesfull
disjunctive subset with 85 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 230 43
sample: [[-0.08012869 0.38857973 -0.07925352 0.89470798]]
trajectory optimization failed
failed to extend
i: 231 43
sample: [[-0.04878367 -0.88780602 0.05476301 -0.13674683]]
trajectory optimization failed
failed to extend
i: 232 43
sample: [[ 0.0576759 0.77433086 0.01207798 -0.6921143 ]]
trajectory optimization succesfull
disjunctive subset with 87 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 233 44
sample: [[ 0.05235293 -0.24357175 0.09082585 0.84080168]]
trajectory optimization failed
failed to extend
i: 234 44
sample: [[ 0.02641144 0.39804478 -0.01223677 -0.77303058]]
trajectory optimization failed
failed to extend
i: 235 44
sample: [[ 0.01489824 0.58372138 -0.01487109 0.36946452]]
trajectory optimization failed
failed to extend
i: 236 44
sample: [[4.37104117e-04 8.00387786e-01 3.42331943e-02 1.31231010e-01]]
trajectory optimization failed
failed to extend
i: 237 44
sample: [[-0.08640715 0.4741245 -0.03647683 0.29690634]]
trajectory optimization failed
failed to extend
i: 238 44
sample: [[-0.01102836 0.72069269 0.07937983 -0.30314378]]
trajectory optimization failed
failed to extend
i: 239 44
sample: [[ 0.07029387 0.95665223 0.05231191 -0.11193734]]
trajectory optimization failed
failed to extend
i: 240 44
sample: [[ 0.02946814 -0.9548515 0.005607 -0.04119387]]
trajectory optimization failed
failed to extend
i: 241 44
sample: [[ 0.06952992 0.46895767 -0.06319955 0.78131277]]
trajectory optimization failed
failed to extend
i: 242 44
sample: [[-0.05158909 -0.65259829 0.06051129 0.56984075]]
trajectory optimization succesfull
disjunctive subset with 89 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 243 45
sample: [[ 0.04367699 0.29340012 0.08566529 -0.82811586]]
trajectory optimization failed
failed to extend
i: 244 45
sample: [[-0.06353474 0.09934472 0.02983747 0.21993777]]
trajectory optimization failed
failed to extend
i: 245 45
sample: [[-0.05999901 -0.85252344 0.01235864 -0.3081597 ]]
trajectory optimization failed
failed to extend
i: 246 45
sample: [[ 0.02769002 0.94179431 -0.08710938 0.86651223]]
trajectory optimization failed
failed to extend
i: 247 45
sample: [[-0.08658705 0.50460897 -0.02395176 -0.0184314 ]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 91 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 248 46
sample: [[-0.05155025 -0.74755703 -0.08335848 0.59323363]]
trajectory optimization failed
failed to extend
i: 249 46
sample: [[-0.00457176 -0.14159122 0.03875137 -0.49404969]]
trajectory optimization failed
failed to extend
i: 250 46
sample: [[ 0.05821839 0.27472431 0.08636916 -0.28991987]]
trajectory optimization failed
failed to extend
i: 251 46
sample: [[-0.05533925 -0.45602042 0.02545734 -0.10476515]]
trajectory optimization failed
failed to extend
i: 252 46
sample: [[0.07535224 0.65210961 0.03557904 0.31763181]]
trajectory optimization failed
failed to extend
i: 253 46
sample: [[ 0.08162891 0.94546718 -0.02624875 -0.50387659]]
trajectory optimization failed
failed to extend
i: 254 46
sample: [[ 0.07892586 -0.29101676 -0.08088439 -0.46058617]]
trajectory optimization failed
failed to extend
i: 255 46
sample: [[-0.09938559 0.13488981 0.05765209 -0.36256735]]
trajectory optimization succesfull
disjunctive subset with 93 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 256 47
sample: [[-0.0012486 -0.16242165 0.08988674 -0.87697293]]
trajectory optimization failed
failed to extend
i: 257 47
sample: [[-0.00891422 0.39883393 0.01570451 -0.06425988]]
trajectory optimization failed
failed to extend
i: 258 47
sample: [[-0.01076283 -0.59839783 -0.05106887 -0.87164184]]
trajectory optimization failed
failed to extend
i: 259 47
sample: [[ 0.02643959 0.27622464 0.00629592 -0.91074245]]
trajectory optimization failed
failed to extend
i: 260 47
sample: [[-0.02881204 0.13764032 -0.0270158 -0.28304835]]
trajectory optimization failed
failed to extend
i: 261 47
sample: [[ 0.06348198 0.0296512 0.07417868 -0.32120522]]
trajectory optimization failed
failed to extend
i: 262 47
sample: [[-0.02852915 -0.46726423 -0.08432355 -0.94204838]]
trajectory optimization failed
failed to extend
i: 263 47
sample: [[-0.09815181 -0.68698011 0.04598506 -0.3491397 ]]
trajectory optimization failed
failed to extend
i: 264 47
sample: [[ 0.09114556 0.55934945 -0.02897437 -0.96996187]]
trajectory optimization failed
failed to extend
i: 265 47
sample: [[ 0.05925798 0.1379005 -0.084744 -0.49783284]]
trajectory optimization failed
failed to extend
i: 266 47
sample: [[-0.01501636 -0.26593363 -0.03332925 0.65916911]]
trajectory optimization failed
failed to extend
i: 267 47
sample: [[-0.0978106 -0.75663548 -0.07754513 0.770682 ]]
trajectory optimization failed
failed to extend
i: 268 47
sample: [[-0.00192801 -0.16167819 0.04732594 0.88345764]]
trajectory optimization failed
failed to extend
i: 269 47
sample: [[ 0.06357087 -0.14335379 -0.04644865 -0.79600197]]
trajectory optimization failed
failed to extend
i: 270 47
sample: [[-0.01379795 -0.15095776 0.02367055 -0.6262118 ]]
trajectory optimization failed
failed to extend
i: 271 47
sample: [[-0.09070569 0.98075739 0.03133285 -0.01472325]]
trajectory optimization failed
failed to extend
i: 272 47
sample: [[-0.05140408 -0.72485263 -0.0905586 -0.91203306]]
trajectory optimization failed
failed to extend
i: 273 47
sample: [[ 0.08382744 -0.06073274 0.0424736 0.37951801]]
trajectory optimization failed
failed to extend
i: 274 47
sample: [[ 0.08632306 -0.25272858 0.0051405 -0.24867256]]
trajectory optimization failed
failed to extend
i: 275 47
sample: [[ 0.08744283 -0.77098792 0.06634856 0.44354852]]
trajectory optimization failed
failed to extend
i: 276 47
sample: [[ 0.06387425 -0.90348647 0.0538593 0.27649193]]
trajectory optimization failed
failed to extend
i: 277 47
sample: [[ 0.0960871 -0.78378681 -0.04563294 -0.28532668]]
trajectory optimization failed
failed to extend
i: 278 47
sample: [[ 0.02014042 0.26005735 0.0388105 -0.20852577]]
trajectory optimization succesfull
disjunctive subset with 95 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 279 48
sample: [[ 0.02885073 0.83554578 -0.06117163 -0.68230892]]
trajectory optimization failed
failed to extend
i: 280 48
sample: [[ 0.04458257 0.95263618 0.02937878 -0.26480003]]
trajectory optimization failed
failed to extend
i: 281 48
sample: [[ 0.04123493 -0.26590371 -0.0700253 -0.33004682]]
trajectory optimization failed
failed to extend
i: 282 48
sample: [[-0.00454899 -0.83079283 -0.08257962 0.59297182]]
trajectory optimization failed
failed to extend
i: 283 48
sample: [[-0.05088139 -0.43180955 0.08868035 0.01245646]]
trajectory optimization succesfull
disjunctive subset with 97 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
i: 284 49
sample: [[ 0.01611884 0.48026739 0.04907777 -0.3219587 ]]
trajectory optimization succesfull
contact mode change detected
disjunctive subset with 99 circumbodies
********** Set up a mixed-integer optimization problem **********
polytopic trajectory optimization succesfull
Visualization¶
[8]:
fig,ax=plt.subplots()
pp.visualize([Omega]+list_of_all_nodes,a=0.01,ax=ax,fig=fig,tuple_of_projection_dimensions=(0,2),\
title='Hybrid MPC Feasible Set %d Branches'%branch)
ax.plot([1,-1],[-0.9,1.1])
ax.set_xlim([-0.17,0.17])
ax.set_ylim([-0.15,0.15])
# Synthesis
ax.plot([0.2,-0.2],[-0.1,0.3] ,LineWidth=2,linestyle=':',color='black')
ax.set_xlabel(r'$x$',FontSize=30)
ax.set_ylabel(r'$\theta$',FontSize=30)
ax.set_title(r"""%d Branches %d AH-polytopes
projected into $x-\theta$ planes"""%(branch,len(list_of_all_nodes)),FontSize=30)
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
/usr/lib/python3/dist-packages/matplotlib/colors.py:235: VisibleDeprecationWarning: Creating an ndarray from ragged nested sequences (which is a list-or-tuple of lists-or-tuples-or ndarrays with different lengths or shapes) is deprecated. If you meant to do this, you must specify 'dtype=object' when creating the ndarray
c = np.array(c)
[8]:
Text(0.5, 1.0, '50 Branches 301 AH-polytopes \nprojected into $x-\\theta$ planes')
[15]:
slicer=pp.unitbox(4).H_polytope
slicer.h[1,0],slicer.h[3,0],slicer.h[5,0],slicer.h[7,0]=0.01,0.01,0.01,0.01
sliced=[pp.intersection(Q,slicer) for Q in list_of_all_nodes]
i=0
for Q in sliced:
Q.color=list_of_all_nodes[i].color
i+=1
fig,ax=plt.subplots()
# for Q in sliced:
# print( pp.check_non_empty(Q))
pp.visualize([Q for Q in sliced if pp.check_non_empty(Q)],\
a=0.01,ax=ax,fig=fig,tuple_of_projection_dimensions=(0,2),\
title='Hybrid MPC Feasible Set')
ax.plot([1,-1],[-0.9,1.1])
ax.set_xlim([-0.17,0.17])
ax.set_ylim([-0.15,0.15])
ax.plot([0.2,-0.2],[-0.1,0.3] ,LineWidth=2,linestyle=':',color='black')
ax.set_xlabel(r'$x$',FontSize=30)
ax.set_ylabel(r'$\theta$',FontSize=30)
ax.set_title(r"""%d Branches %d AH-polytopes
sliced with $x-\theta$ planes"""%(branch,len(list_of_all_nodes)),FontSize=30)
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
[15]:
Text(0.5, 1.0, '50 Branches 301 AH-polytopes \nsliced with $x-\\theta$ planes')
A Trajectory¶
[21]:
fig,ax=plt.subplots()
for Q in list_of_all_nodes:
Q.color='green'
pp.visualize(list_of_all_nodes,a=0.01,ax=ax,fig=fig,tuple_of_projection_dimensions=(0,2),\
title='Hybrid MPC Feasible Set %d Branches'%branch, alpha=0.3)
ax.plot([1,-1],[-0.9,1.1])
ax.set_xlim([-0.17,0.17])
ax.set_ylim([-0.15,0.15])
# Synthesis
ax.plot([0.2,-0.2],[-0.1,0.3] ,LineWidth=2,linestyle=':',color='black')
ax.set_xlabel(r'$x$',FontSize=30)
ax.set_ylabel(r'$\theta$',FontSize=30)
x=xS
mu[T,'free'],mu[T,'contact']=1,1
ax.plot( [x[t][0] for t in range(T+1)] , [x[t][2] for t in range(T+1)], color='black' )
ax.plot( [x[t][0] for t in range(T+1) if mu[t,'free']==0] , \
[x[t][2] for t in range(T) if mu[t,'free']==0],'o',\
color='red')
ax.plot( [x[t][0] for t in range(T+1) if mu[t,'free']==1] , \
[x[t][2] for t in range(T+1) if mu[t,'free']==1],'o',\
color='blue')
ax.plot([0.2,-0.05],[-0.1,0.15] ,LineWidth=2,linestyle=':',color='black')
ax.set_title(r'A Trajectory With Contact',FontSize=30)
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
projection on 0 and 2 dimensions
[21]:
Text(0.5, 1.0, 'A Trajectory With Contact')
We generate random points and see
[ ]:
Trials=500
covered=0
false_positive=0
feasible=0
L=np.array([0.1,1,0.1,1])
for N in range(Trials):
x0=np.random.uniform(-L,L).reshape(4,1)
print(N)
try:
x,u,mu=pwa.point_trajectory(myS,x0,T=40,goal=np.zeros((4,1)),Q=np.eye(4)*1)
feasible+=1
covered+=pwa.in_the_tree(x0,list_of_H_polytopes)
except:
false_positive+=pwa.in_the_tree(x0,list_of_H_polytopes)
print("feasible: %d covered: %d"%(feasible,covered))
print("infeasible: %d false positive: %d"%(Trials-feasible,false_positive))
[22]:
print("feasible: %d covered: %d"%(feasible,covered))
print("infeasible: %d false positive: %d"%(Trials-feasible,false_positive))
feasible: 107 covered: 54
infeasible: 393 false positive: 6
Objects¶
pypolycontain has the following polytopic objects.
H-polytope¶
-
class
pypolycontain.objects.H_polytope(H, h, symbolic=False, color='red')¶ An H-polytope is a set defined as follows:
\[\mathbb{P}=\{x \in \mathbb{R}^n | H x \le h \},\]where
- Attributes:
- \(H \in \mathbb{R}^{q \times n}\): numpy.ndarray[float[[q,n]]
- \(h\in \mathbb{R}^q\) : numpy.ndarray[float[[q,1]]
define the hyperplanes. The inequality is interpreted element-wise and q is the number of hyperplanes.
V-polytope¶
-
class
pypolycontain.objects.V_polytope(list_of_vertices)¶ V-polytopes are a convex hull of vertices.
\[\mathbb{V}= \{ x \in \mathbb{R}^n | x = \sum_{i=1}^N \lambda_i v_i, \sum_{i=1}^N \lambda_i=1, \lambda_i \ge 0, i=1,\cdots,N \}\]where each \(v_i, i=1,\cdots,N\) is a point (some or all are effectively vertices).
- Attributes:
- list_of_vertices= list of numpy.ndarray[float[n,1]].
AH-polytope¶
-
class
pypolycontain.objects.AH_polytope(t, T, P, color='blue')¶ An AH_polytope is an affine transformation of an H-polytope and is defined as:
\[\mathbb{Q}=\{t+Tx | x \in \mathbb{R}^p, H x \le h \}\]- Attributes:
- P: The underlying H-polytope \(P:\{x \in \mathbb{R}^p | Hx \le h\}\)
- T: \(\mathbb{R}^{n \times p}\) matrix: linear transformation
- t: \(\mathbb{R}^{n}\) vector: translation
Zonotope¶
-
class
pypolycontain.objects.zonotope(G, x=None, name=None, color='green')¶ A Zonotope is a set defined as follows:
\[\mathbb{Z}=\langle x,G \rangle = \{x + G p | p \in [-1,1]^q \},\]where
- Attributes:
- \(G \in \mathbb{R}^{n \times q}\): numpy.ndarray[float[[n,q]] is the zonotope generator.
- \(x\in \mathbb{R}^n\): numpy.ndarray[float[[n,1]] is the zonotope centroid. Default is zero vector.
The order of the zonotope is defined as \(\frac{q}{n}\).
-
pypolycontain.objects.zonotope.volume(self)¶ Computes the volume of the zonotope in \(\mathbb{n}\) dimensions. The formula is based on the paper in [Gover2002]
[Gover2002] Gover, Eugene, and Nishan Krikorian. “Determinants and the volumes of parallelotopes and zonotopes.” Linear Algebra and its Applications 433, no. 1 (2010): 28-40.
Operations¶
-
pypolycontain.operations.AH_polytope_vertices(P, N=200, epsilon=0.001, solver='Gurobi')¶ Returns N*2 matrix of vertices
-
pypolycontain.operations.AddMatrixInequalityConstraint_classical(mathematical_program, A, X, B)¶
-
pypolycontain.operations.Hausdorff_distance(Q1, Q2, directed=False, ball='infinty_norm', solver='gurobi', k=-1)¶
-
pypolycontain.operations.Lambda_H_Gamma(mathematical_program, Lambda, H_1, H_2, Gamma)¶ Lambda H_1 = H_2 Gamma
-
pypolycontain.operations.Lambda_h_Inequality(mathematical_program, Lambda, beta, H, h_1, h_2)¶ Adds Lambda H-1 le h_2 + H beta to the Mathematical Program
-
pypolycontain.operations.Lambda_h_Inequality_D(mathematical_program, Lambda, beta, H, h_1, h_2, D)¶ Adds Lambda H-1 le h_2 D + H beta to the Mathematical Program
-
pypolycontain.operations.affine_map(T, P, t=None, get_inverse=True)¶ Returns the affine map of a polytope.
-
pypolycontain.operations.bounding_box(Q, solver='Gurobi')¶ Computes the bounding box of a polytope by solving \(2n\) linear programs. Each linear program is in the form:
\[\begin{split}\begin{array}{lll} l_i= & \min & e_i^T x \\ & \text{subject to} & x \in \mathbb{P} \end{array}\end{split}\]and
\[\begin{split}\begin{array}{lll} u_i= & \max & e_i^T x \\ & \text{subject to} & x \in \mathbb{P} \end{array} \end{split}\]where \(l,u\) define the lower and upper corners of the bounding box.
-
pypolycontain.operations.boxing_order_reduction(zonotope, desired_order=1)¶ boxing method for zonotope order reduction inputs: input zonotope , order of the output zonotope output: zonotope
Based on Kopetzki, Anna-Kathrin, Bastian Schurmann, and Matthias Althoff. “Methods for order reduction of zonotopes.” 2017 IEEE 56th Annual Conference on Decision and Control (CDC). IEEE, 2017.
-
pypolycontain.operations.check_non_empty(Q, tol=1e-05, solver='gurobi')¶
-
pypolycontain.operations.check_subset(P1, P2, k=-1)¶ Checks if .math..`P1 subseteq P2` Inputs:
-
pypolycontain.operations.convex_hull(P1, P2)¶ - Inputs:
- P1, P2: AH_polytopes
- Output:
- returns :math:` ext{ConvexHull}(mathbb{P}_1,mathbb{P}_2)` as an AH-polytope
-
pypolycontain.operations.convex_hull_of_point_and_polytope(x, Q)¶ - Inputs:
- x: numpy n*1 array Q: AH-polytope in R^n
- Returns:
- AH-polytope representing convexhull(x,Q)
\[\text{conv}(x,Q):=\{y | y= \lambda q + (1-\lambda) x, q \in Q\}.\]
-
pypolycontain.operations.decompose(zonotope, dimensions)¶ @author: kasra Decompising a given set into bunch of fewer dimensional sets such that the Cartesian product of those sets is a subset of the given set.
-
pypolycontain.operations.directed_Hausdorff_hyperbox(b1, b2)¶ The directed Hausdorff hyperbox min epsilon such that b1 in b2+epsilon
-
pypolycontain.operations.distance_hyperbox(b1, b2)¶ The distance between boxes
-
pypolycontain.operations.distance_point_polytope(P, x, ball='infinity', solver='Gurobi')¶ Computes the distance of point x from AH-polytope Q
-
pypolycontain.operations.distance_polytopes(Q1, Q2, ball='infinity', solver='gurobi')¶ Finds the closest two points in two polytopes and their distance. It is zero if polytopes have non-empty intersection
-
pypolycontain.operations.get_nonzero_cost_vectors(cost)¶
-
pypolycontain.operations.intersection(P1, P2)¶ - Inputs:
- P1, P2: polytopic objects
- Output:
returns \(\mathbb{P}_1 \cap \mathbb{P}_2\) as an AH-polytope
If both objects are H-polytopes, return H-polytope
-
pypolycontain.operations.intersection_old(P1, P2)¶ - Inputs:
- P1, P2: AH_polytopes \(\mathbb{P}_1,\mathbb{P}_2\). Converted to AH-polytopes
- Output:
- returns \(\mathbb{P}_1 \cap \mathbb{P}_2\) as an AH-polytope
-
pypolycontain.operations.make_ball(n, norm)¶
-
pypolycontain.operations.minkowski_sum(P1, P2)¶ - Inputs:
- P1, P2: AH_polytopes
- Returns:
- returns the Mkinkowski sum \(P_1 \oplus P_2\) as an AH-polytope.
Background: The Minkowski sum of two sets is defined as:
\[A \oplus B = \{ a + b \big | a \in A, b \in B\}.\]
-
pypolycontain.operations.pca_order_reduction(zonotope, desired_order=1)¶ PCA method for zonotope order reduction inputs: input zonotope , order of the output zonotope output: zonotope
Based on Kopetzki, Anna-Kathrin, Bastian Schürmann, and Matthias Althoff. “Methods for order reduction of zonotopes.” 2017 IEEE 56th Annual Conference on Decision and Control (CDC). IEEE, 2017.
-
pypolycontain.operations.point_membership(Q, x, tol=1e-05, solver='gurobi')¶
-
pypolycontain.operations.point_membership_fuzzy(Q, x, tol=1e-05, solver='gurobi')¶ Fuzzy membership check. If x contains NaN, the entry is unconstrained @param Q: Polytope in R^n @param x: n*1 numpy array, may contain NaNs @param tol: @param solver: solver to use @return: boolean of whether x is in Q
-
pypolycontain.operations.positive_matrix(mathematical_program, Lambda)¶ All elements are non-negative
-
pypolycontain.operations.sorting_generator(G, desired_numberofcolumns)¶ The goal is deviding the generator into to parts. One part that is used for zonotope order reduction methods. And the other part which is used to enforce the reduced zonotope to have the desire order.
-
pypolycontain.operations.translate(t, P)¶ Shifts the polytope by t vector
Conversions¶
-
pypolycontain.conversions.AH_to_H_old(Q, P0, solver='Gurobi')¶ Converting Q to an H-polytope using an optimization-based method
WARNING: To be deprecated
-
pypolycontain.conversions.AH_to_V(P, N=360, epsilon=0.001, solver='Gurobi')¶ Returns the V-polytope form of a 2D pp.AH_polytope. The method is based on ray shooting.
- Inputs:
- P: AH-polytope
- N
defualt=360: number of rays - solver:
default=Gurobi. The linear-programming optimization solver.
- Returns:
- V: matrix
Note
This method only works for 2D AH-polytopes and its for visualization. For generic use, first use H-V on \(\mathbb{P}\) and then apply affine transformation. Note that H-V uses elimination-based vertex enumeration method that is not scalable.
-
pypolycontain.conversions.H_to_V(P)¶ Returns the vertices of an H_polytope.
- Inputs:
- P: H_polytope in \(\mathbb{R}^n\)
- Output:
- V: list of vertices. Each vertex is numpy.ndarray[float[n,1]]
- Method:
- The method is based on double description method, and is using pycddlib.
Warning
This method can be very slow or numerically unstable for polytopes in high dimensions and/or large number of hyperplanes
-
pypolycontain.conversions.to_AH_polytope(P)¶ Converts the polytopic object P into an AH-polytope. If applied on a AH-polytope, a deepcopy is returned
-
pypolycontain.conversions.to_V(P, N=500)¶ returns the vertices of the polytopic object $P$ in a vertical stack form.
-
pypolycontain.conversions.vcube(n)¶ \(2^n \times n\) array of vectors of vertices in unit cube in \(\mathbb{R^n}\)
-
pypolycontain.conversions.zonotope_to_V(Z)¶ Finds the vertices of a zonotope
Visualization¶
Important
We only support 2D visualization.
-
pypolycontain.visualize.visualize(list_of_objects, fig=None, ax=None, a=0.5, alpha=0.8, tuple_of_projection_dimensions=[0, 1], title='pypolycontain visualization', show_vertices=False, TitleSize=15, FontSize=15, equal_axis=False, grid=True, N_points=1000, figsize=(8, 8))¶ Visualization.
- inputs:
- list_of_objects:
- fig:
- tuple_of_projection_dimensions: