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