orientation.getColumn(2, zOrientation); double originalYaw = RotationTools.computeYaw(originalFootstepFound.getOrientation());
@Override public double getFootstepValue(FootstepDataMessage footstepData) { Matrix3d rotationMatrix = new Matrix3d(); rotationMatrix.set(footstepData.getOrientation()); Vector3d footstepNormal = new Vector3d(); rotationMatrix.getColumn(2, footstepNormal); double offHorizontalAngle = Math.acos(footstepNormal.getZ()); if (offHorizontalAngle > parameters.getMaxAngle() || footstepNormal.getZ() < 0) return Double.NEGATIVE_INFINITY; double value = slopeGain * offHorizontalAngle; if (footstepData.predictedContactPoints == null || footstepData.predictedContactPoints.isEmpty()) return Double.NEGATIVE_INFINITY; ConvexPolygon2d supportPolygon = new ConvexPolygon2d(footstepData.getPredictedContactPoints()); supportPolygon.update(); double inPlaneArea = supportPolygon.getArea(); double horizonalArea = inPlaneArea * footstepNormal.getZ(); if (horizonalArea < parameters.getMinArea()) return Double.NEGATIVE_INFINITY; value += horizonalArea * areaGain; return value; }
private TerrainObject3D createConvexPolygonTerrainObject(RigidBodyTransform transformToWorld) { Matrix3d rotationToWorld = new Matrix3d(); transformToWorld.getRotation(rotationToWorld); Vector3d normal = new Vector3d(); rotationToWorld.getColumn(2, normal); Vector3d centroid = new Vector3d(); transformToWorld.getTranslation(centroid); int nPoints = 5; double radius = 0.23; ConvexPolygon2d convexPolygon = createContactPolygon(centroid, nPoints, radius); TerrainObject3D contact = new RotatableConvexPolygonTerrainObject(normal, convexPolygon, centroid.getZ(), YoAppearance.DarkGray()); return contact; }
rotationMatrix.set(footstepData.getOrientation()); Vector3d footNormal = new Vector3d(); rotationMatrix.getColumn(2, footNormal); Plane3d footPlane = new Plane3d(footstepData.getLocation(), footNormal);