[simspark-cvs] simspark/simulations/soccer/plugin/driveeffector driveaction.h, NONE, 1.1.2.1 driveeffector.cpp, NONE, 1.1.2.1 driveeffector.h, NONE, 1.1.2.1 driveeffector_c.cpp, NONE, 1.1.2.1

[Oliver O. <fr...@us...>]

Update of /cvsroot/simspark/simspark/simulations/soccer/plugin/driveeffector
In directory sc8-pr-cvs8.sourceforge.net:/tmp/cvs-serv24718/driveeffector

Added Files:
      Tag: projectx
	driveaction.h driveeffector.cpp driveeffector.h 
	driveeffector_c.cpp 
Log Message:
soccer plugins from rcssserver3D

--- NEW FILE: driveeffector.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: driveeffector.cpp,v 1.1.2.1 2007/03/07 10:39:28 fruit Exp $

   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; version 2 of the License.

   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., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "driveaction.h"
#include "driveeffector.h"
#include <salt/gmath.h>
#include <zeitgeist/logserver/logserver.h>
#include <oxygen/physicsserver/spherecollider.h>
#include <soccer/soccerbase/soccerbase.h>

using namespace boost;
using namespace oxygen;
using namespace salt;

DriveEffector::DriveEffector() : oxygen::Effector(),
                                 mForceFactor(60.0),
                                 mMaxPower(100.0), mConsumption(1.0/18000.0)
{
}

DriveEffector::~DriveEffector()
{
}

bool
DriveEffector::Realize(boost::shared_ptr<ActionObject> action)
{
    if (mBody.get() == 0)
    {
        return false;
    }

    shared_ptr<BaseNode> parent =
        shared_dynamic_cast<BaseNode>(make_shared(GetParent()));

    if (parent.get() == 0)
    {
        GetLog()->Error()
            << "ERROR: (DriveEffector) parent node is not derived from BaseNode\n";
        return false;
    }

    shared_ptr<DriveAction> driveAction = shared_dynamic_cast<DriveAction>(action);

    if (driveAction.get() == 0)
    {
        GetLog()->Error()
            << "ERROR: (DriveEffector) cannot realize an unknown ActionObject\n";
        return false;
    }

    mForce = driveAction->GetForce();

    // cut down the drive power vector to maximum length
    if (mForce.SquareLength() > mMaxPower * mMaxPower)
    {
        mForce.Normalize();
        mForce *= mMaxPower;
    }

    if (mForceErrorRNG.get() != 0)
    {
        mForce[0] = mForce[0] * (*(mForceErrorRNG.get()))() * mForceFactor;
        mForce[1] = mForce[1] * (*(mForceErrorRNG.get()))() * mForceFactor;
        mForce[2] = mForce[2] * (*(mForceErrorRNG.get()))() * mForceFactor;
    } else {
        mForce = mForce * mForceFactor;
    }
    return true;
}

shared_ptr<ActionObject>
DriveEffector::GetActionObject(const Predicate& predicate)
{
  if (predicate.name != GetPredicate())
    {
      GetLog()->Error() << "ERROR: (DriveEffector) invalid predicate"
                        << predicate.name << "\n";
      return shared_ptr<ActionObject>();
    }

  Vector3f force;
  if (! predicate.GetValue(predicate.begin(), force))
  {
      GetLog()->Error()
          << "ERROR: (DriveEffector) Vector3f parameter expected\n";
      return shared_ptr<ActionObject>(new ActionObject(GetPredicate()));
  }

  return shared_ptr<ActionObject>(new DriveAction(GetPredicate(),force));
}

void
DriveEffector::OnLink()
{
    SoccerBase::GetTransformParent(*this,mTransformParent);
    SoccerBase::GetBody(*this,mBody);
    SoccerBase::GetAgentState(*this,mAgentState);

    shared_ptr<SphereCollider> geom =
        shared_dynamic_cast<SphereCollider>(mTransformParent->GetChild("geometry"));

    mMaxDistance = 0.001;
    if (geom.get() == 0)
    {
        GetLog()->Error() << "ERROR: (DriveEffector) parent node has "
                          << "no 'geometry' sphere child\n";
    } else {
            mMaxDistance += geom->GetRadius();
    }
}

void
DriveEffector::OnUnlink()
{
    mForceErrorRNG.reset();
    mTransformParent.reset();
    mBody.reset();
}

void
DriveEffector::SetForceFactor(float force_factor)
{
    mForceFactor = salt::gAbs(force_factor);
}

void
DriveEffector::SetSigma(float sigma)
{
    NormalRngPtr rng(new salt::NormalRNG<>(1.0,sigma));
    mForceErrorRNG = rng;
}

void
DriveEffector::SetMaxPower(float max_power)
{
    mMaxPower = max_power;
}

void
DriveEffector::PrePhysicsUpdateInternal(float deltaTime)
{
    Effector::PrePhysicsUpdateInternal(deltaTime);
    if (mBody.get() == 0 ||
        mForce.Length() <= std::numeric_limits<float>::epsilon())
    {
        return;
    }

    Vector3f vec = mTransformParent->GetWorldTransform().Pos();
    if (vec.z() > mMaxDistance) return;

    if (mAgentState->ReduceBattery(mForce.Length() * mConsumption))
    {
        mBody->AddForce(SoccerBase::FlipView(mForce,mAgentState->GetTeamIndex()));
    }
}

