@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { floatingJoint.getTransformToWorld(transformToParent); } };
private synchronized void updateCurrentBox3d() { floatingJoint.getTransformToWorld(temporaryTransform3D); frameBox.setTransform(temporaryTransform3D); }
public void getBodyTransformToWorld(RigidBodyTransform transformToWorld) { getFloatingJoint().getTransformToWorld(transformToWorld); }
private synchronized void updateCurrentSphere3d() { floatingJoint.getTransformToWorld(temporaryTransform3D); currentSphere3d.set(originalSphere3d); currentSphere3d.applyTransform(temporaryTransform3D); } }
public void setFullRobotModelRootJointPositionAndOrientationToMatchRobot(FloatingJointBasics sixDoFJoint, FloatingJoint floatingJoint) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); floatingJoint.getTransformToWorld(transformToWorld); sixDoFJoint.setJointConfiguration(transformToWorld); }
cubeA.getTransformToWorld(transformToWorld);
root1.getTransformToWorld(transformToWorld); RigidBodyTransform transformToBody = new RigidBodyTransform(); transformToBody.setAndInvert(transformToWorld);
public void testBoxCloseButNoCollisions() { ScsCollisionDetector collisionDetector = createCollisionDetector(); FloatingJoint cubeA = cube(collisionDetector, "A", 10, 0.5, 1.0, 1.5); FloatingJoint cubeB = cube(collisionDetector, "B", 10, 0.75, 1.2, 1.7); double a[] = new double[] { 0.5 + 0.75, 1.0 + 1.2, 1.5 + 1.7 }; // add a bit of separation to ensure they don't collide double tau = 0.001; // should just barely not intersect for (int i = 0; i < 3; i++) { double Tx, Ty, Tz; Tx = Ty = Tz = 0.0; if (i == 0) Tx = a[i] / 2.0 + tau; if (i == 1) Ty = a[i] / 2.0 + tau; if (i == 2) Tz = a[i] / 2.0 + tau; cubeA.setPosition(Tx, Ty, Tz); cubeB.setPosition(-Tx, -Ty, -Tz); cubeA.getRobot().update(); cubeB.getRobot().update(); RigidBodyTransform transformToWorld = new RigidBodyTransform(); cubeA.getTransformToWorld(transformToWorld); CollisionDetectionResult result = new CollisionDetectionResult(); collisionDetector.performCollisionDetection(result); assertEquals(0, result.getNumberOfCollisions()); } }
floatingJoint.getTransformToWorld(positionAndRotation); sixDoFJoint.setJointConfiguration(positionAndRotation);
floatingJoint.getTransformToWorld(positionAndRotation); sixDoFJoint.setPositionAndRotation(positionAndRotation);