robotfactory.java

一款机器人仿真软件,功能上与microsoft robotics studio有些相似,但基于Java平台.突出特点是给出了传感器仿真,如声纳,激光等.

/*
 *  Simbad - Robot Simulator
 *  Copyright (C) 2004 Louis Hugues
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful, 
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 -----------------------------------------------------------------------------
 * $Author: sioulseuguh $ 
 * $Date: 2005/08/07 12:24:57 $
 * $Revision: 1.13 $
 * $Source: /cvsroot/simbad/src/simbad/sim/RobotFactory.java,v $
 */
package simbad.sim;

import javax.media.j3d.Transform3D;
import javax.vecmath.Vector3d;

/**
 *  A helper class to aid building robot bodies and sensors.
 * It only contains only static methods and thus should not be instanciated. 
 * 
 * 
 * 
 */
public class RobotFactory extends Factory {

    
    /**
     * Adds a prebuild camera sensor to the agent. Image resolution is 100x100 pixels.
     * Camera is situated on the top of the agent.
     * @param agent
     * @return the sensor object
     */
    static public CameraSensor addCameraSensor(Agent agent) {
        double agentHeight = agent.getHeight();
        float cameraBodyRadius = 0.1f;
        CameraSensor camera = new CameraSensor(cameraBodyRadius, 100, 100);
        camera.setUpdatePerSecond(3);
        camera.setName("Camera");
        Vector3d pos = new Vector3d(0.0, (agentHeight / 2)
                + (cameraBodyRadius * 3) / 4, 0);
        agent.addSensorDevice(camera, pos, 0);
        return camera;
    }

    

    /**
     * Adds a prebuild belt of sonar sensor to the agent.
     * @param agent
     * @return the sensor object
     */
    static public RangeSensorBelt addSonarBeltSensor(Agent agent) {
        double agentHeight = agent.getHeight();
        double agentRadius = agent.getRadius();
        RangeSensorBelt sonarBelt = new RangeSensorBelt((float) agentRadius,
                0f, 1.5f, 9, RangeSensorBelt.TYPE_SONAR,0);
        sonarBelt.setUpdatePerSecond(3);
        sonarBelt.setName("sonars");
        Vector3d pos = new Vector3d(0, agentHeight / 2, 0.0);
        agent.addSensorDevice(sonarBelt, pos, 0);
        return sonarBelt;
    }
    /**
     * Adds a prebuild belt of sonar sensor to the agent.
     * @param agent
     * @param nbSonars the number of sonars.
     * @return the sensor object
     */
    static public RangeSensorBelt addSonarBeltSensor(Agent agent,int nbSonars) {
        double agentHeight = agent.getHeight();
        double agentRadius = agent.getRadius();
        RangeSensorBelt sonarBelt = new RangeSensorBelt((float) agentRadius,
                0f, 1.5f, nbSonars, RangeSensorBelt.TYPE_SONAR,0);
        sonarBelt.setUpdatePerSecond(3);
        sonarBelt.setName("sonars");
        Vector3d pos = new Vector3d(0, agentHeight / 2, 0.0);
        agent.addSensorDevice(sonarBelt, pos, 0);
        return sonarBelt;
    }

    /**
     * Adds a prebuild belt of bumpers sensor to the agent.
     * @param agent
     * @return the sensor object
     */
    static public RangeSensorBelt addBumperBeltSensor(Agent agent) {
       // double agentHeight = agent.getHeight();
        double agentRadius = agent.getRadius();
        RangeSensorBelt bumperBelt = new RangeSensorBelt((float) agentRadius-0.1f,
                0f, 0.2f, 9, RangeSensorBelt.TYPE_BUMPER,0);
        bumperBelt.setUpdatePerSecond(6);
        bumperBelt.setName("bumpers");
        Vector3d pos = new Vector3d(0, 0, 0.0);
        agent.addSensorDevice(bumperBelt, pos, 0);
        return bumperBelt;
    }
    /**
     * Adds a prebuild belt of bumpers sensor to the agent. Specify the number of sensors.
     * @param agent
     * @param nbSensors
     * @return the sensor object
     */
    static public RangeSensorBelt addBumperBeltSensor(Agent agent,int  nbSensors) {
       // double agentHeight = agent.getHeight();
        double agentRadius = agent.getRadius();
        RangeSensorBelt bumperBelt = new RangeSensorBelt((float) agentRadius-0.1f,
                0f, 0.2f, nbSensors, RangeSensorBelt.TYPE_BUMPER,0);
        bumperBelt.setUpdatePerSecond(6);
        bumperBelt.setName("bumpers");
        Vector3d pos = new Vector3d(0, 0, 0.0);
        agent.addSensorDevice(bumperBelt, pos, 0);
        return bumperBelt;
    }

