/** * Averages the goal positions from the MotionFrames into a single * PositionMap. The MotionFrames' start positions, timing, and * FrameSources are ignored. * @return PositionMap with averaged goal positions from the MotionFrames */ @Override public PosMap combineFrames(long time, long interval, PosMap curPos, Map<? extends MotionFrame<PosMap>, ? extends FrameSource<PosMap>> frames) { //Use a HashMap instead of a PositionMap since the sum may be greater //than 1.0 before averaging. Map<Id, Double> posSums = new HashMap(); Map<Id, Integer> count = new HashMap(); for(MotionFrame f : frames.keySet()){ sumGoalPositions(f, posSums, count); } PosMap pos = myPositionMapFactory.getValue(); for(Entry<Id,Double> e : posSums.entrySet()){ Id id = e.getKey(); double goal = e.getValue(); double c = count.get(id); double avg = goal/c; avg = Utils.bound(avg, 0.0, 1.0); NormalizedDouble val = new NormalizedDouble(avg); pos.put(id, val); } return pos; }
val += velocities.get(i) * (double)interval; val = Utils.bound(val, 0.0, 1.0); pos.put(i, new NormalizedDouble(val));
private void addJoint(JointId jointId, MotionFrame frame, double startPercent, double stopPercent){ NormalizedDouble normAbsStart = myStartPositions.get(jointId); NormalizedDouble normAbsStop = myGoalPositions.get(jointId); if(normAbsStart == null || normAbsStop == null){ return; } double absStart = normAbsStart.getValue(); double absStop = normAbsStop.getValue(); double range = absStop - absStart; double start = startPercent*range + absStart; start = Utils.bound(start, 0.0, 1.0); double stop = stopPercent*range + absStart; stop = Utils.bound(stop, 0.0, 1.0); frame.getPreviousPositions().put(jointId, new NormalizedDouble(start)); frame.getGoalPositions().put(jointId, new NormalizedDouble(stop)); }