void
DriveEffector::SetConsumption(float consume_time)
{
    mConsumption = 1.0 / consume_time;
}



--- NEW FILE: driveaction.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: driveaction.h,v 1.1.2.1 2007/03/07 10:39:28 fruit Exp $

   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; version 2 of the License.

   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., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef DRIVEACTION_H
#define DRIVEACTION_H

#include <oxygen/gamecontrolserver/actionobject.h>
#include <salt/vector.h>

class DriveAction : public oxygen::ActionObject
{
public:
    DriveAction(const std::string& predicate, salt::Vector3f force)
        : ActionObject(predicate), mForce(force) {}
    virtual ~DriveAction() {}

    /** returns the stored force vector */
    const salt::Vector3f& GetForce() { return mForce; }

protected:
    /** the force to be applied by the DriveEffector */
    salt::Vector3f mForce;
};

#endif // DRIVEACTION_H

--- NEW FILE: driveeffector_c.cpp ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: driveeffector_c.cpp,v 1.1.2.1 2007/03/07 10:39:29 fruit Exp $

   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; version 2 of the License.

   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., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#include "driveeffector.h"

using namespace oxygen;

FUNCTION(DriveEffector,setForceFactor)
{
    float inForceFactor;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inForceFactor))
        )
        {
            return false;
        }

    obj->SetForceFactor(inForceFactor);
    return true;
}

FUNCTION(DriveEffector,setSigma)
{
    float inSigma;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inSigma))
        )
        {
            return false;
        }

    obj->SetSigma(inSigma);
    return true;
}

FUNCTION(DriveEffector,setMaxPower)
{
    float inMaxPower;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inMaxPower))
        )
        {
            return false;
        }

    obj->SetMaxPower(inMaxPower);
    return true;
}

FUNCTION(DriveEffector,setConsumption)
{
    float inConsumption;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in.begin(), inConsumption))
        )
        {
            return false;
        }

    obj->SetConsumption(inConsumption);
    return true;
}

void
CLASS(DriveEffector)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Effector);
    DEFINE_FUNCTION(setForceFactor);
    DEFINE_FUNCTION(setSigma);
    DEFINE_FUNCTION(setMaxPower);
    DEFINE_FUNCTION(setConsumption);
}

--- NEW FILE: driveeffector.h ---
/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: driveeffector.h,v 1.1.2.1 2007/03/07 10:39:28 fruit Exp $

   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; version 2 of the License.

   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., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#ifndef DRIVEEFFECTOR_H
#define DRIVEEFFECTOR_H

#include <salt/random.h>
#include <oxygen/agentaspect/effector.h>
#include <oxygen/physicsserver/body.h>
#include <soccer/agentstate/agentstate.h>

class DriveEffector : public oxygen::Effector
{
public:
    DriveEffector();
    virtual ~DriveEffector();

    /** realizes the action described by the ActionObject */
    virtual bool Realize(boost::shared_ptr<oxygen::ActionObject> action);

    /** returns the name of the predicate this effector implements. */
    virtual std::string GetPredicate() { return "drive"; }

    /** constructs an Actionobject, describing a predicate */
    virtual boost::shared_ptr<oxygen::ActionObject>
    GetActionObject(const oxygen::Predicate& predicate);

    /** Set the force factor.
     *
     * The drive power vector is multiplied by this factor divided by the maximum length
     * of the drive power vector.
     */
    void SetForceFactor(float force_factor);

    /** Set the error distribution for calculating the applied force.
     *
     * The force applied to a sphere is the drive power vector multiplied by
     * the force factor divided by (maximum length of the drive power vector + Error).
     * The error is normaly distributed around zero with a given sigma. If sigma is <= 0,
     * no error will be applied.
     */
    void SetSigma(float sigma);

    /** Set the maximum length of the drive power vector. */
    void SetMaxPower(float max_power);

    /** Set the battery consumption.
     *  How long can you be driving full speed with a fully charged battery?
     *  \param consume_time time (in milliseconds) the battery works
     *                      driving full speed
     */
    void SetConsumption(float consume_time);

protected:
    virtual void PrePhysicsUpdateInternal(float deltaTime);

    /** setup the reference to the agents body node */
    virtual void OnLink();

    /** remove the reference to the agents body node */
    virtual void OnUnlink();

protected:
    typedef boost::shared_ptr<salt::NormalRNG<> > NormalRngPtr;

    /** the reference to the parent transform node */
    boost::shared_ptr<oxygen::Transform> mTransformParent;
    /** the reference to the parents body node */
    boost::shared_ptr<oxygen::Body> mBody;
    //! a reference to the agent state
    boost::shared_ptr<AgentState> mAgentState;

    /** the force that should be applied to the agent body */
    salt::Vector3f mForce;

    /** the maximum distance from the plane */
    float mMaxDistance;

    /** The force factor is the force applied to the body if the length of
        the drive power vector is greater or equal to mMaxDrivePower */
    float mForceFactor;

    /** random number generator for the error distribution of the applied force */
    NormalRngPtr mForceErrorRNG;

    /** The maximum length of the drive power vector. */
    float mMaxPower;

    /** The battery consumption for driving one second with full speed */
    double mConsumption;
};

DECLARE_CLASS(DriveEffector);

#endif // DRIVEEFFECTOR_H