def OutputGeometryPose(self, context, poses):
assert self.body_frame_id.is_valid()
assert self.elevator_frame_id.is_valid()
lh = 0.317 # elevator hinge.
state = GliderState(self.get_input_port(0).Eval(context))
body_pose = RigidTransform(RotationMatrix.MakeYRotation(state.pitch),
[state.x, 0, state.z])
elevator_pose = RigidTransform(
RotationMatrix.MakeYRotation(state.pitch + state.elevator), [
state.x - lh * np.cos(state.pitch), 0,
state.z + lh * np.sin(state.pitch)
])
poses.get_mutable_value().set_value(self.body_frame_id, body_pose)
poses.get_mutable_value().set_value(self.elevator_frame_id,
elevator_pose)
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)