/** * getTransformToParent * * Returns a Transform3D that can be applied to a vector defined in this frame * in order to obtain the equivalent vector in the parent frame * * @return Transform3D */ public RigidBodyTransform getTransformToParent() { return new RigidBodyTransform(transformToParent); }
public static RigidBodyTransform createTranslationTransform(Vector3d translation) { RigidBodyTransform transform = new RigidBodyTransform(); transform.setTranslation(translation); return transform; }
public static RigidBodyTransform transformLocalX(RigidBodyTransform originalTransform, double magnitude) { RigidBodyTransform transform = new RigidBodyTransform(originalTransform); Vector3d localTranslation = new Vector3d(magnitude, 0.0, 0.0); RigidBodyTransform postTranslation = new RigidBodyTransform(); postTranslation.setTranslationAndIdentityRotation(localTranslation); transform.multiply(postTranslation); return transform; }
public static RevoluteJoint addRevoluteJoint(String jointName, RigidBody parentBody, Vector3d jointOffset, Vector3d jointAxis) { RigidBodyTransform transformToParent = new RigidBodyTransform(); transformToParent.setTranslationAndIdentityRotation(jointOffset); return addRevoluteJoint(jointName, parentBody, transformToParent, jointAxis); }
public static ReferenceFrame constructFrameWithUnchangingTransformFromParent(String frameName, ReferenceFrame parentFrame, RigidBodyTransform transformFromParent) { RigidBodyTransform transformToParent = new RigidBodyTransform(transformFromParent); transformToParent.invert(); return constructFrameWithUnchangingTransformToParent(frameName, parentFrame, transformToParent); }
/** * Add a rotation to the current transform. */ public final void applyRotationX(double angle) { RigidBodyTransform temp = new RigidBodyTransform(); temp.setRotationRollAndZeroTranslation(angle); multiply(temp); }
@Override public void changeFrameAndProjectToXYPlane(ReferenceFrame desiredFrame) { // this is in the correct frame already if (desiredFrame == referenceFrame) return; if (temporaryTransformToDesiredFrame == null) temporaryTransformToDesiredFrame = new RigidBodyTransform(); referenceFrame.getTransformToDesiredFrame(temporaryTransformToDesiredFrame, desiredFrame); applyTransformAndProjectToXYPlane(temporaryTransformToDesiredFrame); referenceFrame = desiredFrame; }
@Override public void changeFrame(ReferenceFrame desiredFrame) { // this is in the correct frame already if (desiredFrame == referenceFrame) return; if (temporaryTransformHToDesiredFrame == null) temporaryTransformHToDesiredFrame = new RigidBodyTransform(); referenceFrame.getTransformToDesiredFrame(temporaryTransformHToDesiredFrame, desiredFrame); applyTransform(temporaryTransformHToDesiredFrame); this.referenceFrame = desiredFrame; }
@Override public void changeFrameAndProjectToXYPlane(ReferenceFrame desiredFrame) { if (desiredFrame == referenceFrame) return; // this is in the correct frame already if (temporaryTransformToDesiredFrame == null) temporaryTransformToDesiredFrame = new RigidBodyTransform(); referenceFrame.getTransformToDesiredFrame(temporaryTransformToDesiredFrame, desiredFrame); applyTransformAndProjectToXYPlane(temporaryTransformToDesiredFrame); referenceFrame = desiredFrame; }
public double getEffectiveDistanceToFramePose(FramePose framePose, double radiusOfRotation) { checkReferenceFrameMatch(framePose); RigidBodyTransform transformThis = new RigidBodyTransform(); this.getPose(transformThis); transformThis.invert(); RigidBodyTransform transformThat = new RigidBodyTransform(); framePose.getPose(transformThat); transformThat.invert(); return TransformTools.getSizeOfTransformBetweenTwoWithRotationScaled(transformThis, transformThat, radiusOfRotation); }
public static void main(String[] args) { RigidBodyTransform t = new RigidBodyTransform(); t.setTranslationAndIdentityRotation(new Vector3d(4.5, 6.6, 22)); System.out.println(getTransformedPoint(new Point3d(0.0, 1.0, 4.6), t)); }
public void setSoleFrame(FramePose newSolePoseInWorldFrame) { newSolePoseInWorldFrame.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); RigidBodyTransform transformFromSoleToWorld = new RigidBodyTransform(); newSolePoseInWorldFrame.getPose(transformFromSoleToWorld); RigidBodyTransform transformFromShinToWorld = new RigidBodyTransform(); transformFromShinToWorld.multiply(transformFromSoleToWorld, shinFrame.getTransformToDesiredFrame(soleFrame)); shinFrame.setPoseAndUpdate(transformFromShinToWorld); }
public static List<RevoluteJoint> createRandomChainRobot(String prefix, Vector3d[] jointAxes, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody(prefix + "RootBody", rootBodyFrame); return createRandomChainRobot(prefix, rootBody, jointAxes, random); }
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); }
protected Point3d getPoint(int index, float range) { Point3d p = new Point3d(range, 0.0, 0.0); RigidBodyTransform transform = new RigidBodyTransform(); getInterpolatedTransform(index, transform); transform.multiply(getSweepTransform(index)); transform.transform(p); return p; }
@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); }
private static void setUpWall3D(CombinedTerrainObject3D combinedTerrainObject, double[] xy, double width, double length, double height, double yawDegrees, AppearanceDefinition app) { double x = xy[0]; double y = xy[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3d(x, y, height / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3d(location, length, width, height), app); combinedTerrainObject.addTerrainObject(newBox); }
private static void setUpWall(CombinedTerrainObject3D combinedTerrainObject, double[] xy, double width, double length, double height, double yawDegrees, AppearanceDefinition app) { double x = xy[0]; double y = xy[1]; RigidBodyTransform location = new RigidBodyTransform(); location.setRotationYawAndZeroTranslation(Math.toRadians(yawDegrees)); location.setTranslation(new Vector3d(x, y, height / 2)); RotatableBoxTerrainObject newBox = new RotatableBoxTerrainObject(new Box3d(location, length, width, height), app); combinedTerrainObject.addTerrainObject(newBox); }
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); }
private void addBlock(double yaw, Vector3d position, Vector3d dimensions, AppearanceDefinition color) { AxisAngle4d orientation = new AxisAngle4d(new Vector3d(0.0, 0.0, 1.0), yaw); RigidBodyTransform blockPose = new RigidBodyTransform(orientation, position); Box3d block = new Box3d(blockPose, dimensions.getX(), dimensions.getY(), dimensions.getZ()); terrainObject.addTerrainObject(new RotatableBoxTerrainObject(block, color)); }