public void setBackwardTransform(RigidBodyTransform backwardTransform) { this.backwardTransform.set(backwardTransform); backwardTransformIsOutOfDate = false; forwardTransformIsOutOfDate = true; }
protected void setTransformToParent(RigidBodyTransform transformToParent) { this.transformToParent.set(transformToParent); this.transformToRootID = Long.MIN_VALUE; // transformToParent.normalize(); }
/** * Set this transform equal to the RigidBodyTransformed sent as an argument. * @param transform */ public RigidBodyTransform(RigidBodyTransform transform) { set(transform); }
/** * Create transformation matrix from Matrix4f * * @param mat4d */ public RigidBodyTransform(Matrix4f matrix) { set(matrix); }
public void setTransformFromWorld(RigidBodyTransform transformFromWorld, double time) { synchronized (lidarTransform) { this.lidarTransform.set(transformFromWorld); this.time = time; } }
/** * Create RigidBodyTransform from AxisAngle4f and Tuple3f * * @param axisAngle * @param translation */ public RigidBodyTransform(AxisAngle4f axisAngle, Vector3f translation) { set(axisAngle, translation); }
/** * Get the transform from local coordinates to world coordinates. * @param transformToPack used to store the transform. */ public void getTransformToWorld(RigidBodyTransform transformToPack) { transformToPack.set(fromLocalToWorldTransform); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(rotation, translation); } }
/** * Create transformation matrix from Matrix4d * * @param mat4d */ public RigidBodyTransform(Matrix4d matrix) { set(matrix); }
public TimeStampedTransform3D(RigidBodyTransform transform3D, long timeStamp) { this.transform3D.set(transform3D); this.timeStamp = timeStamp; }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(cameraRigidTransform); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(transformFromMocapHeadToRobotHead); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(mocapHeadPoseInZUp); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.set(mocapRigidBodyTransform); } };
public void setTransformFromControlFrameToEndEffectorBodyFixedFrame(RigidBodyTransform transformFromControlFrameToEndEffectorBodyFixedFrame) { endEffectorToControlFrameOffset.set(transformFromControlFrameToEndEffectorBodyFixedFrame); endEffectorToControlFrameOffset.invert(); }
public void translateThenRotate(RigidBodyTransform translateThenRotateTransform) { tempTransform.set(translateThenRotateTransform); transform.multiply(transform, tempTransform); }
public void setPose(Point3d position, Quat4d orientation) { RigidBodyTransform fromShape = getTransformFromShapeFrameUnsafe(); fromShape.set(orientation, position); setTransformFromShapeFrame(fromShape); }
public void corruptTransformToParentPreMultiply(RigidBodyTransform preCorruptionTransform) { if (this.preCorruptionTransform == null) { this.preCorruptionTransform = new RigidBodyTransform(); } this.preCorruptionTransform.set(preCorruptionTransform); this.update(); }
private void computeRootJointToWorldTransform(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, RigidBodyTransform rootJointToWorldToPack, RigidBodyTransform estimationLinkTransform) { // H_{root}^{estimation} rootJoint.getFrameAfterJoint().getTransformToDesiredFrame(tempRootJointFrameToEstimationFrame, estimationFrame); // H_{root}^{w} = H_{estimation}^{w} * H_{root}^{estimation} rootJointToWorldToPack.set(estimationLinkTransform); rootJointToWorldToPack.multiply(tempRootJointFrameToEstimationFrame); }
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); }