@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); } }
@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); }
/** * 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; } }
/** * Creates a RemoteJoint with the given values. * @param robot RemoteRobot this RemoteJoint belongs to * @param def JointDefinition for initializing this Joint */ public RemoteJoint(RemoteRobot robot, JointDefinition def){ if(robot == null || def == null){ throw new NullPointerException(); } myRobot = robot; myJointId = def.getJointId(); myRobotJointId = new Robot.JointId(myRobot.getRobotId(), myJointId); myName = def.getName(); myDefaultPosition = def.getDefaultPosition(); myCachedGoalPosition = def.getGoalPosition(); myEnabledFlag = def.getEnabled(); myRange = new DoubleRange(0.0, 1.0); myProperties = new HashMap<String, JointProperty>(); for(JointPropDefinition p : def.getJointProperties()){ JointProperty<Double> prop = new RemoteJointProperty<Double>( p.getPropertyName(), p.getDisplayName(), Double.class, p.getInitialValue(), new DoubleRange(p.getMinValue(), p.getMaxValue())); myProperties.put(p.getPropertyName(), prop); } }
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)); }
private boolean isMatch(RobotRequest req, RobotResponseHeader resp){ if(req == null || resp == null){ theLogger.info("Received null request or response header, unable to determine match."); throw new NullPointerException(); } if(!req.getRobotId().equals(resp.getRobotId())){ theLogger.log(Level.INFO, "Requested Robot Id ({0}) does not match Response Robot Id ({1}).", new Object[]{req.getRobotId(), resp.getRobotId()}); return false; }else if(!req.getRequestType().equals(resp.getRequestType())){ theLogger.log(Level.INFO, "Request Type ({0}) does not match Response Type ({1}).", new Object[]{req.getRequestType(), resp.getRequestType()}); return false; /*}else if(!req.getSourceId().equals(resp.getDestinationId())){ return false; }else if(!req.getDestinationId().equals(resp.getSourceId())){ return false; */}else if(req.getTimestampMillisecUTC() != resp.getRequestTimestampMillisecUTC()){ theLogger.log(Level.INFO, "Request Timestamp ({0}) does not match Response Request Timestamp ({1}).", new Object[]{req.getTimestampMillisecUTC(), resp.getRequestTimestampMillisecUTC()}); return false; } return true; } }
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 Map<Integer,Double> advanceAnimation(long time, long interval){ MotionFrame frame = getMovements(time, interval); if(frame == null){ return null; } return frame.getGoalPositions(); }
private RobotResponseHeader getHeader(RobotRequest req){ return myResponseFactory.createHeader( getRobotId(), mySourceId, myDestinationId, req.getRequestType(), req.getTimestampMillisecUTC()); }
/** * Retrieves the Joint specified in the RobotRequest. * @param req RobotRequest specifying a Joint * @return Joint specified in the RobotRequest */ protected Joint getRequestedJoint(RobotRequest req){ Integer jIdInt = req.getRequestIndex(); if(jIdInt == null){ throw new NullPointerException(); } Joint.Id jId = new Joint.Id(jIdInt); Robot.Id rId = req.getRobotId(); JointId jointId = new Robot.JointId(rId, jId); Joint j = getRobot().getJoint(jointId); if(j == null){ throw new NullPointerException(); } return j; }
private Boolean makeStatusRequest(String requestType, long timeout){ RobotStatusResponse resp = makeBlockingRequest( RobotStatusResponse.class, requestType, timeout); if(resp == null){ theLogger.log(Level.WARNING, "Received null status for: {0}, from: {1}", new Object[]{requestType, myRobotId.getRobtIdString()}); return null; } return resp.getStatusResponse(); }
/** * Sends a MotionFrame to move a RemoteRobot. * @param frame MotionFrame to send */ public void sendMovement(MotionFrame frame){ MotionFrameEvent mfe = myMotionFrameAdapter.createMotionFrameEvent( mySourceId, myDestinationId, frame); myMotionFrameSender.notifyListeners(mfe); }
private RobotPositionMap makePositionRequest( String requestType, long timeout){ RobotPositionResponse resp = makeBlockingRequest( RobotPositionResponse.class, requestType, timeout); if(resp == null){ theLogger.log(Level.WARNING, "Received null positions for: {0}, from: {1}", new Object[]{requestType, myRobotId.getRobtIdString()}); return null; } return resp.getPositionMap(); }
@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); } }
private void updateRobotDefinition(){ RobotDefinitionResponse robotDef = myRobotClient.requestRobotDefinition(); if(robotDef == null){ theLogger.warning("RobotRequest timed out. " + "Unable to update definition."); throw new NullPointerException(); } for(JointDefinition def : robotDef.getJointDefinitions()){ RemoteJoint rj = new RemoteJoint(this, def); addJoint(rj); } } /**
@Override public void handleEvent(RobotRequest event) { String reqType = event.getRequestType(); if(reqType == null){ theLogger.info("Received RobotRequest with null RequestType.");
MotionFrame frame = new DefaultMotionFrame(); 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;
@Override public Map<Integer,Double> advanceAnimation(long time, long interval){ MotionFrame frame = getMovements(time, interval); if(frame == null){ return null; } return frame.getGoalPositions(); }
private Boolean makeStatusRequestForJoint(String requestType, JointId jointId, long timeout){ RobotStatusResponse resp = makeBlockingRequestForJoint( requestType, jointId, timeout); if(resp == null){ theLogger.log(Level.WARNING, "Received null positions for: {0}, from: {1}", new Object[]{requestType, myRobotId.getRobtIdString()}); return null; } return resp.getStatusResponse(); }
new DefaultMotionFrame<RobotPositionMap>(); frame.setFrameLengthMillisec(moveLengthMilliSec); frame.setTimestampMillisecUTC(currentTimeUTC); frame.setPreviousPositions(new RobotPositionHashMap()); frame.setGoalPositions(new RobotPositionHashMap());