/** * Rotates this shape by angle of {@code yaw} about the z-axis of this shape local coordinates. * * @param yaw the angle to rotate about the local z-axis. */ public final void appendYawRotation(double yaw) { shapePose.appendYawRotation(yaw); }
public static void appendRotation(RigidBodyTransform transformToModify, double angle, Axis axis) { switch (axis) { case X: transformToModify.appendRollRotation(angle); break; case Y: transformToModify.appendPitchRotation(angle); break; case Z: transformToModify.appendYawRotation(angle); break; default: throw new RuntimeException("Unhandled value of Axis: " + axis); } }
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); }
break; case YAW: ret.appendYawRotation(configuration[0]); break; case SO3: ret.appendYawRotation(theta3); break;
/** * 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); }
@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 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 static void main(String[] args) { RigidBodyTransform t1 = new RigidBodyTransform(); Vector3D rotationVector = new Vector3D(); DenseMatrix64F rotationVectorMatrix = new DenseMatrix64F(3, 1); t1.appendYawRotation(Math.PI / 8.0); t1.getRotation(rotationVector); rotationVector.get(rotationVectorMatrix); SelectionMatrix3D selectionMatrix3d = new SelectionMatrix3D(); selectionMatrix3d.selectZAxis(false); DenseMatrix64F selectionMatrix = new DenseMatrix64F(3, 3); selectionMatrix3d.getFullSelectionMatrixInFrame(null, selectionMatrix); DenseMatrix64F result = new DenseMatrix64F(3, 1); CommonOps.mult(selectionMatrix, rotationVectorMatrix, result); System.out.println(result); rotationVector.set(result); RotationMatrix rm = new RotationMatrix(rotationVector); System.out.println(rm); } }
break; case YAW: randomPose.appendYawRotation(value); break; default:
double translationX = centerToSideLine * (1 + ratioToInnerPolygon) / 2; double pitchAngle = -Math.atan(depthToCentroid / (centerToSideLine * (1 - ratioToInnerPolygon))); sideTransform.appendYawRotation(yawAngle); sideTransform.appendTranslation(translationX, 0.0, -depthToCentroid / 2); sideTransform.appendPitchRotation(pitchAngle);
transform2.appendYawRotation(-Math.PI / 4.0); transform2.appendRollRotation(Math.PI / 2.0); PlanarRegion region2 = new PlanarRegion(transform2, polygonsRegion2);
transform.appendYawRotation(yaw); transform.invert(); baseBlockFrame = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("baseFrame", ReferenceFrame.getWorldFrame(), transform);
break; case 2: pose.appendYawRotation(Math.PI); break; default: break; case 2: pose.appendYawRotation(angle); break; default: pose2.appendYawRotation(Math.PI / 2.0); pose2.appendYawRotation(Math.PI / 2.0); pose2.appendPitchRotation(Math.PI / 2.0); pose2.appendYawRotation(Math.PI / 2.0); pose2.appendRollRotation(Math.PI / 2.0);
pose2.appendYawRotation(Math.PI / 2.0); pose2.appendYawRotation(Math.PI / 2.0); pose2.appendPitchRotation(Math.PI / 2.0); pose2.appendYawRotation(Math.PI / 2.0); pose2.appendRollRotation(Math.PI / 2.0);
actual.appendYawRotation(yaw); assertTrue(actual.hasRotation()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expected, actual, EPS); actual.set(original); assertFalse(actual.hasRotation()); actual.appendYawRotation(yaw); assertTrue(actual.hasRotation()); actual.set(original); assertTrue(actual.hasRotation()); actual.appendYawRotation(yaw); assertTrue(actual.hasRotation()); actual.set(original); assertTrue(actual.hasRotation()); actual.appendYawRotation(yaw); assertFalse(actual.hasRotation());
protected FootstepDataListMessage createYawingForwardWalkingFootstepMessage() transform.appendYawRotation(0.5); ReferenceFrame referenceFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("yawing", ReferenceFrame.getWorldFrame(), transform);
rotationTransform.appendYawRotation(theta);