MovingZUpFrame frameTwo = new MovingZUpFrame(nonZUpFrameTwo, nonZUpFrameTwo.getName() + "ZUp"); MovingMidFootZUpGroundFrame movingMidFrameZUpFrame = new MovingMidFootZUpGroundFrame("midFrame" + i, frameOne, frameTwo); for (Entry<MovingMidFootZUpGroundFrame, NumericalMovingReferenceFrame> entry : entrySet) entry.getKey().getTwistOfFrame(expectedTwist); entry.getValue().getTwistOfFrame(actualTwist); expectedTwist.setBodyFrame(entry.getValue());
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { poseOne.setToZero(frameOne); poseTwo.setToZero(frameTwo); poseOne.changeFrame(getParent()); poseTwo.changeFrame(getParent()); pose.setToZero(getParent()); pose.interpolate(poseOne, poseTwo, 0.5); pose.setZ(Math.min(poseOne.getZ(), poseTwo.getZ())); pose.get(transformToParent); }
MovingZUpFrame frameTwo = new MovingZUpFrame(nonZUpFrameTwo, nonZUpFrameTwo.getName() + "ZUp"); MovingMidFootZUpGroundFrame movingMidFrameZUpFrame = new MovingMidFootZUpGroundFrame("midFrame" + i, frameOne, frameTwo); MidFootZUpGroundFrame midFrameZUpFrame = new MidFootZUpGroundFrame("midFrame" + i, frameOne, frameTwo); actualTransform.set(entry.getValue().getTransformToRoot());
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);
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { twistRelativeToParentToPack.setToZero(this, getParent(), this); TwistReadOnly twistOfMidFootFrame = midFootZUpGroundFrame.getTwistOfFrame(); angularVelocity.setIncludingFrame(twistOfMidFootFrame.getAngularPart()); linearVelocity.setIncludingFrame(twistOfMidFootFrame.getLinearPart()); pelvisFrame.getTwistRelativeToOther(midFootZUpGroundFrame, twistOfPelvisRelativeToMidFootFrame); linearPelvisVelocity.setIncludingFrame(twistOfPelvisRelativeToMidFootFrame.getLinearPart()); linearPelvisVelocity.changeFrame(this); angularVelocity.changeFrame(this); linearVelocity.changeFrame(this); linearVelocity.add(linearPelvisVelocity.getX(), 0.0, 0.0); offset.set(pelvisPosition.getX(), 0.0, 0.0); linearVelocityOffset.cross(angularVelocity, offset); linearVelocity.add(linearVelocityOffset); twistRelativeToParentToPack.getAngularPart().set(angularVelocity); twistRelativeToParentToPack.getLinearPart().set(linearVelocity); } }
@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(); }
midFootZUpGroundFrame = new MovingMidFootZUpGroundFrame("midFeetZUpAverageYaw", localSoleZUpFrames.get(RobotSide.LEFT), localSoleZUpFrames.get(RobotSide.RIGHT));
@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); } }