def test_frame_velocity(self):
frame_velocity = FrameVelocity()
self.assertTrue(isinstance(frame_velocity, BasicVector))
self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity))
self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity))
self.assertEqual(frame_velocity.size(), FrameVelocity.kSize)
# - Accessors.
self.assertTrue(
isinstance(frame_velocity.get_velocity(), SpatialVelocity))
# - Value.
self.assertTrue(
np.allclose(frame_velocity.get_velocity().rotational(), 3 * [0.]))
self.assertTrue(
np.allclose(frame_velocity.get_velocity().translational(),
3 * [0.]))
# - Mutators.
w = [0.1, 0.3, 0.5]
v = [0., 1., 2.]
frame_velocity.set_velocity(SpatialVelocity(w=w, v=v))
self.assertTrue(
np.allclose(frame_velocity.get_velocity().rotational(), w))
self.assertTrue(
np.allclose(frame_velocity.get_velocity().translational(), v))
# - Ensure ordering is ((wx, wy, wz), (vx, vy, vz))
velocity_expected = np.hstack((w, v))
velocity_actual = frame_velocity.get_value()
self.assertTrue(np.allclose(velocity_actual, velocity_expected))
# - Fully-parameterized constructor.
frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
self.assertTrue(
np.allclose(frame_velocity1.get_velocity().rotational(), w))
self.assertTrue(
np.allclose(frame_velocity1.get_velocity().translational(), v))
def test_frame_velocity(self):
frame_velocity = FrameVelocity()
self.assertTrue(isinstance(frame_velocity, BasicVector))
self.assertTrue(isinstance(copy.copy(frame_velocity), FrameVelocity))
self.assertTrue(isinstance(frame_velocity.Clone(), FrameVelocity))
self.assertEqual(frame_velocity.size(), FrameVelocity.kSize)
# - Accessors.
self.assertTrue(isinstance(
frame_velocity.get_velocity(), SpatialVelocity))
# - Value.
self.assertTrue(np.allclose(
frame_velocity.get_velocity().rotational(), 3 * [0.]))
self.assertTrue(np.allclose(
frame_velocity.get_velocity().translational(), 3 * [0.]))
# - Mutators.
w = [0.1, 0.3, 0.5]
v = [0., 1., 2.]
frame_velocity.set_velocity(SpatialVelocity(w=w, v=v))
self.assertTrue(np.allclose(
frame_velocity.get_velocity().rotational(), w))
self.assertTrue(np.allclose(
frame_velocity.get_velocity().translational(), v))
# - Ensure ordering is ((wx, wy, wz), (vx, vy, vz))
velocity_expected = np.hstack((w, v))
velocity_actual = frame_velocity.get_value()
self.assertTrue(np.allclose(velocity_actual, velocity_expected))
# - Fully-parameterized constructor.
frame_velocity1 = FrameVelocity(SpatialVelocity(w=w, v=v))
self.assertTrue(np.allclose(
frame_velocity1.get_velocity().rotational(), w))
self.assertTrue(np.allclose(
frame_velocity1.get_velocity().translational(), v))
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())