Add Point3dToMatrix method to JamaU
Display goal in JArm2D
Add getArmEndPointMatrix to use only Matrix out of the GUI