/** * Retrieves a point that lies in this planar region. * This point is also used as the origin of the local coordinate system of this planar region. * @param pointToPack used to store the point coordinates. */ public void getPointInRegion(Point3d pointToPack) { fromLocalToWorldTransform.getTranslation(pointToPack); }
/** * Retrieves a point that lies in this planar region. * This point is also used as the origin of the local coordinate system of this planar region. * @param pointToPack used to store the point coordinates. */ public void getPointInRegion(Point3f pointToPack) { fromLocalToWorldTransform.getTranslation(pointToPack); }
/** * Pack rotation part into Matrix3f and translation part into Tuple3f * * @param matrixToPack * @param translationToPack */ public void get(Matrix3f matrixToPack, Tuple3f translationToPack) { getRotation(matrixToPack); getTranslation(translationToPack); }
public void setPose(RigidBodyTransform transform) { transform.getTranslation(position); transform.getRotation(orientation); }
/** * Convert and pack rotation part of transform into Quat4d and pack * translation into Tuple3d. * * @param quaternionToPack * @param translationToPack */ public void get(Quat4d quaternionToPack, Tuple3d translationToPack) { getRotation(quaternionToPack); getTranslation(translationToPack); }
/** * Pack rotation part into Matrix3d and translation part into Tuple3d * * @param matrixToPack * @param translationPack */ public void get(Matrix3d matrixToPack, Tuple3d translationPack) { getRotation(matrixToPack); getTranslation(translationPack); }
/** * Convert and pack rotation part of transform into Quat4f and pack * translation into Vector3f. * * @param quaternionToPack * @param translationToPack */ public void get(Quat4f quaternionToPack, Tuple3f translationToPack) { getRotation(quaternionToPack); getTranslation(translationToPack); }
private void checkIsTransformationInPlane(RigidBodyTransform transformToNewFrame, Point3d transformedPoint) { transformToNewFrame.getTranslation(temporaryTranslation); if (Math.abs(temporaryTranslation.getZ() - transformedPoint.getZ()) > epsilon) throw new RuntimeException("Cannot transform FramePoint2d to a plane with a different surface normal"); }
public static double getMagnitudeOfTranslation(RigidBodyTransform rigidBodyTransform) { Vector3d vector3d = new Vector3d(); rigidBodyTransform.getTranslation(vector3d); return vector3d.length(); }
private void sendCorrectionUpdatePacket() { errorBetweenCorrectedAndLocalizationTransform.getTranslation(translationalResidualError); totalErrorBetweenPelvisAndLocalizationTransform.getTranslation(translationalTotalError); double absoluteResidualError = translationalResidualError.length(); double absoluteTotalError = translationalTotalError.length(); PelvisPoseErrorPacket pelvisPoseErrorPacket = new PelvisPoseErrorPacket((float) absoluteResidualError, (float) absoluteTotalError, hasMapBeenReset); hasMapBeenReset = false; pelvisPoseCorrectionCommunicator.sendPelvisPoseErrorPacket(pelvisPoseErrorPacket); }
public void setPositionAndRotation(RigidBodyTransform transform) { RotationTools.convertTransformToQuaternion(transform, jointRotation); if (!RotationTools.isQuaternionNormalized(jointRotation)) { throw new AssertionError("quaternion is not normalized. " + jointRotation); } transform.getTranslation(jointTranslation); this.afterJointFrame.setRotation(jointRotation); this.afterJointFrame.setTranslation(jointTranslation); }
public FrameVector getTranslationNOTDueToRotationAboutFrame(FramePose otherPose) { checkReferenceFrameMatch(otherPose); RigidBodyTransform transformToOtherPose = getTransformFromThisToThat(otherPose); FrameVector ret = new FrameVector(referenceFrame); transformToOtherPose.getTranslation(ret.tuple); return ret; }
private void sendCorrectionUpdatePacket() { totalRotationErrorFrame.get(totalRotationError); totalTranslationErrorFrame.get(totalTranslationError); totalError.set(totalRotationError, totalTranslationError); errorBetweenCurrentPositionAndCorrected.getTranslation(translationalResidualError); double absoluteResidualError = translationalResidualError.length(); totalError.getTranslation(translationalTotalError); double absoluteTotalError = translationalTotalError.length(); PelvisPoseErrorPacket pelvisPoseErrorPacket = new PelvisPoseErrorPacket((float) absoluteTotalError, (float) absoluteResidualError, false); pelvisPoseCorrectionCommunicator.sendPelvisPoseErrorPacket(pelvisPoseErrorPacket); }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3d positionInWorld = new Vector3d(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
protected void setPositionInWorld(HumanoidFloatingRootJointRobot robot) { robot.update(); robot.getRootJointToWorldTransform(rootToWorld); rootToWorld.getTranslation(offset); Vector3d positionInWorld = new Vector3d(); rootToWorld.getTranslation(positionInWorld); GroundContactPoint gc1 = robot.getFootGroundContactPoints(RobotSide.LEFT).get(0); double pelvisToFoot = positionInWorld.getZ() - gc1.getPositionPoint().getZ(); offset.setZ(groundZ + pelvisToFoot); robot.setPositionInWorld(offset); }
public VehiclePosePacket(RigidBodyTransform transformFromVehicleToWorld) { Matrix3d rotationMatrix = new Matrix3d(); transformFromVehicleToWorld.getRotation(rotationMatrix); orientation = new Quat4d(); RotationTools.convertMatrixToQuaternion(rotationMatrix, orientation); Vector3d translation = new Vector3d(); transformFromVehicleToWorld.getTranslation(translation); position = new Point3d(translation); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { Vector3d translationToWorld = new Vector3d(); transformToWorld.getTranslation(translationToWorld); this.yoFramePoint.set(translationToWorld); FrameOrientation orientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), transformToWorld); double[] yawPitchRoll = orientation.getYawPitchRoll(); yoFrameOrientation.setYawPitchRoll(yawPitchRoll[0], yawPitchRoll[1], yawPitchRoll[2]); }
public void setPose(RigidBodyTransform rigidBodyTransform, boolean notifyListeners) { rigidBodyTransform.getTranslation(tempFramePoint.getGeometryObject()); rigidBodyTransform.getRotation(tempFrameOrientation.getGeometryObject()); position.set(tempFramePoint.getGeometryObject()); orientation.set(tempFrameOrientation.getGeometryObject()); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { transformToWorld.getTranslation(translationToWorld); x.set(translationToWorld.getX()); y.set(translationToWorld.getY()); z.set(translationToWorld.getZ()); orientation.setIncludingFrame(ReferenceFrame.getWorldFrame(), transformToWorld); setOrientation(orientation); }
protected Footstep createFootstepPlacedAtBipedfootLocation(RobotSide side) { ReferenceFrame soleFrame = soleFrames.get(side); Vector3d translation = new Vector3d(); Quat4d rotation = new Quat4d(); soleFrame.getTransformToWorldFrame().getTranslation(translation); soleFrame.getTransformToWorldFrame().getRotation(rotation); FramePose2d solePose2d = new FramePose2d(soleFrame, new Point2d(translation.getX(), translation.getY()), RotationTools.computeYaw(rotation)); Footstep foot = createFootstep(side, solePose2d); return foot; }