I'd also recommend making a more fleshed out problem. You could use a single conical pendulum or your 3 -link conical pendulum or any other simple problem that is 3D so that you really are able to test this class fully.