def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) self.assertTrue(isinstance(bundle.get_pose(0), Isometry3)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle.set_pose(kIndex, Isometry3(q, p)) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) self.assertTrue( (bundle.get_pose(kIndex).matrix() == Isometry3(q, p).matrix()).all()) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def test_pose_bundle(self): num_poses = 7 bundle = PoseBundle(num_poses) # - Accessors. self.assertEqual(bundle.get_num_poses(), num_poses) with catch_drake_warnings(expected_count=1): self.assertTrue(isinstance(bundle.get_pose(0), Isometry3)) self.assertTrue(isinstance(bundle.get_transform(0), RigidTransform)) self.assertTrue(isinstance(bundle.get_velocity(0), FrameVelocity)) # - Mutators. kIndex = 5 p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) with catch_drake_warnings(expected_count=2): bundle.set_pose(kIndex, Isometry3(q, p)) self.assertTrue((bundle.get_pose(kIndex).matrix() == Isometry3(q, p).matrix()).all()) pose = RigidTransform(quaternion=q, p=p) bundle.set_transform(kIndex, pose) self.assertTrue((bundle.get_transform(kIndex).GetAsMatrix34() == pose.GetAsMatrix34()).all()) w = [0.1, 0.3, 0.5] v = [0., 1., 2.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) bundle.set_velocity(kIndex, frame_velocity) vel_actual = bundle.get_velocity(kIndex).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) name = "Alice" bundle.set_name(kIndex, name) self.assertEqual(bundle.get_name(kIndex), name) instance_id = 42 # Supply a random instance id. bundle.set_model_instance_id(kIndex, instance_id) self.assertEqual(bundle.get_model_instance_id(kIndex), instance_id)
def test_joints_api(self): # Verify construction from both Isometry3d and 4x4 arrays, # and sanity-check that the accessors function. name = "z" prismatic_joint_np = PrismaticJoint(name, np.eye(4), np.array([0., 0., 1.])) prismatic_joint_isom = PrismaticJoint(name, Isometry3.Identity(), np.array([0., 0., 1.])) self.assertEqual(prismatic_joint_isom.get_num_positions(), 1) self.assertEqual(prismatic_joint_isom.get_name(), name) name = "theta" revolute_joint_np = RevoluteJoint(name, np.eye(4), np.array([0., 0., 1.])) revolute_joint_isom = RevoluteJoint(name, Isometry3.Identity(), np.array([0., 0., 1.])) self.assertEqual(revolute_joint_isom.get_num_positions(), 1) self.assertEqual(revolute_joint_isom.get_name(), name) name = "fixed" fixed_joint_np = FixedJoint(name, np.eye(4)) fixed_joint_isom = FixedJoint(name, Isometry3.Identity()) self.assertEqual(fixed_joint_isom.get_num_positions(), 0) self.assertEqual(fixed_joint_isom.get_name(), name) name = "rpy" rpy_floating_joint_np = RollPitchYawFloatingJoint( name, np.eye(4)) rpy_floating_joint_isom = RollPitchYawFloatingJoint( name, Isometry3.Identity()) self.assertEqual(rpy_floating_joint_isom.get_num_positions(), 6) self.assertEqual(rpy_floating_joint_isom.get_name(), name)
def test_AddMinimumDistanceConstraint(self): ik = self.ik_two_bodies W = self.plant.world_frame() B1 = self.body1_frame B2 = self.body2_frame min_distance = 0.1 tol = 1e-2 radius1 = 0.1 radius2 = 0.2 ik.AddMinimumDistanceConstraint(minimal_distance=min_distance) context = self.plant.CreateDefaultContext() R_I = np.eye(3) self.plant.SetFreeBodyPose(context, B1.body(), Isometry3(R_I, [0, 0, 0.01])) self.plant.SetFreeBodyPose(context, B2.body(), Isometry3(R_I, [0, 0, -0.01])) def get_min_distance_actual(): X = partial(self.plant.CalcRelativeTransform, context) distance = norm(X(W, B1).translation() - X(W, B2).translation()) return distance - radius1 - radius2 self.assertLess(get_min_distance_actual(), min_distance - tol) self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context)) result = self.prog.Solve() self.assertEqual(result, mp.SolutionResult.kSolutionFound) self.plant.SetPositions(context, self.prog.GetSolution(ik.q())) self.assertGreater(get_min_distance_actual(), min_distance - tol)
def test_pose_selector(self): kScanDistance = 4. rg = make_two_lane_road() lane = rg.junction(0).segment(0).lane(0) pose0 = PoseVector() pose0.set_translation([1., 0., 0.]) pose1 = PoseVector() # N.B. Set pose1 3 meters ahead of pose0. pose1.set_translation([4., 0., 0.]) bundle = PoseBundle(num_poses=2) bundle.set_pose(0, Isometry3(Quaternion(), pose0.get_translation())) bundle.set_pose(1, Isometry3(Quaternion(), pose1.get_translation())) closest_pose = PoseSelector.FindSingleClosestPose( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, side=AheadOrBehind.kAhead, path_or_branches=ScanStrategy.kPath) self.assertEqual(closest_pose.odometry.lane.id().string(), lane.id().string()) self.assertTrue(closest_pose.distance == 3.) closest_pair = PoseSelector.FindClosestPair( lane=lane, ego_pose=pose0, traffic_poses=bundle, scan_distance=kScanDistance, path_or_branches=ScanStrategy.kPath) self.assertEqual( closest_pair[AheadOrBehind.kAhead].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kAhead].distance, 3.) self.assertEqual( closest_pair[AheadOrBehind.kBehind].odometry.lane.id().string(), lane.id().string()) self.assertEqual(closest_pair[AheadOrBehind.kBehind].distance, float('inf')) lane_pos = LanePosition(s=1., r=0., h=0.) road_pos = RoadPosition(lane=lane, pos=lane_pos) w = [1., 2., 3.] v = [-4., -5., -6.] frame_velocity = FrameVelocity(SpatialVelocity(w=w, v=v)) road_odom = RoadOdometry(road_position=road_pos, frame_velocity=frame_velocity) sigma_v = PoseSelector.GetSigmaVelocity(road_odom) self.assertEqual(sigma_v, v[0])
def ComputeDoorPose(door_points, door_colors, model_path): """Finds a good 4x4 pose of the door from the segmented points. @param door_points An Nx3 numpy array of door points. @param door_colors An Nx3 numpy array of corresponding door colors. @return X_MS A 4x4 numpy array of the best-fit door pose. """ model_door = np.load(model_path) num_sample_points = min(door_points.shape[0], 250) X_MS, error = AlignSceneToModel(door_points, model_door, num_sample_points=num_sample_points) # if the best fit matrix isn't exactly an Isometry3, fix it try: Isometry3(X_MS) except: # make valid Isometry3 sin_th = X_MS[1, 0] cos_th = X_MS[0, 0] if sin_th > 0: theta = np.arccos(np.clip(cos_th, -1.0, 1.0)) else: theta = -np.arccos(np.clip(cos_th, -1.0, 1.0)) X_MS[0, 0] = np.cos(theta) X_MS[1, 1] = np.cos(theta) X_MS[0, 1] = -np.sin(theta) X_MS[1, 0] = np.sin(theta) return X_MS
def __init__(self, meshcat_viz, draw_period=_DEFAULT_PUBLISH_PERIOD, name="point_cloud", X_WP=Isometry3.Identity(), default_rgb=[255., 255., 255.]): """ Args: meshcat_viz: Either a native meshcat.Visualizer or a pydrake MeshcatVisualizer object. draw_period: The rate at which this class publishes to the visualizer. name: The string name of the meshcat object. X_WP: Pose of point cloud frame ``P`` in meshcat world frame ``W``. Default is identity. default_rgb: RGB value for published points if the PointCloud does not provide RGB values. """ LeafSystem.__init__(self) self._meshcat_viz = _get_native_visualizer(meshcat_viz) self._X_WP = X_WP self._default_rgb = np.array(default_rgb) self._name = name self.set_name('meshcat_point_cloud_visualizer_' + name) self.DeclarePeriodicPublish(draw_period, 0.0) self.DeclareAbstractInputPort("point_cloud_P", AbstractValue.Make(mut.PointCloud()))
def GetFoamBrickPose(brick_points, brick_colors): """Finds a good 4x4 pose of the brick from the segmented points. @param brick_points An Nx3 numpy array of brick points. @param brick_colors An Nx3 numpy array of corresponding brick colors. @return X_MS A 4x4 numpy array of the best-fit brick pose. """ model_brick = np.load("models/foam_brick_model.npy") num_sample_points = min(brick_points.shape[0], 250) X_MS, error = AlignSceneToModel( brick_points, model_brick, num_sample_points=num_sample_points) # if the best fit matrix isn't exactly an Isometry3, fix it try: Isometry3(X_MS) except: # make valid Isometry3 sin_th = X_MS[1, 0] cos_th = X_MS[0, 0] if sin_th > 0: theta = np.arccos(np.clip(cos_th, -1.0, 1.0)) else: theta = -np.arccos(np.clip(cos_th, -1.0, 1.0)) X_MS[0, 0] = np.cos(theta) X_MS[1, 1] = np.cos(theta) X_MS[0, 1] = -np.sin(theta) X_MS[1, 0] = np.sin(theta) return X_MS
def test_pose_vector(self): value = PoseVector() self.assertTrue(isinstance(value, BasicVector)) self.assertTrue(isinstance(copy.copy(value), PoseVector)) self.assertTrue(isinstance(value.Clone(), PoseVector)) self.assertEqual(value.size(), PoseVector.kSize) # - Accessors. self.assertTrue(isinstance(value.get_isometry(), Isometry3)) self.assertTrue(isinstance(value.get_rotation(), Quaternion)) self.assertTrue(isinstance(value.get_translation(), np.ndarray)) # - Value. self.assertTrue( np.allclose(value.get_isometry().matrix(), np.eye(4, 4))) # - Mutators. p = [0, 1, 2] q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) X_expected = Isometry3(q, p) value.set_translation(p) value.set_rotation(q) self.assertTrue( np.allclose(value.get_isometry().matrix(), X_expected.matrix())) # - Ensure ordering is ((px, py, pz), (qw, qx, qy, qz)) vector_expected = np.hstack((p, q.wxyz())) vector_actual = value.get_value() self.assertTrue(np.allclose(vector_actual, vector_expected)) # - Fully-parameterized constructor. value1 = PoseVector(rotation=q, translation=p) self.assertTrue( np.allclose(value1.get_isometry().matrix(), X_expected.matrix()))
def __init__(self, config_file, viz=False, segment_scene_function=None, get_pose_function=None): """ A system that takes in 3 Drake PointClouds and ImageRgba8U from RGBDCameras and determines the pose of an object in them. The user must supply a segmentation function and pose alignment to determine the pose. If these functions aren't supplied, the returned pose will always be the 4x4 identity matrix. @param config_file str. A path to a .yml configuration file for the cameras. @param viz bool. If True, save the aligned and segmented point clouds as serialized numpy arrays. @param segment_scene_function A Python function that returns a subset of the scene point cloud. See self.SegmentScene for more details. @param get_pose_function A Python function that calculates a pose from a segmented point cloud. See self.GetPose for more details. @system{ @input_port{camera_left_rgb}, @input_port{camera_middle_rgb}, @input_port{camera_right_rgb}, @input_port{left_point_cloud}, @input_port{middle_point_cloud}, @input_port{right_point_cloud}, @output_port{X_WObject} } """ LeafSystem.__init__(self) # TODO(kmuhlrad): Remove once Drake PointCloud object supports RGB # fields. self.left_rgb = self._DeclareAbstractInputPort( "camera_left_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.middle_rgb = self._DeclareAbstractInputPort( "camera_middle_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.right_rgb = self._DeclareAbstractInputPort( "camera_right_rgb", AbstractValue.Make(ImageRgba8U(848, 480))) self.left_depth = self._DeclareAbstractInputPort( "left_point_cloud", AbstractValue.Make(mut.PointCloud())) self.middle_depth = self._DeclareAbstractInputPort( "middle_point_cloud", AbstractValue.Make(mut.PointCloud())) self.right_depth = self._DeclareAbstractInputPort( "right_point_cloud", AbstractValue.Make(mut.PointCloud())) self._DeclareAbstractOutputPort( "X_WObject", lambda: AbstractValue.Make(Isometry3.Identity()), self._DoCalcOutput) self.segment_scene_function = segment_scene_function self.get_pose_function = get_pose_function self._LoadConfigFile(config_file) self.viz = viz
def GetEndEffectorWorldAlignedFrame(): X_EEa = Isometry3.Identity() X_EEa.set_rotation(np.array([[ 0., 1., 0, ], [0, 0, 1], [1, 0, 0]])) return X_EEa
def from_yaml(cls, d): """Creates from a dict (from YAML).""" return cls( name=d["name"], X_WF=Isometry3( rotation=d["X_WF"]["rotation"], translation=d["X_WF"]["translation"], ), )
def test_collision_element_api(self): # Verify construction from both Isometry3d and 4x4 arrays. box_element = shapes.Box([1.0, 1.0, 1.0]) box_collision_element_np = CollisionElement(box_element, np.eye(4)) box_collision_element_isom = CollisionElement( box_element, Isometry3.Identity()) body = RigidBody() box_collision_element_isom.set_body(body) self.assertEqual(box_collision_element_isom.get_body(), body)
def test_multibody_add_frame(self): plant = MultibodyPlant() frame = plant.AddFrame( frame=FixedOffsetFrame(name="frame", P=plant.world_frame(), X_PF=Isometry3.Identity(), model_instance=None)) self.assertIsInstance(frame, Frame) np.testing.assert_equal(np.eye(4), frame.GetFixedPoseInBodyFrame().matrix())
def test_visual_element_api(self): material_in = [0.3, 0.4, 0.5, 0.6] material_in_2 = [0.6, 0.7, 0.8, 0.9] box = shapes.Box(size=[1., 1., 1.]) visual_element_np = shapes.VisualElement(box, np.eye(4), material_in) visual_element_isom = shapes.VisualElement(box, Isometry3.Identity(), material_in) self.assertTrue( np.allclose(visual_element_np.getMaterial(), material_in)) visual_element_np.setMaterial(material_in_2) self.assertTrue( np.allclose(visual_element_np.getMaterial(), material_in_2))
def test_AddMinimumDistanceConstraint(self): ik = self.ik_two_bodies W = self.plant.world_frame() B1 = self.body1_frame B2 = self.body2_frame min_distance = 0.1 tol = 1e-2 radius1 = 0.1 radius2 = 0.2 ik.AddMinimumDistanceConstraint(minimal_distance=min_distance) context = self.plant.CreateDefaultContext() R_I = np.eye(3) self.plant.SetFreeBodyPose(context, B1.body(), Isometry3(R_I, [0, 0, 0.01])) self.plant.SetFreeBodyPose(context, B2.body(), Isometry3(R_I, [0, 0, -0.01])) def get_min_distance_actual(): X = partial(self.plant.CalcRelativeTransform, context) distance = norm(X(W, B1).translation() - X(W, B2).translation()) return distance - radius1 - radius2 self.assertLess(get_min_distance_actual(), min_distance - tol) self.prog.SetInitialGuess(ik.q(), self.plant.GetPositions(context)) result = mp.Solve(self.prog) self.assertTrue(result.is_success()) q_val = result.GetSolution(ik.q()) self.plant.SetPositions(context, q_val) self.assertGreater(get_min_distance_actual(), min_distance - tol) with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) self.assertEqual(self.prog.Solve(), mp.SolutionResult.kSolutionFound) self.assertTrue(np.allclose(self.prog.GetSolution(ik.q()), q_val)) self.assertEqual(len(w), 2)
def test_manipulation_station_add_iiwa_and_wsg_explicitly(self): station = ManipulationStation() parser = Parser(station.get_mutable_multibody_plant(), station.get_mutable_scene_graph()) plant = station.get_mutable_multibody_plant() # Add models for iiwa and wsg iiwa_model_file = FindResourceOrThrow( "drake/manipulation/models/iiwa_description/iiwa7/" "iiwa7_no_collision.sdf") iiwa = parser.AddModelFromFile(iiwa_model_file, "iiwa") X_WI = Isometry3.Identity() plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0", iiwa), X_WI) wsg_model_file = FindResourceOrThrow( "drake/manipulation/models/wsg_50_description/sdf/" "schunk_wsg_50.sdf") wsg = parser.AddModelFromFile(wsg_model_file, "gripper") X_7G = Isometry3.Identity() plant.WeldFrames(plant.GetFrameByName("iiwa_link_7", iiwa), plant.GetFrameByName("body", wsg), X_7G) # Register models for the controller. station.RegisterIiwaControllerModel( iiwa_model_file, iiwa, plant.world_frame(), plant.GetFrameByName("iiwa_link_0", iiwa), X_WI) station.RegisterWsgControllerModel( wsg_model_file, wsg, plant.GetFrameByName("iiwa_link_7", iiwa), plant.GetFrameByName("body", wsg), X_7G) # Finalize station.Finalize() # This WSG gripper model has 2 independent dof, and the IIWA model # has 7. self.assertEqual(plant.num_positions(), 9) self.assertEqual(plant.num_velocities(), 9)
def test_idm_controller(self): rg = make_two_lane_road() idm = IdmController( road=rg, path_or_branches=ScanStrategy.kPath, road_position_strategy=RoadPositionStrategy.kExhaustiveSearch, period_sec=0.) context = idm.CreateDefaultContext() output = idm.AllocateOutput() # Fix the inputs. pose_vector1 = PoseVector() pose_vector1.set_translation([1., 2., 3.]) ego_pose_index = idm.ego_pose_input().get_index() context.FixInputPort(ego_pose_index, pose_vector1) w = [0., 0., 0.] v = [1., 0., 0.] frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v)) ego_velocity_index = idm.ego_velocity_input().get_index() context.FixInputPort(ego_velocity_index, frame_velocity1) pose_vector2 = PoseVector() pose_vector2.set_translation([6., 0., 0.]) bundle = PoseBundle(num_poses=1) bundle.set_pose( 0, Isometry3(Quaternion(), pose_vector2.get_translation())) traffic_index = idm.traffic_input().get_index() context.FixInputPort(traffic_index, framework.AbstractValue.Make(bundle)) # Verify the inputs. pose_vector_eval = idm.EvalVectorInput(context, 0) self.assertTrue( np.allclose(pose_vector1.get_translation(), pose_vector_eval.get_translation())) frame_velocity_eval = idm.EvalVectorInput(context, 1) self.assertTrue( np.allclose(frame_velocity1.get_velocity().translational(), frame_velocity_eval.get_velocity().translational())) self.assertTrue( np.allclose(frame_velocity1.get_velocity().rotational(), frame_velocity_eval.get_velocity().rotational())) # Verify the outputs. idm.CalcOutput(context, output) accel_command_index = idm.acceleration_output().get_index() accel = output.get_vector_data(accel_command_index) self.assertEqual(len(accel.get_value()), 1) self.assertTrue(accel.get_value() < 0.)
def test_frame_pose_vector_api(self): obj = mut.FramePoseVector() frame_id = mut.FrameId.get_new_id() obj.set_value(id=frame_id, value=Isometry3.Identity()) self.assertEqual(obj.size(), 1) self.assertIsInstance(obj.value(id=frame_id), Isometry3) self.assertTrue(obj.has_id(id=frame_id)) self.assertIsInstance(obj.frame_ids(), list) self.assertIsInstance(obj.frame_ids()[0], mut.FrameId) obj.clear() self.assertEqual(obj.size(), 0) with catch_drake_warnings(expected_count=1): mut.FramePoseVector(source_id=mut.SourceId.get_new_id(), ids=[mut.FrameId.get_new_id()])
def test_multibody_add_joint(self): """ Tests joint constructors and `AddJoint`. """ instance_file = FindResourceOrThrow( "drake/examples/double_pendulum/models/double_pendulum.sdf") # Add different joints between multiple model instances. # TODO(eric.cousineau): Remove the multiple instances and use # programmatically constructed bodies once this API is exposed in # Python. num_joints = 2 plant = MultibodyPlant() parser = Parser(plant) instances = [] for i in range(num_joints + 1): instance = parser.AddModelFromFile(instance_file, "instance_{}".format(i)) instances.append(instance) proximal_frame = "base" distal_frame = "lower_link" joints = [ RevoluteJoint( name="revolve_things", frame_on_parent=plant.GetBodyByName(distal_frame, instances[1]).body_frame(), frame_on_child=plant.GetBodyByName(proximal_frame, instances[2]).body_frame(), axis=[0, 0, 1], damping=0.), WeldJoint( name="weld_things", parent_frame_P=plant.GetBodyByName(distal_frame, instances[0]).body_frame(), child_frame_C=plant.GetBodyByName(proximal_frame, instances[1]).body_frame(), X_PC=Isometry3.Identity()), ] for joint in joints: joint_out = plant.AddJoint(joint) self.assertIs(joint, joint_out) # The model is now complete. plant.Finalize() for joint in joints: self._test_joint_api(joint)
def test_mbp_overloads(self): file_name = FindResourceOrThrow( "drake/multibody/benchmarks/acrobot/acrobot.sdf") plant = MultibodyPlant(0.0) Parser(plant).AddModelFromFile(file_name) plant.Finalize() context = plant.CreateDefaultContext() frame = plant.GetFrameByName("Link2") parameters = mut.DifferentialInverseKinematicsParameters(2, 2) mut.DoDifferentialInverseKinematics(plant, context, np.zeros(6), frame, parameters) mut.DoDifferentialInverseKinematics(plant, context, Isometry3.Identity(), frame, parameters)
def test_rigid_transform(self): def check_equality(X_actual, X_expected_matrix): # TODO(eric.cousineau): Use `IsNearlyEqualTo`. self.assertIsInstance(X_actual, mut.RigidTransform) self.assertTrue( np.allclose(X_actual.GetAsMatrix4(), X_expected_matrix)) # - Constructors. X_I = np.eye(4) check_equality(mut.RigidTransform(), X_I) check_equality(mut.RigidTransform(other=mut.RigidTransform()), X_I) check_equality(copy.copy(mut.RigidTransform()), X_I) R_I = mut.RotationMatrix() p_I = np.zeros(3) rpy_I = mut.RollPitchYaw(0, 0, 0) quaternion_I = Quaternion.Identity() angle = np.pi * 0 axis = [0, 0, 1] angle_axis = AngleAxis(angle=angle, axis=axis) check_equality(mut.RigidTransform(R=R_I, p=p_I), X_I) check_equality(mut.RigidTransform(rpy=rpy_I, p=p_I), X_I) check_equality(mut.RigidTransform(quaternion=quaternion_I, p=p_I), X_I) check_equality(mut.RigidTransform(theta_lambda=angle_axis, p=p_I), X_I) check_equality(mut.RigidTransform(R=R_I), X_I) check_equality(mut.RigidTransform(p=p_I), X_I) # - Accessors, mutators, and general methods. X = mut.RigidTransform() X.set(R=R_I, p=p_I) X.SetFromIsometry3(pose=Isometry3.Identity()) check_equality(mut.RigidTransform.Identity(), X_I) self.assertIsInstance(X.rotation(), mut.RotationMatrix) X.set_rotation(R=R_I) self.assertIsInstance(X.translation(), np.ndarray) X.set_translation(p=np.zeros(3)) self.assertTrue(np.allclose(X.GetAsMatrix4(), X_I)) self.assertTrue(np.allclose(X.GetAsMatrix34(), X_I[:3])) self.assertIsInstance(X.GetAsIsometry3(), Isometry3) check_equality(X.inverse(), X_I) self.assertIsInstance( X.multiply(other=mut.RigidTransform()), mut.RigidTransform) self.assertIsInstance(X.multiply(p_BoQ_B=p_I), np.ndarray) if six.PY3: self.assertIsInstance( eval("X @ mut.RigidTransform()"), mut.RigidTransform) self.assertIsInstance(eval("X @ [0, 0, 0]"), np.ndarray)
def test_multibody_plant_construction_api(self): builder = DiagramBuilder() plant, scene_graph = AddMultibodyPlantSceneGraph(builder) spatial_inertia = SpatialInertia() body = plant.AddRigidBody(name="new_body", M_BBo_B=spatial_inertia) box = Box(width=0.5, depth=1.0, height=2.0) body_X_BG = Isometry3() body_friction = CoulombFriction(static_friction=0.6, dynamic_friction=0.5) plant.RegisterVisualGeometry(body=body, X_BG=body_X_BG, shape=box, name="new_body_visual", diffuse_color=[1., 0.64, 0.0, 0.5]) plant.RegisterCollisionGeometry(body=body, X_BG=body_X_BG, shape=box, name="new_body_collision", coulomb_friction=body_friction)
def test_pose_aggregator(self): aggregator = PoseAggregator() # - Set-up. instance_id1 = 5 # Supply a random instance id. port1 = aggregator.AddSingleInput("pose_only", instance_id1) self.assertEqual(port1.get_data_type(), PortDataType.kVectorValued) self.assertEqual(port1.size(), PoseVector.kSize) instance_id2 = 42 # Supply another random, but unique, id. ports2 = aggregator.AddSinglePoseAndVelocityInput( "pose_and_velocity", instance_id2) self.assertEqual(ports2.pose_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.pose_input_port.size(), PoseVector.kSize) self.assertEqual(ports2.velocity_input_port.get_data_type(), PortDataType.kVectorValued) self.assertEqual(ports2.velocity_input_port.size(), FrameVelocity.kSize) num_poses = 1 port3 = aggregator.AddBundleInput("pose_bundle", num_poses) self.assertEqual(port3.get_data_type(), PortDataType.kAbstractValued) # - CalcOutput. self.assertEqual( aggregator.get_output_port(0).get_data_type(), PortDataType.kAbstractValued) context = aggregator.CreateDefaultContext() output = aggregator.AllocateOutput() p1 = [0, 1, 2] pose1 = PoseVector() pose1.set_translation(p1) p2 = [5, 7, 9] pose2 = PoseVector() pose2.set_translation(p2) w = [0.3, 0.4, 0.5] v = [0.5, 0.6, 0.7] velocity = FrameVelocity() velocity.set_velocity(SpatialVelocity(w=w, v=v)) p3 = [50, 70, 90] q3 = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9])) bundle = PoseBundle(num_poses) bundle.set_pose(0, Isometry3(q3, p3)) bundle_value = AbstractValue.Make(bundle) aggregator.get_input_port(0).FixValue(context, pose1) aggregator.get_input_port(1).FixValue(context, pose2) aggregator.get_input_port(2).FixValue(context, velocity) aggregator.get_input_port(3).FixValue(context, bundle_value) aggregator.CalcOutput(context, output) value = output.get_data(0).get_value() self.assertEqual(value.get_num_poses(), 3) isom1_actual = Isometry3() isom1_actual.set_translation(p1) self.assertTrue( (value.get_pose(0).matrix() == isom1_actual.matrix()).all()) isom2_actual = Isometry3() isom2_actual.set_translation(p2) self.assertTrue( (value.get_pose(1).matrix() == isom2_actual.matrix()).all()) vel_actual = value.get_velocity(1).get_velocity() self.assertTrue(np.allclose(vel_actual.rotational(), w)) self.assertTrue(np.allclose(vel_actual.translational(), v)) self.assertTrue( (value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all())
def se3_from_xyz(xyz): return Isometry3(np.eye(3), xyz)
controller = builder.AddSystem(QuadrotorController(plant, y_traj, DURATION)) builder.Connect(controller.get_output_port(0), plant.get_input_port(0)) builder.Connect(plant.get_output_port(0), controller.get_input_port(0)) # Set up visualization and env in MeshCat from pydrake.geometry import Box from pydrake.common.eigen_geometry import Isometry3 from pydrake.all import AddMultibodyPlantSceneGraph from pydrake.multibody.tree import SpatialInertia, UnitInertia env_plant, scene_graph = AddMultibodyPlantSceneGraph(builder) for idx, object_ in enumerate(OBJECTS): sz, trans, rot = object_ body = env_plant.AddRigidBody( str(idx + 1), SpatialInertia(1.0, np.zeros(3), UnitInertia(1.0, 1.0, 1.0))) pos = Isometry3() pos.set_translation(trans) # np.array([0,1,1]) pos.set_rotation(rot) # rm([1,0,0], 90) env_plant.RegisterVisualGeometry(body, pos, Box(sz[0], sz[1], sz[2]), str(idx + 1) + 'v', np.array([1, 1, 1, 1]), scene_graph) env_plant.Finalize() plant.RegisterGeometry(scene_graph) builder.Connect(plant.get_geometry_pose_output_port(), scene_graph.get_source_pose_port(plant.source_id())) meshcat = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=None)) builder.Connect(scene_graph.get_pose_bundle_output_port(), meshcat.get_input_port(0)) # end setup for visualization
"path": "data/test_pbr_mats/Metal09/Metal09" }), (".*cupboard_body.*", { "material_type": "CC0_texture", "path": "data/test_pbr_mats/Wood15/Wood15" }), (".*top_and_bottom.*", { "material_type": "CC0_texture", "path": "data/test_pbr_mats/Wood15/Wood15" }), (".*cupboard.*door.*", { "material_type": "CC0_texture", "path": "data/test_pbr_mats/Wood08/Wood08" })], global_transform=Isometry3( translation=[0, 0, 0], quaternion=Quaternion(offset_quat_base)), out_prefix="/tmp/manipulation_station_ycb/")) builder.Connect(station.GetOutputPort("pose_bundle"), blender_cam.get_input_port(0)) if args.log_bbox: bbox_source = builder.AddSystem( BoundingBoxBundleYamlSource( args.log_bbox, publish_period=blender_cam.draw_period)) builder.Connect(bbox_source.get_output_port(0), blender_cam.get_input_port(1)) if args.log: def buildTrajectorySource(ts, knots):
def test_contact_force(self): """A block sitting on a table.""" object_file_path = FindResourceOrThrow( "drake/examples/manipulation_station/models/061_foam_brick.sdf") table_file_path = FindResourceOrThrow( "drake/examples/kuka_iiwa_arm/models/table/" "extra_heavy_duty_table_surface_only_collision.sdf") # T: tabletop frame. X_TObject = Isometry3.Identity() X_TObject.set_translation([0, 0, 0.2]) builder = DiagramBuilder() plant = MultibodyPlant(0.002) _, scene_graph = AddMultibodyPlantSceneGraph(builder, plant) object_model = Parser(plant=plant).AddModelFromFile(object_file_path) table_model = Parser(plant=plant).AddModelFromFile(table_file_path) # Weld table to world. plant.WeldFrames(A=plant.world_frame(), B=plant.GetFrameByName("link", table_model)) plant.AddForceElement(UniformGravityFieldElement()) plant.Finalize() # Add meshcat visualizer. viz = builder.AddSystem( MeshcatVisualizer(scene_graph, zmq_url=None, open_browser=False)) builder.Connect(scene_graph.get_pose_bundle_output_port(), viz.get_input_port(0)) # Add contact visualizer. contact_viz = builder.AddSystem( MeshcatContactVisualizer(meshcat_viz=viz, force_threshold=0, contact_force_scale=10, plant=plant)) contact_input_port = contact_viz.GetInputPort("contact_results") builder.Connect(plant.GetOutputPort("contact_results"), contact_input_port) builder.Connect(scene_graph.get_pose_bundle_output_port(), contact_viz.GetInputPort("pose_bundle")) diagram = builder.Build() diagram_context = diagram.CreateDefaultContext() mbp_context = diagram.GetMutableSubsystemContext( plant, diagram_context) X_WT = plant.CalcRelativeTransform(mbp_context, plant.world_frame(), plant.GetFrameByName("top_center")) plant.SetFreeBodyPose(mbp_context, plant.GetBodyByName("base_link", object_model), X_WT.multiply(X_TObject)) simulator = Simulator(diagram, diagram_context) simulator.set_publish_every_time_step(False) simulator.StepTo(1.0) contact_viz_context = (diagram.GetMutableSubsystemContext( contact_viz, diagram_context)) contact_results = contact_viz.EvalAbstractInput( contact_viz_context, contact_input_port.get_index()).get_value() self.assertGreater(contact_results.num_contacts(), 0) self.assertEqual(contact_viz._contact_key_counter, 4)
pc_to_pose.GetInputPort("camera_right_rgb")) # connect the XYZ point clouds to PointCloudToPoseSystem builder.Connect(left_dut.point_cloud_output_port(), pc_to_pose.GetInputPort("left_point_cloud")) builder.Connect(middle_dut.point_cloud_output_port(), pc_to_pose.GetInputPort("middle_point_cloud")) builder.Connect(right_dut.point_cloud_output_port(), pc_to_pose.GetInputPort("right_point_cloud")) diagram = builder.Build() simulator = Simulator(diagram) if not use_hardware: X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.get_mutable_multibody_plant().tree().SetFreeBodyPoseOrThrow( station.get_mutable_multibody_plant().GetBodyByName( "base_link", brick), X_WObject, station.GetMutableSubsystemContext( station.get_mutable_multibody_plant(), station_context)) context = diagram.GetMutableSubsystemContext(pc_to_pose, simulator.get_mutable_context()) # prints the pose of the brick, of type Isometry3 print pc_to_pose.GetOutputPort("X_WObject").Eval(context)
def GetDoorPose(config_file, viz=False, left_door_angle=0.0, right_door_angle=0.0, num_trials=1): """Estimates the pose of the foam brick in a ManipulationStation setup. @param config_file str. The path to a camera configuration file. @param viz bool. If True, save point clouds to numpy arrays. @return An Isometry3 representing the pose of the door. """ builder = DiagramBuilder() output = add_camera_system(builder, config_file, viz, build_station_real_world, build_station_simulation) if output[0]: # use_hardware is True use_hardware, station, pc_to_pose_left, pc_to_pose_right = output else: use_hardware, station, pc_to_pose_left, pc_to_pose_right, brick = output diagram = builder.Build() simulator = Simulator(diagram) if not use_hardware: X_WObject = Isometry3.Identity() X_WObject.set_translation([.6, 0, 0]) station_context = diagram.GetMutableSubsystemContext( station, simulator.get_mutable_context()) station.get_mutable_multibody_plant().tree().SetFreeBodyPoseOrThrow( station.get_mutable_multibody_plant().GetBodyByName( "base_link", brick), X_WObject, station.GetMutableSubsystemContext( station.get_mutable_multibody_plant(), station_context)) left_hinge_joint = station.get_mutable_multibody_plant( ).GetJointByName("left_door_hinge") left_hinge_joint.set_angle( station.GetMutableSubsystemContext( station.get_mutable_multibody_plant(), station_context), -left_door_angle) right_hinge_joint = station.get_mutable_multibody_plant( ).GetJointByName("right_door_hinge") right_hinge_joint.set_angle( station.GetMutableSubsystemContext( station.get_mutable_multibody_plant(), station_context), right_door_angle) left_context = diagram.GetMutableSubsystemContext( pc_to_pose_left, simulator.get_mutable_context()) right_context = diagram.GetMutableSubsystemContext( pc_to_pose_right, simulator.get_mutable_context()) # returns the pose of the brick, of type Isometry3 isometries = {"left_door": [], "right_door": []} for _ in range(num_trials): isometries["left_door"].append( pc_to_pose_left.GetOutputPort("X_WObject").Eval(left_context)) isometries["right_door"].append( pc_to_pose_right.GetOutputPort("X_WObject").Eval(right_context)) return isometries