/** * 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 move(RobotPositionMap positions, long lenMillisec) { MotionFrame frame = new DefaultMotionFrame(); frame.setFrameLengthMillisec(lenMillisec); frame.setGoalPositions(positions); frame.setPreviousPositions(myPreviousPositions); frame.setTimestampMillisecUTC(TimeUtils.now()); myRobotClient.sendMovement(frame); myPreviousPositions = positions; setGoals(myPreviousPositions); }
@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); } }
frame.setTimestampMillisecUTC(time); frame.setFrameLengthMillisec(interval); for(Entry<Robot.JointId,NormalizedDouble> e : currentPos.entrySet()){ disableAtGoal(); frame.setGoalPositions(goals); return frame;
@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 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)); }
@Override public Map<Integer,Double> advanceAnimation(long time, long interval){ MotionFrame frame = getMovements(time, interval); if(frame == null){ return null; } return frame.getGoalPositions(); }
frame.setTimestampMillisecUTC(currentTimeUTC); frame.setFrameLengthMillisec(moveLengthMilliSec); frame.setPreviousPositions(new RobotPositionHashMap(myPreviousPositions)); RobotPositionMap goals = new Robot.RobotPositionHashMap(myGoalPositions.size()); for(Entry<JointId,NormalizedDouble> e : myGoalPositions.entrySet()){ goals.put(jointId, interval); frame.setGoalPositions(goals); return frame;
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); } } }
frame.setFrameLengthMillisec(moveLengthMilliSec); frame.setTimestampMillisecUTC(currentTimeUTC); frame.setPreviousPositions(new RobotPositionHashMap()); frame.setGoalPositions(new RobotPositionHashMap());
Map<Id, Double> velocities = new HashMap(); if(!frames.isEmpty()){ return frames.keySet().iterator().next().getGoalPositions();
frame.setTimestampMillisecUTC(time); frame.setFrameLengthMillisec(interval); myPreviousPositions.clear(); long cur = getCurrentTime(time); myPreviousPositions.put(djId, val); frame.setPreviousPositions(myPreviousPositions); myPreviousPositions.clear(); long next = getCurrentTime(time+interval); myPreviousPositions.put(djId, val); frame.setGoalPositions(myPreviousPositions); long unrampedTime = cur - myRampTimeMillisec; if(isComplete(unrampedTime)){
frame.setTimestampMillisecUTC(time); frame.setFrameLengthMillisec(interval); myPreviousPositions.clear(); long cur = getCurrentTime(time); myPreviousPositions.put(djId, val); frame.setPreviousPositions(myPreviousPositions); myPreviousPositions.clear(); cur = getCurrentTime(time+interval); myPreviousPositions.put(djId, val); frame.setGoalPositions(myPreviousPositions); if(isComplete(cur)){ complete(time);