[simspark-cvs] simspark/spark/plugin/sparkagent universaljointaction.h,NONE,1.1 universaljointeffect

SourceForge Headquarters 225 Broadway Suite 1600 San Diego, CA 92101 +1 (858) 422-6466

Update of /cvsroot/simspark/simspark/spark/plugin/sparkagent
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv2082

Modified Files:
	Makefile.am export.cpp hingeeffector.cpp 
Added Files:
	universaljointaction.h universaljointeffector.cpp 
	universaljointeffector.h universaljointeffector_c.cpp 
	universaljointperceptor.cpp universaljointperceptor.h 
	universaljointperceptor_c.cpp 
Log Message:
(sync with rcssserver3d cvs, thanks Joschka)
- initial version of action, perceptor, and effector
  for the universal joint
- removed some commented out code in hingeeffector.cpp
- added exports for the universal joint perceptor and
  effector

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

   this file is part of rcssserver3D
   Thu Jan 4 2006
   Copyright (C) 2006 RoboCup Soccer Server 3D Maintenance Group

   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 UNIVERSALJOINTPERCEPTOR_H
#define UNIVERSALJOINTPERCEPTOR_H

#include <oxygen/agentaspect/perceptor.h>
#include <oxygen/physicsserver/universaljoint.h>

class UniversalJointPerceptor : public oxygen::Perceptor
{
public:
    UniversalJointPerceptor();
    virtual ~UniversalJointPerceptor();

    //! \return true, if valid data is available and false otherwise.
    bool Percept(boost::shared_ptr<oxygen::PredicateList> predList);

protected:
    virtual void OnLink();
    virtual void OnUnlink();

    void InsertAxisAngle(oxygen::Predicate& predicate, oxygen::Joint::EAxisIndex idx);
    void InsertAxisRate(oxygen::Predicate& predicate, oxygen::Joint::EAxisIndex idx);

protected:
    /** cached reference to the monitor joint */
    boost::shared_ptr<oxygen::UniversalJoint> mJoint;
};

DECLARE_CLASS(UniversalJointPerceptor);

#endif //UNIVERSALJOINTPERCEPTOR_H

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

   this file is part of rcssserver3D
   Thu Jan 4 2006
   Copyright (C) 2006 RoboCup Soccer Server 3D Maintenance Group

   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 UNIVERSALJOINTEFFECTOR_H
#define UNIVERSALJOINTEFFECTOR_H

#include <oxygen/agentaspect/effector.h>
#include <oxygen/physicsserver/universaljoint.h>

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

    /** 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 GetName(); }

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

protected:
    /** setup the reference to the HingeJoint parent node */
    virtual void OnLink();

    /** remove the reference to the HingeJoint parent node */
    virtual void OnUnlink();

protected:
    /** cached reference to the monitor joint */
    boost::shared_ptr<oxygen::UniversalJoint> mJoint;

};

DECLARE_CLASS(UniversalJointEffector);

#endif // UNIVERSALJOINTEFFECTOR_H

Index: export.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/plugin/sparkagent/export.cpp,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** export.cpp	2 Jan 2006 20:38:32 -0000	1.3
--- export.cpp	6 Jan 2006 13:57:36 -0000	1.4
***************
*** 26,29 ****
--- 26,31 ----
  #include "hinge2effector.h"
  #include "timeperceptor.h"
+ #include "universaljointeffector.h"
+ #include "universaljointperceptor.h"

  ZEITGEIST_EXPORT_BEGIN()
***************
*** 33,35 ****
--- 35,39 ----
      ZEITGEIST_EXPORT(HingeEffector);
      ZEITGEIST_EXPORT(HingePerceptor);
+     ZEITGEIST_EXPORT(UniversalJointEffector);
+     ZEITGEIST_EXPORT(UniversalJointPerceptor);
  ZEITGEIST_EXPORT_END()

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

   this file is part of rcssserver3D
   Thu Jan 4 2006
   Copyright (C) 2006 RoboCup Soccer Server 3D Maintenance Group

   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 "universaljointeffector.h"
