protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); transformToParent.setTranslation(offset); } }
public static RigidBodyTransform yawPitchDegreesTransform(Vector3D center, double yawCCWDegrees, double pitchDownDegrees) { RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawPitchRoll(Math.toRadians(yawCCWDegrees), Math.toRadians(pitchDownDegrees), 0.0); location.setTranslation(center); return location; }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setTranslation(new Vector3D(0.0, robotSide.negateIfRightSide(0.15), 0.0)); } };
private RigidBodyTransform[] createRandomCorrectionTargets(int numTargets) { RigidBodyTransform[] targets = new RigidBodyTransform[numTargets]; for (int i = 0; i < numTargets; i++) { targets[i] = new RigidBodyTransform(); targets[i].setRotationEulerAndZeroTranslation(0, 0, random.nextDouble() * 2.0 * Math.PI); targets[i].setTranslation(RandomGeometry.nextVector3D(random, 1.0)); } return targets; }
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; }
/** * Creates a transform that transforms to the given point and rotates to make the z axis align * with the normal vector. */ public static RigidBodyTransform createTransformFromPointAndZAxis(FramePoint3D point, FrameVector3D zAxis) { RigidBodyTransform ret = new RigidBodyTransform(); ret.setRotation(EuclidGeometryTools.axisAngleFromZUpToVector3D(zAxis)); ret.setTranslation(point); return ret; }
void trainSingleDetector(double yaw, double pitch, double roll, double distance) { OrganizedPointCloud cloud = renderCloud(yaw, pitch, roll, distance); int[] mask = makeMaskByZThresholing(cloud, 1.5f); writeMask(mask, cloud.width, cloud.height); LineModTemplate template = LineModInterface.trainTemplateBytes(cloud, mask); RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationEulerAndZeroTranslation(roll, pitch, yaw); transform.setTranslation(0, 0, distance); template.transform = transform; byteFeatures.add(template); }
public static CombinedTerrainObject3D addValveTextureBox(Vector3D position, double yaw) { YoAppearanceTexture valveTexture = new YoAppearanceTexture("/images/red-valve.jpg"); double boxSideLength = 1.0; CombinedTerrainObject3D valveTerrainObject = new CombinedTerrainObject3D("ValveBox"); RigidBodyTransform location = new RigidBodyTransform(); location.setRotationEulerAndZeroTranslation(Math.toRadians(90.0), 0.0, yaw - Math.toRadians(90.0)); location.setTranslation(position); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, boxSideLength, boxSideLength, boxSideLength), valveTexture); valveTerrainObject.addTerrainObject(newBox); return valveTerrainObject; }
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); }
public static void tranformSe3IntoTransform3D(Se3_F64 from, RigidBodyTransform to) { DenseMatrix64F R = from.getR(); Vector3D_F64 T = from.getT(); RotationMatrix Rd = new RotationMatrix(R); to.setRotation(Rd); to.setTranslation(new Vector3D(T.x, T.y, T.z)); }
public static CombinedTerrainObject3D addFiducial(Vector3D position, double yaw, Fiducial fiducial) { YoAppearanceTexture fiducialTexture = new YoAppearanceTexture(fiducial.getPathString()); double boxSideLength = 1.0; CombinedTerrainObject3D fiducualTerrainObject = new CombinedTerrainObject3D(fiducial.name()); RigidBodyTransform location = new RigidBodyTransform(); location.setRotationEulerAndZeroTranslation(Math.toRadians(90.0), 0.0, yaw - Math.toRadians(90.0)); location.setTranslation(position); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, boxSideLength, boxSideLength, boxSideLength), fiducialTexture); fiducualTerrainObject.addTerrainObject(newBox); return fiducualTerrainObject; }
public void setFromAnklePose(FramePose3DReadOnly anklePose, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(anklePose.getOrientation()); tempTransform.setTranslation(anklePose.getPosition()); tempTransform.multiplyInvertOther(transformFromAnkleToSole); footstepPose.setIncludingFrame(anklePose.getReferenceFrame(), tempTransform); }
public static Affine createAffineFromQuaternionAndTuple(Quaternion32 quaternion, Tuple3DBasics translation) { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotation(quaternion); transform.setTranslation(translation.getX(), translation.getY(), translation.getZ()); return convertRigidBodyTransformToAffine(transform); }
public void getAnklePose(FramePose3D poseToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); poseToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform); }
public static Affine createAffineFromQuaternionAndTuple(Quaternion quaternion, Tuple3DBasics translation) { RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotation(quaternion); transform.setTranslation(translation.getX(), translation.getY(), translation.getZ()); return convertRigidBodyTransformToAffine(transform); }
private static void setTranslationSettingZAndPreservingXAndY(Point3D highestVertex, RigidBodyTransform transformToReturn) { Vector3D newTranslation = new Vector3D(highestVertex.getX(), highestVertex.getY(), 0.0); transformToReturn.transform(newTranslation); newTranslation.scale(-1.0); newTranslation.add(highestVertex); transformToReturn.setTranslation(newTranslation); }
public void getAnkleOrientation(FrameQuaternion orientationToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); orientationToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform.getRotationMatrix()); }
private static PoseReferenceFrame createTranslatedZUpFrame(String name, Point3D frameCenterPoint) { PoseReferenceFrame translatedZUpFrame = new PoseReferenceFrame(name, ReferenceFrame.getWorldFrame()); RigidBodyTransform transform3D = new RigidBodyTransform(); transform3D.setTranslation(new Vector3D(frameCenterPoint)); FramePose3D framePose = new FramePose3D(ReferenceFrame.getWorldFrame(), transform3D); translatedZUpFrame.setPoseAndUpdate(framePose); translatedZUpFrame.update(); return translatedZUpFrame; }
private static CombinedTerrainObject3D setUpGround(String name) { CombinedTerrainObject3D combinedTerrainObject = new CombinedTerrainObject3D(name); YoAppearanceTexture texture = new YoAppearanceTexture("Textures/brick.png"); RigidBodyTransform location = new RigidBodyTransform(); location.setTranslation(new Vector3D(0, 0, -0.5)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3D(location, 10, 10, 1), texture); combinedTerrainObject.addTerrainObject(newBox); return combinedTerrainObject; } }
private Footstep generateFootstepFromLocationAndOrientation(RobotSide robotSide, double[] positionArray, double[] orientationArray) { Footstep footstep = new Footstep(robotSide); Point3D position = new Point3D(positionArray); Quaternion orientation = new Quaternion(orientationArray); RigidBodyTransform anklePose = new RigidBodyTransform(); anklePose.setRotation(orientation); anklePose.setTranslation(position); FramePose3D pose = new FramePose3D(ReferenceFrame.getWorldFrame(), anklePose); footstep.setFromAnklePose(pose, transformsFromAnkleToSole.get(robotSide)); return footstep; }