class TestGeometryOptimization(unittest.TestCase):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.A = np.eye(3)
self.b = [1.0, 1.0, 1.0]
self.prog = MathematicalProgram()
self.x = self.prog.NewContinuousVariables(3, "x")
self.t = self.prog.NewContinuousVariables(1, "t")
def test_point_convex_set(self):
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2*p)
np.testing.assert_array_equal(point.x(), 2*p)
point.set_x(x=p)
# TODO(SeanCurtis-TRI): This doesn't test the constructor that
# builds from shape.
def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
with self.assertRaisesRegex(
RuntimeError, ".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
h_box = mut.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(
h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(
h_box.ChebyshevCenter(), [0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
def test_hyper_ellipsoid(self):
ellipsoid = mut.Hyperellipsoid(A=self.A, center=self.b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), self.A)
np.testing.assert_array_equal(ellipsoid.center(), self.b)
self.assertTrue(ellipsoid.PointInSet(x=self.b, tol=0.0))
ellipsoid.AddPointInSetConstraints(self.prog, self.x)
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
e_ball = mut.Hyperellipsoid.MakeAxisAligned(
radius=[1, 1, 1], center=self.b)
np.testing.assert_array_equal(e_ball.A(), self.A)
np.testing.assert_array_equal(e_ball.center(), self.b)
e_ball2 = mut.Hyperellipsoid.MakeHypersphere(
radius=1, center=self.b)
np.testing.assert_array_equal(e_ball2.A(), self.A)
np.testing.assert_array_equal(e_ball2.center(), self.b)
e_ball3 = mut.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), self.A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
def test_minkowski_sum(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
sum = mut.MinkowskiSum(setA=point, setB=hpoly)
self.assertEqual(sum.ambient_dimension(), 3)
self.assertEqual(sum.num_terms(), 2)
sum2 = mut.MinkowskiSum(sets=[point, hpoly])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_terms(), 2)
self.assertIsInstance(sum2.term(0), mut.Point)
def test_v_polytope(self):
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(self.prog, self.x[0:2])
v_box = mut.VPolytope.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10)
v_unit_box = mut.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3))
self.assertTrue(v_from_h.PointInSet([0, 0, 0]))
def test_cartesian_product(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
h_box = mut.HPolyhedron.MakeBox(
lb=[-1, -1, -1], ub=[1, 1, 1])
sum = mut.CartesianProduct(setA=point, setB=h_box)
self.assertEqual(sum.ambient_dimension(), 6)
self.assertEqual(sum.num_factors(), 2)
sum2 = mut.CartesianProduct(sets=[point, h_box])
self.assertEqual(sum2.ambient_dimension(), 6)
self.assertEqual(sum2.num_factors(), 2)
self.assertIsInstance(sum2.factor(0), mut.Point)
sum2 = mut.CartesianProduct(
sets=[point, h_box], A=np.eye(6, 3), b=[0, 1, 2, 3, 4, 5])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_factors(), 2)
self.assertIsInstance(sum2.factor(1), mut.HPolyhedron)
def test_make_from_scene_graph_and_iris(self):
"""
Tests the make from scene graph and iris functionality together as
the Iris code makes obstacles from geometries registered in SceneGraph.
"""
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(
source_id=source_id, frame=GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Box(1., 1., 1.),
name="box"))
cylinder_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Cylinder(1., 1.),
name="cylinder"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id, frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Sphere(1.), name="sphere"))
capsule_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Capsule(1., 1.0),
name="capsule"))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
H = mut.HPolyhedron(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
C = mut.CartesianProduct(
query_object=query_object, geometry_id=cylinder_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(C.ambient_dimension(), 3)
E = mut.Hyperellipsoid(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
S = mut.MinkowskiSum(
query_object=query_object, geometry_id=capsule_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(S.ambient_dimension(), 3)
P = mut.Point(
query_object=query_object, geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id(),
maximum_allowable_radius=1.5)
self.assertEqual(P.ambient_dimension(), 3)
V = mut.VPolytope(
query_object=query_object, geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
obstacles = mut.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
options.relative_termination_threshold = 0.01
self.assertNotIn("object at 0x", repr(options))
region = mut.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.HPolyhedron)
obstacles = [
mut.HPolyhedron.MakeUnitBox(3),
mut.Hyperellipsoid.MakeUnitBall(3),
mut.Point([0, 0, 0]),
mut.VPolytope.MakeUnitBox(3)]
region = mut.Iris(
obstacles=obstacles, sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(
lb=[-5, -5, -5], ub=[5, 5, 5]), options=options)
self.assertIsInstance(region, mut.HPolyhedron)
def test_iris_cspace(self):
limits_urdf = """
<robot name="limits">
<link name="movable">
<collision>
<geometry><box size="1 1 1"/></geometry>
</collision>
</link>
<joint name="movable" type="prismatic">
<axis xyz="1 0 0"/>
<limit lower="-2" upper="2"/>
<parent link="world"/>
<child link="movable"/>
</joint>
</robot>"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant).AddModelFromString(limits_urdf, "urdf")
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
options = mut.IrisOptions()
with catch_drake_warnings(expected_count=1):
region = mut.IrisInConfigurationSpace(
plant=plant, context=plant.GetMyContextFromRoot(context),
sample=[0], options=options)
plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [0])
region = mut.IrisInConfigurationSpace(
plant=plant, context=plant.GetMyContextFromRoot(context),
options=options)
self.assertIsInstance(region, mut.ConvexSet)
self.assertEqual(region.ambient_dimension(), 1)
self.assertTrue(region.PointInSet([1.0]))
self.assertFalse(region.PointInSet([3.0]))
def test_graph_of_convex_sets(self):
spp = mut.GraphOfConvexSets()
source = spp.AddVertex(set=mut.Point([0.1]), name="source")
target = spp.AddVertex(set=mut.Point([0.2]), name="target")
edge0 = spp.AddEdge(u=source, v=target, name="edge0")
edge1 = spp.AddEdge(u_id=source.id(), v_id=target.id(), name="edge1")
self.assertEqual(len(spp.Vertices()), 2)
self.assertEqual(len(spp.Edges()), 2)
result = spp.SolveShortestPath(
source_id=source.id(), target_id=target.id(),
convex_relaxation=True)
self.assertIsInstance(result, MathematicalProgramResult)
self.assertIsInstance(spp.SolveShortestPath(
source=source, target=target, convex_relaxation=True),
MathematicalProgramResult)
self.assertIn("source", spp.GetGraphvizString(
result=result, show_slacks=True, precision=2, scientific=False))
# Vertex
self.assertIsInstance(source.id(), mut.GraphOfConvexSets.VertexId)
self.assertEqual(source.ambient_dimension(), 1)
self.assertEqual(source.name(), "source")
self.assertIsInstance(source.x()[0], Variable)
self.assertIsInstance(source.set(), mut.Point)
np.testing.assert_array_almost_equal(
source.GetSolution(result), [0.1], 1e-6)
# Edge
self.assertAlmostEqual(edge0.GetSolutionCost(result=result), 0.0, 1e-6)
np.testing.assert_array_almost_equal(
edge0.GetSolutionPhiXu(result=result), [0.1], 1e-6)
np.testing.assert_array_almost_equal(
edge0.GetSolutionPhiXv(result=result), [0.2], 1e-6)
self.assertIsInstance(edge0.id(), mut.GraphOfConvexSets.EdgeId)
self.assertEqual(edge0.name(), "edge0")
self.assertEqual(edge0.u(), source)
self.assertEqual(edge0.v(), target)
self.assertIsInstance(edge0.phi(), Variable)
self.assertIsInstance(edge0.xu()[0], Variable)
self.assertIsInstance(edge0.xv()[0], Variable)
var, binding = edge0.AddCost(e=1.0+edge0.xu()[0])
self.assertIsInstance(var, Variable)
self.assertIsInstance(binding, Binding[Cost])
var, binding = edge0.AddCost(binding=binding)
self.assertIsInstance(var, Variable)
self.assertIsInstance(binding, Binding[Cost])
binding = edge0.AddConstraint(f=(edge0.xu()[0] == edge0.xv()[0]))
self.assertIsInstance(binding, Binding[Constraint])
binding = edge0.AddConstraint(binding=binding)
self.assertIsInstance(binding, Binding[Constraint])
edge0.AddPhiConstraint(phi_value=False)
edge0.ClearPhiConstraints()
# Remove Edges
self.assertEqual(len(spp.Edges()), 2)
spp.RemoveEdge(edge1.id())
self.assertEqual(len(spp.Edges()), 1)
spp.RemoveEdge(edge0)
self.assertEqual(len(spp.Edges()), 0)
# Remove Vertices
self.assertEqual(len(spp.Vertices()), 2)
spp.RemoveVertex(source.id())
self.assertEqual(len(spp.Vertices()), 1)
spp.RemoveVertex(target)
self.assertEqual(len(spp.Vertices()), 0)
class TestGeometryOptimization(unittest.TestCase):
def __init__(self, *args, **kwargs):
super().__init__(*args, **kwargs)
self.A = np.eye(3)
self.b = [1.0, 1.0, 1.0]
self.prog = MathematicalProgram()
self.x = self.prog.NewContinuousVariables(3, "x")
self.t = self.prog.NewContinuousVariables(1, "t")[0]
self.Ay = np.array([[1., 0.], [0., 1.], [1., 0.]])
self.by = np.ones(3)
self.cz = np.ones(2)
self.dz = 1.
self.y = self.prog.NewContinuousVariables(2, "y")
self.z = self.prog.NewContinuousVariables(2, "z")
def test_point_convex_set(self):
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2 * p)
np.testing.assert_array_equal(point.x(), 2 * p)
point.set_x(x=p)
assert_pickle(self, point, lambda S: S.x())
# TODO(SeanCurtis-TRI): This doesn't test the constructor that
# builds from shape.
def test_h_polyhedron(self):
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), self.A)
np.testing.assert_array_equal(hpoly.b(), self.b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
self.assertFalse(hpoly.IsBounded())
hpoly.AddPointInSetConstraints(self.prog, self.x)
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = hpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
with self.assertRaisesRegex(RuntimeError,
".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
assert_pickle(self, hpoly, lambda S: np.vstack((S.A(), S.b())))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(h_box.IntersectsWith(hpoly))
h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(h_box.MaximumVolumeInscribedEllipsoid(),
mut.Hyperellipsoid)
np.testing.assert_array_almost_equal(h_box.ChebyshevCenter(),
[0, 0, 0])
h2 = h_box.CartesianProduct(other=h_unit_box)
self.assertIsInstance(h2, mut.HPolyhedron)
self.assertEqual(h2.ambient_dimension(), 6)
h3 = h_box.CartesianPower(n=3)
self.assertIsInstance(h3, mut.HPolyhedron)
self.assertEqual(h3.ambient_dimension(), 9)
h4 = h_box.Intersection(other=h_unit_box)
self.assertIsInstance(h4, mut.HPolyhedron)
self.assertEqual(h4.ambient_dimension(), 3)
h5 = h_box.PontryaginDifference(other=h_unit_box)
self.assertIsInstance(h5, mut.HPolyhedron)
np.testing.assert_array_equal(h5.A(), h_box.A())
np.testing.assert_array_equal(h5.b(), np.zeros(6))
def test_hyper_ellipsoid(self):
ellipsoid = mut.Hyperellipsoid(A=self.A, center=self.b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), self.A)
np.testing.assert_array_equal(ellipsoid.center(), self.b)
self.assertTrue(ellipsoid.PointInSet(x=self.b, tol=0.0))
ellipsoid.AddPointInSetConstraints(self.prog, self.x)
constraints = ellipsoid.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x, t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = ellipsoid.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay,
b=self.by,
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
p = np.array([11.1, 12.2, 13.3])
point = mut.Point(p)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
assert_pickle(self, ellipsoid, lambda S: np.vstack(
(S.A(), S.center())))
e_ball = mut.Hyperellipsoid.MakeAxisAligned(radius=[1, 1, 1],
center=self.b)
np.testing.assert_array_equal(e_ball.A(), self.A)
np.testing.assert_array_equal(e_ball.center(), self.b)
e_ball2 = mut.Hyperellipsoid.MakeHypersphere(radius=1, center=self.b)
np.testing.assert_array_equal(e_ball2.A(), self.A)
np.testing.assert_array_equal(e_ball2.center(), self.b)
e_ball3 = mut.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), self.A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
def test_minkowski_sum(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
hpoly = mut.HPolyhedron(A=self.A, b=self.b)
sum = mut.MinkowskiSum(setA=point, setB=hpoly)
self.assertEqual(sum.ambient_dimension(), 3)
self.assertEqual(sum.num_terms(), 2)
sum2 = mut.MinkowskiSum(sets=[point, hpoly])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_terms(), 2)
self.assertIsInstance(sum2.term(0), mut.Point)
def test_v_polytope(self):
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(self.prog, self.x[0:2])
constraints = vpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog, x=self.x[:2], t=self.t)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
constraints = vpoly.AddPointInNonnegativeScalingConstraints(
prog=self.prog,
A=self.Ay[:2],
b=self.by[:2],
c=self.cz,
d=self.dz,
x=self.y,
t=self.z)
self.assertGreaterEqual(len(constraints), 2)
self.assertIsInstance(constraints[0], Binding[Constraint])
assert_pickle(self, vpoly, lambda S: S.vertices())
v_box = mut.VPolytope.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
self.assertAlmostEqual(v_box.CalcVolume(), 8, 1E-10)
v_unit_box = mut.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
v_from_h = mut.VPolytope(H=mut.HPolyhedron.MakeUnitBox(dim=3))
self.assertTrue(v_from_h.PointInSet([0, 0, 0]))
# Test creating a vpolytope from a non-minimal set of vertices
# 2D: Random points inside a circle
r = 2.0
n = 400
vertices = np.zeros((2, n + 4))
theta = np.linspace(0, 2 * np.pi, n, endpoint=False)
vertices[0, 0:n] = r * np.cos(theta)
vertices[1, 0:n] = r * np.sin(theta)
vertices[:, n:] = np.array([[r / 2, r / 3, r / 4, r / 5],
[r / 2, r / 3, r / 4, r / 5]])
vpoly = mut.VPolytope(vertices=vertices).GetMinimalRepresentation()
self.assertAlmostEqual(vpoly.CalcVolume(), np.pi * r * r, delta=1e-3)
self.assertEqual(vpoly.vertices().shape[1], n)
# Calculate the length of the path that visits all the vertices
# sequentially.
# If the vertices are in clockwise/counter-clockwise order,
# the length of the path will coincide with the perimeter of a
# circle.
self.assertAlmostEqual(self._calculate_path_length(vpoly.vertices()),
2 * np.pi * r,
delta=1e-3)
# 3D: Random points inside a box
a = 2.0
vertices = np.array(
[[0, a, 0, a, 0, a, 0, a, a / 2, a / 3, a / 4, a / 5],
[0, 0, a, a, 0, 0, a, a, a / 2, a / 3, a / 4, a / 5],
[0, 0, 0, 0, a, a, a, a, a / 2, a / 3, a / 4, a / 5]])
vpoly = mut.VPolytope(vertices=vertices).GetMinimalRepresentation()
self.assertAlmostEqual(vpoly.CalcVolume(), a * a * a)
self.assertEqual(vpoly.vertices().shape[1], 8)
def _calculate_path_length(self, vertices):
n = vertices.shape[1]
length = 0
for i in range(n):
j = (i + 1) % n
diff = vertices[:, i] - vertices[:, j]
length += np.sqrt(np.dot(diff, diff))
return length
def test_cartesian_product(self):
point = mut.Point(np.array([11.1, 12.2, 13.3]))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
sum = mut.CartesianProduct(setA=point, setB=h_box)
self.assertEqual(sum.ambient_dimension(), 6)
self.assertEqual(sum.num_factors(), 2)
sum2 = mut.CartesianProduct(sets=[point, h_box])
self.assertEqual(sum2.ambient_dimension(), 6)
self.assertEqual(sum2.num_factors(), 2)
self.assertIsInstance(sum2.factor(0), mut.Point)
sum2 = mut.CartesianProduct(sets=[point, h_box],
A=np.eye(6, 3),
b=[0, 1, 2, 3, 4, 5])
self.assertEqual(sum2.ambient_dimension(), 3)
self.assertEqual(sum2.num_factors(), 2)
self.assertIsInstance(sum2.factor(1), mut.HPolyhedron)
def test_intersection(self):
point = mut.Point(np.array([0.1, 0.2, 0.3]))
h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1])
intersect = mut.Intersection(setA=point, setB=h_box)
self.assertEqual(intersect.ambient_dimension(), 3)
self.assertEqual(intersect.num_elements(), 2)
intersect2 = mut.Intersection(sets=[point, h_box])
self.assertEqual(intersect2.ambient_dimension(), 3)
self.assertEqual(intersect2.num_elements(), 2)
self.assertIsInstance(intersect2.element(0), mut.Point)
def test_make_from_scene_graph_and_iris(self):
"""
Tests the make from scene graph and iris functionality together as
the Iris code makes obstacles from geometries registered in SceneGraph.
"""
scene_graph = SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(source_id=source_id,
frame=GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Box(1., 1., 1.),
name="box"))
cylinder_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Cylinder(1., 1.),
name="cylinder"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Sphere(1.),
name="sphere"))
capsule_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=GeometryInstance(X_PG=RigidTransform(),
shape=Capsule(1., 1.0),
name="capsule"))
context = scene_graph.CreateDefaultContext()
pose_vector = FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
H = mut.HPolyhedron(query_object=query_object,
geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
C = mut.CartesianProduct(query_object=query_object,
geometry_id=cylinder_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(C.ambient_dimension(), 3)
E = mut.Hyperellipsoid(query_object=query_object,
geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
S = mut.MinkowskiSum(query_object=query_object,
geometry_id=capsule_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(S.ambient_dimension(), 3)
P = mut.Point(query_object=query_object,
geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id(),
maximum_allowable_radius=1.5)
self.assertEqual(P.ambient_dimension(), 3)
V = mut.VPolytope(query_object=query_object,
geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
obstacles = mut.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
options.relative_termination_threshold = 0.01
self.assertNotIn("object at 0x", repr(options))
region = mut.Iris(obstacles=obstacles,
sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(lb=[-5, -5, -5],
ub=[5, 5, 5]),
options=options)
self.assertIsInstance(region, mut.HPolyhedron)
obstacles = [
mut.HPolyhedron.MakeUnitBox(3),
mut.Hyperellipsoid.MakeUnitBall(3),
mut.Point([0, 0, 0]),
mut.VPolytope.MakeUnitBox(3)
]
region = mut.Iris(obstacles=obstacles,
sample=[2, 3.4, 5],
domain=mut.HPolyhedron.MakeBox(lb=[-5, -5, -5],
ub=[5, 5, 5]),
options=options)
self.assertIsInstance(region, mut.HPolyhedron)
def test_iris_cspace(self):
limits_urdf = """
<robot name="limits">
<link name="movable">
<collision>
<geometry><box size="1 1 1"/></geometry>
</collision>
</link>
<joint name="movable" type="prismatic">
<axis xyz="1 0 0"/>
<limit lower="-2" upper="2"/>
<parent link="world"/>
<child link="movable"/>
</joint>
</robot>"""
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
Parser(plant).AddModelFromString(limits_urdf, "urdf")
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
options = mut.IrisOptions()
plant.SetPositions(plant.GetMyMutableContextFromRoot(context), [0])
region = mut.IrisInConfigurationSpace(
plant=plant,
context=plant.GetMyContextFromRoot(context),
options=options)
self.assertIsInstance(region, mut.ConvexSet)
self.assertEqual(region.ambient_dimension(), 1)
self.assertTrue(region.PointInSet([1.0]))
self.assertFalse(region.PointInSet([3.0]))
def test_graph_of_convex_sets(self):
spp = mut.GraphOfConvexSets()
source = spp.AddVertex(set=mut.Point([0.1]), name="source")
target = spp.AddVertex(set=mut.Point([0.2]), name="target")
edge0 = spp.AddEdge(u=source, v=target, name="edge0")
edge1 = spp.AddEdge(u_id=source.id(), v_id=target.id(), name="edge1")
self.assertEqual(len(spp.Vertices()), 2)
self.assertEqual(len(spp.Edges()), 2)
result = spp.SolveShortestPath(source_id=source.id(),
target_id=target.id(),
convex_relaxation=True)
self.assertIsInstance(result, MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source_id=source.id(),
target_id=target.id(),
convex_relaxation=True,
solver=ClpSolver()),
MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source_id=source.id(),
target_id=target.id(),
convex_relaxation=True,
solver_options=SolverOptions()),
MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source=source,
target=target,
convex_relaxation=True),
MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source=source,
target=target,
convex_relaxation=True,
solver=ClpSolver()),
MathematicalProgramResult)
self.assertIsInstance(
spp.SolveShortestPath(source=source,
target=target,
convex_relaxation=True,
solver_options=SolverOptions()),
MathematicalProgramResult)
self.assertIn(
"source",
spp.GetGraphvizString(result=result,
show_slacks=True,
precision=2,
scientific=False))
# Vertex
self.assertIsInstance(source.id(), mut.GraphOfConvexSets.VertexId)
self.assertEqual(source.ambient_dimension(), 1)
self.assertEqual(source.name(), "source")
self.assertIsInstance(source.x()[0], Variable)
self.assertIsInstance(source.set(), mut.Point)
np.testing.assert_array_almost_equal(source.GetSolution(result), [0.1],
1e-6)
# Edge
self.assertAlmostEqual(edge0.GetSolutionCost(result=result), 0.0, 1e-6)
np.testing.assert_array_almost_equal(
edge0.GetSolutionPhiXu(result=result), [0.1], 1e-6)
np.testing.assert_array_almost_equal(
edge0.GetSolutionPhiXv(result=result), [0.2], 1e-6)
self.assertIsInstance(edge0.id(), mut.GraphOfConvexSets.EdgeId)
self.assertEqual(edge0.name(), "edge0")
self.assertEqual(edge0.u(), source)
self.assertEqual(edge0.v(), target)
self.assertIsInstance(edge0.phi(), Variable)
self.assertIsInstance(edge0.xu()[0], Variable)
self.assertIsInstance(edge0.xv()[0], Variable)
var, binding = edge0.AddCost(e=1.0 + edge0.xu()[0])
self.assertIsInstance(var, Variable)
self.assertIsInstance(binding, Binding[Cost])
var, binding = edge0.AddCost(binding=binding)
self.assertIsInstance(var, Variable)
self.assertIsInstance(binding, Binding[Cost])
self.assertEqual(len(edge0.GetCosts()), 2)
binding = edge0.AddConstraint(f=(edge0.xu()[0] == edge0.xv()[0]))
self.assertIsInstance(binding, Binding[Constraint])
binding = edge0.AddConstraint(binding=binding)
self.assertIsInstance(binding, Binding[Constraint])
self.assertEqual(len(edge0.GetConstraints()), 2)
edge0.AddPhiConstraint(phi_value=False)
edge0.ClearPhiConstraints()
# Remove Edges
self.assertEqual(len(spp.Edges()), 2)
spp.RemoveEdge(edge1.id())
self.assertEqual(len(spp.Edges()), 1)
spp.RemoveEdge(edge0)
self.assertEqual(len(spp.Edges()), 0)
# Remove Vertices
self.assertEqual(len(spp.Vertices()), 2)
spp.RemoveVertex(source.id())
self.assertEqual(len(spp.Vertices()), 1)
spp.RemoveVertex(target)
self.assertEqual(len(spp.Vertices()), 0)
class PPTrajectory():
def __init__(self, sample_times, num_vars, degree, continuity_degree):
self.sample_times = sample_times
self.n = num_vars
self.degree = degree
self.prog = MathematicalProgram()
self.coeffs = []
for i in range(len(sample_times)):
self.coeffs.append(
self.prog.NewContinuousVariables(num_vars, degree + 1, "C"))
self.result = None
# Add continuity constraints
for s in range(len(sample_times) - 1):
trel = sample_times[s + 1] - sample_times[s]
coeffs = self.coeffs[s]
for var in range(self.n):
for deg in range(continuity_degree + 1):
# Don't use eval here, because I want left and right
# values of the same time
left_val = 0
for d in range(deg, self.degree + 1):
left_val += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
right_val = self.coeffs[s + 1][var,
deg] * math.factorial(deg)
self.prog.AddLinearConstraint(left_val == right_val)
# Add cost to minimize highest order terms
for s in range(len(sample_times) - 1):
self.prog.AddQuadraticCost(np.eye(num_vars), np.zeros(
(num_vars, 1)), self.coeffs[s][:, -1])
def eval(self, t, derivative_order=0):
if derivative_order > self.degree:
return 0
s = 0
while s < len(self.sample_times) - 1 and t >= self.sample_times[s + 1]:
s += 1
trel = t - self.sample_times[s]
if self.result is None:
coeffs = self.coeffs[s]
else:
coeffs = self.result.GetSolution(self.coeffs[s])
deg = derivative_order
val = 0 * coeffs[:, 0]
for var in range(self.n):
for d in range(deg, self.degree + 1):
val[var] += coeffs[var, d]*np.power(trel, d-deg) * \
math.factorial(d)/math.factorial(d-deg)
return val
def add_constraint(self, t, derivative_order, lb, ub=None):
'''
Adds a constraint of the form d^deg lb <= x(t) / dt^deg <= ub
'''
if ub is None:
ub = lb
assert (derivative_order <= self.degree)
val = self.eval(t, derivative_order)
self.prog.AddLinearConstraint(val, lb, ub)
def generate(self):
self.result = Solve(self.prog)
assert (self.result.is_success())
context = context_ad
# Do forward kinematics
plant.SetPositions(context, iiwa, q)
X_WL7 = plant.CalcRelativeTransform(context, resolve_frame(plant, W),
resolve_frame(plant, L7))
p_TL7 = X_WL7.translation() - p_WT
# WARNING: If you return a scalar for a constraint, or a vector for
# a cost, you may get the following cryptic error:
# "Unable to cast Python instance to c++ type"
link_7_distance_to_target_vector = lambda q: [link_7_distance_to_target(q)]
# New program: Using a custom evaluator
prog = MathematicalProgram()
q = prog.NewContinuousVariables(plant_f.num_positions())
# Define nominal configuration
q0 = np.zeros(plant_f.num_positions())
# Add basic cost. (This will be parsed into a quadratic cost)
prog.AddCost((q - q0).dot(q - q0))
# Add constraint based on custom evaluator.
prog.AddConstraint(link_7_distance_to_target_vector,
lb=[0.1],
ub=[0.2],
vars=q)
result = Solve(prog, initial_guess=q0)
from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve
import numpy as np
# Creat an empty MathematicalProgram named prog (with no decision variables, constraints or costs)
prog = MathematicalProgram()
# Add two decision variables x[0], x[1]
x = prog.NewContinuousVariables(2, "x")
# Add a symbolic linear expression as the cost
cost_1 = prog.AddLinearCost(x[0] + 3 * x[1] + 2)
# Print the newly added cost
print(cost_1)
# The newly added cost is stored in prog.linear_costs()
print(prog.linear_costs()[0])
cost_2 = prog.AddLinearCost(2 * x[1] + 3)
print(f"number of linear cost objects: {len(prog.linear_costs())}")
# Can also add costs in a vector coefficient form
cost_3 = prog.AddLinearCost([3., 4.], 5.0, x)
print(cost_3)
# Can also just call the generic "AddCost".
# If drake thinks its linear, gets added to linear costs list
cost_4 = prog.AddCost(x[0] + 3 * x[1] + 5)
print(f"Number of linear cost objects after calling AddCost: {len(prog.linear_costs())}")
# New program, now with constraints
prog = MathematicalProgram()
def compute_input(self, x, xd, initial_guess=None):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda1')
z = prog.NewBinaryVariables(rows=self.T, cols=self.nf, name='z')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
prog.AddConstraint(z[0, 0] == 0)
prog.AddConstraint(z[0, 1] == 0)
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints with big M = self.contact
prog.AddConstraint(ge(contact[t], 0))
prog.AddConstraint(contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) >= 0)
prog.AddConstraint(contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) >= 0)
# Mixed Integer Constraints
M = self.contact_max
prog.AddConstraint(contact[t, 0] <= M)
prog.AddConstraint(contact[t, 1] <= M)
prog.AddConstraint(contact[t, 0] <= M * z[t, 0])
prog.AddConstraint(contact[t, 1] <= M * z[t, 1])
prog.AddConstraint(
contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d']) <= M *
(1 - z[t, 0]))
prog.AddConstraint(
contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d']) <= M *
(1 - z[t, 1]))
prog.AddConstraint(z[t, 0] + z[t, 1] == 1)
# Add Input Constraints. Contact Constraints already enforced in big-M
# prog.AddConstraint(le(u[t], self.input_max))
# prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver_id = IpoptSolver().solver_id()
elif (self.solver == "snopt"):
solver_id = SnoptSolver().solver_id()
elif (self.solver == "osqp"):
solver_id = OsqpSolver().solver_id()
elif (self.solver == "mosek"):
solver_id = MosekSolver().solver_id()
elif (self.solver == "gurobi"):
solver_id = GurobiSolver().solver_id()
solver = MixedIntegerBranchAndBound(prog, solver_id)
#result = solver.Solve(prog, initial_guess)
result = solver.Solve()
if result != result.kSolutionFound:
raise ValueError('Infeasible optimization problem')
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt
x(0) <= x(1)
this example also demonstrates how to add a callback to the program, and use it to visualize the progression of the solver.
Adapted from the MathematicalProgram tutorial on the Drake website: https://drake.mit.edu
"""
from pydrake.solvers.mathematicalprogram import MathematicalProgram, Solve
import numpy as np
import matplotlib.pyplot as plt
# Create empty MathematicalProgram
prog = MathematicalProgram()
# Add 2 continuous decision variables
# x is a numpy array - we can use an optional second argument to name the variables
x = prog.NewContinuousVariables(2)
#print(x)
prog.AddConstraint(x[0] + x[1] == 1)
prog.AddConstraint(x[0] <= x[1])
prog.AddCost(x[0]**2 + x[1]**2)
# Make and add a visualization callback
fig = plt.figure()
curve_x = np.linspace(1, 10, 100)
ax = plt.gca()
ax.plot(curve_x, 9. / curve_x)
ax.plot(-curve_x, -9. / curve_x)
ax.plot(0, 0, 'o')
x_init = [4., 5.]
point_x, = ax.plot(x_init[0], x_init[1], 'x')
ax.axis('equal')
from pydrake.solvers.mathematicalprogram import MathematicalProgram import numpy as np import matplotlib.pyplot as plt from pydrake.solvers.mathematicalprogram import Solve prog = MathematicalProgram() x = prog.NewContinuousVariables(2) print(x) print(1 + 2 * x[0] + 3 * x[1] + 4 * x[1]) y = prog.NewContinuousVariables(2, "dog") print(y) print(y[0] + y[0] + y[1] * y[1] * y[1]) var_matrix = prog.NewContinuousVariables(3, 2, "A") print(var_matrix) # Add the constraint x(0) * x(1) = 1 to prog prog.AddConstraint(x[0] * x[1] == 1) prog.AddConstraint(x[0] >= 0) prog.AddConstraint(x[0] - x[1] <= 0) prog.AddCost(x[0]**2 + 3) prog.AddCost(x[0] + x[1]) # New optimization program, all the way through to solving prog = MathematicalProgram() x = prog.NewContinuousVariables(2) prog.AddConstraint(x[0] + x[1] == 1)
def plot_sublevelset_expression(ax, e, vertices=51, **kwargs):
"""
Plots the 2D sub-level set e(x) <= 1, which must contain the origin.
Args:
ax: the matplotlib axis to receive the plot
e: a symbolic expression in two variables
vertices: number of sample points along the boundary
kwargs: are passed to the matplotlib fill method
Returns:
the return values from matplotlib's fill command.
"""
x = list(e.GetVariables())
assert len(x) == 2, "e must be an expression in two variables"
# Handle the special case where e is a degree 2 polynomial.
if e.is_polynomial():
p = Polynomial(e)
if p.TotalDegree() == 2:
env = {a: 0 for a in x}
c = e.Evaluate(env)
e1 = e.Jacobian(x)
b = Evaluate(e1, env)
e2 = Jacobian(e1, x)
A = 0.5 * Evaluate(e2, env)
return plot_sublevelset_quadratic(ax, A, b, c, vertices, **kwargs)
# Find the level-set in polar coordinates, by sampling theta and
# root-finding (on the scalar expression) to find a rplus and rminus.
Xplus = np.empty((2, vertices))
Xminus = np.empty((2, vertices))
i = 0
for theta in np.linspace(0, np.pi, vertices):
prog = MathematicalProgram()
r = prog.NewContinuousVariables(1, "r")[0]
env = {x[0]: r * np.cos(theta), x[1]: r * np.sin(theta)}
scalar = e.Substitute(env)
b = prog.AddBoundingBoxConstraint(0, np.inf, r)
prog.AddConstraint(scalar == 1)
prog.AddQuadraticCost([1], [0], [r])
prog.SetInitialGuess(r, 0.1) # or anything non-zero.
result = Solve(prog)
assert result.is_success(), "Failed to find the level set"
rplus = result.GetSolution(r)
Xplus[0, i] = rplus * np.cos(theta)
Xplus[1, i] = rplus * np.sin(theta)
b.evaluator().UpdateLowerBound([-np.inf])
b.evaluator().UpdateUpperBound([0])
prog.SetInitialGuess(r, -0.1) # or anything non-zero.
result = Solve(prog)
assert result.is_success(), "Failed to find the level set"
rminus = result.GetSolution(r)
Xminus[0, i] = rminus * np.cos(theta)
Xminus[1, i] = rminus * np.sin(theta)
i = i + 1
return ax.fill(np.hstack((Xplus[0, :], Xminus[0, :])),
np.hstack((Xplus[1, :], Xminus[1, :])), **kwargs)
mp.AddConstraint(theta2 <= np.pi).evaluator().set_description(
"Constrain theta2 <= np.pi")
mp.AddConstraint(
theta3 >= 0.0).evaluator().set_description("Constrain theta3 >= 0.0")
mp.AddConstraint(theta3 <= np.pi).evaluator().set_description(
"Constrain theta3 <= np.pi")
if __name__ == "__main__":
mp = MathematicalProgram()
state_over_time = np.zeros(shape=(NUM_TIME_STEPS, STATE_SIZE),
dtype=pydrake.symbolic.Variable)
tau234_over_time = np.zeros(shape=(NUM_TIME_STEPS, TORQUE_SIZE),
dtype=pydrake.symbolic.Variable)
initial_state = mp.NewContinuousVariables(8, "state_0")
initial_theta1 = initial_state[0]
initial_theta2 = initial_state[1]
initial_theta3 = initial_state[2]
initial_theta4 = initial_state[3]
# Constrain initial velocity to be 0
# for j in range(4, 8):
# mp.AddConstraint(initial_state[j] <= 0.0).evaluator().set_description("Constrain initial_state[%d] <= 0.0" % j)
# mp.AddConstraint(initial_state[j] >= 0.0).evaluator().set_description("Constrain initial_state[%d] >= 0.0" % j)
# constrain_theta123(mp, initial_theta1, initial_theta2, initial_theta3)
mp.AddConstraint(initial_theta1 <= -np.pi / 3.0)
mp.AddConstraint(initial_theta1 >= -np.pi / 3.0)
mp.AddConstraint(initial_theta2 <= np.pi / 3.0)
mp.AddConstraint(initial_theta2 >= np.pi / 3.0)
def test_optimization(self):
"""Tests geometry::optimization bindings"""
A = np.eye(3)
b = [1.0, 1.0, 1.0]
prog = MathematicalProgram()
x = prog.NewContinuousVariables(3, "x")
t = prog.NewContinuousVariables(1, "t")
# Test Point.
p = np.array([11.1, 12.2, 13.3])
point = mut.optimization.Point(p)
self.assertEqual(point.ambient_dimension(), 3)
np.testing.assert_array_equal(point.x(), p)
point.set_x(x=2 * p)
np.testing.assert_array_equal(point.x(), 2 * p)
point.set_x(x=p)
# Test HPolyhedron.
hpoly = mut.optimization.HPolyhedron(A=A, b=b)
self.assertEqual(hpoly.ambient_dimension(), 3)
np.testing.assert_array_equal(hpoly.A(), A)
np.testing.assert_array_equal(hpoly.b(), b)
self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0))
hpoly.AddPointInSetConstraints(prog, x)
with self.assertRaisesRegex(RuntimeError,
".*not implemented yet for HPolyhedron.*"):
hpoly.ToShapeWithPose()
h_box = mut.optimization.HPolyhedron.MakeBox(lb=[-1, -1, -1],
ub=[1, 1, 1])
h_unit_box = mut.optimization.HPolyhedron.MakeUnitBox(dim=3)
np.testing.assert_array_equal(h_box.A(), h_unit_box.A())
np.testing.assert_array_equal(h_box.b(), h_unit_box.b())
self.assertIsInstance(h_box.MaximumVolumeInscribedEllipsoid(),
mut.optimization.Hyperellipsoid)
np.testing.assert_array_almost_equal(h_box.ChebyshevCenter(),
[0, 0, 0])
# Test Hyperellipsoid.
ellipsoid = mut.optimization.Hyperellipsoid(A=A, center=b)
self.assertEqual(ellipsoid.ambient_dimension(), 3)
np.testing.assert_array_equal(ellipsoid.A(), A)
np.testing.assert_array_equal(ellipsoid.center(), b)
self.assertTrue(ellipsoid.PointInSet(x=b, tol=0.0))
ellipsoid.AddPointInSetConstraints(prog, x)
shape, pose = ellipsoid.ToShapeWithPose()
self.assertIsInstance(shape, mut.Ellipsoid)
self.assertIsInstance(pose, RigidTransform)
scale, witness = ellipsoid.MinimumUniformScalingToTouch(point)
self.assertTrue(scale > 0.0)
np.testing.assert_array_almost_equal(witness, p)
e_ball = mut.optimization.Hyperellipsoid.MakeAxisAligned(
radius=[1, 1, 1], center=b)
np.testing.assert_array_equal(e_ball.A(), A)
np.testing.assert_array_equal(e_ball.center(), b)
e_ball2 = mut.optimization.Hyperellipsoid.MakeHypersphere(radius=1,
center=b)
np.testing.assert_array_equal(e_ball2.A(), A)
np.testing.assert_array_equal(e_ball2.center(), b)
e_ball3 = mut.optimization.Hyperellipsoid.MakeUnitBall(dim=3)
np.testing.assert_array_equal(e_ball3.A(), A)
np.testing.assert_array_equal(e_ball3.center(), [0, 0, 0])
# Test VPolytope.
vertices = np.array([[0.0, 1.0, 2.0], [3.0, 7.0, 5.0]])
vpoly = mut.optimization.VPolytope(vertices=vertices)
self.assertEqual(vpoly.ambient_dimension(), 2)
np.testing.assert_array_equal(vpoly.vertices(), vertices)
self.assertTrue(vpoly.PointInSet(x=[1.0, 5.0], tol=1e-8))
vpoly.AddPointInSetConstraints(prog, x[0:2])
v_box = mut.optimization.VPolytope.MakeBox(lb=[-1, -1, -1],
ub=[1, 1, 1])
self.assertTrue(v_box.PointInSet([0, 0, 0]))
v_unit_box = mut.optimization.VPolytope.MakeUnitBox(dim=3)
self.assertTrue(v_unit_box.PointInSet([0, 0, 0]))
# Test remaining ConvexSet methods using these instances.
self.assertIsInstance(hpoly.Clone(), mut.optimization.HPolyhedron)
self.assertTrue(ellipsoid.IsBounded())
hpoly.AddPointInNonnegativeScalingConstraints(prog=prog, x=x, t=t[0])
# Test MakeFromSceneGraph methods.
scene_graph = mut.SceneGraph()
source_id = scene_graph.RegisterSource("source")
frame_id = scene_graph.RegisterFrame(source_id=source_id,
frame=mut.GeometryFrame("frame"))
box_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Box(1., 1., 1.),
name="sphere"))
sphere_geometry_id = scene_graph.RegisterGeometry(
source_id=source_id,
frame_id=frame_id,
geometry=mut.GeometryInstance(X_PG=RigidTransform(),
shape=mut.Sphere(1.),
name="sphere"))
context = scene_graph.CreateDefaultContext()
pose_vector = mut.FramePoseVector()
pose_vector.set_value(frame_id, RigidTransform())
scene_graph.get_source_pose_port(source_id).FixValue(
context, pose_vector)
query_object = scene_graph.get_query_output_port().Eval(context)
H = mut.optimization.HPolyhedron(
query_object=query_object,
geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(H.ambient_dimension(), 3)
E = mut.optimization.Hyperellipsoid(
query_object=query_object,
geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(E.ambient_dimension(), 3)
P = mut.optimization.Point(
query_object=query_object,
geometry_id=sphere_geometry_id,
reference_frame=scene_graph.world_frame_id(),
maximum_allowable_radius=1.5)
self.assertEqual(P.ambient_dimension(), 3)
V = mut.optimization.VPolytope(
query_object=query_object,
geometry_id=box_geometry_id,
reference_frame=scene_graph.world_frame_id())
self.assertEqual(V.ambient_dimension(), 3)
# Test Iris.
obstacles = mut.optimization.MakeIrisObstacles(
query_object=query_object,
reference_frame=scene_graph.world_frame_id())
options = mut.optimization.IrisOptions()
options.require_sample_point_is_contained = True
options.iteration_limit = 1
options.termination_threshold = 0.1
region = mut.optimization.Iris(
obstacles=obstacles,
sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(lb=[-5, -5, -5],
ub=[5, 5, 5]),
options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
obstacles = [
mut.optimization.HPolyhedron.MakeUnitBox(3),
mut.optimization.Hyperellipsoid.MakeUnitBall(3),
mut.optimization.Point([0, 0, 0]),
mut.optimization.VPolytope.MakeUnitBox(3)
]
region = mut.optimization.Iris(
obstacles=obstacles,
sample=[2, 3.4, 5],
domain=mut.optimization.HPolyhedron.MakeBox(lb=[-5, -5, -5],
ub=[5, 5, 5]),
options=options)
self.assertIsInstance(region, mut.optimization.HPolyhedron)
def compute_input(self, x, xd, initial_guess=None, tol=0.0):
prog = MathematicalProgram()
# Joint configuration states & Contact forces
q = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='q')
v = prog.NewContinuousVariables(rows=self.T + 1,
cols=self.nq,
name='v')
u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
contact = prog.NewContinuousVariables(rows=self.T,
cols=self.nf,
name='lambda')
#
alpha = prog.NewContinuousVariables(rows=self.T, cols=2, name='alpha')
beta = prog.NewContinuousVariables(rows=self.T, cols=2, name='beta')
# Add Initial Condition Constraint
prog.AddConstraint(eq(q[0], np.array(x[0:3])))
prog.AddConstraint(eq(v[0], np.array(x[3:6])))
# Add Final Condition Constraint
prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))
# Add Dynamics Constraints
for t in range(self.T):
# Add Dynamics Constraints
prog.AddConstraint(
eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))
prog.AddConstraint(v[t + 1, 0] == (
v[t, 0] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
prog.AddConstraint(v[t + 1,
1] == (v[t, 1] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 1] +
contact[t, 0] - contact[t, 1])))
prog.AddConstraint(v[t + 1, 2] == (
v[t, 2] + self.sim.params['h'] *
(-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))
# Add Contact Constraints
prog.AddConstraint(ge(alpha[t], 0))
prog.AddConstraint(ge(beta[t], 0))
prog.AddConstraint(alpha[t, 0] == contact[t, 0])
prog.AddConstraint(alpha[t, 1] == contact[t, 1])
prog.AddConstraint(
beta[t, 0] == (contact[t, 0] + self.sim.params['k'] *
(q[t, 1] - q[t, 0] - self.sim.params['d'])))
prog.AddConstraint(
beta[t, 1] == (contact[t, 1] + self.sim.params['k'] *
(q[t, 2] - q[t, 1] - self.sim.params['d'])))
# Complementarity constraints. Start with relaxed version and start constraining.
prog.AddConstraint(alpha[t, 0] * beta[t, 0] <= tol)
prog.AddConstraint(alpha[t, 1] * beta[t, 1] <= tol)
# Add Input Constraints and Contact Constraints
prog.AddConstraint(le(contact[t], self.contact_max))
prog.AddConstraint(ge(contact[t], -self.contact_max))
prog.AddConstraint(le(u[t], self.input_max))
prog.AddConstraint(ge(u[t], -self.input_max))
# Add Costs
prog.AddCost(u[t].dot(u[t]))
# Set Initial Guess as empty. Otherwise, start from last solver iteration.
if (type(initial_guess) == type(None)):
initial_guess = np.empty(prog.num_vars())
# Populate initial guess by linearly interpolating between initial
# and final states
#qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
uinit = np.tile(np.array([0, 0]), (self.T, 1))
finit = np.tile(np.array([0, 0]), (self.T, 1))
prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
prog.SetDecisionVariableValueInVector(contact, finit,
initial_guess)
# Solve the program
if (self.solver == "ipopt"):
solver = IpoptSolver()
elif (self.solver == "snopt"):
solver = SnoptSolver()
result = solver.Solve(prog, initial_guess)
if (self.solver == "ipopt"):
print("Ipopt Solver Status: ",
result.get_solver_details().status, ", meaning ",
result.get_solver_details().ConvertStatusToString())
elif (self.solver == "snopt"):
val = result.get_solver_details().info
status = self.snopt_status(val)
print("Snopt Solver Status: ",
result.get_solver_details().info, ", meaning ", status)
sol = result.GetSolution()
q_opt = result.GetSolution(q)
v_opt = result.GetSolution(v)
u_opt = result.GetSolution(u)
f_opt = result.GetSolution(contact)
return sol, q_opt, v_opt, u_opt, f_opt