public MovingMidFootZUpGroundFrame(String name, MovingZUpFrame frameOne, MovingZUpFrame frameTwo) { super(name, frameOne.getRootFrame(), true); if (frameOne == frameTwo) throw new IllegalArgumentException("The frames have to be different."); frameOne.verifySameRoots(frameTwo); this.frameOne = frameOne; this.frameTwo = frameTwo; }
MovingZUpFrame zUpFrame = new MovingZUpFrame(randomMovingFrame, "blopButZUp"); zUpFrame.update(); expectedTwist.setAngularPartZ(RotationTools.computeYawRate(yawPitchRoll, originalAngularVelocity, true)); assertTrue(expectedTwist.epsilonEquals(zUpFrame.getTwistOfFrame(), EPSILON));
MovingZUpFrame zUpFrame = new MovingZUpFrame(randomMovingFrame, "blopButZUp"); zUpFrame.update(); expectedTransform.setRotationYaw(originalTransform.getRotationMatrix().getYaw()); EuclidCoreTestTools.assertRigidBodyTransformEquals(expectedTransform, zUpFrame.getTransformToWorldFrame(), EPSILON);
MovingZUpFrame frameOne = new MovingZUpFrame(nonZUpFrameOne, nonZUpFrameOne.getName() + "ZUp"); MovingZUpFrame frameTwo = new MovingZUpFrame(nonZUpFrameTwo, nonZUpFrameTwo.getName() + "ZUp");
MovingZUpFrame zupFrameAfterJoint = new MovingZUpFrame(frameAfterJoint, frameAfterJoint.getName() + "ZUp"); for (Entry<MovingZUpFrame, NumericalMovingReferenceFrame> entry : entrySet) entry.getKey().getTwistOfFrame(expectedTwist); entry.getValue().getTwistOfFrame(actualTwist); expectedTwist.setBodyFrame(entry.getValue());
MovingZUpFrame movingZUpFrameAfterJoint = new MovingZUpFrame(frameAfterJoint, frameAfterJoint.getName() + "ZUp"); actualTransform.set(entry.getValue().getTransformToRoot());
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { TwistReadOnly twistOfFrameOne = frameOne.getTwistOfFrame(); TwistReadOnly twistOfFrameTwo = frameTwo.getTwistOfFrame(); linearVelocityOne.setIncludingFrame(twistOfFrameOne.getLinearPart()); linearVelocityTwo.setIncludingFrame(twistOfFrameTwo.getLinearPart()); linearVelocityOne.changeFrame(this); linearVelocityTwo.changeFrame(this); linearVelocity.setToZero(this); linearVelocity.interpolate(linearVelocityOne, linearVelocityTwo, 0.5); twistRelativeToParentToPack.setToZero(this, getParent(), this); twistRelativeToParentToPack.getLinearPart().set(linearVelocity); if (poseOne.getZ() < poseTwo.getZ()) twistRelativeToParentToPack.setLinearPartZ(linearVelocityOne.getZ()); else twistRelativeToParentToPack.setLinearPartZ(linearVelocityTwo.getZ()); double wz = 0.5 * (twistOfFrameOne.getAngularPartZ() + twistOfFrameTwo.getAngularPartZ()); twistRelativeToParentToPack.setAngularPartZ(wz); } }
@Override public void updateFrames() { fullRobotModel.updateFrames(); pelvisZUpFrame.update(); for (RobotSide robotSide : RobotSide.values) { ankleZUpFrames.get(robotSide).update(); ReferenceFrame handZUpFrame = handZUpFrames.get(robotSide); if (handZUpFrame != null) { handZUpFrame.update(); } footReferenceFrames.get(robotSide).update(); soleFrames.get(robotSide).update(); soleZUpFrames.get(robotSide).update(); } midFeetZUpFrame.update(); midFootZUpGroundFrame.update(); midFeetUnderPelvisWalkDirectionFrame.update(); centerOfMassFrame.update(); }
MovingZUpFrame frameOne = new MovingZUpFrame(nonZUpFrameOne, nonZUpFrameOne.getName() + "ZUp"); MovingZUpFrame frameTwo = new MovingZUpFrame(nonZUpFrameTwo, nonZUpFrameTwo.getName() + "ZUp");
MovingReferenceFrame frameTwo = createMovingReferenceFrame("frameTwo", worldFrame, random, timeProvider); MovingReferenceFrame pelvisFrame = createMovingReferenceFrame("pelvisFrame", worldFrame, random, timeProvider); MovingZUpFrame frameOneZUp = new MovingZUpFrame(frameOne, frameOne.getName() + "ZUp"); MovingZUpFrame frameTwoZUp = new MovingZUpFrame(frameTwo, frameTwo.getName() + "ZUp"); MovingMidFootZUpGroundFrame midFootZUpGroundFrame = new MovingMidFootZUpGroundFrame("midFootZUp", frameOneZUp, frameTwoZUp); MovingWalkingReferenceFrame walkingReferenceFrame = new MovingWalkingReferenceFrame("walkingFrame", pelvisFrame, midFootZUpGroundFrame);
pelvisZUpFrame = new MovingZUpFrame(pelvisFrame, "pelvisZUpFrame"); footReferenceFrames.put(robotSide, footFrame); MovingZUpFrame ankleZUpFrame = new MovingZUpFrame(footFrame, robotSide.getCamelCaseNameForStartOfExpression() + "AnkleZUp"); ankleZUpFrames.put(robotSide, ankleZUpFrame); if (handFrame != null) MovingZUpFrame handZUpFrame = new MovingZUpFrame(handFrame, robotSide.getCamelCaseNameForStartOfExpression() + "HandZUp"); handZUpFrames.put(robotSide, handZUpFrame); soleFrames.put(robotSide, soleFrame); MovingZUpFrame soleZUpFrame = new MovingZUpFrame(soleFrame, soleFrame.getName() + "ZUp"); localSoleZUpFrames.put(robotSide, soleZUpFrame); soleZUpFrames.put(robotSide, soleZUpFrame);