@Override public void initialize() { rotationFrozenOffset.setIdentity(); updateRootJointOrientationAndAngularVelocity(); }
@Override public void setToNaN() { super.setIdentity(); super.mul(Double.NaN); }
rotmat.setIdentity();
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front vertex-centered break; case 1: m.rotX(Math.PI); // back face-centered break; case 2: double angle = Math.PI - 0.5 * TETRAHEDRAL_ANGLE; // Side edge-centered m.rotX(angle); break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
public OrientationLatencyCorruptor(String namePrefix, int latencyTicks, YoVariableRegistry parentRegistry) { registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); index = new IntegerYoVariable(namePrefix + "Index", registry); this.latencyTicks = latencyTicks; parentRegistry.addChild(registry); orientations = new Matrix3d[latencyTicks+1]; for (int i=0; i<=latencyTicks; i++) { Matrix3d orientation = new Matrix3d(); orientation.setIdentity(); orientations[i] = orientation; } }
public void setMomentOfInertia(double Ixx, double Iyy, double Izz) { massMomentOfInertiaPart.setIdentity(); massMomentOfInertiaPart.setM00(Ixx); massMomentOfInertiaPart.setM11(Iyy); massMomentOfInertiaPart.setM22(Izz); }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // C4 vertex-centered break; case 1: m.rotX(-0.5 * TETRAHEDRAL_ANGLE); // C3 face-centered 2.0*Math.PI/3 Matrix3d m1 = new Matrix3d(); m1.rotZ(Math.PI/4); m.mul(m1); break; case 2: m.rotY(Math.PI/4); // side face-centered break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front vertex-centered break; case 1: m.rotX(-0.6523581397843639); // back face-centered -0.5535743588970415 m.rotX(Math.toRadians(-26)); break; case 2: m.rotZ(Math.PI/2); Matrix3d m1 = new Matrix3d(); m1.rotX(-1.0172219678978445); m.mul(m1); break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front break; case 1: m.rotY(Math.PI/2); // left break; case 2: m.rotY(Math.PI); // back break; case 3: m.rotY(-Math.PI/2); // right break; case 4: m.rotX(Math.PI/2); // top break; case 5: m.rotX(-Math.PI/2); // bottom break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; } }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front break; case 1: m.rotX(Math.PI/2); // side edge-centered break; case 2: m.rotY(Math.PI/n); // side face-centered Matrix3d m1 = new Matrix3d(); m1.rotX(Math.PI/2); m.mul(m1); break; case 3: m.set(flipX()); // back break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
private boolean exponentialCoordinatesOK() { Twist twist = new Twist(jacobian.getEndEffectorFrame(), jacobian.getBaseFrame(), jacobian.getJacobianFrame(), error); Matrix3d rotationCheck = new Matrix3d(); rotationCheck.setIdentity(); Vector3d positionCheck = new Vector3d(); ScrewTestTools.integrate(rotationCheck, positionCheck, 1.0, twist); RigidBodyTransform transformCheck = new RigidBodyTransform(rotationCheck, positionCheck); return transformCheck.epsilonEquals(errorTransform, 1e-5); }
@Override public void initializeForFrozenState() { rotationFrozenOffset.setIdentity(); // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); double initialYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); rootJointYawOffsetFromFrozenState.set(initialYaw); rotationFrozenOffset.rotZ(initialYaw); yoRootJointFrameQuaternion.setToZero(); yoRootJointFrameOrientation.setToZero(); yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
@Override protected void computeRotationTranslation(Transform3d transform3D) { reshapingMatrix.setIdentity(); reshapingMatrix.setM00(reshapingMatrix.getM00() * radii.getX()); reshapingMatrix.setM11(reshapingMatrix.getM11() * radii.getY()); reshapingMatrix.setM22(reshapingMatrix.getM22() * radii.getZ()); reshapingTransform.setRotationAndZeroTranslation(reshapingMatrix); transform3D.setIdentity(); translationVector.set(x.getDoubleValue(), y.getDoubleValue(), z.getDoubleValue()); transform3D.setScale(scale); transform3D.setRotationEulerAndZeroTranslation(roll.getDoubleValue(), pitch.getDoubleValue(), yaw.getDoubleValue()); transform3D.setTranslation(translationVector); transform3D.multiply(reshapingTransform); }
private void setupSymmetricModeListeners() oppositeSignForYawAndRollOnly.setIdentity(); oppositeSignForYawAndRollOnly.setM00(-1.0); oppositeSignForYawAndRollOnly.setM22(-1.0);
private void setupSymmetricModeListeners() oppositeSignForYawAndRollOnly.setIdentity(); oppositeSignForYawAndRollOnly.setM00(-1.0); oppositeSignForYawAndRollOnly.setM22(-1.0);
parentRegistry.addChild(registry); rotationError.setIdentity(); pelvisRotation.setIdentity();
phiJPhi.setIdentity();