RotationTools.computeQuaternionFromYawAndZNormal(yaw, new Vector3D(0.0, 0.0, 1.0), orientation); footstepData.getOrientation().set(orientation);
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3d planeNormal) { Point3d position = footstep.getLocation(); Quat4d orientation = footstep.getOrientation(); RigidBodyTransform solePose = new RigidBodyTransform(); double yaw = RotationTools.computeYaw(orientation); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); }
@Override public void adjustFootstepWithoutHeightmap(FootstepDataMessage footstep, double height, Vector3D planeNormal) { Point3D position = footstep.getLocation(); Quaternion orientation = footstep.getOrientation(); RigidBodyTransform solePose = new RigidBodyTransform(); double yaw = orientation.getYaw(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); }
intersectionToPack.setVectorWithoutChecks(intersectPlaneNormal); RotationTools.computeQuaternionFromYawAndZNormal(0.0, intersectionToPack.getNormalizedVector(), tempIntersectPlaneReferenceFrame.get().getOrientationUnsafe()); tempIntersectPlaneReferenceFrame.get().setPositionWithoutChecksAndUpdate(planeOne.getPoint());
private Footstep generateFootstep(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3D planeNormal) { double yaw = footPose2d.getYaw(); Point3D position = new Point3D(footPose2d.getX(), footPose2d.getY(), height); Quaternion orientation = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame(), position, orientation); Footstep footstep = new Footstep(robotSide, footstepPose); return footstep; }
@Override public Footstep generateFootstepWithoutHeightMap(FramePose2D footPose2d, RigidBodyBasics foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3D planeNormal) { double yaw = footPose2d.getYaw(); Point3D position = new Point3D(footPose2d.getX(), footPose2d.getY(), height); Quaternion orientation = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); FramePose3D solePose = new FramePose3D(ReferenceFrame.getWorldFrame(), position, orientation); Footstep footstep = new Footstep(robotSide, solePose); return footstep; }
@Override public Footstep generateFootstepWithoutHeightMap(FramePose2d footPose2d, RigidBody foot, ReferenceFrame soleFrame, RobotSide robotSide, double height, Vector3d planeNormal) { double yaw = footPose2d.getYaw(); Point3d position = new Point3d(footPose2d.getX(), footPose2d.getY(), height); Quat4d orientation = new Quat4d(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); Footstep footstep = new Footstep(foot, robotSide, soleFrame); footstep.setSolePose(new FramePose(ReferenceFrame.getWorldFrame(), position, orientation)); return footstep; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testRandomGetQuaternionFromYawAndZNormal() { int numTests = 100; Random random = new Random(7362L); Vector3D normal = new Vector3D(); Quaternion quatSolution = new Quaternion(); for (int i = 0; i < numTests; i++) { Quaternion quatToPack = RandomGeometry.nextQuaternion(random); RotationMatrix rotationMatrixToPack = new RotationMatrix(); double yaw = quatToPack.getYaw(); rotationMatrixToPack.set(quatToPack); rotationMatrixToPack.getColumn(2, normal); RotationTools.computeQuaternionFromYawAndZNormal(yaw, normal, quatSolution); boolean quaternionsAreEqual = RotationTools.quaternionEpsilonEquals(quatToPack, quatSolution, EPSILON); assertTrue(quaternionsAreEqual); } }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3d planeNormal) { Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); double yaw = RotationTools.computeYaw(orientation); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); footstep.setSolePose(new FramePose(ReferenceFrame.getWorldFrame(), new Point3d(position), orientation)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetQuaternionFromYawAndZNormal() { double yaw = 1.0; double pitch = 0.4; double roll = 0.8; RotationMatrix rotationMatrixToPack = new RotationMatrix(); rotationMatrixToPack.setYawPitchRoll(yaw, pitch, roll); Vector3D normal = new Vector3D(); rotationMatrixToPack.getColumn(2, normal); Quaternion quatToPack = new Quaternion(); quatToPack.set(rotationMatrixToPack); Quaternion quatSolution = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, normal, quatSolution); assertTrue(RotationTools.quaternionEpsilonEquals(quatToPack, quatSolution, EPSILON)); yaw = -Math.PI - 0.01; pitch = Math.PI / 2.0 - 1e-3; roll = 0.8; rotationMatrixToPack.setYawPitchRoll(yaw, pitch, roll); normal = new Vector3D(); rotationMatrixToPack.getColumn(2, normal); quatToPack.set(rotationMatrixToPack); quatSolution = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, normal, quatSolution); assertTrue(RotationTools.quaternionEpsilonEquals(quatToPack, quatSolution, EPSILON)); }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3D planeNormal) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FramePose3D footstepPose = new FramePose3D(); footstep.getPose(footstepPose); Point3D position = new Point3D(footstepPose.getPosition()); double yaw = footstep.getFootstepPose().getYaw(); Quaternion orientation = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); footstepPose.set(new Point3D(position), orientation); footstep.setPose(footstepPose); }
RotationTools.computeQuaternionFromYawAndZNormal(yaw, facePlane.getNormalCopy(), newOrientation); FootstepDataMessage currentFaceFootstep = new FootstepDataMessage(footstep.getRobotSide(), new Point3d(x, y, facePlane.getZOnPlane(x, y)), newOrientation);
RotationTools.computeQuaternionFromYawAndZNormal(yaw, facePlane.getNormalCopy(), newOrientation); FootstepDataMessage currentFaceFootstep = HumanoidMessageTools.createFootstepDataMessage(RobotSide.fromByte(footstep.getRobotSide()), new Point3D(x, y, facePlane.getZOnPlane(x, y)), newOrientation); currentPredictedContactPoints = getPredictedContactPointsForFootstep(currentFaceFootstep, points, distanceTolerance);
RotationTools.computeQuaternionFromYawAndZNormal(footYaw, verticalUnitVector, currentFootstep.getOrientation()); RotationTools.computeQuaternionFromYawAndZNormal(footYaw, verticalUnitVector, currentFootstep.getOrientation());
private FootstepDataListMessage createFootstepsForSideSteppingOverSmallWall() { Quaternion orientation = new Quaternion(); Vector3D verticalVector = new Vector3D(0.0, 0.0, 1.0); FootstepDataListMessage footstepDataList = new FootstepDataListMessage(); RotationTools.computeQuaternionFromYawAndZNormal(3.0/4.0*Math.PI, verticalVector, orientation); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(-4.318, -3.912, 0.0), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(-4.198, -3.792, 0.0), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(-4.598, -4.192, 0.0), new Quaternion(orientation), TrajectoryType.OBSTACLE_CLEARANCE, 0.24)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(-4.318, -3.912, 0.0), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(-4.728, -4.322, 0.0), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(-4.598, -4.192, 0.0), new Quaternion(orientation), TrajectoryType.OBSTACLE_CLEARANCE, 0.24)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(-4.858, -4.452, 0.0), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(-4.688, -4.282, 0.0), new Quaternion(orientation))); return footstepDataList; }
private FootstepDataListMessage createFootstepsForSideSteppingOverSmallPlatform() { Quaternion orientation = new Quaternion(); Vector3D verticalVector = new Vector3D(0.0, 0.0, 1.0); FootstepDataListMessage footstepDataList = new FootstepDataListMessage(); RotationTools.computeQuaternionFromYawAndZNormal(3.0/4.0*Math.PI, verticalVector, orientation); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(-3.418, -5.012, 0.156), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(-3.238, -4.832, 0.0), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(-3.518, -5.112, 0.156), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(-3.388, -4.982, 0.156), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(-3.688, -5.282, 0.0), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(-3.518, -5.112, 0.156), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(-3.858, -5.452, 0.0), new Quaternion(orientation))); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(-3.688, -5.282, 0.0), new Quaternion(orientation))); return footstepDataList; }
RotationTools.computeQuaternionFromYawAndZNormal(soleYaw.getDoubleValue(), planeNormal, planeOrientation); planePose.setOrientation(planeOrientation);
private FootstepDataListMessage createFootstepsWithHighSwing() { double swingHeight = 0.5; double swingTime = 1.5; double transferTime = 1.0; Quaternion orientation = new Quaternion(); Vector3D verticalVector = new Vector3D(0.0, 0.0, 1.0); FootstepDataListMessage footstepDataList = HumanoidMessageTools.createFootstepDataListMessage(swingTime, transferTime); RotationTools.computeQuaternionFromYawAndZNormal(0.0 / 4.0 * Math.PI, verticalVector, orientation); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(0.0, .15, 0.0), new Quaternion(orientation), null, TrajectoryType.OBSTACLE_CLEARANCE, swingHeight)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(0.0, -.15, 0.0), new Quaternion(orientation), null, TrajectoryType.OBSTACLE_CLEARANCE, swingHeight)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(0.4, .15, 0.0), new Quaternion(orientation), null, TrajectoryType.OBSTACLE_CLEARANCE, swingHeight)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(0.8, -.15, 0.0), new Quaternion(orientation), null, TrajectoryType.OBSTACLE_CLEARANCE, swingHeight)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(1.2, .15, 0.0), new Quaternion(orientation), null, TrajectoryType.OBSTACLE_CLEARANCE, swingHeight)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(1.6, -.15, 0.0), new Quaternion(orientation), null, TrajectoryType.OBSTACLE_CLEARANCE, swingHeight)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(2.0, .15, 0.0), new Quaternion(orientation), null, TrajectoryType.OBSTACLE_CLEARANCE, swingHeight)); footstepDataList.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(2.0, -.15, 0.0), new Quaternion(orientation), null, TrajectoryType.OBSTACLE_CLEARANCE, swingHeight)); return footstepDataList; } }