[simspark-cvs] simspark/simulations/soccer/plugin/kickeffector kickaction.h, NONE, 1.1.2.1 kickeffector.cpp, NONE, 1.1.2.1 kickeffector.h, NONE, 1.1.2.1 kickeffector_c.cpp, NONE, 1.1.2.1

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

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

Added Files:
      Tag: projectx
	kickaction.h kickeffector.cpp kickeffector.h 
	kickeffector_c.cpp 
Log Message:
soccer plugins from rcssserver3D

--- NEW FILE: kickeffector.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: kickeffector.h,v 1.1.2.1 2007/03/07 10:39:38 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 KICKEFFECTOR_H
#define KICKEFFECTOR_H

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

class BallStateAspect;

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

    /** 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 "kick"; }

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

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

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

    /** set the kick margin (the area within objects are kickable) */
    void SetKickMargin(float margin);

    /** Set the force factor.
     *
     * The kick power vector is multiplied by this factor.
     */
    void SetForceFactor(float force_factor);

    /** Set the torque factor.
     *
     * The kick torque vector is multiplied by this factor.
     */
    void SetTorqueFactor(float torque_factor);

    /** Set the number of steps the force is applied.
     * \param steps the number of steps to apply force and torque.
     */
    void SetSteps(int steps);

    /** Set the noise parameters.
     *  If used, the noise values are normally distributed around 0.0.
     *  Using this method, the sigmas of the distributions can be set.
     *  Values <= 0 mean that no noise will be added.
     *
     * \param sigma_force for noise of the applied force
     * \param sigma_theta for noise of the angle in the x-y plane
     * \param sigma_phi_end for noise of the latitudal angle at the end of the range
     * \param sigma_phi_mid for noise of the latitudal angle in the middle of the angle range
     */
    void SetNoiseParams(double sigma_force,
                        double sigma_theta,
                        double sigma_phi_end,
                        double sigma_phi_mid);

    /** Set the maximum kick power. */
    void SetMaxPower(float max_power);

    /** Set the latitudal angle range
     * \param min minimum latitudal kick angle in degrees
     * \param max maximum latitudal kick angle in degrees
     *
     * min has to be smaller than max.
     */
    void SetAngleRange(float min, float max);

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

    /** reference to the body node of the ball */
    boost::shared_ptr<oxygen::Body> mBallBody;
    /** reference to the body node of the ball */
    boost::shared_ptr<Ball> mBall;
    /** reference to the agent aspect */
    boost::shared_ptr<oxygen::AgentAspect> mAgent;

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

    /** reference to the ball state aspect */
    boost::shared_ptr<BallStateAspect> mBallStateAspect;

private:
    /** the margin where objects can be kicked */
    float mKickMargin;
    /** radius of the player */
    float mPlayerRadius;
    /** radius of the ball */
    float mBallRadius;
    /** force factor */
    float mForceFactor;
    /** force factor */
    float mTorqueFactor;
    /** the maximum kick power */
    float mMaxPower;
    /** the minimal kick angle */
    float mMinAngle;
    /** the maximum kick angle */
    float mMaxAngle;
    /** number of simulation steps for applying kick force */
    int mSteps;
    /** sigma for angle error at the end of the range (latitudal angle) */
    double mSigmaPhiEnd;
    /** sigma for angle error in the middle of the range (latitudal angle) */
    double mSigmaPhiMid;
};

DECLARE_CLASS(KickEffector);

#endif // KICKEFFECTOR_H

--- NEW FILE: kickeffector.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: kickeffector.cpp,v 1.1.2.1 2007/03/07 10:39:37 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 "kickaction.h"
#include "kickeffector.h"
#include <salt/random.h>
#include <zeitgeist/logserver/logserver.h>
#include <oxygen/sceneserver/transform.h>
#include <oxygen/physicsserver/spherecollider.h>
#include <soccer/soccerbase/soccerbase.h>

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

KickEffector::KickEffector()
    : oxygen::Effector(),
      mKickMargin(0.04),mPlayerRadius(0.0),mBallRadius(0.0),
      mForceFactor(4.0),mTorqueFactor(0.1),
      mMaxPower(100.0),
      mMinAngle(0.0),mMaxAngle(50.0), mSteps(10),
      mSigmaPhiEnd(0.9), mSigmaPhiMid(4.5)
{
}

KickEffector::~KickEffector()
{
}

