org.robokind.api.motion.blending.VelocityCombiner Maven / Gradle / Ivy

Go to download
/*
 *  Copyright (c) 2011 Hanson Robokind LLC. All rights reserved.
 * 
 *  Redistribution and use in source and binary forms, with or without modification, are
 *  permitted provided that the following conditions are met:
 *  
 *     1. Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *  
 *     2. 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.
 *  
 *  THIS SOFTWARE IS PROVIDED BY HANSON ROBOKIND LLC "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 HANSON ROBOKIND LLC 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.
 *  
 *  The views and conclusions contained in the software and documentation are those of the
 *  authors and should not be interpreted as representing official policies, either expressed
 *  or implied, of Hanson Robokind LLC.
 */

package org.robokind.api.motion.blending;

import java.util.HashMap;
import java.util.Map;
import java.util.Map.Entry;
import org.robokind.api.common.position.NormalizedDouble;
import org.jflux.api.core.Source;
import org.robokind.api.motion.protocol.MotionFrame;
import org.robokind.api.motion.protocol.JointPositionMap;
import org.robokind.api.common.utils.Utils;

/**
 * Blends Frames by combining the velocities contributed from each MotionFrame.
 * 
 * @param  type of id used in the PositionMaps being combined
 * @param  type of PositionMaps being combined
 * @author Matthew Stevenson
 */
public class VelocityCombiner<
        Id, PosMap extends JointPositionMap> implements 
        FrameCombiner,FrameSource,PosMap> {
    private Source myPositionMapFactory;    
    
    /**
     * Creates a new VelocityCombiner.
     * @param posMapFact factory for creating new PositionMaps
     */
    public VelocityCombiner(Source posMapFact){
        if(posMapFact == null){
            throw new NullPointerException();
        }
        myPositionMapFactory = posMapFact;
    }
    
    @Override
    public PosMap combineFrames(long time, long interval, PosMap curPos, 
            Map, 
                    ? extends FrameSource> frames) {
        PosMap pos = myPositionMapFactory.getValue();
        Map velocities = new HashMap();
        if(!frames.isEmpty()){
            return frames.keySet().iterator().next().getGoalPositions();
        }
        for(MotionFrame f : frames.keySet()){
            for(Entry e : getVelocities(f).entrySet()){
                Id id = e.getKey();
                double vel = e.getValue();
                if(velocities.containsKey(id)){
                    vel += velocities.get(id);
                }
                velocities.put(id, vel);
            }
        }
        for(Id i : curPos.keySet()){
            if(!velocities.containsKey(i)){
                continue;
            }
            NormalizedDouble goal = curPos.get(i);
            double val = goal.getValue();
            val += velocities.get(i) * (double)interval;
            val = Utils.bound(val, 0.0, 1.0);
            pos.put(i, new NormalizedDouble(val));
        }
        return pos;
    }
    
    /**
     * Return the MotionFrame velocities.
     * @param frame MotionFrame to use to calculate velocities
     * @return MotionFrame velocities
     */
    private Map getVelocities(MotionFrame frame){
        Map 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;
    }
}