@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { transformToParent.setRotationAndZeroTranslation(rotation); }
public void rotate(Matrix3d rotationMatrix) { tempTransform.setRotationAndZeroTranslation(rotationMatrix); transform.multiply(transform, tempTransform); }
public void rotate(Quat4d rotationQuaternion) { tempTransform.setRotationAndZeroTranslation(rotationQuaternion); transform.multiply(transform, tempTransform); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempAxisAngle.set(axis, angle); transformToParent.setRotationAndZeroTranslation(tempAxisAngle); } }
@Override public void variableChanged(YoVariable<?> v) { AxisAngle4d axisAngle = new AxisAngle4d(jointAxis, offset.getDoubleValue()); preCorruptionTransform.setRotationAndZeroTranslation(axisAngle); frameBeforeJoint.corruptTransformToParentPreMultiply(preCorruptionTransform); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { initialOrientation.getFrameOrientationIncludingFrame(localFrameOrientation); localFrameOrientation.changeFrame(parentFrame); localFrameOrientation.getMatrix3d(localRotation); transformToParent.setRotationAndZeroTranslation(localRotation); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { currentOrientation.getFrameOrientationIncludingFrame(localFrameOrientation); localFrameOrientation.changeFrame(parentFrame); localFrameOrientation.getMatrix3d(localRotation); transformToParent.setRotationAndZeroTranslation(localRotation); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { positionError.changeFrame(bodyFrame); GeometryTools.getAxisAngleFromZUpToVector(positionError.getVector(), rotationToControlFrame); transformToParent.setRotationAndZeroTranslation(rotationToControlFrame); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { initialOrientation.getFrameOrientationIncludingFrame(localFrameOrientation); localFrameOrientation.changeFrame(parentFrame); localFrameOrientation.getMatrix3d(localRotation); transformToParent.setRotationAndZeroTranslation(localRotation); } };
public static RigidBodyTransform generateRandomTransform(Random random) { RigidBodyTransform ret = new RigidBodyTransform(); ret.setRotationAndZeroTranslation(RandomTools.generateRandomRotation(random)); ret.setTranslation(RandomTools.generateRandomVector(random)); return ret; }
private void measureOrientationInFusedFrame(FrameOrientation orientationToPack, IMUSensorReadOnly imu) { // R_{IMU}^{world} imu.getOrientationMeasurement(rotationFromIMUToWorld); transformFromIMUToWorld.setRotationAndZeroTranslation(rotationFromIMUToWorld); // R_{Fused IMU}^{IMU} fusedMeasurementFrame.getTransformToDesiredFrame(transformFromFusedIMUToIMU, imu.getMeasurementFrame()); // R_{Fused IMU}^{world} = R_{IMU}^{world} * R_{Fused IMU}^{IMU} transformFromFusedIMUToWorld.multiply(transformFromIMUToWorld, transformFromFusedIMUToIMU); transformFromFusedIMUToWorld.getRotation(rotationFromFusedIMUToWorld); orientationToPack.setIncludingFrame(fusedMeasurementFrame, rotationFromFusedIMUToWorld); }
private FootstepDataMessage adjustFootstepForAnkleHeight(FootstepDataMessage footstep){ FootstepDataMessage copy = new FootstepDataMessage(footstep); Point3d ankleOffset = new Point3d(0, 0, ankleHeight); RigidBodyTransform footTransform = new RigidBodyTransform(); footTransform.setRotationAndZeroTranslation(copy.getOrientation()); footTransform.transform(ankleOffset); copy.getLocation().add(ankleOffset); return copy; }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { x.set(xAxis.getX(), xAxis.getY(), 0.0); z.set(0.0, 0.0, 1.0); y.cross(z, x); rotation.setColumn(0, x); rotation.setColumn(1, y); rotation.setColumn(2, z); transformToParent.setRotationAndZeroTranslation(rotation); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { x.set(xAxis.getX(), xAxis.getY(), 0.0); z.set(0.0, 0.0, 1.0); y.cross(z, x); rotation.setColumn(0, x); rotation.setColumn(1, y); rotation.setColumn(2, z); transformToParent.setRotationAndZeroTranslation(rotation); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { QuaternionPose pose = viconClient.getQuaternionPose(bodyName); if (pose == null) { pose = new QuaternionPose(); } setDataValid(pose.dataValid); bodyToWorldQuaternion.set(pose.qx, pose.qy, pose.qz, pose.qw); bodyToWorldTransform.setRotationAndZeroTranslation(bodyToWorldQuaternion); bodyToWorldTranslation.set(pose.xPosition, pose.yPosition, pose.zPosition); bodyToWorldTransform.setTranslation(bodyToWorldTranslation); transformToParent.set(bodyToWorldTransform); setLastUpdateTimeStamp(viconClient.getModelReadingTimeStamp(bodyName)); }
private void convertOrientationAndSetOnOutputPort() { transformFromIMUToWorld.setRotationAndZeroTranslation(orientationInputPort.getData()); estimationFrame.getTransformToDesiredFrame(transformFromEstimationFrameToIMUFrame, orientationMeasurementFrame); transformFromEstimationToWorld.multiply(transformFromIMUToWorld, transformFromEstimationFrameToIMUFrame); transformFromEstimationToWorld.getRotation(rotationFromEstimationToWorld); tempOrientationEstimationFrame.setIncludingFrame(ReferenceFrame.getWorldFrame(), rotationFromEstimationToWorld); // Introduce simulated IMU drift tempOrientationEstimationFrame.getYawPitchRoll(estimationFrameYawPitchRoll); estimationFrameYawPitchRoll[0] += imuSimulatedDriftYawAngle.getDoubleValue(); tempOrientationEstimationFrame.setYawPitchRoll(estimationFrameYawPitchRoll); orientationOutputPort.setData(tempOrientationEstimationFrame); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempPoint.setToZero(frameBeforeHipPitchJoint); tempPoint.changeFrame(endEffectorFrame); footToHipAxis.setIncludingFrame(tempPoint); footToHipAxis.changeFrame(getParent()); GeometryTools.getAxisAngleFromZUpToVector(footToHipAxis.getVector(), anklePitchRotationToParentFrame); anklePitchPosition.setToZero(endEffectorFrame); anklePitchPosition.changeFrame(getParent()); anklePitchPosition.get(anklePitchToParentFrame); transformToParent.setRotationAndZeroTranslation(anklePitchRotationToParentFrame); transformToParent.setTranslation(anklePitchToParentFrame); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempPoint.setToZero(frameBeforeHipPitchJoint); tempPoint.changeFrame(endEffectorFrame); footToHipAxis.setIncludingFrame(tempPoint); footToHipAxis.changeFrame(getParent()); GeometryTools.getAxisAngleFromZUpToVector(footToHipAxis.getVector(), hipPitchRotationToParentFrame); hipPitchPosition.setToZero(frameBeforeHipPitchJoint); hipPitchPosition.changeFrame(getParent()); hipPitchPosition.get(hipPitchToParentFrame); transformToParent.setRotationAndZeroTranslation(hipPitchRotationToParentFrame); transformToParent.setTranslation(hipPitchToParentFrame); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { eX.sub(p2.getPoint(), p1.getPoint()); eX.normalize(); eY.sub(p3.getPoint(), p1.getPoint()); // temp only eZ.cross(eX, eY); eZ.normalize(); eY.cross(eZ, eX); rotation.setColumn(0, eX); rotation.setColumn(1, eY); rotation.setColumn(2, eZ); transformToParent.setRotationAndZeroTranslation(rotation); transformToParent.setTranslation(p1.getX(), p1.getY(), p1.getZ()); } }
@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); }