protected void setPinTransform3D(RigidBodyTransform t1, Vector3d u_i, double rotAng) { t1.setIdentity(); axisAngle.set(u_i, rotAng); t1.setRotation(axisAngle); // t1.setRotation(); }