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
