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 test_pure_pursuit(self):
rg = make_two_lane_road()
lane = rg.junction(0).segment(0).lane(0)
pure_pursuit = PurePursuitController()
context = pure_pursuit.CreateDefaultContext()
output = pure_pursuit.AllocateOutput()
# Fix the inputs.
ld_value = framework.AbstractValue.Make(
LaneDirection(lane=lane, with_s=True))
lane_index = pure_pursuit.lane_input().get_index()
context.FixInputPort(lane_index, ld_value)
pos = [1., 2., 3.] # An aribtrary position with the lane.
pose_vector = PoseVector()
pose_vector.set_translation(pos)
pose_index = pure_pursuit.ego_pose_input().get_index()
context.FixInputPort(pose_index, pose_vector)
# Verify the inputs.
pose_vector_eval = pure_pursuit.EvalVectorInput(context, pose_index)
self.assertTrue(np.allclose(pose_vector.get_translation(),
pose_vector_eval.get_translation()))
# Verify the outputs.
pure_pursuit.CalcOutput(context, output)
command_index = pure_pursuit.steering_command_output().get_index()
steering = output.get_vector_data(command_index)
self.assertEqual(len(steering.get_value()), 1)
self.assertTrue(steering.get_value() < 0.)
def test_pure_pursuit(self):
rg = make_two_lane_road()
lane = rg.junction(0).segment(0).lane(0)
pure_pursuit = PurePursuitController()
context = pure_pursuit.CreateDefaultContext()
output = pure_pursuit.AllocateOutput()
# Fix the inputs.
ld_value = framework.AbstractValue.Make(
LaneDirection(lane=lane, with_s=True))
lane_index = pure_pursuit.lane_input().get_index()
context.FixInputPort(lane_index, ld_value)
pos = [1., 2., 3.] # An arbitrary position with the lane.
pose_vector = PoseVector()
pose_vector.set_translation(pos)
pose_index = pure_pursuit.ego_pose_input().get_index()
context.FixInputPort(pose_index, pose_vector)
# Verify the inputs.
pose_vector_eval = pure_pursuit.EvalVectorInput(context, pose_index)
self.assertTrue(np.allclose(pose_vector.get_translation(),
pose_vector_eval.get_translation()))
# Verify the outputs.
pure_pursuit.CalcOutput(context, output)
command_index = pure_pursuit.steering_command_output().get_index()
steering = output.get_vector_data(command_index)
self.assertEqual(len(steering.get_value()), 1)
self.assertTrue(steering.get_value() < 0.)
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 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 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_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.
with catch_drake_warnings(expected_count=1):
self.assertTrue(isinstance(value.get_isometry(), Isometry3))
self.assertTrue(isinstance(value.get_transform(), RigidTransform))
self.assertTrue(isinstance(
value.get_rotation(), Quaternion))
self.assertTrue(isinstance(
value.get_translation(), np.ndarray))
# - Value.
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value.get_isometry().matrix(), np.eye(4, 4)))
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), np.eye(4, 4)))
# - Mutators.
p = [0, 1, 2]
q = Quaternion(wxyz=normalized([0.1, 0.3, 0.7, 0.9]))
X_expected = RigidTransform(quaternion=q, p=p)
value.set_translation(p)
value.set_rotation(q)
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value.get_isometry().matrix(), X_expected.GetAsMatrix4()))
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4()))
# - 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)
with catch_drake_warnings(expected_count=1):
self.assertTrue(np.allclose(
value1.get_isometry().matrix(), X_expected.GetAsMatrix4()))
self.assertTrue(np.allclose(
value1.get_transform().GetAsMatrix4(), X_expected.GetAsMatrix4()))
# Test mutation via RigidTransform
p2 = [10, 20, 30]
q2 = Quaternion(wxyz=normalized([0.2, 0.3, 0.5, 0.8]))
X2_expected = RigidTransform(quaternion=q2, p=p2)
value.set_transform(X2_expected)
self.assertTrue(np.allclose(
value.get_transform().GetAsMatrix4(), X2_expected.GetAsMatrix4()))
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_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 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_transform(0, RigidTransform(quaternion=q3, p=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)
pose1_actual = RigidTransform(p=p1)
self.assertTrue((value.get_transform(0).GetAsMatrix34() ==
pose1_actual.GetAsMatrix34()).all())
pose2_actual = RigidTransform(p=p2)
self.assertTrue((value.get_transform(1).GetAsMatrix34() ==
pose2_actual.GetAsMatrix34()).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))
pose3_actual = RigidTransform(quaternion=q3, p=p3)
self.assertTrue((value.get_transform(2).GetAsMatrix34() ==
pose3_actual.GetAsMatrix34()).all())
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)
context.FixInputPort(0, pose1)
context.FixInputPort(1, pose2)
context.FixInputPort(2, velocity)
context.FixInputPort(3, 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())