/** * 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); } }