/** * Translate this shape with the given (x, y, z) components that are expressed in the local * coordinates of this shape. * * @param x the x-component of the translation to append to this shape pose. * @param y the y-component of the translation to append to this shape pose. * @param z the z-component of the translation to append to this shape pose. */ public final void appendTranslation(double x, double y, double z) { shapePose.appendTranslation(x, y, z); }
/** * Translate this shape with the given {@code translation} that is expressed in the local * coordinates of this shape. * * @param translation the translation to append to the pose of this shape. Not modified. */ public final void appendTranslation(Tuple3DReadOnly translation) { shapePose.appendTranslation(translation); }
@Override public void updateRighdBodyTransform() { RigidBodyTransform rigidbodyTransform = referenceFrame.getTransformToWorldFrame(); rigidbodyTransform.appendTranslation(translationToCenter); transform = rigidbodyTransform; }
private TerrainObject3D createBollard(int row, int column, Point2D positionInGrid, double height, double radius) { AppearanceDefinition bollardAppearance = YoAppearance.Yellow(); RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(getWorldCoordinate(row, column)); location.appendTranslation(positionInGrid.getX(), positionInGrid.getY(), height / 2); return new CylinderTerrainObject(location, height, radius, bollardAppearance); }
private TerrainObject3D setUpFlatGrid(int row, int column) { AppearanceDefinition gridAppearance = new YoAppearanceTexture("Textures/asphalt.png"); RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(getWorldCoordinate(row, column)); location.appendTranslation(0.0, 0.0, -flatGridHeight / 2); return new RotatableBoxTerrainObject(new Box3D(location, gridLength, gridWidth, flatGridHeight), gridAppearance); }
private TerrainObject3D createBump(int row, int column, Point2D positionInGrid, double height, double run, double width, double rotateYaw) { AppearanceDefinition bumpAppearance = YoAppearance.DarkGrey(); double radius = height / 2 + run * run / 8 / height; double alpha = Math.asin(run / 2 / radius); double depth = radius * Math.cos(alpha); RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(getWorldCoordinate(row, column)); location.appendTranslation(positionInGrid.getX(), positionInGrid.getY(), -depth); location.appendYawRotation(rotateYaw); location.appendPitchRotation(Math.PI / 2); return new CylinderTerrainObject(location, width - bumpSideMargin, radius, bumpAppearance); }
private CombinedTerrainObject3D setUpStormGrate(int row, int column) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D("StormGrate"); AppearanceDefinition crossLineAppearance = YoAppearance.DarkGrey(); double depthCrossLine = 0.05; double thickCrossLine = 0.02; int numberOfCrossLines = 15; double intervalOfCrossLines = gridLength / (numberOfCrossLines + 1); for (int i = 0; i < numberOfCrossLines; i++) { RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(getWorldCoordinate(row, column)); location.appendTranslation(gridLength / 2 - intervalOfCrossLines * (i + 1), 0.0, -depthCrossLine / 2); Box3D crossLine = new Box3D(location, thickCrossLine, gridWidth, depthCrossLine); combinedTerrainObject.addRotatableBox(crossLine, crossLineAppearance); } return combinedTerrainObject; }
@Override public Graphics3DObject getGraphicObject() { RigidBodyTransform rigidbodyTransform = transform; Point3D translationToCreate = new Point3D(0, 0, -sizeZ / 2); rigidbodyTransform.appendTranslation(translationToCreate); graphicObject.transform(rigidbodyTransform); AppearanceDefinition app; app = YoAppearance.Yellow(); graphicObject.addCube(sizeX, sizeY, sizeZ, app); return graphicObject; }
public static CombinedTerrainObject3D setUpGround(String name) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); int groundDimension = 20; double unitLength = 1.0; double groundThickness = 1.0; YoAppearanceTexture texture = new YoAppearanceTexture("Textures/brick.png"); for (int i = 0; i < groundDimension; i++) { for (int j = 0; j < groundDimension; j++) { RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(new Vector3D(groundDimension / 2 * unitLength, groundDimension / 2 * unitLength, -groundThickness / 2)); location.appendTranslation(new Vector3D(-unitLength * (i + 0.5), -unitLength * (j + 0.5), 0.0)); RotatableBoxTerrainObject unitBox = new RotatableBoxTerrainObject(new Box3D(location, unitLength, unitLength, groundThickness), texture); combinedTerrainObject.addTerrainObject(unitBox); } } return combinedTerrainObject; }
private CombinedTerrainObject3D setUpGround(String name) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); int lateralDimension = 15; int sideDimension = 10; double leteralOffset = 2.0; double unitLength = 1.0; double groundThickness = 0.3; YoAppearanceTexture texture = new YoAppearanceTexture("Textures/brick.png"); for (int i = 0; i < lateralDimension; i++) { for (int j = 0; j < sideDimension; j++) { RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(new Vector3D(lateralDimension / 2 * unitLength + leteralOffset, sideDimension / 2 * unitLength, -groundThickness / 2)); location.appendTranslation(new Vector3D(-unitLength * (i + 0.5) + leteralOffset, -unitLength * (j + 0.5), 0.0)); RotatableBoxTerrainObject unitBox = new RotatableBoxTerrainObject(new Box3D(location, unitLength, unitLength, groundThickness), texture); combinedTerrainObject.addTerrainObject(unitBox); } } return combinedTerrainObject; }
private CombinedTerrainObject3D setUpInclinedSurface(int startRow, int endRow, int startColumn, int endColumn) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D("InclinedSurface"); AppearanceDefinition inclinedSurfaceAppearance = YoAppearance.Grey(); AppearanceDefinition wallAppearance = new YoAppearanceTexture("Textures/cinderBlock2.jpeg"); YoAppearance.makeTransparent(wallAppearance, 0.7f); int rampLengthDimension = (endColumn - startColumn + 1); int rampWidthDimension = (endRow - startRow + 1); double wallThickness = 0.05; double wallHeight = gridWidth; double rampWidth = rampWidthDimension * gridLength - wallThickness; double rampLength = rampLengthDimension * gridWidth; double centerX = (getWorldCoordinate(startRow, startColumn).getX() + getWorldCoordinate(endRow, endColumn).getX()) / 2 + wallThickness / 2; double centerY = (getWorldCoordinate(startRow, startColumn).getY() + getWorldCoordinate(endRow, endColumn).getY()) / 2; RotatableRampTerrainObject inclinedSurface = new RotatableRampTerrainObject(centerX, centerY, rampLength, rampWidth, flatGridHeight, -90, inclinedSurfaceAppearance); combinedTerrainObject.addTerrainObject(inclinedSurface); for (int i = 0; i < rampLengthDimension; i++) { RigidBodyTransform wallLocation = new RigidBodyTransform(); wallLocation.appendTranslation(centerX, centerY, 0.0); wallLocation.appendTranslation(-wallThickness / 2 - rampWidth / 2, 0.0, wallHeight / 2); wallLocation.appendTranslation(0.0, rampLength / 2, 0.0); wallLocation.appendTranslation(0.0, -wallHeight * (i + 0.5), 0.0); combinedTerrainObject.addTerrainObject(new RotatableBoxTerrainObject(wallLocation, wallThickness, wallHeight, wallHeight, wallAppearance)); } return combinedTerrainObject; }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { //this fixes a threading issue. Generating garbage on purpose. RigidBodyTransform tempTransform = new RigidBodyTransform(); screenPosition.changeFrame(pixelsFrame); tempTransform.setIdentity(); tempTranslation.set(screenPosition.getX() + getPlotterWidthPixels() / 2.0, screenPosition.getY() - getPlotterHeightPixels() / 2.0, 0.0); tempTransform.appendTranslation(tempTranslation); tempTransform.appendYawRotation(screenRotation); tempTranslation.set(-getPlotterWidthPixels() / 2.0, getPlotterHeightPixels() / 2.0, 0.0); tempTransform.appendTranslation(tempTranslation); tempTransform.appendPitchRotation(Math.PI); tempTransform.appendYawRotation(Math.PI); transformToParent.set(tempTransform); } };
private TerrainObject3D createTable(int row, int column, Point2D positionInGrid) { RigidBodyTransform location = new RigidBodyTransform(); location.appendTranslation(getWorldCoordinate(row, column)); location.appendTranslation(positionInGrid.getX(), positionInGrid.getY(), 0.0); double xStart = location.getTranslationX() - tableLength / 2; double yStart = location.getTranslationY() - tableWidth / 2; double xEnd = location.getTranslationX() + tableLength / 2; double yEnd = location.getTranslationY() + tableWidth / 2; double zStart = tableHeight - tableThickness + getWorldCoordinate(row, column).getZ(); double zEnd = tableHeight + getWorldCoordinate(row, column).getZ(); return new SimpleTableTerrainObject(xStart, yStart, xEnd, yEnd, zStart, zEnd); }
private RegularPolygon(double centerToPoints, int numberOfPoints) { this.centerToPoints = centerToPoints; polygon = new ConvexPolygon2D(); for (int i = 0; i < numberOfPoints; i++) { double intervalAngle = 2 * Math.PI / numberOfPoints; RigidBodyTransform vertexTransform = new RigidBodyTransform(); vertexTransform.appendYawRotation(intervalAngle * i); vertexTransform.appendTranslation(centerToPoints, 0.0, 0.0); polygon.addVertex(vertexTransform.getTranslationX(), vertexTransform.getTranslationY()); } polygon.update(); }
public ValkyrieCollisionMeshDefinitionDataHolder(ValkyrieJointMap jointMap) { RigidBodyTransform ankleOffset = new RigidBodyTransform(); ankleOffset.appendTranslation((ValkyriePhysicalProperties.footForward - ValkyriePhysicalProperties.footLength * 0.5), 0.0, -ValkyriePhysicalProperties.ankleHeight * 0.5); CollisionMeshDefinitionData rightFootCollisionMeshData = new BoxCollisionMeshDefinitionData(jointMap.getLegJointName(RobotSide.RIGHT, LegJointName.ANKLE_ROLL), ValkyriePhysicalProperties.footLength, ValkyriePhysicalProperties.footWidth, ValkyriePhysicalProperties.ankleHeight); rightFootCollisionMeshData.setTransformToParentJoint(ankleOffset); addCollisionMeshDefinitionData(rightFootCollisionMeshData); CollisionMeshDefinitionData leftFootCollisionMeshData = new BoxCollisionMeshDefinitionData(jointMap.getLegJointName(RobotSide.LEFT, LegJointName.ANKLE_ROLL), ValkyriePhysicalProperties.footLength, ValkyriePhysicalProperties.footWidth, ValkyriePhysicalProperties.ankleHeight); leftFootCollisionMeshData.setTransformToParentJoint(ankleOffset); addCollisionMeshDefinitionData(leftFootCollisionMeshData); } }
/** * X-axis of hand is located toward Sky. * Z-axis of hand is located toward (wallNormalVector). */ public static Pose3D computeCuttingWallTrajectory(double time, double trajectoryTime, double cuttingRadius, boolean cuttingDirectionCW, Point3D cuttingCenterPosition, Vector3D wallNormalVector) { Vector3D xAxisRotationMatrix = new Vector3D(0, 0, 1); Vector3D yAxisRotationMatrix = new Vector3D(); Vector3D zAxisRotationMatrix = new Vector3D(wallNormalVector); zAxisRotationMatrix.normalize(); yAxisRotationMatrix.cross(zAxisRotationMatrix, xAxisRotationMatrix); RotationMatrix cuttingRotationMatrix = new RotationMatrix(); cuttingRotationMatrix.setColumns(xAxisRotationMatrix, yAxisRotationMatrix, zAxisRotationMatrix); RigidBodyTransform handControl = new RigidBodyTransform(cuttingRotationMatrix, cuttingCenterPosition); double phase = time / trajectoryTime; handControl.appendYawRotation(cuttingDirectionCW ? -phase * 2 * Math.PI : phase * 2 * Math.PI); handControl.appendTranslation(0, cuttingRadius, 0); handControl.appendYawRotation(cuttingDirectionCW ? phase * 2 * Math.PI : -phase * 2 * Math.PI); return new Pose3D(handControl); }
/** * Valkyrie is able to complete only half rotating motion. */ public static Pose3D computeClosingValveTrajectory(double time, double trajectoryTime, double radius, boolean closingDirectionCW, double closingAngle, Point3D valveCenterPosition, Vector3D valveNormalVector) { Vector3D xAxisHandFrame = new Vector3D(valveNormalVector); Vector3D yAxisHandFrame = new Vector3D(); Vector3D zAxisHandFrame = new Vector3D(0, 0, 1); xAxisHandFrame.negate(); xAxisHandFrame.normalize(); yAxisHandFrame.cross(zAxisHandFrame, xAxisHandFrame); RotationMatrix handFrame = new RotationMatrix(); handFrame.setColumns(xAxisHandFrame, yAxisHandFrame, zAxisHandFrame); handFrame.appendRollRotation(closingDirectionCW ? -0.5 * Math.PI : 0.5 * Math.PI); RigidBodyTransform handControl = new RigidBodyTransform(handFrame, valveCenterPosition); double phase = time / trajectoryTime; handControl.appendRollRotation(closingDirectionCW ? phase * closingAngle : -phase * closingAngle); handControl.appendTranslation(0, -radius, 0); return new Pose3D(handControl); }
private synchronized void processFootstepPath(FootstepPlan plan) { meshBuilder.clear(); SideDependentList<Color> colors = solutionWasReceived.get() ? solutionFootstepColors : intermediateFootstepColors; FramePose3D footPose = new FramePose3D(); RigidBodyTransform transformToWorld = new RigidBodyTransform(); ConvexPolygon2D foothold = new ConvexPolygon2D(); for (int i = 0; i < plan.getNumberOfSteps(); i++) { SimpleFootstep footstep = plan.getFootstep(i); Color regionColor = colors.get(footstep.getRobotSide()); footstep.getSoleFramePose(footPose); footPose.get(transformToWorld); transformToWorld.appendTranslation(0.0, 0.0, 0.01); if (footstep.hasFoothold()) footstep.getFoothold(foothold); else foothold.set(defaultFootPolygon); Point2D[] vertices = new Point2D[foothold.getNumberOfVertices()]; for (int j = 0; j < vertices.length; j++) { vertices[j] = new Point2D(foothold.getVertex(j)); } meshBuilder.addMultiLine(transformToWorld, vertices, 0.01, regionColor, true); meshBuilder.addPolygon(transformToWorld, foothold, regionColor); } meshReference.set(new Pair<>(meshBuilder.generateMesh(), meshBuilder.generateMaterial())); }
private CombinedTerrainObject3D setUpVan(String name, Point2D doorLocation) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); combinedTerrainObject.addBox(doorLocation.getX() - dummyVanLength / 2, doorLocation.getY(), doorLocation.getX() + dummyVanLength / 2, doorLocation.getY() + dummyVanWidth, dummyVanWheelRadius, dummyVanWheelRadius + dummyVanHeight, YoAppearance.Gray()); RigidBodyTransform frontWheel = new RigidBodyTransform(); RigidBodyTransform rearWheel = new RigidBodyTransform(); frontWheel.setTranslation(doorLocation.getX() + dummyVanWheelDistance / 2, doorLocation.getY() + dummyVanWidth / 2, dummyVanWheelRadius); rearWheel.setTranslation(doorLocation.getX() - dummyVanWheelDistance / 2, doorLocation.getY() + dummyVanWidth / 2, dummyVanWheelRadius); frontWheel.appendRollRotation(Math.PI / 2); rearWheel.appendRollRotation(Math.PI / 2); double offset = 0.05; combinedTerrainObject.addCylinder(frontWheel, dummyVanWidth - offset, dummyVanWheelRadius, YoAppearance.DarkGray()); combinedTerrainObject.addCylinder(rearWheel, dummyVanWidth - offset, dummyVanWheelRadius, YoAppearance.DarkGray()); combinedTerrainObject.addBox(doorLocation.getX(), doorLocation.getY() - dummyDoorThickness, doorLocation.getX() + dummyDoorWidth, doorLocation.getY(), dummyDoorHeightOffset, dummyDoorHeightOffset + dummyDoorHeight, YoAppearance.DarkGray()); knob.appendTranslation(doorLocation.getX(), doorLocation.getY(), dummyDoorHeightOffset + dummyDoorHeight / 2); knob.appendTranslation(dummyDoorKnobLength / 2 + dummyDoorKnobDiameter, -dummyDoorThickness - dummyDoorKnobDiameter, 0); knob.appendPitchRotation(Math.PI / 2); combinedTerrainObject.addCylinder(knob, dummyDoorKnobLength, dummyDoorKnobDiameter / 2, YoAppearance.Gray()); return combinedTerrainObject; }
private void processFootMesh(FootstepNode node, FootstepNodeSnapData snapData, boolean valid) { meshBuilder.clear(); RigidBodyTransform planarTransformToWorld = new RigidBodyTransform(); FootstepNodeTools.getNodeTransform(node, planarTransformToWorld); RigidBodyTransform snappedTransformToWorld = new RigidBodyTransform(); ConvexPolygon2D foothold = snapData.getCroppedFoothold(); FootstepNodeTools.getSnappedNodeTransform(node, snapData.getSnapTransform(), snappedTransformToWorld); snappedTransformToWorld.appendTranslation(0.0, 0.0, 0.01); planarTransformToWorld.setTranslationZ(snappedTransformToWorld.getTranslationZ() + 0.1); Color regionColor = valid ? Color.GREEN : Color.RED; regionColor = Color.hsb(regionColor.getHue(), 0.9, 1.0); Point2D[] vertices = new Point2D[foothold.getNumberOfVertices()]; for (int j = 0; j < vertices.length; j++) { vertices[j] = new Point2D(foothold.getVertex(j)); } meshBuilder.addMultiLine(snappedTransformToWorld, vertices, 0.01, regionColor, true); meshBuilder.addPolygon(snappedTransformToWorld, foothold, regionColor); // TODO add mesh of planar footstep meshView.setOpacity(0.9); meshView.setMesh(meshBuilder.generateMesh()); meshView.setMaterial(meshBuilder.generateMaterial()); }