/** * 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; }
private Point2D projectOnPlane(RigidBodyTransform plane, Vector3D point) { RigidBodyTransform planeInv = new RigidBodyTransform(plane); planeInv.invert(); planeInv.transform(point); return new Point2D(point.getX(), point.getY()); }
private RigidBodyTransform getWiggleTransformInWorldFrame(RigidBodyTransform wiggleTransformLocalToLocal) { RigidBodyTransform wiggleTransformWorldToWorld = new RigidBodyTransform(); RigidBodyTransform localToWorld = new RigidBodyTransform(); planarRegionToPack.getTransformToWorld(localToWorld); RigidBodyTransform worldToLocal = new RigidBodyTransform(localToWorld); worldToLocal.invert(); wiggleTransformWorldToWorld.set(localToWorld); wiggleTransformWorldToWorld.multiply(wiggleTransformLocalToLocal); wiggleTransformWorldToWorld.multiply(worldToLocal); return wiggleTransformWorldToWorld; }
public void translate(double x, double y, double z) { tempTransform.setTranslationAndIdentityRotation(x, y, z); transform.set(transform); transform.multiply(tempTransform); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setIdentity(); transformToParent.setTranslation(originVector); }
public void translateThenRotateEuler(Vector3D translationVector, Vector3D eulerAngles) { tempTransform.setRotationEulerAndZeroTranslation(eulerAngles); tempTransform.setTranslation(translationVector); transform.set(transform); transform.multiply(tempTransform); }
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; }
private static void addNodeDataToFootstepPlan(FootstepPlan footstepPlan, FootstepNodeDataMessage nodeData) { RobotSide robotSide = RobotSide.fromByte(nodeData.getRobotSide()); RigidBodyTransform footstepPose = new RigidBodyTransform(); footstepPose.setRotationYawAndZeroTranslation(nodeData.getYawIndex() * FootstepNode.gridSizeYaw); footstepPose.setTranslationX(nodeData.getXIndex() * FootstepNode.gridSizeXY); footstepPose.setTranslationY(nodeData.getYIndex() * FootstepNode.gridSizeXY); RigidBodyTransform snapTransform = new RigidBodyTransform(); snapTransform.set(nodeData.getSnapRotation(), nodeData.getSnapTranslation()); snapTransform.transform(footstepPose); footstepPlan.addFootstep(robotSide, new FramePose3D(ReferenceFrame.getWorldFrame(), footstepPose)); }
private RigidBodyTransform randomTransform(Random random) { RigidBodyTransform transform = new RigidBodyTransform(); RigidBodyTransform tempTransform = new RigidBodyTransform(); transform.setRotationRollAndZeroTranslation(2 * Math.PI * random.nextDouble()); tempTransform.setRotationPitchAndZeroTranslation(2 * Math.PI * random.nextDouble()); transform.multiply(tempTransform); tempTransform.setRotationYawAndZeroTranslation(2 * Math.PI * random.nextDouble()); transform.multiply(tempTransform); double[] matrix = new double[16]; transform.get(matrix); matrix[3] = random.nextDouble(); matrix[7] = random.nextDouble(); matrix[11] = random.nextDouble(); return transform; } }
public void getAnklePosition(FramePoint3D positionToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); positionToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform.getTranslationVector()); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newPose = new RigidBodyTransform(); pinJointTransform.setRotationYawAndZeroTranslation(steeringWheelPinJoint.getQYoVariable().getDoubleValue()); newPose.set(originalSteeringWheelPose); newPose.multiply(pinJointTransform); steeringWheelFrame.setPoseAndUpdate(newPose); super.updateAllGroundContactPointVelocities(); }
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; }
public void updateRobotLocationBasedOnMultisensePose(RigidBodyTransform headPose) { RigidBodyTransform pelvisPose = new RigidBodyTransform(); RigidBodyTransform transformFromHeadToPelvis = pelvisFrame.getTransformToDesiredFrame(headFrame); pelvisPose.set(headPose); pelvisPose.multiply(transformFromHeadToPelvis); atomicPelvisPose.set(pelvisPose); }
private static void setupCamera(DRCSimulationTestHelper drcSimulationTestHelper) { OffsetAndYawRobotInitialSetup startingLocationOffset = location.getStartingLocationOffset(); Point3D cameraFocus = new Point3D(startingLocationOffset.getAdditionalOffset()); cameraFocus.addZ(1.0); RigidBodyTransform transform = new RigidBodyTransform(); transform.setRotationYawAndZeroTranslation(startingLocationOffset.getYaw()); Point3D cameraPosition = new Point3D(10.0, 5.0, cameraFocus.getZ() + 2.0); transform.transform(cameraPosition); drcSimulationTestHelper.setupCameraForUnitTest(cameraFocus, cameraPosition); }
public void getPose(RigidBodyTransform rigidBodyTransformToPack) { orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); rigidBodyTransformToPack.setRotation(tempFrameOrientation); rigidBodyTransformToPack.setTranslation(position); }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform pinJointTransform = new RigidBodyTransform(); RigidBodyTransform newValvePose = new RigidBodyTransform(); pinJointTransform.setRotationRollAndZeroTranslation(valvePinJoint.getQYoVariable().getDoubleValue()); newValvePose.set(originalValvePose); newValvePose.multiply(pinJointTransform); valveFrame.setPoseAndUpdate(newValvePose); super.updateAllGroundContactPointVelocities(); }
public static RigidBodyTransform shiftInSoleFrame(Vector2D shiftVector, RigidBodyTransform soleTransform) { RigidBodyTransform shiftTransform = new RigidBodyTransform(); shiftTransform.setTranslation(new Vector3D(shiftVector.getX(), shiftVector.getY(), 0.0)); soleTransform.multiply(shiftTransform); return soleTransform; }
@Override public void updateAllGroundContactPointVelocities() { RigidBodyTransform sliderJointTransform = new RigidBodyTransform(); RigidBodyTransform newButtonPose = new RigidBodyTransform(); buttonPushVector.scale(buttonSliderJoint.getQYoVariable().getDoubleValue()); sliderJointTransform.setTranslationAndIdentityRotation(buttonPushVector); buttonPushVector.normalize(); newButtonPose.set(originalButtonTransform); newButtonPose.multiply(sliderJointTransform); buttonFrame.setPoseAndUpdate(newButtonPose); super.updateAllGroundContactPointVelocities(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { //TODO: Combine with RotationTools.removePitchAndRollFromTransform(). origin.getReferenceFrame().getTransformToDesiredFrame(nonZUpToWorld, worldFrame); nonZUpToWorld.getRotation(nonZUpToWorldRotation); double yaw = nonZUpToWorldRotation.getYaw(); euler.set(0.0, 0.0, yaw); transformToParent.setRotationEulerAndZeroTranslation(euler); originPoint3d.set(origin); nonZUpToWorld.transform(originPoint3d); translation.set(originPoint3d); transformToParent.setTranslation(translation); } }
public void getAnkleOrientation(FrameQuaternion orientationToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); orientationToPack.setIncludingFrame(footstepPose.getReferenceFrame(), tempTransform.getRotationMatrix()); }