    /**
     * Adds a prebuild light sensor  to the agent.
     * @param agent
     * @return the sensor object
     */
    static public LightSensor addLightSensor(Agent agent) {
        double agentHeight = agent.getHeight();
        LightSensor sensor = new LightSensor();
        sensor.setUpdatePerSecond(3);
        sensor.setName("Light Sensor");
        Vector3d pos = new Vector3d(0.0, (agentHeight / 2), 0);
        agent.addSensorDevice(sensor, pos, -Math.PI / 2);
        return sensor;
    }

    /**
     * Adds a prebuild light sensor  to the agent at a given position.
     * @param agent
     * @return the sensor object
     */
    static public LightSensor addLightSensor(Agent agent, Vector3d pos,
            float angle, String label) {
        LightSensor sensor = new LightSensor();
        sensor.setUpdatePerSecond(3);
        sensor.setName("Light Sensor " + label);
        agent.addSensorDevice(sensor, pos, angle);
        return sensor;
    }

    /**
     * Adds a prebuild light sensor on the left of the agent .
     * @param agent
     * @return the sensor object
     */
    static public LightSensor addLightSensorLeft(Agent agent) {
        Vector3d front = new Vector3d(agent.getRadius()+0.5, 0, 0);
        Transform3D t3d = new Transform3D();
        t3d.rotY(Math.PI / 4);
        Vector3d left = new Vector3d(front);
        t3d.transform(left);
        return RobotFactory.addLightSensor(agent, left, (float) Math.PI / 4,
                "left");
    }

    /**
     * Adds a prebuild light sensor on the right of the agent .
     * @param agent
     * @return the sensor object
     */
    static public LightSensor addLightSensorRight(Agent agent) {
        Vector3d front = new Vector3d(agent.getRadius()+0.5, 0, 0);
        Transform3D t3d = new Transform3D();
        t3d.rotY(-Math.PI / 4);
        Vector3d right = new Vector3d(front);
        t3d.transform(right);
        return RobotFactory.addLightSensor(agent, right, (float) -Math.PI / 4,
                "left");
    }

    
    
    /**
     * Adds a prebuild key  input  to the agent.
     * @param agent
     * @return the sensor object
     */
    static public KeyInputSensor addKeyInputSensor(Agent agent) {
        KeyInputSensor sensor = new KeyInputSensor();
      
        sensor.setUpdatePerSecond(3);
        sensor.setName("key input");
        // position has no meaning in the case of that sensor.
        Vector3d pos = new Vector3d(0, 0.0, 0.0);
        agent.addSensorDevice(sensor, pos, 0);
        return sensor;
    }
    /////////////////////////////////////////////////////////////////////////////////////
    //  Prebuild Actuators 
    
    
    /**
     * Adds a prebuild lamp actuator on the top  of the agent .
     * @param agent
     * @return the actuator object
     */
    static public LampActuator addLamp(Agent agent) {
        LampActuator la = new LampActuator(0.15f);
        la.setName("lamp");
        la.setUpdatePerSecond(5f);
        agent.addActuatorDevice(la, new Vector3d(0,agent.getHeight(),0),0);
       
        return la;
    }
    
    

    /////////////////////////////////////////////////////////////////////////////////////
    //  Prebuild KinelaticModels

    /** Sets the kinematic model to be DifferentialDrive (instead of DefaultKinematic)
     * @param agent
     */
    static public DifferentialKinematic setDifferentialDriveKinematicModel(Agent agent){
       agent.setKinematicModel(new DifferentialKinematic(agent.getRadius()));
       return (DifferentialKinematic)agent.getKinematicModel();
    }
    
}