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