bool
KickEffector::Realize(boost::shared_ptr<ActionObject> action)
{
    // this should also include the case when there is no ball
    // (because then there will be no body, neither).
    if (mBallBody.get() == 0)
    {
        return false;
    }

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

    shared_ptr<KickAction> kickAction =
        shared_dynamic_cast<KickAction>(action);

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

    // if the agent doesn't have a body, we're done (this should never happen)
    if (mBall.get() == 0) return true;

    Vector3f force =
        mBallBody->GetWorldTransform().Pos() -
        mAgent->GetWorldTransform().Pos();

    // the ball can be kicked if the distance is
    // less then Ball-Radius + Player-Radius + KickMargin AND
    // the player is close to the ground
    if (mAgent->GetWorldTransform().Pos().z() > mPlayerRadius + 0.01 ||
        force.Length() > mPlayerRadius + mBallRadius + mKickMargin)
    {
        // ball is out of reach, or player is in the air:
        // kick has no effect
        return true;
    }

    // get the kick angle in the horizontal plane
    double theta = salt::gArcTan2(force[1], force[0]);
    if (mThetaErrorRNG.get() != 0)
    {
        theta += (*(mThetaErrorRNG.get()))();
    }

    float phi = salt::gMin(salt::gMax(kickAction->GetAngle(), mMinAngle), mMaxAngle);
    if (mSigmaPhiEnd > 0.0 || mSigmaPhiMid > 0.0)
    {
        // f will be close to 0.0 if the angle is near the minimum or the maximum.
        // f will be close to 1.0 if the angle is somewhere in the middle of the range.
        float f = 1.0 - 2.0 * salt::gAbs((phi - mMinAngle) / (mMaxAngle - mMinAngle) - 0.5);
        // f is set to a number between mSigmaPhiEnd and mSigmaPhiMid
        f = salt::gMax(mSigmaPhiEnd + f * (mSigmaPhiMid-mSigmaPhiEnd), 0.0);
        phi = salt::NormalRNG<>(phi,f)();
    }
    phi = salt::gDegToRad(90.0-phi);

    // x = r * cos(theta) * sin(90 - phi), with r = 1.0
    force[0] = salt::gCos(theta) * salt::gSin(phi);
    // y = r * sin(theta) * sin(90 - phi), with r = 1.0
    force[1] = salt::gSin(theta) * salt::gSin(phi);
    // z = r * cos(90 - phi),              with r = 1.0
    force[2] = salt::gCos(phi);

    float kick_power = salt::gMin(salt::gMax(kickAction->GetPower(), 1.0f), mMaxPower);
    if (mForceErrorRNG.get() != 0)
    {
        kick_power += (*(mForceErrorRNG.get()))();
    }

    force *= (mForceFactor * kick_power);

    const Vector3f torque(-mTorqueFactor*force[1]/salt::g2PI,
                          mTorqueFactor*force[0]/salt::g2PI,
                          0.0);

    mBall->SetAcceleration(mSteps,force,torque,mAgent);

    mBallStateAspect->UpdateLastKickingAgent(mAgent);

    return true;
}

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

      Predicate::Iterator iter = predicate.begin();

      float angle;
      if (! predicate.AdvanceValue(iter, angle))
      {
          GetLog()->Error()
              << "ERROR: (KickEffector) kick angle parameter expected\n";
          break;
      }

      float power;
      if (! predicate.AdvanceValue(iter, power))
          {
              GetLog()->Error()
                  << "ERROR: (KickEffector) kick power expected\n";
              break;
          }

      // construct the KickAction object
      return shared_ptr<KickAction>(new KickAction(GetPredicate(),angle,power));

  } while (0);

  // some error happened
  return shared_ptr<ActionObject>();
}

void
KickEffector::OnLink()
{
    SoccerBase::GetBall(*this,mBall);
    SoccerBase::GetBallBody(*this,mBallBody);

    mAgent = shared_dynamic_cast<AgentAspect>(make_shared(GetParent()));

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

    shared_ptr<SphereCollider> geom =
        shared_dynamic_cast<SphereCollider>(mAgent->GetChild("geometry"));
    if (geom.get() == 0)
    {
        GetLog()->Error()
            << "ERROR: (KickEffector) parent node has no SphereCollider child\n";
    } else
        {
            mPlayerRadius = geom->GetRadius();
        }

    if (! SoccerBase::GetBallCollider(*this,geom))
    {
        GetLog()->Error()
            << "ERROR: (KickEffector) ball node has no SphereCollider child\n";
    } else
    {
        mBallRadius = geom->GetRadius();
    }

    if (mBallStateAspect.get() == 0)
    {
        mBallStateAspect = shared_dynamic_cast<BallStateAspect>
            (GetCore()->Get("/sys/server/gamecontrol/BallStateAspect"));
        if (mBallStateAspect.get() == 0) return;
    }
}

