Thread: [simspark-svn] SF.net SVN: simspark:[121] trunk (Page 2)

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

Revision: 121
          http://simspark.svn.sourceforge.net/simspark/?rev=121&view=rev
Author:   yxu
Date:     2009-12-15 11:29:57 +0000 (Tue, 15 Dec 2009)

Log Message:
-----------

* add accelerometer in the chest of NAO
* make some functions const

Modified Paths:
--------------
    trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg
    trunk/spark/lib/oxygen/physicsserver/body.cpp
    trunk/spark/lib/oxygen/physicsserver/body.h
    trunk/spark/lib/oxygen/physicsserver/odeobject.cpp
    trunk/spark/lib/oxygen/physicsserver/odeobject.h
    trunk/spark/lib/oxygen/sceneserver/basenode.cpp
    trunk/spark/lib/oxygen/sceneserver/basenode.h
    trunk/spark/plugin/CMakeLists.txt

Added Paths:
-----------
    trunk/spark/plugin/accelerometer/
    trunk/spark/plugin/accelerometer/CMakeLists.txt
    trunk/spark/plugin/accelerometer/accelerometer.cpp
    trunk/spark/plugin/accelerometer/accelerometer.h
    trunk/spark/plugin/accelerometer/accelerometer_c.cpp
    trunk/spark/plugin/accelerometer/export.cpp

Modified: trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg
===================================================================

--- trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg	2009-12-14 11:14:38 UTC (rev 120)
+++ trunk/rcssserver3d/data/rsg/agent/nao/nao.rsg	2009-12-15 11:29:57 UTC (rev 121)
@@ -71,6 +71,7 @@
         )
 
         (nd GyroRatePerceptor (setName torso))
+	(nd Accelerometer (setName torso))
 
         ;(nd HMDPPerceptor)
         ;(nd HMDPEffector)

Modified: trunk/spark/lib/oxygen/physicsserver/body.cpp
===================================================================
--- trunk/spark/lib/oxygen/physicsserver/body.cpp	2009-12-14 11:14:38 UTC (rev 120)
+++ trunk/spark/lib/oxygen/physicsserver/body.cpp	2009-12-15 11:29:57 UTC (rev 121)
@@ -18,6 +18,9 @@
    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
+#include "salt/tvector.h"
+
+
 #include "body.h"
 #include "world.h"
 #include "../sceneserver/scene.h"
@@ -413,6 +416,15 @@
     return Vector3f(vel[0], vel[1], vel[2]);
 }
 
+Vector3f
+Body::GetLocalAngularVelocity() const
+{
+    const dReal* vel = dBodyGetAngularVel(mODEBody);
+    Vector3f w;
+    dBodyVectorFromWorld(mODEBody, vel[0], vel[1], vel[2], w.GetData());
+    return w;
+}
+
 void
 Body::SetAngularVelocity(const Vector3f& vel)
 {
@@ -531,6 +543,12 @@
     dBodyAddForce(mODEBody, force.x(), force.y(), force.z());
 }
 
+Vector3f Body::GetForce() const
+{
+    const dReal* f = dBodyGetForce(mODEBody);
+    return Vector3f(f[0], f[1], f[2]);
+}
+
 void
 Body::AddTorque(const Vector3f& torque)
 {

Modified: trunk/spark/lib/oxygen/physicsserver/body.h
===================================================================
--- trunk/spark/lib/oxygen/physicsserver/body.h	2009-12-14 11:14:38 UTC (rev 120)
+++ trunk/spark/lib/oxygen/physicsserver/body.h	2009-12-15 11:29:57 UTC (rev 121)
@@ -205,6 +205,9 @@
     /** gets the rotation of this body */
     salt::Matrix GetRotation() const;
 
+    /** returns the current angular velocity of this body in local coordination */
+    salt::Vector3f GetLocalAngularVelocity() const;
+
     /** returns the current angular velocity of this body */
     salt::Vector3f GetAngularVelocity() const;
 
@@ -217,6 +220,9 @@
     /** applies a force to the managed ODE body */
     void AddForce(const salt::Vector3f& force);
 
+    /** returns the total force applied to this body */
+    salt::Vector3f GetForce() const;
+
     /** aplies a  force to the managed ODE body */
     void AddTorque(const salt::Vector3f& torque);
 

Modified: trunk/spark/lib/oxygen/physicsserver/odeobject.cpp
===================================================================
--- trunk/spark/lib/oxygen/physicsserver/odeobject.cpp	2009-12-14 11:14:38 UTC (rev 120)
+++ trunk/spark/lib/oxygen/physicsserver/odeobject.cpp	2009-12-15 11:29:57 UTC (rev 121)
@@ -42,7 +42,7 @@
 }
 
 /** returns the world node */
