Menu
▾
▴
[simspark-svn] SF.net SVN: simspark:[121] trunk
|
From: <yx...@us...> - 2009-12-15 11:30:07
|
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.
|