#include "universaljointaction.h"

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

UniversalJointEffector::UniversalJointEffector() : Effector()
{
    SetName("universaljoint");
}

UniversalJointEffector::~UniversalJointEffector()
{
}

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

    shared_ptr<UniversalJointAction> universalAction =
        shared_dynamic_cast<UniversalJointAction>(action);

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

    mJoint->SetParameter(dParamVel, universalAction->GetMotorVelocity(Joint::AI_FIRST));
    mJoint->SetParameter(dParamVel2, universalAction->GetMotorVelocity(Joint::AI_SECOND));

    return true;
}

shared_ptr<ActionObject> UniversalJointEffector::GetActionObject(const Predicate& predicate)
{
    for(;;)
        {
            if (mJoint.get() == 0)
                {
                    break;
                }

            if (predicate.name != GetPredicate())
                {
                    GetLog()->Error()
                        << "ERROR: (UniversalJointEffector) invalid predicate"
                        << predicate.name << "\n";
                    break;
                }

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

            float velocity1;
            float velocity2;

            if (! predicate.AdvanceValue(iter, velocity1))
                {
                    GetLog()->Error()
                        << "ERROR: (UniversalJointEffector) motor velocity1 expected\n";
                    break;
                }
            if (! predicate.AdvanceValue(iter, velocity2))
                {
                    GetLog()->Error()
                        << "ERROR: (UniversalJointEffector) motor velocity2 expected\n";
                    break;
                }

            return shared_ptr<UniversalJointAction>(new  UniversalJointAction(GetPredicate(),velocity1,velocity2));
        }

    return shared_ptr<ActionObject>();
}

void UniversalJointEffector::OnLink()
{
    mJoint = make_shared(FindParentSupportingClass<UniversalJoint>());

    if (mJoint.get() == 0)
        {
            GetLog()->Error()
                << "(UniversalJointEffector) ERROR: found no UniversalJoint parent\n";
        }
}

void UniversalJointEffector::OnUnlink()
{
    mJoint.reset();
}

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

   this file is part of rcssserver3D
   Thu Jan 4 2006
   Copyright (C) 2006 RoboCup Soccer Server 3D Maintenance Group

   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 "universaljointperceptor.h"
#include <zeitgeist/logserver/logserver.h>

using namespace oxygen;
using namespace zeitgeist;
using namespace boost;
using namespace std;

UniversalJointPerceptor::UniversalJointPerceptor() : Perceptor()
{
}

UniversalJointPerceptor::~UniversalJointPerceptor()
{
}

void UniversalJointPerceptor::OnLink()
{
    mJoint = make_shared(FindParentSupportingClass<UniversalJoint>());

    if (mJoint.get() == 0)
        {
            GetLog()->Error()
                << "(UniversalJointPerceptor) ERROR: found no UniversalJoint parent\n";
        }

}

void UniversalJointPerceptor::OnUnlink()
{
    mJoint.reset();
}

void UniversalJointPerceptor::InsertAxisAngle(Predicate& predicate, Joint::EAxisIndex idx)
{
    ParameterList& axisElement = predicate.parameter.AddList();
    axisElement.AddValue(string("axis"));
    axisElement.AddValue(mJoint->GetAngle(idx));
}

void UniversalJointPerceptor::InsertAxisRate(Predicate& predicate, Joint::EAxisIndex idx)
{
    ParameterList& axisElement = predicate.parameter.AddList();
    axisElement.AddValue(string("rate"));
    axisElement.AddValue(mJoint->GetAngleRate(idx));
}

bool UniversalJointPerceptor::Percept(boost::shared_ptr<oxygen::PredicateList> predList)
{
    if (mJoint.get() == 0)
        {
            return false;
        }

    Predicate& predicate = predList->AddPredicate();
    predicate.name = "UJ";
    predicate.parameter.Clear();

    ParameterList& nameElement = predicate.parameter.AddList();
    nameElement.AddValue(string("name"));
    nameElement.AddValue(GetName());

    InsertAxisAngle(predicate, Joint::AI_FIRST);
    InsertAxisRate(predicate, Joint::AI_FIRST);
    InsertAxisAngle(predicate, Joint::AI_SECOND);
    InsertAxisRate(predicate, Joint::AI_SECOND);

    return true;
}

