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()))