private void sumGoalPositions(MotionFrame<PosMap> f, Map<Id, Double> posSums, Map<Id, Integer> count) { PosMap goals = f.getGoalPositions(); if(goals == null){ return; } for(Entry<Id,NormalizedDouble> e : goals.entrySet()){ Id id = (Id)e.getKey(); NormalizedDouble goal = e.getValue(); double val = goal.getValue(); int c = 1; if(posSums.containsKey(id)){ val += posSums.get(id); c += count.get(id); } posSums.put(id, val); count.put(id, c); } } }
@Override public void blend(long time, long interval, Map<? extends MF, ? extends FS> frames) { if(frames == null || frames.isEmpty() || myFrameCombiner == null || myOutput == null){ return; } PosMap curPos = myOutput.getPositions(); if(curPos == null || curPos.isEmpty()){ return; } PosMap pos = myFrameCombiner.combineFrames(time, interval, curPos, frames); myOutput.write(pos, interval); } }
/** * 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; }
/** * Return the MotionFrame velocities. * @param frame MotionFrame to use to calculate velocities * @return MotionFrame velocities */ private Map<Id,Double> getVelocities(MotionFrame<PosMap> frame){ Map<Id,Double> vels = new HashMap(); if(frame.getFrameLengthMillisec() <= 0){ return vels; } for(Id i : frame.getGoalPositions().keySet()){ NormalizedDouble goal = frame.getGoalPositions().get(i); NormalizedDouble prev = frame.getPreviousPositions().get(i); if(goal == null || prev == null){ vels.put(i, 0.0); } double diff = goal.getValue() - prev.getValue(); double vel = diff/frame.getFrameLengthMillisec(); vels.put(i, vel); } return vels; } }
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)); }