@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setRotationYawAndZeroTranslation(rotation); transformToParent.setTranslation(translation); }
private static RigidBodyTransform createRandomTransform(Random random) { RigidBodyTransform transformReturn = new RigidBodyTransform(); RigidBodyTransform transformTemp = new RigidBodyTransform(); transformReturn.setRotationRollAndZeroTranslation(random.nextDouble()); transformTemp.setRotationPitchAndZeroTranslation(random.nextDouble()); transformReturn.multiply(transformTemp); transformTemp.setRotationYawAndZeroTranslation(random.nextDouble()); transformReturn.multiply(transformTemp); transformReturn.setTranslation(new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble())); return transformReturn; }
private static void setUpWall(CombinedTerrainObject3D combinedTerrainObject, double[] xy, double width, double length, double height, double yawDegrees, AppearanceDefinition app) { double x = xy[0]; double y = xy[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(x, y, height / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, length, width, height), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpFloatingStair(CombinedTerrainObject3D combinedTerrainObject, double[] centerPoint, double width, double tread, double thickness, double stairTopHeight, double yawDegrees, AppearanceDefinition app) { double xCenter = centerPoint[0]; double yCenter = centerPoint[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(xCenter, yCenter, stairTopHeight - thickness / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, tread, width, thickness), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpWall(CombinedTerrainObject3D combinedTerrainObject, double[] xy, double width, double length, double height, double yawDegrees, AppearanceDefinition app) { double x = xy[0]; double y = xy[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(x, y, height / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, length, width, height), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpWall(CombinedTerrainObject3D combinedTerrainObject, double[] xy, double width, double length, double height, double yawDegrees, AppearanceDefinition app) { double x = xy[0]; double y = xy[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(x, y, height / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, length, width, height), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpFloatingStair(CombinedTerrainObject3D combinedTerrainObject, double[] centerPoint, double width, double tread, double thickness, double stairTopHeight, double yawDegrees, AppearanceDefinition app) { double xCenter = centerPoint[0]; double yCenter = centerPoint[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(xCenter, yCenter, stairTopHeight - thickness / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, tread, width, thickness), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpWall(CombinedTerrainObject3D combinedTerrainObject, double[] xy, double width, double length, double height, double yawDegrees, AppearanceDefinition app) { double x = xy[0]; double y = xy[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(x, y, height / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, length, width, height), app); combinedTerrainObject.addTerrainObject(newBox); }
/** * Computes a node-to-world RigidBodyTransform from the node's x, y and yaw. This transform * will always have no z translation, pitch and roll. * @param node * @param nodeToWorldTransformToPack */ public static void getNodeTransform(FootstepNode node, RigidBodyTransform nodeToWorldTransformToPack) { double soleYaw = node.getYaw(); Point3D solePosition = new Point3D(node.getX(), node.getY(), 0.0); nodeToWorldTransformToPack.setRotationYawAndZeroTranslation(soleYaw); nodeToWorldTransformToPack.setTranslation(solePosition); }
private void setUpCinderBlock(CombinedTerrainObject3D combinedTerrainObject, double[] centerPoint, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; AppearanceDefinition app = cinderBlockAppearance; double xCenter = centerPoint[0]; double yCenter = centerPoint[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(xCenter, yCenter, cinderBlockHeight / 2 + numberFlatSupports * cinderBlockHeight)); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject(new Box3D(location, cinderBlockLength + overlapToPreventGaps, cinderBlockWidth + overlapToPreventGaps, cinderBlockHeight + overlapToPreventGaps), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpCinderBlock(CombinedTerrainObject3D combinedTerrainObject, double[] centerPoint, int numberFlatSupports, double yawDegrees) { if (numberFlatSupports < 0) return; AppearanceDefinition app = cinderBlockAppearance; double xCenter = centerPoint[0]; double yCenter = centerPoint[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3D(xCenter, yCenter, cinderBlockHeight / 2 + numberFlatSupports * cinderBlockHeight)); RotatableCinderBlockTerrainObject newBox = new RotatableCinderBlockTerrainObject( new Box3D(location, cinderBlockLength + overlapToPreventGaps, cinderBlockWidth + overlapToPreventGaps, cinderBlockHeight + overlapToPreventGaps), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3D(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }
@Test public void testExampleUsage() { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationYawAndZeroTranslation(Math.PI / 2.0); transform.setTranslation(new Vector3D(2.0, 0.0, 3.0)); // example use Ramp3D ramp3d = new Ramp3D(transform, 1.0, 1.0, 1.0); Point3D pointToCheck = new Point3D(2.0, 0.0, 4.0); assertFalse(ramp3d.isInsideOrOnSurface(pointToCheck)); assertEquals(Math.toRadians(45.0), ramp3d.getRampIncline(), 1e-7); }
private static void setUpSlopedBox(CombinedTerrainObject3D combinedTerrainObject, double xCenter, double yCenter, double zCenter, double xLength, double yLength, double zLength, double slopeRadians, double yawDegrees, AppearanceDefinition app) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); RigidBodyTransform tilt = new RigidBodyTransform(); tilt.setRotationPitchAndZeroTranslation(-slopeRadians); location.multiply(tilt); location.setTranslation(new Vector3D(xCenter, yCenter, zCenter)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, xLength, yLength, zLength), app); combinedTerrainObject.addTerrainObject(newBox); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newPose = new RigidBodyTransform(); pinJointTransform.setRotationYawAndZeroTranslation(steeringWheelPinJoint.getQYoVariable().getDoubleValue()); newPose.set(originalSteeringWheelPose); newPose.multiply(pinJointTransform); steeringWheelFrame.setPoseAndUpdate(newPose); super.updateAllGroundContactPointVelocities(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { poseOne.setToZero(frameOne); poseTwo.setToZero(frameTwo); poseOne.changeFrame(worldFrame); poseTwo.changeFrame(worldFrame); framePose.interpolate(poseOne, poseTwo, 0.5); transformToParent.setIdentity(); transformToParent.setRotationYawAndZeroTranslation(framePose.getYaw()); transformToParent.setTranslation(framePose.getPosition()); transformToParent.setTranslationZ(Math.min(poseOne.getZ(), poseTwo.getZ())); } }
private static void setupCamera(DRCSimulationTestHelper drcSimulationTestHelper) { OffsetAndYawRobotInitialSetup startingLocationOffset = location.getStartingLocationOffset(); Point3D cameraFocus = new Point3D(startingLocationOffset.getAdditionalOffset()); cameraFocus.addZ(1.0); RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationYawAndZeroTranslation(startingLocationOffset.getYaw()); Point3D cameraPosition = new Point3D(10.0, 5.0, cameraFocus.getZ() + 2.0); transform.transform(cameraPosition); drcSimulationTestHelper.setupCameraForUnitTest(cameraFocus, cameraPosition); }
private static void setupCamera(DRCSimulationTestHelper drcSimulationTestHelper) { OffsetAndYawRobotInitialSetup startingLocationOffset = location.getStartingLocationOffset(); Point3D cameraFocus = new Point3D(startingLocationOffset.getAdditionalOffset()); cameraFocus.addZ(1.0); RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationYawAndZeroTranslation(startingLocationOffset.getYaw()); Point3D cameraPosition = new Point3D(10.0, 5.0, cameraFocus.getZ() + 2.0); transform.transform(cameraPosition); drcSimulationTestHelper.setupCameraForUnitTest(cameraFocus, cameraPosition); }
private static void addNodeDataToFootstepPlan(FootstepPlan footstepPlan, FootstepNodeDataMessage nodeData) { RobotSide robotSide = RobotSide.fromByte(nodeData.getRobotSide()); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(nodeData.getYawIndex() * FootstepNode.gridSizeYaw); footstepPose.setTranslationX(nodeData.getXIndex() * FootstepNode.gridSizeXY); footstepPose.setTranslationY(nodeData.getYIndex() * FootstepNode.gridSizeXY); RigidBodyTransform snapTransform = new RigidBodyTransform(); snapTransform.set(nodeData.getSnapRotation(), nodeData.getSnapTranslation()); snapTransform.transform(footstepPose); footstepPlan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); }
@Test public void testSimpleMethodCalls() { Ramp3D ramp3d = new Ramp3D(1.0, 1.0, 1.0); // can apply transform and test a point that switches sides of the ramp RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationYawAndZeroTranslation(Math.PI / 2.0); Point3D p1 = new Point3D(new double[] {0.4, 0.2, 0.0}); Point3D p2 = new Point3D(new double[] {0.4, -0.2, 0.0}); assertTrue(ramp3d.isInsideOrOnSurface(p1)); assertTrue(ramp3d.isInsideOrOnSurface(p2)); ramp3d.applyTransform(transform); assertTrue(ramp3d.isInsideOrOnSurface(p1)); assertFalse(ramp3d.isInsideOrOnSurface(p2)); }