-shared_ptr<World> ODEObject::GetWorld()
+shared_ptr<World> ODEObject::GetWorld() const
 {
     shared_ptr<Scene> scene = GetScene();
     if (scene.get() == 0)

Modified: trunk/spark/lib/oxygen/physicsserver/odeobject.h
===================================================================
--- trunk/spark/lib/oxygen/physicsserver/odeobject.h	2009-12-14 11:14:38 UTC (rev 120)
+++ trunk/spark/lib/oxygen/physicsserver/odeobject.h	2009-12-15 11:29:57 UTC (rev 121)
@@ -61,10 +61,10 @@
     /** destroy the managed ODE object */
     virtual void DestroyODEObject() = 0;
 
-protected:
     /** returns the world node */
-    boost::shared_ptr<World> GetWorld();
+    boost::shared_ptr<World> GetWorld() const;
 
+protected:
     /** finds the nearest parent space node */
     boost::shared_ptr<Space> GetSpace();
 

Modified: trunk/spark/lib/oxygen/sceneserver/basenode.cpp
===================================================================
--- trunk/spark/lib/oxygen/sceneserver/basenode.cpp	2009-12-14 11:14:38 UTC (rev 120)
+++ trunk/spark/lib/oxygen/sceneserver/basenode.cpp	2009-12-15 11:29:57 UTC (rev 121)
@@ -141,7 +141,7 @@
         }
 }
 