void
KickEffector::OnUnlink()
{
    mForceErrorRNG.reset();
    mThetaErrorRNG.reset();
    mBallBody.reset();
    mAgent.reset();
}

void
KickEffector::SetKickMargin(float margin)
{
    mKickMargin = margin;
}

void
KickEffector::SetNoiseParams(double sigma_force, double sigma_theta,
                             double sigma_phi_end, double sigma_phi_mid)
{
    NormalRngPtr rng(new salt::NormalRNG<>(0.0,sigma_force));
    mForceErrorRNG = rng;
    NormalRngPtr rng2(new salt::NormalRNG<>(0.0,sigma_theta));
    mThetaErrorRNG = rng2;
    mSigmaPhiEnd = sigma_phi_end;
    mSigmaPhiMid = sigma_phi_mid;
}

void
KickEffector::SetForceFactor(float force_factor)
{
    mForceFactor = force_factor;
}

void
KickEffector::SetTorqueFactor(float torque_factor)
{
    mTorqueFactor = torque_factor;
}

void
KickEffector::SetSteps(int steps)
{
    mSteps = steps;
}

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

void
KickEffector::SetAngleRange(float min, float max)
{
    if (max <= min)
    {
        GetLog()->Error()
            << "ERROR: (KickEffector) min. kick angle should be < max kick angle\n";
        return;
    }
    mMinAngle = min;
    mMaxAngle = max;
}

--- NEW FILE: kickaction.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: kickaction.h,v 1.1.2.1 2007/03/07 10:39:37 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 KICKACTION_H
#define KICKACTION_H

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

class KickAction : public oxygen::ActionObject
{
public:
    KickAction(const std::string& predicate, float phi, float power)
        : ActionObject(predicate), mKickAngle(phi),mPower(power) {}

    virtual ~KickAction() {}

    /** returns the stored kick power */
    float GetPower() const { return mPower; }

    /** returns the stored kick type */
    float GetAngle() { return mKickAngle; }

protected:
    /** the amount of force to be applied to be the ball */
    float mPower;

    /** the kick angle to be applied to the ball */
    float mKickAngle;
};

#endif // KICKACTION_H

--- NEW FILE: kickeffector_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: kickeffector_c.cpp,v 1.1.2.1 2007/03/07 10:39:38 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 "kickeffector.h"

using namespace oxygen;

FUNCTION(KickEffector,setKickMargin)
{
    float inMargin;

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

    obj->SetKickMargin(inMargin);
    return true;
}

FUNCTION(KickEffector,setForceFactor)
{
    float inForceFactor;

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

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

FUNCTION(KickEffector,setTorqueFactor)
{
    float inTorqueFactor;

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

    obj->SetTorqueFactor(inTorqueFactor);
    return true;
}

FUNCTION(KickEffector,setSteps)
{
    int steps;

    if (
        (in.GetSize() != 1) ||
        (! in.GetValue(in[0], steps))
        )
        {
            return false;
        }

    obj->SetSteps(steps);
    return true;
}

FUNCTION(KickEffector,setNoiseParams)
{
    float inForce;
    float inTheta;
    float inPhiEnd;
    float inPhiMid;

    if (
        (in.GetSize() != 4) ||
        (! in.GetValue(in[0], inForce)) ||
        (! in.GetValue(in[1], inTheta)) ||
        (! in.GetValue(in[2], inPhiEnd)) ||
        (! in.GetValue(in[3], inPhiMid))
        )
        {
            return false;
        }

    obj->SetNoiseParams(inForce,inTheta,inPhiEnd,inPhiMid);
    return true;
}

FUNCTION(KickEffector,setMaxPower)
{
    float inMaxPower;

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

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

FUNCTION(KickEffector,setAngleRange)
{
    float inMin;
    float inMax;

    if (
        (in.GetSize() != 2) ||
        (! in.GetValue(in[0], inMin)) ||
        (! in.GetValue(in[1], inMax))
        )
        {
            return false;
        }

    obj->SetAngleRange(inMin,inMax);
    return true;
}

void CLASS(KickEffector)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Effector);
    DEFINE_FUNCTION(setKickMargin);
    DEFINE_FUNCTION(setForceFactor);
    DEFINE_FUNCTION(setTorqueFactor);
    DEFINE_FUNCTION(setSteps);
    DEFINE_FUNCTION(setNoiseParams);
    DEFINE_FUNCTION(setMaxPower);
    DEFINE_FUNCTION(setAngleRange);
}