def construct_environment(masses: typing.List, box_sizes: typing.List):
"""
Construct an environment with many free boxes.
@param masses masses[i] is the mass of box i.
@param box_sizes box_sizes[i] is the size of box i.
"""
builder = DiagramBuilder_[AutoDiffXd]()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, 0.0)
# Add the ground as a big box.
ground_box = plant.AddRigidBody(
"ground", SpatialInertia(1, np.array([0, 0, 0]), UnitInertia(1, 1, 1)))
X_WG = RigidTransform([0, 0, -0.05])
ground_geometry_id = plant.RegisterCollisionGeometry(
ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
CoulombFriction(0.9, 0.8))
plant.RegisterVisualGeometry(
ground_box, RigidTransform(), Box(10, 10, 0.1), "ground",
[0.5, 0.5, 0.5, 1.])
plant.WeldFrames(plant.world_frame(), ground_box.body_frame(), X_WG)
# Add boxes
assert isinstance(masses, list)
assert isinstance(box_sizes, list)
assert len(masses) == len(box_sizes)
num_boxes = len(masses)
boxes = []
boxes_geometry_id = []
for i in range(num_boxes):
box_name = f"box{i}"
boxes.append(plant.AddRigidBody(
box_name, SpatialInertia(
masses[i], np.array([0, 0, 0]), UnitInertia(1, 1, 1))))
box_shape = Box(box_sizes[i][0], box_sizes[i][1], box_sizes[i][2])
boxes_geometry_id.append(plant.RegisterCollisionGeometry(
boxes[i], RigidTransform(), box_shape, f"{box_name}_box",
CoulombFriction(0.9, 0.8)))
plant.RegisterVisualGeometry(
ground_box, RigidTransform(), box_shape, f"{box_name}_box",
[0.5, 0.5, 0.5, 1.])
# Add small spheres at the corners of the box.
vertices = np.array([
[1, 1, 1], [1, 1, -1], [1, -1, 1], [1, -1, -1], [-1, 1, 1],
[-1, -1, 1], [-1, 1, -1], [-1, -1, -1]]) *\
np.tile(box_sizes[i]/2, (8, 1))
sphere_shape = Sphere(0.001)
for j in range(8):
plant.RegisterCollisionGeometry(
boxes[i], RigidTransform(vertices[j]), sphere_shape,
f"{box_name}_sphere{j}", CoulombFriction(0.9, 0.8))
plant.RegisterVisualGeometry(
boxes[i], RigidTransform(vertices[j]), sphere_shape,
f"{box_name}_sphere{j}", [0.5, 0.5, 0.5, 1])
plant.Finalize()
diagram = builder.Build()
return Environment(
plant=plant, scene_graph=scene_graph, diagram=diagram, boxes=boxes,
ground_geometry_id=ground_geometry_id,
boxes_geometry_id=boxes_geometry_id)
def _convert_geom(self, geom):
"""Given an lcmt_viewer_geometry_data, parse it into a tuple of (Shape,
Rgba, RigidTransform)."""
shape = None
if geom.type == lcmt_viewer_geometry_data.BOX:
(width, depth, height) = geom.float_data
shape = Box(width=width, depth=depth, height=height)
elif geom.type == lcmt_viewer_geometry_data.CAPSULE:
(radius, length) = geom.float_data
shape = Capsule(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.CYLINDER:
(radius, length) = geom.float_data
shape = Cylinder(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.ELLIPSOID:
(a, b, c) = geom.float_data
shape = Ellipsoid(a=a, b=b, c=c)
elif geom.type == lcmt_viewer_geometry_data.MESH:
(scale_x, scale_y, scale_z) = geom.float_data
filename = geom.string_data
assert scale_x == scale_y and scale_y == scale_z
shape = Mesh(absolute_filename=filename, scale=scale_x)
elif geom.type == lcmt_viewer_geometry_data.SPHERE:
(radius,) = geom.float_data
shape = Sphere(radius=radius)
else:
_logger.warning(f"Unknown geom.type of {geom.type}")
return (None, None, None)
rgba = Rgba(*geom.color)
pose = self._to_pose(geom.position, geom.quaternion)
return (shape, rgba, pose)
def add_vis_object(self, name, color):
source_id = self._sg.RegisterSource(name)
frame_id = self._sg.RegisterFrame(source_id, GeometryFrame(f"{name}_frame"))
geom = GeometryInstance(RigidTransform(), Sphere(0.014), f"{name}_geometry")
geom.set_illustration_properties(
MakePhongIllustrationProperties(color)
)
goal_id = self._sg.RegisterGeometry(source_id, frame_id, geom)
goal_vis = self._builder.AddSystem(VisObject(frame_id))
self._builder.Connect(goal_vis.get_output_port(0),
self._sg.get_source_pose_port(source_id))
return goal_vis
def _convert_geom(self, geom):
"""Given an lcmt_viewer_geometry_data, parse it into a tuple of (Shape,
Rgba, RigidTransform)."""
shape = None
if geom.type == lcmt_viewer_geometry_data.BOX:
(width, depth, height) = geom.float_data
shape = Box(width=width, depth=depth, height=height)
elif geom.type == lcmt_viewer_geometry_data.CAPSULE:
(radius, length) = geom.float_data
shape = Capsule(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.CYLINDER:
(radius, length) = geom.float_data
shape = Cylinder(radius=radius, length=length)
elif geom.type == lcmt_viewer_geometry_data.ELLIPSOID:
(a, b, c) = geom.float_data
shape = Ellipsoid(a=a, b=b, c=c)
elif geom.type == lcmt_viewer_geometry_data.MESH and geom.string_data:
# A mesh to be loaded from a file.
(scale_x, scale_y, scale_z) = geom.float_data
filename = geom.string_data
assert scale_x == scale_y and scale_y == scale_z
shape = Mesh(absolute_filename=filename, scale=scale_x)
elif geom.type == lcmt_viewer_geometry_data.MESH:
assert not geom.string_data
# A mesh with the data inline, i.e.,
# V | T | v0 | v1 | ... vN | t0 | t1 | ... | tM
# where
# V: The number of vertices.
# T: The number of triangles.
# N: 3V, the number of floating point values for the V vertices.
# M: 3T, the number of vertex indices for the T triangles.
_logger.warning("Meldis cannot yet display hydroelastic collision "
"meshes; that geometry will be ignored.")
elif geom.type == lcmt_viewer_geometry_data.SPHERE:
(radius,) = geom.float_data
shape = Sphere(radius=radius)
else:
_logger.warning(f"Unknown geom.type of {geom.type}")
return (None, None, None)
rgba = Rgba(*geom.color)
pose = self._to_pose(geom.position, geom.quaternion)
return (shape, rgba, pose)
def add_goal_region_visual_geometry(mbp, goal_position, goal_delta):
''' Adds a 5cm cube at the specified pose. Uses a planar floating base
in the x-z plane. '''
shape = Box(goal_delta, goal_delta, goal_delta)
no_mass_no_inertia = SpatialInertia(mass=0.,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(0., 0., 0.))
shape = Sphere(0.05)
model_instance = mbp.AddModelInstance("goal_vis")
vis_origin_frame = mbp.AddFrame(
frame=FixedOffsetFrame(name="goal_vis_origin",
P=mbp.world_frame(),
X_PF=RigidTransform(
p=(goal_position +
np.array([0., 0.5, 0.])))))
body = mbp.AddRigidBody("goal_vis", model_instance, no_mass_no_inertia)
mbp.WeldFrames(vis_origin_frame, body.body_frame())
mbp.RegisterVisualGeometry(body, RigidTransform(), shape, "goal_vis",
[0.4, 0.9, 0.4, 0.35])
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 plot_mathematical_program(meshcat,
path,
prog,
X,
Y,
result=None,
point_size=0.05):
assert prog.num_vars() == 2
assert X.size == Y.size
N = X.size
values = np.vstack((X.reshape(-1), Y.reshape(-1)))
costs = prog.GetAllCosts()
# Vectorized multiply for the quadratic form.
# Z = (D*np.matmul(Q,D)).sum(0).reshape(nx, ny)
if costs:
Z = prog.EvalBindingVectorized(costs[0], values)
for b in costs[1:]:
Z = Z + prog.EvalBindingVectorized(b, values)
cv = f"{path}/constraints"
for binding in prog.GetAllConstraints():
if isinstance(binding.evaluator(), BoundingBoxConstraint):
c = binding.evaluator()
var_indices = [
int(prog.decision_variable_index()[v.get_id()])
for v in binding.variables()
]
satisfied = np.array(
c.CheckSatisfiedVectorized(values[var_indices, :],
0.001)).reshape(1, -1)
if costs:
Z[~satisfied] = np.nan
v = f"{cv}/{type(c).__name__}"
Zc = np.zeros(Z.shape)
Zc[satisfied] = np.nan
plot_surface(meshcat,
v,
X,
Y,
Zc.reshape(X.shape),
rgba=Rgba(1.0, .2, .2, 1.0),
wireframe=True)
else:
Zc = prog.EvalBindingVectorized(binding, values)
evaluator = binding.evaluator()
low = evaluator.lower_bound()
up = evaluator.upper_bound()
cvb = f"{cv}/{type(evaluator).__name__}"
for index in range(Zc.shape[0]):
# TODO(russt): Plot infeasible points in a different color.
infeasible = np.logical_or(Zc[index, :] < low[index],
Zc[index, :] > up[index])
plot_surface(meshcat,
f"{cvb}/{index}",
X,
Y,
Zc[index, :].reshape(X.shape),
rgba=Rgba(1.0, .3, 1.0, 1.0),
wireframe=True)
if costs:
plot_surface(meshcat,
f"{path}/objective",
X,
Y,
Z.reshape(X.shape),
rgba=Rgba(.3, 1.0, .3, 1.0),
wireframe=True)
if result:
v = f"{path}/solution"
meshcat.SetObject(v, Sphere(point_size), Rgba(.3, 1.0, .3, 1.0))
x_solution = result.get_x_val()
meshcat.SetTransform(
v,
RigidTransform(
[x_solution[0], x_solution[1],
result.get_optimal_cost()]))