Index: Makefile.am
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/plugin/sparkagent/Makefile.am,v
retrieving revision 1.2
retrieving revision 1.3
diff -C2 -d -r1.2 -r1.3
*** Makefile.am	2 Jan 2006 18:11:33 -0000	1.2
--- Makefile.am	6 Jan 2006 13:57:36 -0000	1.3
***************
*** 19,23 ****
  	hingeperceptor.h \
  	hingeperceptor.cpp \
! 	hingeperceptor_c.cpp

  sparkagent_la_LDFLAGS = -module -version-info 0:0:0
--- 19,30 ----
  	hingeperceptor.h \
  	hingeperceptor.cpp \
! 	hingeperceptor_c.cpp \
! 	universaljointaction.h \
! 	universaljointeffector.h \
! 	universaljointeffector.cpp \
! 	universaljointeffector_c.cpp \
! 	universaljointperceptor.h \
! 	universaljointperceptor.cpp \
! 	universaljointperceptor_c.cpp

  sparkagent_la_LDFLAGS = -module -version-info 0:0:0

Index: hingeeffector.cpp
===================================================================
RCS file: /cvsroot/simspark/simspark/spark/plugin/sparkagent/hingeeffector.cpp,v
retrieving revision 1.1
retrieving revision 1.2
diff -C2 -d -r1.1 -r1.2
*** hingeeffector.cpp	2 Jan 2006 18:11:33 -0000	1.1
--- hingeeffector.cpp	6 Jan 2006 13:57:36 -0000	1.2
***************
*** 54,70 ****
      }

- /*
-     Vector3f axis = mJoint->GetAxis();
- 
-     GetLog()->Error() << "(HingeEffector) axis is ("
-                       << axis[0] << "," << axis[1] << "," << axis[2] << ")\n";
-     
-     GetLog()->Error() << "(HingeEffector) current vel = "
-                       << mJoint->GetAngularMotorVelocity(Joint::AI_FIRST) << "\n";
- 
-     GetLog()->Error() << "(HingeEffector) set vel to "
-                       << hingeAction->GetMotorVelocity() << "\n";
- */
- 
      mJoint->SetParameter(dParamVel, hingeAction->GetMotorVelocity());

--- 54,57 ----

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

   this file is part of rcssserver3D
   Thu Jan 4 2006
   Copyright (C) 2006 RoboCup Soccer Server 3D Maintenance Group

   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 UNIVERSALJOINTACTION_H
#define UNIVERSALJOINTACTION_H

#include <oxygen/gamecontrolserver/actionobject.h>

class UniversalJointAction : public oxygen::ActionObject
{
public:
    UniversalJointAction(const std::string& predicate, float velocity1, float velocity2)
        : ActionObject(predicate), mVelocityAxis1(velocity1), mVelocityAxis2(velocity2) {};

    virtual ~UniversalJointAction() {}

    float GetMotorVelocity(oxygen::Joint::EAxisIndex idx) 
    { 
        if (oxygen::Joint::AI_FIRST) 
            return mVelocityAxis1; 
        else 
            return mVelocityAxis2;
    }

protected:
    float mVelocityAxis1;
    float mVelocityAxis2;
};

#endif // UNIVERSALJOINTACTION_H

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

   this file is part of rcssserver3D
   Fri Jan 6 2006
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2006 RoboCup Soccer Server 3D Maintenance Group

   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 "universaljointeffector.h"

void CLASS(UniversalJointEffector)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Effector);
}

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

   this file is part of rcssserver3D
   Thu Jan 5 2006
   Copyright (C) 2006 RoboCup Soccer Server 3D Maintenance Group

   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 "universaljointperceptor.h"

void CLASS(UniversalJointPerceptor)::DefineClass()
{
    DEFINE_BASECLASS(oxygen/Perceptor);
}