-shared_ptr<Scene> BaseNode::GetScene()
+shared_ptr<Scene> BaseNode::GetScene() const
 {
     // is this node the scene node ?
     shared_ptr<Scene> self =

Modified: trunk/spark/lib/oxygen/sceneserver/basenode.h
===================================================================
--- trunk/spark/lib/oxygen/sceneserver/basenode.h	2009-12-14 11:14:38 UTC (rev 120)
+++ trunk/spark/lib/oxygen/sceneserver/basenode.h	2009-12-15 11:29:57 UTC (rev 121)
@@ -96,7 +96,7 @@
     void UpdateHierarchy();
 
     /** moves up the hierarchy, until it finds a scene */
-    boost::shared_ptr<Scene> GetScene();
+    boost::shared_ptr<Scene> GetScene() const;
 
     /** enables debug mode controls */
     void EnableDebugMode();

Modified: trunk/spark/plugin/CMakeLists.txt
===================================================================
--- trunk/spark/plugin/CMakeLists.txt	2009-12-14 11:14:38 UTC (rev 120)
+++ trunk/spark/plugin/CMakeLists.txt	2009-12-15 11:29:57 UTC (rev 121)
@@ -9,6 +9,7 @@
       set(spark_libs ${spark_libs} /System/Library/Frameworks/CoreFoundation.framework)
 endif (APPLE AND USE_COREFOUNDATION) 
 
+add_subdirectory(accelerometer)
 add_subdirectory(filesystemstd)
 add_subdirectory(filesystemzip)
 add_subdirectory(forceeffector)

Added: trunk/spark/plugin/accelerometer/CMakeLists.txt
===================================================================
--- trunk/spark/plugin/accelerometer/CMakeLists.txt	                        (rev 0)
+++ trunk/spark/plugin/accelerometer/CMakeLists.txt	2009-12-15 11:29:57 UTC (rev 121)
@@ -0,0 +1,16 @@
+
+set(accelerometer_LIB_SRCS
+   export.cpp
+   accelerometer.h
+   accelerometer.cpp
+   accelerometer_c.cpp
+)
+
+add_library(accelerometer MODULE ${accelerometer_LIB_SRCS})
+
+target_link_libraries(accelerometer ${spark_libs} ${Boost_LIBRARIES})
+
+set_target_properties(accelerometer PROPERTIES VERSION 0.0.0 SOVERSION 0)
+if (NOT APPLE)
+   install(TARGETS accelerometer DESTINATION ${LIBDIR}/${CMAKE_PROJECT_NAME})
+endif (NOT APPLE)

Added: trunk/spark/plugin/accelerometer/accelerometer.cpp
===================================================================
--- trunk/spark/plugin/accelerometer/accelerometer.cpp	                        (rev 0)
+++ trunk/spark/plugin/accelerometer/accelerometer.cpp	2009-12-15 11:29:57 UTC (rev 121)
@@ -0,0 +1,93 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+   this file is part of rcssserver3D
+   Copyright (C) 2008 RoboCup Soccer Server 3D Maintenance Group
+   $Id$
+
+   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 "oxygen/physicsserver/world.h"
+
+
+#include "oxygen/physicsserver/body.h"
+
+
+#include "accelerometer.h"
+#include <oxygen/sceneserver/transform.h>
+
+// using namespace kerosin;
+using namespace oxygen;
+using namespace boost;
+using namespace zeitgeist;
+using namespace salt;
+using namespace std;
+
+Accelerometer::Accelerometer() : oxygen::Perceptor()
+{
+}
+
+Accelerometer::~Accelerometer()
+{
+}
+
+void Accelerometer::OnLink()
+{
+    shared_ptr<Transform> transformParent = shared_static_cast<Transform>
+        (FindParentSupportingClass<Transform>().lock());
+
+    mBody = shared_static_cast<const Body>
+        (transformParent->GetChildOfClass("Body"));
+
+    mGravity = mBody->GetWorld()->GetGravity();
+    mLastVel = mBody->GetVelocity();
+}
+
+bool Accelerometer::Percept(boost::shared_ptr<PredicateList> predList)
+{
+    Predicate &predicate = predList->AddPredicate();
+    predicate.name = "ACC";
+    predicate.parameter.Clear();
+
+    ParameterList &nameElement = predicate.parameter.AddList();
+    nameElement.AddValue(std::string("n"));
+    nameElement.AddValue(GetName());
+
+    ParameterList &ratesElement = predicate.parameter.AddList();
+    ratesElement.AddValue(std::string("a"));
+    ratesElement.AddValue(mAcc.x());
+    ratesElement.AddValue(mAcc.y());
+    ratesElement.AddValue(mAcc.z());
+
+    return true;
+}
+
+void Accelerometer::PrePhysicsUpdateInternal(float deltaTime)
+{
+//    Vector3f F = mBody->GetForce();
+//    float mass = mBody->GetMass();
+//    mAcc = F / mass - mGravity;
+
+    Vector3f vel = mBody->GetVelocity();
+    Vector3f acc = (vel - mLastVel) / deltaTime;
+    acc -= mGravity;
+
+    Matrix invRot = mBody->GetRotation();
+    invRot.InvertRotationMatrix();
+    acc = invRot * acc;
+
+    float k = 0.9;
+    mAcc = k*mAcc + (1-k)*acc;
+    mLastVel = vel;
+}

Added: trunk/spark/plugin/accelerometer/accelerometer.h
===================================================================
--- trunk/spark/plugin/accelerometer/accelerometer.h	                        (rev 0)
+++ trunk/spark/plugin/accelerometer/accelerometer.h	2009-12-15 11:29:57 UTC (rev 121)
@@ -0,0 +1,49 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+   this file is part of rcssserver3D
+   Copyright (C) 2008 RoboCup Soccer Server 3D Maintenance Group
+   $Id:$
+
+   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 ACCELEROMETER_H
+#define ACCELEROMETER_H
+
+#include <oxygen/agentaspect/perceptor.h>
+#include <oxygen/physicsserver/body.h>
+
+class Accelerometer : public oxygen::Perceptor {
+public:
+    Accelerometer();
+    virtual ~Accelerometer();
+
+    //! \return true, if valid data is available and false otherwise.
+    bool Percept(boost::shared_ptr<oxygen::PredicateList> predList);
+
+    virtual void OnLink();
+
+protected:
+    virtual void PrePhysicsUpdateInternal(float deltaTime);
+
+private:
+    boost::shared_ptr<const oxygen::Body> mBody;
+    salt::Vector3f mAcc;
+    salt::Vector3f mGravity;
+    salt::Vector3f mLastVel;
+};
+
+DECLARE_CLASS(Accelerometer);
+
+#endif //ACCELEROMETER_H

Added: trunk/spark/plugin/accelerometer/accelerometer_c.cpp
===================================================================
--- trunk/spark/plugin/accelerometer/accelerometer_c.cpp	                        (rev 0)
+++ trunk/spark/plugin/accelerometer/accelerometer_c.cpp	2009-12-15 11:29:57 UTC (rev 121)
@@ -0,0 +1,29 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+   this file is part of rcssserver3D
+   Copyright (C) 2008 RoboCup Soccer Server 3D Maintenance Group
+   $Id:$
+
+   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 "accelerometer.h"
+
+using namespace boost;
+using namespace oxygen;
+
+void CLASS(Accelerometer)::DefineClass()
+{
+    DEFINE_BASECLASS(oxygen/Perceptor);
+}

Added: trunk/spark/plugin/accelerometer/export.cpp
===================================================================
--- trunk/spark/plugin/accelerometer/export.cpp	                        (rev 0)
+++ trunk/spark/plugin/accelerometer/export.cpp	2009-12-15 11:29:57 UTC (rev 121)
@@ -0,0 +1,26 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+   this file is part of rcssserver3D
+   Copyright (C) 2008 RoboCup Soccer Server 3D Maintenance Group
+   $Id:$
+
+   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 "accelerometer.h"
+#include <zeitgeist/zeitgeist.h>
+
+ZEITGEIST_EXPORT_BEGIN()
+        ZEITGEIST_EXPORT(Accelerometer);
+ZEITGEIST_EXPORT_END()


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.


