/** * Gets the read-only reference of the position of this shape. * * @return the position of this shape. */ public Tuple3DReadOnly getPosition() { return shapePose.getTranslationVector(); }
public Vector3DReadOnly getDoorKnobGraspingPoint() { return knob.getTranslationVector(); }
/** * Compares {@code this} to {@code other} to determine if the two spheres are geometrically similar, * i.e. the position of each sphere is geometrically similar given {@code epsilon} and the * difference between the radius of each sphere is less than or equal to {@code epsilon}. * * @param other the sphere to compare to. Not modified. * @param epsilon the tolerance of the comparison. * @return {@code true} if the two boxes represent the same geometry, {@code false} otherwise. */ @Override public boolean geometricallyEquals(Sphere3D other, double epsilon) { return Math.abs(radius - other.radius) <= epsilon && shapePose.getTranslationVector().geometricallyEquals(other.shapePose.getTranslationVector(), epsilon); } }
public static double getMagnitudeOfTranslation(RigidBodyTransform rigidBodyTransform) { return rigidBodyTransform.getTranslationVector().length(); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { yoFramePoint.set(transformToWorld.getTranslationVector()); yoFrameYawPitchRoll.set(transformToWorld.getRotationMatrix()); }
/** * Sets this pose 3D to match the given rigid-body transform. * * @param rigidBodyTransform the transform use to set this pose 3D. Not modified. */ default void set(RigidBodyTransform rigidBodyTransform) { setPosition(rigidBodyTransform.getTranslationVector()); setOrientation(rigidBodyTransform.getRotationMatrix()); }
public static double getDistance(RigidBodyTransform from, RigidBodyTransform to, double positionWeight, double orientationWeight) { Point3D pointFrom = new Point3D(from.getTranslationVector()); Quaternion orientationFrom = new Quaternion(from.getRotationMatrix()); Point3D pointTo = new Point3D(to.getTranslationVector()); Quaternion orientationTo = new Quaternion(to.getRotationMatrix()); double positionDistance = positionWeight * pointFrom.distance(pointTo); double orientationDistance = orientationWeight * orientationFrom.distance(orientationTo); double distance = positionDistance + orientationDistance; return distance; }
public void getAnklePosition2d(FramePoint2D position2dToPack, RigidBodyTransform transformFromAnkleToSole) { tempTransform.setRotation(footstepPose.getOrientation()); tempTransform.setTranslation(footstepPose.getPosition()); tempTransform.multiply(transformFromAnkleToSole); double x = tempTransform.getTranslationVector().getX(); double y = tempTransform.getTranslationVector().getY(); position2dToPack.setIncludingFrame(footstepPose.getReferenceFrame(), x, y); }
public static void packExtrapolatedTransform(RigidBodyTransform from, RigidBodyTransform to, double ratio, RigidBodyTransform toPack) { Point3D pointToPack = new Point3D(); RotationMatrix orientationToPack = new RotationMatrix(); packExtrapolatedPoint(from.getTranslationVector(), to.getTranslationVector(), ratio, pointToPack); packExtrapolatedOrienation(from.getRotationMatrix(), to.getRotationMatrix(), ratio, orientationToPack); toPack.setIdentity(); toPack.setTranslation(pointToPack); toPack.setRotation(orientationToPack); }
@Test public void testGetTranslation() throws Exception { Random random = new Random(2345L); RigidBodyTransform transform = EuclidCoreRandomTools.nextRigidBodyTransform(random); Vector3D translation = new Vector3D(); transform.getTranslation(translation); for (int row = 0; row < 3; row++) assertTrue(translation.getElement(row) == transform.getElement(row, 3)); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); translation.set(transform.getTranslationX(), transform.getTranslationY(), transform.getTranslationZ()); EuclidCoreTestTools.assertTuple3DEquals(translation, transform.getTranslationVector(), EPS); }
@Override public double compute(FootstepNode startNode, FootstepNode endNode) { RigidBodyTransform startNodeTransform = new RigidBodyTransform(); RigidBodyTransform endNodeTransform = new RigidBodyTransform(); FootstepNodeSnapData endNodeData = snapper.getSnapData(endNode); FootstepNodeSnapData startNodeData = snapper.getSnapData(startNode); if (startNodeData == null || endNodeData == null) return 0.0; FootstepNodeTools.getSnappedNodeTransform(startNode, startNodeData.getSnapTransform(), startNodeTransform); FootstepNodeTools.getSnappedNodeTransform(endNode, endNodeData.getSnapTransform(), endNodeTransform); double heightChange = endNodeTransform.getTranslationVector().getZ() - startNodeTransform.getTranslationVector().getZ(); if (heightChange > 0.0) return costParameters.getStepUpWeight() * Math.pow(stepHeightScalar * heightChange, 2.0); else return costParameters.getStepDownWeight() * Math.pow(stepHeightScalar * heightChange, 2.0); } }
/** * Second pass that can be done iteratively and each iteration is independent. * <p> * This pass is only needed when the {@code jacobianFrame} is at the center of mass and that this * calculator has to update. It recomputes the intermediate variable {@link #centerOfMassTimesMass} * at the center of mass frame so it can be used in the next and last pass. * </p> */ public void passTwo() { centerOfMassTimesMass.setIncludingFrame(centerOfMass); centerOfMassTimesMass.sub(jacobianFrame.getTransformToRoot().getTranslationVector()); centerOfMassTimesMass.setReferenceFrame(jacobianFrame); centerOfMassTimesMass.scale(subTreeMass); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { position.set(transformToWorld.getTranslationVector()); if (isUsingYawPitchRoll()) yawPitchRoll.set(transformToWorld.getRotationMatrix()); else quaternion.set(transformToWorld.getRotationMatrix()); }
/** * Appends the given {@code transform} to this pose 3D. * * @param transform the rigid-body transform to append to this pose 3D. Not modified. */ default void appendTransform(RigidBodyTransform transform) { QuaternionTools.addTransform(getOrientation(), transform.getTranslationVector(), getPosition()); getOrientation().append(transform.getRotationMatrix()); }
public static VehiclePosePacket createVehiclePosePacket(RigidBodyTransform transformFromVehicleToWorld) { VehiclePosePacket message = new VehiclePosePacket(); message.getOrientation().set(transformFromVehicleToWorld.getRotationMatrix()); message.getPosition().set(transformFromVehicleToWorld.getTranslationVector()); return message; }
/** * trajectory for current transform to goal transform */ public static Pose3D computeLinearTrajectory(double time, double trajectoryTime, RigidBodyTransform from, RigidBodyTransform to) { double progress = time / trajectoryTime; Point3D fromPoint = new Point3D(from.getTranslationVector()); Point3D toPoint = new Point3D(to.getTranslationVector()); Quaternion fromOrienation = new Quaternion(from.getRotationMatrix()); Quaternion toOrienation = new Quaternion(to.getRotationMatrix()); Point3D point = new Point3D(); Quaternion orientation = new Quaternion(); point.interpolate(fromPoint, toPoint, progress); orientation.interpolate(fromOrienation, toOrienation, progress); return new Pose3D(point, orientation); }
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 double compute(FootstepNode startNode, FootstepNode endNode) { RigidBodyTransform startNodeTransform = new RigidBodyTransform(); RigidBodyTransform endNodeTransform = new RigidBodyTransform(); FootstepNodeSnapData endNodeData = snapper.getSnapData(endNode); FootstepNodeSnapData startNodeData = snapper.getSnapData(startNode); if (startNodeData == null || endNodeData == null) return 0.0; FootstepNodeTools.getSnappedNodeTransform(startNode, startNodeData.getSnapTransform(), startNodeTransform); FootstepNodeTools.getSnappedNodeTransform(endNode, endNodeData.getSnapTransform(), endNodeTransform); double heightChange = endNodeTransform.getTranslationVector().getZ() - startNodeTransform.getTranslationVector().getZ(); if (heightChange > 0.0) return costParameters.getStepUpWeight() * heightChange; else return -costParameters.getStepDownWeight() * heightChange; } }
public boolean solveFor(FramePoint3DReadOnly position) { kinematicsToolboxController.requestInitialize(); FramePoint3D desiredPosition = new FramePoint3D(position); desiredPosition.changeFrame(ReferenceFrame.getWorldFrame()); KinematicsToolboxRigidBodyMessage message = MessageTools.createKinematicsToolboxRigidBodyMessage(endEffector, desiredPosition); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(1.0)); message.getControlFramePositionInEndEffector().set(controlFramePoseInEndEffector.getTranslationVector()); message.getControlFrameOrientationInEndEffector().set(controlFramePoseInEndEffector.getRotationMatrix()); commandInputManager.submitMessage(message); return solveAndRetry(50); }
public boolean solveFor(FramePoint3DReadOnly position, FrameQuaternionReadOnly orientation) { kinematicsToolboxController.requestInitialize(); FramePoint3D desiredPosition = new FramePoint3D(position); desiredPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameQuaternion desiredOrientation = new FrameQuaternion(orientation); desiredOrientation.changeFrame(ReferenceFrame.getWorldFrame()); KinematicsToolboxRigidBodyMessage message = MessageTools.createKinematicsToolboxRigidBodyMessage(endEffector, desiredPosition, desiredOrientation); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(1.0)); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(1.0)); message.getControlFramePositionInEndEffector().set(controlFramePoseInEndEffector.getTranslationVector()); message.getControlFrameOrientationInEndEffector().set(controlFramePoseInEndEffector.getRotationMatrix()); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(angularSelection)); commandInputManager.submitMessage(message); return solveAndRetry(maximumNumberOfIterations.getIntegerValue()); }