def AddMeshcatTriad(meshcat,
path,
length=.25,
radius=0.01,
opacity=1.,
X_PT=RigidTransform()):
meshcat.SetTransform(path, X_PT)
# x-axis
X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2),
[length / 2., 0, 0])
meshcat.SetTransform(path + "/x-axis", X_TG)
meshcat.SetObject(path + "/x-axis", Cylinder(radius, length),
Rgba(1, 0, 0, opacity))
# y-axis
X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2),
[0, length / 2., 0])
meshcat.SetTransform(path + "/y-axis", X_TG)
meshcat.SetObject(path + "/y-axis", Cylinder(radius, length),
Rgba(0, 1, 0, opacity))
# z-axis
X_TG = RigidTransform([0, 0, length / 2.])
meshcat.SetTransform(path + "/z-axis", X_TG)
meshcat.SetObject(path + "/z-axis", Cylinder(radius, length),
Rgba(0, 0, 1, opacity))
def add_triad(
source_id,
frame_id,
scene_graph,
*,
length,
radius,
opacity,
X_FT=RigidTransform(),
name="frame",
):
"""
Adds illustration geometry representing the coordinate frame, with the
x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes
point in +x, +y and +z directions, respectively.
Based on [code permalink](https://github.com/RussTedrake/manipulation/blob/5e59811/manipulation/scenarios.py#L367-L414).# noqa
Args:
source_id: The source registered with SceneGraph.
frame_id: A geometry::frame_id registered with scene_graph.
scene_graph: The SceneGraph with which we will register the geometry.
length: the length of each axis in meters.
radius: the radius of each axis in meters.
opacity: the opacity of the coordinate axes, between 0 and 1.
X_FT: a RigidTransform from the triad frame T to the frame_id frame F
name: the added geometry will have names name + " x-axis", etc.
"""
# x-axis
X_TG = RigidTransform(
RotationMatrix.MakeYRotation(np.pi / 2),
[length / 2.0, 0, 0],
)
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " x-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([1, 0, 0, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# y-axis
X_TG = RigidTransform(
RotationMatrix.MakeXRotation(np.pi / 2),
[0, length / 2.0, 0],
)
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " y-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 1, 0, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# z-axis
X_TG = RigidTransform([0, 0, length / 2.0])
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " z-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 0, 1, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
def AddTriad(source_id,
frame_id,
scene_graph,
length=.25,
radius=0.01,
opacity=1.,
X_FT=RigidTransform(),
name="frame"):
"""
Adds illustration geometry representing the coordinate frame, with the
x-axis drawn in red, the y-axis in green and the z-axis in blue. The axes
point in +x, +y and +z directions, respectively.
Args:
source_id: The source registered with SceneGraph.
frame_id: A geometry::frame_id registered with scene_graph.
scene_graph: The SceneGraph with which we will register the geometry.
length: the length of each axis in meters.
radius: the radius of each axis in meters.
opacity: the opacity of the coordinate axes, between 0 and 1.
X_FT: a RigidTransform from the triad frame T to the frame_id frame F
name: the added geometry will have names name + " x-axis", etc.
"""
# x-axis
X_TG = RigidTransform(RotationMatrix.MakeYRotation(np.pi / 2),
[length / 2., 0, 0])
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " x-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([1, 0, 0, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# y-axis
X_TG = RigidTransform(RotationMatrix.MakeXRotation(np.pi / 2),
[0, length / 2., 0])
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " y-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 1, 0, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
# z-axis
X_TG = RigidTransform([0, 0, length / 2.])
geom = GeometryInstance(X_FT.multiply(X_TG), Cylinder(radius, length),
name + " z-axis")
geom.set_illustration_properties(
MakePhongIllustrationProperties([0, 0, 1, opacity]))
scene_graph.RegisterGeometry(source_id, frame_id, geom)
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_rope_and_ground(self, include_ground=True):
if include_ground:
ground_model = add_ground(self.mbp, self.sg)
self._model_ids["ground"] = ground_model
for rope_name, rope_config in iteritems(self._config['env']['ropes']):
X_W = transform_from_dict(rope_config)
rope_model = add_rope(self.mbp, self.sg, self._config['rope'], X_W, rope_name)
self._model_names_to_mask.append(rope_name)
self._model_ids[rope_name] = rope_model
link_length = np.linalg.norm(np.subtract(self._config["env"]["ropes"]["rope_2"]["pos"], self._config["env"]["ropes"]["rope_1"]["pos"]))
lx, ly, lz = self._config["env"]["ropes"]["rope_1"]["pos"]
rx, ry, rz = self._config["env"]["ropes"]["rope_2"]["pos"]
# TODO: not completely right
roll = math.pi / 2 + math.atan2(rz - lz, ry - ly)
pitch = 0
yaw = -math.atan2(rx - lx, ry - ly)
link_I = SpatialInertia(mass=1,
p_PScm_E=np.array([0, 0, 0]),
G_SP_E=UnitInertia(1, 1, 1))
link_name = "middle_link"
link_body = self._mbp.AddRigidBody(link_name, rope_model, link_I)
cylinder_geom = Cylinder(self._config["rope"]["rope_radius"], link_length)
X = RigidTransform()
self._mbp.RegisterVisualGeometry(link_body, X, cylinder_geom,
"middle_vis1", [0.5, 0.5, 0.5, 0.5])
self._mbp.RegisterCollisionGeometry(link_body, X, cylinder_geom,
"middle_collision",
CoulombFriction(0.0, 0.0))
self._mbp.WeldFrames(self._mbp.world_frame(), self._mbp.GetFrameByName(f"middle_link", rope_model), RigidTransform(RollPitchYaw(roll, pitch, yaw), [(lx + rx) / 2.0, (ly + ry) / 2.0, (lz + rz) / 2.0]))
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 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 add_pusher(self):
"""
Adds a cylindrical pusher object
:return:
:rtype:
"""
mbp = self.mbp
parser = self.parser
pusher_model_idx = parser.AddModelFromFile(paths.xy_slide, "pusher")
base_link = mbp.GetBodyByName("base", pusher_model_idx)
# weld it to the world
mbp.WeldFrames(mbp.world_frame(), base_link.body_frame())
self.models['pusher'] = pusher_model_idx
radius = 0.01
length = 0.1
pusher_shape = Cylinder(radius, length)
# This rigid body will be added to the pusher model instance
world_body = mbp.world_body()
pusher_body = mbp.AddRigidBody(
"pusher_body", pusher_model_idx,
SpatialInertia(mass=10.0,
p_PScm_E=np.array([0., 0., 0.]),
G_SP_E=UnitInertia(1.0, 1.0, 1.0)))
self._rigid_bodies['pusher'] = pusher_body
# weld it to EE frame at particular height
translation = np.zeros(3)
translation[
2] = length / 2.0 + 0.001 # give it a little room for collision stuff
T_EE_P = RigidTransform(p=translation)
ee_link = mbp.GetBodyByName("end_effector", pusher_model_idx)
mbp.WeldFrames(ee_link.body_frame(), pusher_body.body_frame(), T_EE_P)
# color it green
color = np.array([0., 1., 0., 1.])
mbp.RegisterVisualGeometry(pusher_body, RigidTransform.Identity(),
pusher_shape, "pusher_vis", color)
mbp.RegisterCollisionGeometry(pusher_body, RigidTransform.Identity(),
pusher_shape, "pusher_collision",
CoulombFriction(0.9, 0.8))
def export_port_func():
# export relevant ports
actuation_input_port = mbp.get_actuation_input_port(
pusher_model_idx)
state_output_port = mbp.get_state_output_port(pusher_model_idx)
self._port_names["pusher_state_output"] = "pusher_state_output"
self._port_names[
"pusher_actuation_input"] = "pusher_actuation_input"
self.builder.ExportOutput(state_output_port,
self._port_names["pusher_state_output"])
self.builder.ExportInput(
actuation_input_port,
self._port_names["pusher_actuation_input"])
self._export_port_functions.append(export_port_func)