def SetPose(self, pose):
"""
@param pose is an Isometry3.
"""
tf = RigidTransform(pose)
self.SetRPY(RollPitchYaw(tf.rotation()))
self.SetXYZ(tf.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(tf.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 draw(self, context):
"""Overrides base with the implementation."""
pose_bundle = self._pose_bundle_port.Eval(context)
view_dir = np.cross(self._T_VW[0, :3], self._T_VW[1, :3])
for frame_i in range(pose_bundle.get_num_poses()):
# SceneGraph currently sets the name in PoseBundle as
# "get_source_name::frame_name".
full_name = pose_bundle.get_name(frame_i)
model_id = pose_bundle.get_model_instance_id(frame_i)
X_WB = RigidTransform(pose_bundle.get_pose(frame_i))
patch_Wlist, _ = self._get_view_patches(full_name, X_WB)
for i, patch_W in enumerate(patch_Wlist):
# Project the object vertices from 3d in world frame W to 2d in
# view frame V (keeps homogeneous portion, removing it later).
patch_V = self._T_VW @ patch_W
# Applies normalization in the perspective transformation
# to make each projected point have z = 1. If the bottom row
# of T_VW is [0, 0, 0, 1], this will result in an
# orthographic projection.
patch_V[0, :] /= patch_V[2, :]
patch_V[1, :] /= patch_V[2, :]
# Cut patch_V down to 2xN.
patch_V = patch_V[:2, :]
# Take a convex hull to get an accurate shape for drawing,
# with verts coming out in ccw order.
if patch_V.shape[1] > 3:
hull = spatial.ConvexHull(patch_V.T)
patch_V = np.vstack([patch_V[:, v]
for v in hull.vertices]).T
body_fill = self._body_fill_dict[full_name][i]
n_verts = body_fill.get_path().vertices.shape[0]
# Update the verts, padding out to the appropriate full # of
# verts by replicating the final vertex.
patch_V = np.pad(patch_V,
((0, 0), (0, n_verts - patch_V.shape[1])),
mode="edge")
body_fill.get_path().vertices[:, :] = patch_V.T
body_fill.zorder = X_WB.translation() @ view_dir
self.ax.set_title('t = {:.1f}'.format(context.get_time()))
def draw(self, context):
"""Overrides base with the implementation."""
pose_bundle = self._pose_bundle_port.Eval(context)
view_dir = np.cross(self._T_VW[0, :3], self._T_VW[1, :3])
for frame_i in range(pose_bundle.get_num_poses()):
# SceneGraph currently sets the name in PoseBundle as
# "get_source_name::frame_name".
full_name = pose_bundle.get_name(frame_i)
model_id = pose_bundle.get_model_instance_id(frame_i)
X_WB = RigidTransform(pose_bundle.get_pose(frame_i))
patch_Wlist, _ = self._get_view_patches(full_name, X_WB)
for i, patch_W in enumerate(patch_Wlist):
# Project the object vertices from 3d in world frame W to 2d in
# view frame V (keeps homogeneous portion, removing it later).
patch_V = self._project_patch(patch_W)
body_fill = self._body_fill_dict[full_name][i]
# Use the latest vertices to update the body_fill.
self._update_body_fill_verts(body_fill, patch_V)
body_fill.zorder = X_WB.translation() @ view_dir
self.ax.set_title('t = {:.1f}'.format(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()))
X_GprepickGpreplace = X_G["prepick"].inverse().multiply(X_G["preplace"])
angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis()
X_GprepickGclearance = RigidTransform(AngleAxis(angle=angle_axis.angle() / 2.0, axis=angle_axis.axis()),
X_GprepickGpreplace.translation() / 2.0 + np.array([0, 0.0, -0.3]))
X_G["clearance"] = X_G["prepick"].multiply(X_GprepickGclearance)
# Precise timings of trajectory
times = {"initial": 0}
X_GinitialGprepick = X_G["initial"].inverse().multiply(X_G["prepick"])
times["prepick"] = times["initial"] + 10.0 * np.linalg.norm(X_GinitialGprepick.translation())
# Allow some time for gripper to close
times["pick_start"] = times["prepick"] + 2.0
times["pick_end"] = times["pick_start"] + 2.0
times["postpick"] = times["pick_end"] + 2.0
time_to_from_clearance = 10.0 * np.linalg.norm(X_GprepickGclearance.translation())
times["clearance"] = times["postpick"] + time_to_from_clearance
times["preplace"] = times["clearance"] + time_to_from_clearance
times["place_start"] = times["preplace"] + 2.0
times["place_end"] = times["place_start"] + 2.0
times["postplace"] = times["place_end"] + 2.0
traj_p_G = make_gripper_position_trajectory(X_G, times)
traj_R_G = make_gripper_orientation_trajectory(X_G, times)
traj_h = make_finger_trajectory(times)
controller_plant = MultibodyPlant(time_step)
controller_parser = Parser(controller_plant)
control_only_panda = controller_parser.AddModelFromFile(panda_arm_hand_file)
controller_plant.WeldFrames(controller_plant.world_frame(), controller_plant.GetFrameByName("panda_link0", control_only_panda), X_AB=RigidTransform(np.array([0.0, 0.5, 0.0])))
