def SetPose(self, pose):
"""
@param pose is an Isometry3.
"""
tf = RigidTransform(pose)
self.SetRPY(RollPitchYaw(tf.rotation()))
self.SetXYZ(pose.translation())
def SetPose(self, pose):
"""
@param pose is an Isometry3.
"""
tf = RigidTransform(pose)
self.SetRPY(RollPitchYaw(tf.rotation()))
self.SetXYZ(pose.translation())
def SetPose(self, pose):
"""
@param pose is a RigidTransform or else any type accepted by
RigidTransform's constructor
"""
tf = RigidTransform(pose)
self.SetRPY(RollPitchYaw(tf.rotation()))
self.SetXYZ(pose.translation())
def SetPose(self, pose):
"""
Sets the current value of the sliders.
Args:
pose: Any viable argument for the RigidTransform
constructor.
"""
tf = RigidTransform(pose)
self.SetRpy(RollPitchYaw(tf.rotation()))
self.SetXyz(tf.translation())
def _DoPublish(self, context, event):
# TODO(russt): Change this to declare a periodic event with a
# callback instead of overriding _DoPublish, pending #9992.
LeafSystem._DoPublish(self, context, event)
if context.get_time() <= 1E-3:
return
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
for frame_i in range(pose_bundle.get_num_poses()):
link_name = pose_bundle.get_name(frame_i)
[source_name, frame_name] = self._parse_name(link_name)
model_id = pose_bundle.get_model_instance_id(frame_i)
# pose_matrix = pose_bundle.get_pose(frame_i)
pose_matrix = pose_bundle.get_transform(frame_i)
for j in range(self.num_link_geometries_by_link_name[link_name]):
offset = RigidTransform(
self.global_transform.GetAsMatrix4().dot(
# pose_matrix.matrix().dot(
pose_matrix.GetAsMatrix4().dot(
self.link_subgeometry_local_tfs_by_link_name[
link_name][j])))
location = offset.translation()
quaternion = offset.rotation().ToQuaternion()
geom_name = self._format_geom_name(source_name, frame_name,
model_id, j)
self.bsi.send_remote_call(
"update_object_parameters",
name="obj_" + geom_name,
location=location.tolist(),
rotation_mode='QUATERNION',
rotation_quaternion=quaternion.tolist())
n_cams = len(self.camera_tfs)
for i in range(len(self.camera_tfs)):
out_filepath = self.out_prefix + "%02d_%08d_label.bmp" % (
i, self.current_publish_num)
im = self.bsi.render_image("cam_%d" % i, filepath=out_filepath)
if self.show_figure:
plt.subplot(1, n_cams, i + 1)
plt.imshow(np.transpose(im, [1, 0, 2]))
if self.save_scene:
scene_filepath = self.out_prefix + "_scene.blend"
self.bsi.send_remote_call("save_current_scene",
path=scene_filepath)
self.current_publish_num += 1
if self.show_figure:
plt.pause(0.01)
print("Rendered a frame at %f seconds sim-time." %
(context.get_time()))
def _write_pose_msg(X_AB, p, q):
# p - position message
# q - quaternion message
X_AB = RigidTransform(X_AB)
p.x, p.y, p.z = X_AB.translation()
q.w, q.x, q.y, q.z = X_AB.rotation().ToQuaternion().wxyz()
def DoPublish(self, context, event):
# TODO(russt): Change this to declare a periodic event with a
# callback instead of overriding _DoPublish, pending #9992.
LeafSystem.DoPublish(self, context, event)
print("In publish at time ", context.get_time())
if context.get_time() <= 1E-3:
return
pose_bundle = self.EvalAbstractInput(context, 0).get_value()
bbox_bundle = self.EvalAbstractInput(context, 1)
if bbox_bundle:
bbox_bundle = bbox_bundle.get_value()
for k in range(bbox_bundle.get_num_bboxes()):
pose = self.global_transform.multiply(
bbox_bundle.get_bbox_pose(k))
if (k + 1) > self.num_registered_bounding_boxes:
# Register a new one
color = bbox_bundle.get_bbox_color(k)
self.bsi.send_remote_call(
"register_material",
name="mat_bb_%d" % k,
material_type="color",
color=bbox_bundle.get_bbox_color(k))
self.bsi.send_remote_call("update_material_parameters",
type="Principled BSDF",
name="mat_bb_%d" % k,
**{"Specular": 0.0})
self.bsi.send_remote_call("update_material_parameters",
type=None,
name="mat_bb_%d" % k,
**{"blend_method": "ADD"})
self.bsi.send_remote_call(
"register_object",
name="obj_bb_%d" % k,
type="cube",
scale=[x / 2 for x in bbox_bundle.get_bbox_scale(k)],
location=pose.translation().tolist(),
quaternion=pose.quaternion().wxyz().tolist(),
material="mat_bb_%d" % k)
self.bsi.send_remote_call("apply_modifier_to_object",
name="obj_bb_%d" % k,
type="WIREFRAME",
thickness=0.1)
self.num_registered_bounding_boxes = k + 1
self.num_visible_bounding_boxes = k + 1
else:
# Update parameters of existing bbox + its material
self.bsi.send_remote_call(
"update_material_parameters",
type="Principled BSDF",
name="mat_bb_%d" % k,
**{"Base Color": bbox_bundle.get_bbox_color(k)})
self.bsi.send_remote_call(
"update_object_parameters",
name="obj_bb_%d" % k,
scale=[x / 2 for x in bbox_bundle.get_bbox_scale(k)],
location=pose.translation().tolist(),
rotation_mode='QUATERNION',
rotation_quaternion=pose.rotation().ToQuaternion(
).wxyz().tolist(),
hide_render=False)
self.num_visible_bounding_boxes = k + 1
for k in range(bbox_bundle.get_num_bboxes(),
self.num_visible_bounding_boxes):
self.bsi.send_remote_call("update_object_parameters",
name="obj_bb_%d" % k,
hide_render=True)
# print('pose_bundle type', type(pose_bundle))
for frame_i in range(pose_bundle.get_num_poses()):
link_name = pose_bundle.get_name(frame_i)
# print('link_name', link_name)
[source_name, frame_name] = self._parse_name(link_name)
model_id = pose_bundle.get_model_instance_id(frame_i)
# pose_matrix = pose_bundle.get_pose(frame_i)
pose_matrix = pose_bundle.get_transform(frame_i)
for j in range(self.num_link_geometries_by_link_name[link_name]):
# I am sure this can be written shorter than this, this bloat is from
# when this was isometry-based.
offset = RigidTransform(
self.global_transform.GetAsMatrix4().dot(
# pose_matrix.matrix().dot(
pose_matrix.GetAsMatrix4().dot(
self.link_subgeometry_local_tfs_by_link_name[
link_name][j])))
location = offset.translation()
quaternion = offset.rotation().ToQuaternion().wxyz()
geom_name = self._format_geom_name(source_name, frame_name, j)
self.bsi.send_remote_call(
"update_object_parameters",
name="obj_" + geom_name,
location=location.tolist(),
rotation_mode='QUATERNION',
rotation_quaternion=quaternion.tolist())
n_cams = len(self.camera_tfs)
for i in range(len(self.camera_tfs)):
out_filepath = self.out_prefix + "%02d_%08d.jpg" % (
i, self.current_publish_num)
self.last_out_filepath = out_filepath
self.bsi.send_remote_call("render",
camera_name="cam_%d" % i,
filepath=out_filepath)
if self.save_scene:
scene_filepath = self.out_prefix + "_scene.blend"
self.bsi.send_remote_call("save_current_scene",
path=scene_filepath)
self.current_publish_num += 1
if self.show_figure:
plt.pause(0.01)
print("Rendered a frame at %f seconds sim-time." %
(context.get_time()))
