@Override public Map<Integer,Double> advanceAnimation(long time, long interval){ MotionFrame frame = getMovements(time, interval); if(frame == null){ return null; } return frame.getGoalPositions(); }
@Override public Map<Integer,Double> advanceAnimation(long time, long interval){ MotionFrame frame = getMovements(time, interval); if(frame == null){ return null; } return frame.getGoalPositions(); }
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); } } }
Map<Id, Double> velocities = new HashMap(); if(!frames.isEmpty()){ return frames.keySet().iterator().next().getGoalPositions();
/** * 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; } }
@Override public void handleEvent(MotionFrameEvent event) { if(event == null){ return; }else if(event.getMotionFrame() == null){ return; } RobotPositionMap goals = event.getMotionFrame().getGoalPositions(); long duration = event.getMotionFrame().getFrameLengthMillisec(); if(goals == null){ return; } move(goals, duration); } }
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)); }