@Override public NormalizedDouble getMax() { return new NormalizedDouble(1.0); } };
public List<BoneProjectionPosition> getRotationListForNormPos(NormalizedDouble normPos) { List<BoneProjectionPosition> rotations = new ArrayList<BoneProjectionPosition>(myBoneProjectionRanges.size()); for(BoneProjectionRange range : myBoneProjectionRanges){ rotations.add(range.makePositionForNormalizedFraction(normPos.getValue())); } return rotations; } public List<BoneProjectionPosition> getRotationListForCurrentGoal() {
@Override public void handlePositions(long x, Map<Integer, Double> positions) { Robot r = getRobot(); if(r == null){ return; } Robot.RobotPositionMap posMap = new Robot.RobotPositionHashMap(positions.size()); for(Entry<Integer,Double> e : positions.entrySet()){ Integer id = e.getKey(); Double rawPos = e.getValue(); if(id == null || rawPos == null){ continue; }else if(!NormalizedDouble.isValid(rawPos)){ continue; } NormalizedDouble val = new NormalizedDouble(rawPos); Joint.Id jId = new Joint.Id(id); Robot.JointId djId = new Robot.JointId(r.getRobotId(), jId); posMap.put(djId, val); } this.putPositions(posMap); }
continue; int sign = goal.compareTo(cur) >= 0 ? 1 : -1; Double newPos = cur.getValue() + interval*myVelocity*sign; if(!NormalizedDouble.isValid(newPos)){ continue; NormalizedDouble pos = new NormalizedDouble(newPos); if(sign > 0){ pos = pos.compareTo(goal) < 0 ? pos : goal; }else{ pos = pos.compareTo(goal) > 0 ? pos : goal;
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)); }
protected boolean isAtGoal(){ RobotPositionMap currentGoals = myRobot.getGoalPositions(); if(currentGoals == null || currentGoals.isEmpty()){ return true; } boolean atGoal = true; for(Entry<Robot.JointId,NormalizedDouble> e : currentGoals.entrySet()){ NormalizedDouble curGoal = e.getValue(); NormalizedDouble target = myTargetPositions.get(e.getKey()); if(target == null || curGoal == null){ continue; } atGoal = atGoal && (target.equals(curGoal)); } return atGoal; }
/** * Creates a new MaestroServo from the given ServoConfig and controller. * @param params ServoConfig for the new Servo * @param controller the Servo's controller */ protected MaestroServo( ServoConfig<MaestroServo.Id> params, MaestroController controller){ super(params,controller); myEnabledFlag = true; myPhysicalId = myConfig.getServoId(); myMinPosition = params.getMinPosition(); myMaxPosition = params.getMaxPosition(); double defInt = params.getDefaultPosition() - myMinPosition; double range = myMaxPosition - myMinPosition; double val = defInt/range; if(!NormalizedDouble.isValid(val)){ throw new IllegalArgumentException("Default Position invalid: " + params.getDefaultPosition()); } myDefaultPosition = new NormalizedDouble(val); myName = params.getName(); myGoalPosition = myDefaultPosition; myLastGoalChangeTimestamp = TimeUtils.now(); mySuspendedFlag = false; }
double val = goal.getValue(); val += velocities.get(i) * (double)interval; val = Utils.bound(val, 0.0, 1.0); pos.put(i, new NormalizedDouble(val));
private boolean positionsEqual(RobotPositionMap targetPositions){ if(myTargetPositions.size() < targetPositions.size()){ return false; } for(Entry<JointId,NormalizedDouble> e : targetPositions.entrySet()){ JointId id = e.getKey(); NormalizedDouble val = e.getValue(); if(id == null || val == null){ continue; } NormalizedDouble newVal = myTargetPositions.get(id); if(!val.equals(newVal)){ return false; } } return true; }
if(id == null || !NormalizedDouble.isValid(v)){ continue; NormalizedDouble val = new NormalizedDouble(v); Joint.Id jId = new Joint.Id(id); Robot.JointId djId = new Robot.JointId(myRobotId, jId);
@Override public NormalizedDouble getMax() { return new NormalizedDouble(1.0); } };
cur = goal; double curVal = cur.getValue(); double diff = goal.getValue() - curVal; double step = diff*timeRatio + curVal; NormalizedDouble interval = new NormalizedDouble(step); goals.put(jointId, interval);
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); } } }
double range = myMaxPosition - myMinPosition; double val = defInt/range; if(!NormalizedDouble.isValid(val)){ throw new IllegalArgumentException("Default Position invalid: " + params.getDefaultPosition()); myDefaultPosition = new NormalizedDouble(val); myName = params.getName();
@Override public NormalizedDouble getMin() { return new NormalizedDouble(0.0); }
private Map<Integer,Double> buildSnapshot(List<Joint> joints){ if(joints == null || joints.isEmpty()){ return Collections.EMPTY_MAP; } Map<Integer,Double> myPositions = new HashMap<Integer, Double>(); for(Joint joint : joints){ int id = joint.getId().getLogicalJointNumber(); if(joint == null || myPositions.containsKey(id)){ continue; } NormalizedDouble n = getJointPosition(joint); if(n == null){ continue; } double pos = n.getValue(); myPositions.put(id, pos); } return myPositions; }
Integer id = e.getKey(); double pos = path.estimatePosition(cur); if(id == null || !NormalizedDouble.isValid(pos)){ continue; NormalizedDouble val = new NormalizedDouble(pos); Joint.Id jId = new Joint.Id(id); Robot.JointId djId = Integer id = e.getKey(); double pos = path.estimatePosition(cur); if(id == null || !NormalizedDouble.isValid(pos)){ continue; NormalizedDouble val = new NormalizedDouble(pos); Joint.Id jId = new Joint.Id(id); Robot.JointId djId =
@Override public NormalizedDouble getMin() { return new NormalizedDouble(0.0); }
/** * Converts a RobotPositionMap to a Map of Integers to Doubles. * If the Robot contains Joints with duplicate Joint Ids, the size of the * RobotPositionMap will be added to the id until it is unique. * @param posMap RobotPositionMap to convert * @return Map of Integer ids to Doubles */ public static Map<Integer,Double> convertMap(RobotPositionMap posMap){ Map<Integer,Double> map = new HashMap<Integer, Double>(); int size = posMap.size(); for(Entry<Robot.JointId,NormalizedDouble> e : posMap.entrySet()){ int i = e.getKey().getJointId().getLogicalJointNumber(); while(map.containsKey(i)){ i+=size; } double pos = e.getValue().getValue(); map.put(i, pos); } return map; } /**
Integer id = e.getKey(); double pos = path.estimatePosition(cur); if(id == null || !NormalizedDouble.isValid(pos)){ continue; NormalizedDouble val = new NormalizedDouble(pos); Joint.Id jId = new Joint.Id(id); Robot.JointId djId = Integer id = e.getKey(); double pos = path.estimatePosition(next); if(id == null || !NormalizedDouble.isValid(pos)){ continue; NormalizedDouble val = new NormalizedDouble(pos); Joint.Id jId = new Joint.Id(id); Robot.JointId djId =