public VirtualChainDataPoint(
ReferenceFrame baseFrame,
ArrayList<ReferenceFrame> referenceFrames,
FramePoint2d centerOfMassProjection)
// public VirtualChainDataPoint(ReferenceFrame baseFrame, ArrayList<ReferenceFrame>
// referenceFrames, FramePoint centerOfMassProjection)
{
centerOfMassProjection.checkReferenceFrameMatch(baseFrame);
if (!centerOfMassProjection.getReferenceFrame().isZupFrame()) {
throw new RuntimeException("centerOfMassProjection is Not a ZUp reference frame!");
}
this.centerOfMassProjection = new FramePoint2d(centerOfMassProjection);
// centerOfMassProjection.changeFrame(baseFrame);
rotationMatrices = new ArrayList<Matrix3d>();
for (int i = 0; i < referenceFrames.size(); i++) {
ReferenceFrame virtualChainFrame = referenceFrames.get(i);
RigidBodyTransform transform3D = virtualChainFrame.getTransformToDesiredFrame(baseFrame);
Matrix3d rotationMatrix = new Matrix3d();
transform3D.get(rotationMatrix);
rotationMatrices.add(rotationMatrix);
}
}
public void updateRobotLocationBasedOnMultisensePose(RigidBodyTransform headPose) {
RigidBodyTransform pelvisPose = new RigidBodyTransform();
RigidBodyTransform transformFromHeadToPelvis =
pelvisFrame.getTransformToDesiredFrame(headFrame);
pelvisPose.multiply(headPose, transformFromHeadToPelvis);
atomicPelvisPose.set(pelvisPose);
}
private Matrix3d computeDesiredFootRotation(
double angleToDestination, RobotSide swingLegSide, ReferenceFrame supportFootFrame) {
RigidBodyTransform supportFootToWorldTransform =
supportFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
Matrix3d supportFootToWorldRotation = new Matrix3d();
supportFootToWorldTransform.get(supportFootToWorldRotation);
double maxTurnInAngle = 0.25;
double maxTurnOutAngle = 0.4;
double amountToYaw;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
amountToYaw = AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI);
break;
}
case LEFT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0);
break;
}
case RIGHT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0);
break;
}
case FORWARD:
{
amountToYaw = angleToDestination;
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
if (swingLegSide == RobotSide.LEFT) {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnInAngle, maxTurnOutAngle);
} else {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnOutAngle, maxTurnInAngle);
}
Matrix3d yawRotation = new Matrix3d();
yawRotation.rotZ(amountToYaw);
Matrix3d ret = new Matrix3d();
ret.mul(yawRotation, supportFootToWorldRotation);
return ret;
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testCommonUsageExample() {
String namePrefix = "point";
String nameSuffix = "toTest";
YoVariableRegistry registry = new YoVariableRegistry("myRegistry");
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
PoseReferenceFrame poseFrame = new PoseReferenceFrame("poseFrame", new FramePose(worldFrame));
FramePoint poseFramePosition = new FramePoint(worldFrame, new Point3d(0.5, 7.7, 9.2));
poseFrame.setPositionAndUpdate(poseFramePosition);
FrameOrientation poseOrientation =
new FrameOrientation(worldFrame, new AxisAngle4d(1.2, 3.9, 4.7, 2.2));
poseFrame.setOrientationAndUpdate(poseOrientation);
YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame, poseFrame);
SimpleSO3TrajectoryPoint simpleTrajectoryPoint = new SimpleSO3TrajectoryPoint();
double time = 3.4;
TransformableQuat4d orientation = new TransformableQuat4d(new Quat4d(0.1, 0.22, 0.34, 0.56));
orientation.normalize();
Vector3d angularVelocity = new Vector3d(1.7, 8.4, 2.2);
simpleTrajectoryPoint.set(time, orientation, angularVelocity);
yoFrameSO3TrajectoryPoint.setIncludingFrame(worldFrame, simpleTrajectoryPoint);
yoFrameSO3TrajectoryPoint.changeFrame(poseFrame);
// Do some checks:
RigidBodyTransform transformToPoseFrame = worldFrame.getTransformToDesiredFrame(poseFrame);
orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(angularVelocity);
namePrefix = "point";
nameSuffix = "toVerify";
YoFrameSO3TrajectoryPoint expectedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, poseFrame);
expectedYoFrameSO3TrajectoryPoint.setTime(time);
expectedYoFrameSO3TrajectoryPoint.setOrientation(orientation);
expectedYoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity);
assertEquals(3.4, yoFrameSO3TrajectoryPoint.getTime(), 1e-7);
assertEquals(3.4, expectedYoFrameSO3TrajectoryPoint.getTime(), 1e-7);
assertTrue(expectedYoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPoint, 1e-10));
}
private void computeOrientationStateOutputBlock() {
estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame());
tempTransform.getRotation(rotationFromEstimationToWorld);
tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData());
tempCenterOfMassPosition.changeFrame(estimationFrame);
ReferenceFrame referenceFrame =
referenceFrameMap.getFrameByName(
pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
tempFramePoint.setIncludingFrame(
referenceFrame,
pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame());
tempFramePoint.changeFrame(estimationFrame);
tempFramePoint.sub(tempCenterOfMassPosition);
tempFramePoint.scale(-1.0);
MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint());
tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix);
MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort));
}
