Project: engagement_generation License: BSD Dependencies: engagement_msgs engagement_srvs bml_msgs bml_srvs bml_realizer Used by: None All Packages

engagement_generation/src/edu/wpi/hri/gen/policy/ref/GestureOption.java Go to the documentation of this file. /* * Software License Agreement (BSD License) * * Copyright (c) 2010, Worcester Polytechnic Institute * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Worcester Polytechnic Institute. nor the names * of its contributors may be used to endorse or promote products * derived from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ package edu.wpi.hri.gen.policy.ref; import java.util.ArrayList; import java.util.List; import edu.wpi.hri.bml.behavior.Behavior; import edu.wpi.hri.bml.behavior.EmitBehavior; import edu.wpi.hri.bml.behavior.GazeBehavior; import edu.wpi.hri.bml.behavior.GestureBehavior; import edu.wpi.hri.bml.behavior.BehaviorEnums.Amplitude; import edu.wpi.hri.bml.behavior.BehaviorEnums.Direction; import edu.wpi.hri.bml.behavior.BehaviorEnums.Distance; import edu.wpi.hri.bml.behavior.BehaviorEnums.GestureType; import edu.wpi.hri.bml.behavior.BehaviorEnums.HandShape; import edu.wpi.hri.bml.behavior.BehaviorEnums.HorizontalLocation; import edu.wpi.hri.bml.behavior.BehaviorEnums.Power; import edu.wpi.hri.bml.behavior.BehaviorEnums.Side; import edu.wpi.hri.bml.behavior.BehaviorEnums.Trajectory; import edu.wpi.hri.bml.behavior.BehaviorEnums.TwoHanded; import edu.wpi.hri.bml.behavior.BehaviorEnums.VerticalLocation; import edu.wpi.hri.gen.GenerationParams; import edu.wpi.hri.gen.comm.BMLEmitListener; import edu.wpi.hri.gen.comm.SituationKnowledge; import edu.wpi.hri.log.Logger; abstract class GestureOption { abstract double getCost(GestureState state, String object, SituationKnowledge situation, NoPointZone noZone); abstract DistractorSet getDistractors(String object, SituationKnowledge situation); abstract double getReliability(); abstract GestureState getNextState(GestureState state, String object, SituationKnowledge situation); abstract GestureBehavior createGesture(Logger logger, String id, String target, boolean required, boolean last); abstract GazeBehavior createGaze(Logger logger, String id, String target, boolean required); abstract EmitBehavior createDG(Behavior source, String actor, String target, BMLEmitListener emit); protected double findGazeCost(GestureState state, String object, SituationKnowledge situation) { Point start = state.getGazePoint(); Point end = situation.get(object); Point head = situation.getRobotHead(); // first find the yaw angle double numerator = start.xyDistanceSquared(end) - head.xyDistanceSquared(start) - end.xyDistanceSquared(head); double denominator = -2 * head.xyDistance(start) * end.xyDistance(head); double yaw = Math.acos(numerator / denominator); // then find the pitch angle numerator = start.xzDistanceSquared(end) - head.xzDistanceSquared(start) - end.xzDistanceSquared(head); denominator = -2 * head.xzDistance(start) * end.xzDistance(head); double pitch = Math.acos(numerator / denominator); // then find the degree maximum (which means max and convert) double max = Math.PI * Math.max(Math.abs(yaw), Math.abs(pitch)) / 180.0; // then multiply by the gaze cost constant return GenerationParams.CONSTANT_GAZE_COST_FACTOR.getDouble() * max; } protected Point findHandPoint(String object, SituationKnowledge situation) { // first find the point and the correct arm to use boolean right = situation.isObjectOnRight(object); Point point = situation.get(object); Point shoulder = right ? situation.getRobotRightShoulder() : situation .getRobotLeftShoulder(); // then check if the arm length is shorter that the object distance double spDist = shoulder.fullDistance(point); double armLength = GenerationParams.ROBOT_ARM_LENGTH.getDouble(); if (spDist > armLength) { // if so, set the point to the maximum extension along that line. double x = shoulder.getX() + ((shoulder.getX() - point.getX()) * (armLength / spDist)); double y = shoulder.getY() + ((shoulder.getY() - point.getY()) * (armLength / spDist)); double z = shoulder.getZ() + ((shoulder.getZ() - point.getZ()) * (armLength / spDist)); point = new Point(x, y, z); } return point; } protected double findPointCost(GestureState state, String object, SituationKnowledge situation, NoPointZone noZone) { Point start = situation.isObjectOnRight(object) ? state .getRobotRightHand() : state.getRobotLeftHand(); Point end = findHandPoint(object, situation); if (noZone.canPointTo(situation.get(object))) return GenerationParams.CONSTANT_POINT_COST_FACTOR.getDouble() * start.fullDistance(end); else return Double.POSITIVE_INFINITY; } protected DistractorSet findGazeDistractors(String object, SituationKnowledge situation) { List<String> distractors = new ArrayList<String>(); Point realPoint = situation.get(object); double sphereSize = GenerationParams.CONSTANT_GAZE_DISTRACTOR_DISTANCE .getDouble(); // for each distractor, ensure it is not the object and find if it is // within the sphere for (String distractor : situation.getAllObjects()) { if ((!distractor.equals(object)) && (realPoint.fullDistance(situation.get(distractor)) < sphereSize)) distractors.add(distractor); } return new DistractorSet(distractors); } protected DistractorSet findPointDistractors(String object, SituationKnowledge situation) { // find all of the data required List<String> distractors = new ArrayList<String>(); Point realPoint = situation.get(object); double rectSize = GenerationParams.CONSTANT_POINT_DISTRACTOR_DISTANCE .getDouble(); // Point handPoint = findHandPoint(object, situation); Point handPoint = situation.isObjectOnRight(object) ? situation .getRobotRightShoulder() : situation.getRobotLeftShoulder(); // now start doing the line intersection algorithm double realHandM = (handPoint.getY() - realPoint.getY()) / (handPoint.getX() - realPoint.getX()); double realHandB = handPoint.getY() - (realHandM * handPoint.getX()); double crossM = -1 / realHandM; // then for each object that exists, check if it is within range of the // line and not the real object for (String distractor : situation.getAllObjects()) { if (!distractor.equals(object)) { Point fakePoint = situation.get(distractor); double crossB = fakePoint.getY() - (crossM * fakePoint.getX()); double x = (crossB - realHandB) / (realHandM - crossM); double y = realHandB + (realHandM * x); Point crossPoint = new Point(x, y, 0); if (fakePoint.xyDistance(crossPoint) < rectSize) distractors.add(distractor); } } return new DistractorSet(distractors); } static final GestureOption LAMBDA = new GestureOption() { @Override double getReliability() { return 0; } @Override DistractorSet getDistractors(String object, SituationKnowledge situation) { return null; } @Override double getCost(GestureState state, String object, SituationKnowledge situation, NoPointZone noZone) { return 0; } @Override GestureState getNextState(GestureState state, String object, SituationKnowledge situation) { return state; }; @Override public String toString() { return "LAMBDA"; } @Override public GazeBehavior createGaze(Logger logger, String id, String target, boolean required) { return null; } @Override public GestureBehavior createGesture(Logger logger, String id, String target, boolean required, boolean last) { return null; } @Override public EmitBehavior createDG(Behavior source, String actor, String target, BMLEmitListener emit) { return null; }; }; static final GestureOption GAZE = new GestureOption() { @Override double getReliability() { return GenerationParams.RELIABILITY_GAZE.getDouble(); } @Override DistractorSet getDistractors(String object, SituationKnowledge situation) { return findGazeDistractors(object, situation); } @Override double getCost(GestureState state, String object, SituationKnowledge situation, NoPointZone noZone) { return findGazeCost(state, object, situation); } @Override GestureState getNextState(GestureState state, String object, SituationKnowledge situation) { return state.nextState(situation.get(object), null, null); }; @Override public String toString() { return "GAZE"; } @Override public GazeBehavior createGaze(Logger logger, String id, String target, boolean required) { return new GazeBehavior(logger, id + "-gaze", target, required); } @Override public GestureBehavior createGesture(Logger logger, String id, String target, boolean required, boolean last) { return null; } @Override public EmitBehavior createDG(Behavior source, String actor, String target, BMLEmitListener emit) { return emit.createDGEmit(actor, new String[] { target }, source, false); }; }; static final GestureOption DIRECTED_GAZE = new GestureOption() { @Override double getReliability() { return GenerationParams.RELIABILITY_DIRECTED_GAZE.getDouble(); } @Override DistractorSet getDistractors(String object, SituationKnowledge situation) { return findGazeDistractors(object, situation); } @Override double getCost(GestureState state, String object, SituationKnowledge situation, NoPointZone noZone) { return findGazeCost(state, object, situation); } @Override GestureState getNextState(GestureState state, String object, SituationKnowledge situation) { return state.nextState(situation.get(object), null, null); }; @Override public String toString() { return "DIRECTED_GAZE"; } @Override public GazeBehavior createGaze(Logger logger, String id, String target, boolean required) { return new GazeBehavior(logger, id + "-gaze", target, required); } @Override public GestureBehavior createGesture(Logger logger, String id, String target, boolean required, boolean last) { return null; } @Override public EmitBehavior createDG(Behavior source, String actor, String target, BMLEmitListener emit) { return emit.createDGEmit(actor, new String[] { target }, source, true); }; }; static final GestureOption DIRECTED_GAZE_POINTING = new GestureOption() { @Override double getReliability() { return GenerationParams.RELIABILITY_DIRECTED_GAZE_POINTING .getDouble(); } @Override DistractorSet getDistractors(String object, SituationKnowledge situation) { return findGazeDistractors(object, situation).intersection( findPointDistractors(object, situation)); } @Override double getCost(GestureState state, String object, SituationKnowledge situation, NoPointZone noZone) { return findGazeCost(state, object, situation) + findPointCost(state, object, situation, noZone); } @Override GestureState getNextState(GestureState state, String object, SituationKnowledge situation) { boolean right = situation.isObjectOnRight(object); Point point = findHandPoint(object, situation); return state.nextState(situation.get(object), right ? null : point, right ? point : null); }; @Override public String toString() { return "DIRECTED_GAZE_POINTING"; }; @Override public GazeBehavior createGaze(Logger logger, String id, String target, boolean required) { return new GazeBehavior(logger, id + "-gaze", target, required); } @Override public GestureBehavior createGesture(Logger logger, String id, String target, boolean required, boolean last) { return new GestureBehavior(logger, id + "-point", GestureType.POINT, Side.BOTH, 1, target, HandShape.INDEXFINGER, Direction.POLAR, Direction.POLAR, Trajectory.STRAIGHT, TwoHanded.PARALLEL, VerticalLocation.CENTER, HorizontalLocation.CENTER, Distance.CONTACT, Direction.POLAR, "", Amplitude.MEDIUM, Power.NORMAL, last, required); } @Override public EmitBehavior createDG(Behavior source, String actor, String target, BMLEmitListener emit) { return emit.createDGEmit(actor, new String[] { target }, source, true); }; }; }

---

engagement_generation
Author(s): Aaron Holroyd (aholroyd (at) wpi (dot) edu)
autogenerated on Fri Sep 9 10:35:40 2011