simpleagent.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:25:03 $
 * $Revision: 1.5 $
 * $Source: /cvsroot/simbad/src/simbad/sim/SimpleAgent.java,v $
 */
package simbad.sim;

import java.util.ArrayList;

import javax.media.j3d.Node;
import javax.media.j3d.Shape3D;
import javax.media.j3d.*;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;



/**
 * This is the  base class for all kinds of physical agents. <br>
 * Implementation note : the agent doesnt have synchronized methods.
 * All thread refering to the agent should do explicit synchronization with synchronized(agent){...}..
 */
public  class SimpleAgent extends BaseObject {

    /** Agent's printable name */
    protected String name;
    /** Collision flag */
    boolean collisionDetected;
    /** Interagent interaction  indicator */
    boolean interactionDetected;
    
    /** Keeps track of agent in physical contact with this agent. null most of the time */
    SimpleAgent veryNearAgent;
      
    /** Parent simulator. */
    private Simulator simulator;
     /** The agent's sensors */
    private ArrayList sensors;
    /** The agent's actuators */
    private ArrayList actuators; 
    
    
     /** Bounds for collision detection */
    private PickBounds collisionPickBounds;
    private BoundingSphere collisionBounds;
    private Point3d collisionBoundsCenter;
 
    /** for intermediate computations and to minimize Gargabe collection*/
    protected Vector3d v1 = new Vector3d();
    protected Transform3D t3d1 = new Transform3D();
    protected Transform3D t3d2 = new Transform3D();
    protected Transform3D t3d3 = new Transform3D();
      
  
    
    /** List of currently interacting agent */
    //private ArrayList interactingAgents;
    /** Start and restart position of the agent */
    private Vector3d startPosition;
    /** Counter incremented on each simulation step */
    private int counter;
    /** Lifetime in seconds since last reset */
    private double lifetime;
    
    protected double odometer;

    /** Position has changed between two steps.*/
    protected boolean positionChanged;

    protected Node body;


    // Physical parameters

     /**Agent's Height in meters */
    protected float height;

    /** Agent's radius  in meters */
    protected float radius;
    
    /** Agent's mass in kilogram */
    protected float mass;

    /** Agent's static  friction coefficient - 0 = no friction */
    protected float staticFrictionCoefficient;
    protected float dynamicFrictionCoefficient;
    
    
    

    /** Current linear acceleration of the center of mass.*/
    protected Vector3d linearAcceleration = new Vector3d();
    /** Current angular acceleration about the center of mass.*/
    protected Vector3d angularAcceleration = new Vector3d();
    /** Current linear velocity of the center of mass.*/
    protected Vector3d linearVelocity = new Vector3d();
    
    /** Current angular velocity about the center of mass.*/
    protected Vector3d angularVelocity = new Vector3d();
    /** Current translation step. */
    protected Vector3d instantTranslation = new Vector3d();
    /** Current rotation step. */
    protected Vector3d instantRotation = new Vector3d();
    
    /** Used for collision picking */
    protected double collisionDistance[] = new double[1];
    protected double collisionRadius;
    
     
    /**
     * Constructs a SimpleAgent.
     * @param pos the starting position.
     * @param name the name of the agent.
     */
    public SimpleAgent(Vector3d pos, String name) {
        this.name = name;
        super.create3D(true);
        startPosition = new Vector3d(pos);
        sensors = new ArrayList();
        actuators= new ArrayList();
       // interactingAgents = new ArrayList();
       
        // reserve collision detection stuff
        collisionBounds = new BoundingSphere();
        collisionPickBounds = new PickBounds();
        collisionBoundsCenter = new Point3d();
        
        detachedFromSceneGraph = false;
  
    }

    protected  void create3D(){ /* Overide */}

    /** Creation phase - called once by the simulator. */
    protected void create(){
        create3D();
        // setup on the floor
        startPosition.add(new Vector3d(0,height/2.0,0));
    }
    
    /** Resets agent variables and position */
    protected  void reset(){
        veryNearAgent = null;
        collisionRadius = radius ;
        counter = 0;
        lifetime = 0;
        odometer =0;
        linearVelocity.set(0,0,0);
        resetPosition();
        resetDevices();
    }
    
    protected  void resetPosition(){
        resetPositionAt(startPosition);
    }
    
    protected  void resetPositionAt(Vector3d newPosition){
        // reattach to graph if necessary
        if (detachedFromSceneGraph) attach();
        resetTransforms();
        collisionDetected =false;
        interactionDetected=false;
        translateTo(newPosition);
    }
    
     
    /** Resets all devices */
    protected void resetDevices() {
        for (int i = 0; i < sensors.size(); i++) {
            SensorDevice sd = (SensorDevice) sensors.get(i);
            if (sd != null)
                sd.reset();

        }
        for (int i = 0; i < actuators.size(); i++) {
            ActuatorDevice ad = (ActuatorDevice) actuators.get(i);
            if (ad != null)
                ad.reset();
        }

    }
 
    /** Update sensor phase - called on each simulation step.  */
    protected  void updateSensors(double elapsedSecond,BranchGroup pickableSceneBranch) {
        
        // don't want to sense its own body while picking (cause unnecessary intersections)
        body.setPickable(false);
        for (int i = 0; i < sensors.size(); i++)
        {
            SensorDevice sd = (SensorDevice)sensors.get(i);
            if (sd == null) continue;
            if (sd instanceof PickSensor){
                ((PickSensor)sd).setPickableSceneBranch(pickableSceneBranch);
            }
            sd.update(elapsedSecond);
        }
        // we are pickable again.
        body.setPickable(true);
     }

    /** Update actuators  phase - called on each simulation step.  */
      protected  void updateActuators(double elapsedSecond) {
        for (int i = 0; i < actuators.size(); i++)
        {
            ActuatorDevice ad = (ActuatorDevice)actuators.get(i);
            if (ad == null) continue;
            ad.update(elapsedSecond);
        }
     }
    
    /** set acceleration applied by motors .*/
    protected  void setMotorsAcceleration(double dt) {
        // no motor by default
        linearAcceleration.set(0,0,0);
        angularAcceleration.set(0,0,0);
    };
    

    
    /** Perform acceleration integration step .*/
    protected void integratesVelocities(double dt){
        v1.set(linearAcceleration);
        v1.scale(dt);
        linearVelocity.add(v1);
        v1.set(angularAcceleration);
        v1.scale(dt);
        angularVelocity.add(v1);
     }
    /** Perform velocities integration step .