Menu
▾
▴
[simspark-svn] SF.net SVN: simspark:[326] branches/realistic_motor
|
From: <yx...@us...> - 2012-09-13 08:05:23
|
Revision: 326
http://simspark.svn.sourceforge.net/simspark/?rev=326&view=rev
Author: yxu
Date: 2012-09-13 08:05:11 +0000 (Thu, 13 Sep 2012)
Log Message:
-----------
Merge branch 'realistic_motor'
* realistic_motor:
revert changes for my testing
doc of angular motor perceptor/effector, battery perceptor
sense temperature
send battery info every second is enough
set stiffness to 1.0 by default
configure for my testing
stiffness effector
remove backlash
Modified Paths:
--------------
branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg
branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/nao.rsg
branches/realistic_motor/rcssserver3d/doc/users/simspark.tex
branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.cpp
branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.h
branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor_c.cpp
branches/realistic_motor/spark/plugin/sparkagent/CMakeLists.txt
branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.cpp
branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.h
branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector_c.cpp
branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor.cpp
branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor.h
branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor_c.cpp
branches/realistic_motor/spark/plugin/sparkagent/export.cpp
Added Paths:
-----------
branches/realistic_motor/spark/plugin/sparkagent/stiffnessaction.h
branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector.cpp
branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector.h
branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector_c.cpp
Removed Paths:
-------------
branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h
Modified: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg 2012-09-13 08:05:11 UTC (rev 326)
@@ -40,7 +40,7 @@
(setNumAxes 1)
(attach $PathBody1 $PathBody2)
(setMotorAxis 0 1 $Axis_X $Axis_Y $Axis_Z)
- (setMaxMotorForce 0 $FMax)
+ (setMaxTorque $FMax)
(setJointMaxSpeed1 $MaxAbsSpeed)
;; constant of motor
(setSpeedConstant $Ke)
@@ -52,13 +52,14 @@
(setProtectionTempeature 80)
(enableFeedback true)
- (setBattery ../../../torso/Battery)
+ (setBattery ../../../body/Battery)
- (nd AngularMotorPerceptor (setName $PerName))
- (nd AngularMotorEffector
- (setName $EffName)
- (setMaxForce $FMax)
- )
+ (nd AngularMotorPerceptor (setName $PerName)
+ (setSenseAngle true)
+ (setSenseTemperature true))
+ (nd AngularMotorEffector (setName $EffName))
+ (nd StiffnessEffector (setName (join $EffName s)))
)
)
+ )
)
Modified: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/nao.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/nao.rsg 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/nao.rsg 2012-09-13 08:05:11 UTC (rev 326)
@@ -65,6 +65,7 @@
(setPower 167184)
(nd BatteryPerceptor
(setName BAT)
+ (setInterval 50)
)
)
Modified: branches/realistic_motor/rcssserver3d/doc/users/simspark.tex
===================================================================
--- branches/realistic_motor/rcssserver3d/doc/users/simspark.tex 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/rcssserver3d/doc/users/simspark.tex 2012-09-13 08:05:11 UTC (rev 326)
@@ -88,6 +88,7 @@
%klaus: What is the zero degree position, is the above correct?
%Joschka: I don't think this is necessarily correct; to my knowledge, the zero position is determined by the orientation and position of the bodies at the time the joint is attached. Is this right?
+%Yuan: Yes, exactly
\begin{itemize}
\item[Message format:] \texttt{(HJ (n <name>) (ax <ax>))}
@@ -100,8 +101,38 @@
\item[Noise model:] None
\end{itemize}
+%-SUB-SUB-SECTION---------------------------------------------------
+%-------------------- AngularMotor Perceptor -----------------------
+%-------------------------------------------------------------------
+\subsubsection{Angular Motor Perceptor}
+\label{sec:AMP}
+The \emph{angular motor} is a more realistic motor which simulates
+power comsuming and tempeature. A \emph{angular motor perceptor}
+recevies information of \emph{angular motor}, it may contains
+information about angle, rate, torque, current, and tempeature,
+depending on configuration.
+The format of message is pretty similar as \emph{Hinge Joint
+ Perceptor} in \ref{sec:HJP}:
+\begin{itemize}
+ \item[Message format:] \texttt{(HJ (n <name>)(ax <ax>)(rt <rt>)(tq <tq>)(ct <ct>)(tp <tp>))}
+ \begin{itemize}
+ \item[\texttt{<name>} -] the name of the corresponding joint
+ \item[\texttt{<ax>} -] the current position angle in degrees and an accuracy
+ of two digits
+ \item[\texttt{<rt>} -] the speed of motor in degrees per second
+ \item[\texttt{<tq>} -] the output torque of motor in Nm
+ \item[\texttt{<ct>} -] the currency of motor in A
+ \item[\texttt{<tp>} -] internal temperature of motor, in $^\circ$C
+ \end{itemize}
+ \item[Example message:] \texttt{(HJ (n laj3) (ax -1.02)(tp 40.0))}
+ \item[Noise model:] None
+\end{itemize}
+
+All the information are updated every cycle, except temperature which
+included in message only when the change is bigger than 1 $^\circ$C.
+
%-SUB-SUB-SECTION---------------------------------------------------
%-------------------- UniversalJoint Perceptor ---------------------
%-------------------------------------------------------------------
@@ -254,7 +285,18 @@
\end{itemize}
+%-SUB-SUB-SECTION---------------------------------------------------
+%----------------------- Battery Perceptor ---------------------------
+%-------------------------------------------------------------------
+\subsubsection{Battery Perceptor}
+\label{sec:batteryperceptor}
+This perceptor gives information about battery of robot.
+\begin{itemize}
+ \item[Message format:] \texttt{(BAT <charge>)}
+ \item[\texttt{<charge>} -] the charge level of battery in range [0, 1], 1 means fully charged.
+ \item[Example message:] \texttt{(BAT 0.9)}
+\end{itemize}
%-SUB-SECTION-------------------------------------------------------
@@ -423,8 +465,8 @@
\item[Example message:] \texttt{(AgentState (temp 48) (battery 75))}
\end{itemize}
+Note: it is not used now.
-
%-SUB-SUB-SECTION---------------------------------------------------
%----------------------- GameState Perceptor -----------------------
%-------------------------------------------------------------------
@@ -613,8 +655,38 @@
\item[Example message:] \texttt{(lae3 5.3)}
\end{itemize}
+%-SUB-SUB-SECTION---------------------------------------------------
+%----------------------- HingeJoint Effector -----------------------
+%-------------------------------------------------------------------
+\subsubsection{Angular Motor Effector}
+\label{sec:AME}
+The \emph{angular motor effector} is able to drive \emph{angular
+ motor}, and the message format is exactly the same as \emph{hinge
+ joint effector} in section \ref{sec:HJE}.
+%-SUB-SUB-SECTION---------------------------------------------------
+%----------------------- HingeJoint Effector -----------------------
+%-------------------------------------------------------------------
+\subsubsection{Stiffness Effector}
+\label{sec:AME}
+The \emph{stiffness effector} is able to change the stiffness of
+\emph{angular motor}, and the stiffness affects the maximum output
+torque of motor: the motor is off when stiffness equals 0, and motor
+has most power when stiffness is 1.
+\begin{itemize}
+ \item[Message format:] \texttt{(<name> <st>)}
+ \begin{itemize}
+ \item[\texttt{<name>} -] the name of the corresponding effector
+ \item[\texttt{<st>} -] the requested stiffness in range [0, 1]
+ \end{itemize}
+ \item[Example message:] \texttt{(lae3s 1.0)}
+\end{itemize}
+
+Note the \emph{angular motor} has temperature protection, it is
+enabled when the motor is too hot, and then the maximum stiffness is
+limited.
+
%-SUB-SUB-SECTION---------------------------------------------------
%--------------------- UniversalJoint Effector ---------------------
%-------------------------------------------------------------------
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.cpp
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.cpp 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -38,9 +38,12 @@
mTempMotor(mTempEnvironment),
mThermalConductivity(0.025),
mHeatCapacity(26),
- mTempProtection(80)
+ mTempProtection(80),
+ mProtectionStiffness(0.8),
+ mMaxTorque(0),
+ mStiffness(1),
+ mActualStiffness(1)
{
-
}
AngularMotor::~AngularMotor()
@@ -163,6 +166,8 @@
float P = mKe * v * mI + Pr;
float E = P * deltaTime;
+ updateStiffnessControl();
+
if ( mBattery.get() != 0 )
{
if (!mBattery->Consume(E))
@@ -171,11 +176,6 @@
}
}
- if (mTempMotor > mTempProtection)
- {
- SetMaxMotorForce(Joint::AI_FIRST, 0);
- }
-
mTempMotor += (Pr - mThermalConductivity*(mTempMotor-mTempEnvironment)) * deltaTime / mHeatCapacity;
}
@@ -222,3 +222,9 @@
return (mTempMotor > mTempProtection);
}
+
+void AngularMotor::updateStiffnessControl()
+{
+ mActualStiffness = mTempMotor < mTempProtection ? mStiffness : std::min(mStiffness, mProtectionStiffness);
+ SetMaxMotorForce(Joint::AI_FIRST, mActualStiffness * mMaxTorque);
+}
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.h
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.h 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.h 2012-09-13 08:05:11 UTC (rev 326)
@@ -130,7 +130,7 @@
float GetCurrent() const { return mI; }
/** get the tempeature of joint */
- float GetTempeature() const { return mTempMotor; }
+ float GetTemperature() const { return mTempMotor; }
/** constant setters */
void setSpeedConstant(float v) { mKe = v; }
@@ -140,16 +140,22 @@
void setThermalConductivity(float v) { mThermalConductivity = v; }
void setHeatCapacity(float v) { mHeatCapacity = v; }
void setProtectionTempeature(float v) { mTempProtection = v; }
+ void setProtectionStiffness(float v) { mProtectionStiffness = v; }
+ void setMaxTorque(float v) { mMaxTorque = v; }
/** returns the torque on the joint */
float GetTorque() const;
+ void SetStiffness(float v) { mStiffness = v; updateStiffnessControl(); }
+
protected:
/** creates a new angularmotor joint */
virtual void OnLink();
/** update energe comsuption and tempeture */
virtual void PrePhysicsUpdateInternal(float deltaTime);
+
+ void updateStiffnessControl();
private:
static boost::shared_ptr<AngularMotorInt> mAngularMotorImp;
@@ -167,7 +173,12 @@
float mThermalConductivity;
float mHeatCapacity;
float mTempProtection;
+ float mProtectionStiffness;
+ float mMaxTorque;
+ float mStiffness;
+ float mActualStiffness;
+
boost::shared_ptr<HingeJoint> mHingeJoint; // HACK: assume connected with hinge joint
};
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor_c.cpp
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor_c.cpp 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor_c.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -180,6 +180,8 @@
SET_ANGULAR_MOTOR_CONSTANT(setThermalConductivity)
SET_ANGULAR_MOTOR_CONSTANT(setHeatCapacity)
SET_ANGULAR_MOTOR_CONSTANT(setProtectionTempeature)
+SET_ANGULAR_MOTOR_CONSTANT(setProtectionStiffness)
+SET_ANGULAR_MOTOR_CONSTANT(setMaxTorque)
void CLASS(AngularMotor)::DefineClass()
{
@@ -202,4 +204,6 @@
DEFINE_FUNCTION(setThermalConductivity);
DEFINE_FUNCTION(setHeatCapacity);
DEFINE_FUNCTION(setProtectionTempeature);
+ DEFINE_FUNCTION(setProtectionStiffness);
+ DEFINE_FUNCTION(setMaxTorque);
}
Modified: branches/realistic_motor/spark/plugin/sparkagent/CMakeLists.txt
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/CMakeLists.txt 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/CMakeLists.txt 2012-09-13 08:05:11 UTC (rev 326)
@@ -27,13 +27,16 @@
universaljointperceptor.h
universaljointperceptor.cpp
universaljointperceptor_c.cpp
- angularmotoraction.h
angularmotoreffector.h
angularmotoreffector.cpp
angularmotoreffector_c.cpp
angularmotorperceptor.h
angularmotorperceptor.cpp
angularmotorperceptor_c.cpp
+ stiffnessaction.h
+ stiffnesseffector.h
+ stiffnesseffector.cpp
+ stiffnesseffector_c.cpp
)
add_library(sparkagent MODULE ${sparkagent_LIB_SRCS})
Deleted: branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h 2012-09-13 08:05:11 UTC (rev 326)
@@ -1,41 +0,0 @@
-/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
-
- this file is part of rcssserver3D
-
- Copyright (C) 2012 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 ANGULAR_MOTOR_ACTION_H
-#define ANGULAR_MOTOR_ACTION_H
-
-#include <oxygen/gamecontrolserver/actionobject.h>
-
-/* only one axis is supported at the moment */
-class AngularMotorAction : public oxygen::ActionObject
-{
-public:
- AngularMotorAction(const std::string& predicate, float velocity, float stiffness)
- : ActionObject(predicate), mVelocity(velocity), mStiffness(stiffness) {}
-
- virtual ~AngularMotorAction() {}
- float GetMotorVelocity() { return mVelocity; }
- float GetStiffness() { return mStiffness; }
-
-protected:
- float mVelocity;
- float mStiffness;
-};
-
-#endif // ANGULAR_MOTOR_ACTION_H
Modified: branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.cpp 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -18,21 +18,18 @@
Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "angularmotoreffector.h"
-#include "angularmotoraction.h"
+#include "hingeaction.h"
#include <oxygen/physicsserver/rigidbody.h>
-using namespace oxygen;
-using namespace zeitgeist;
-using namespace salt;
-using namespace boost;
using namespace std;
+using namespace boost;
+using namespace salt;
+using namespace zeitgeist;
+using namespace oxygen;
+
AngularMotorEffector::AngularMotorEffector()
- : JointEffector<AngularMotor>::JointEffector("amotor"),
- mUseBacklash(false),
- mGearPosition(0),
- mHalfDeadband(1),
- mMaxForce(10)
+ : JointEffector<AngularMotor>::JointEffector("amotor")
{
}
@@ -59,8 +56,7 @@
return false;
}
- boost::shared_ptr<AngularMotorAction> motorAction =
- shared_dynamic_cast<AngularMotorAction>(action);
+ shared_ptr<HingeAction> motorAction = shared_dynamic_cast<HingeAction>(action);
if (motorAction.get() == 0)
{
@@ -71,10 +67,8 @@
}
float finalMotorVel = motorAction->GetMotorVelocity();
- float stiffness = motorAction->GetStiffness();
- stiffness = gClamp(stiffness, 0.0f, 1.0f);
- float maxForce = stiffness * mMaxForce;
+
if (mJoint->IsLimitJointMaxSpeed1())
{
finalMotorVel = (finalMotorVel > 0) ?
@@ -84,30 +78,7 @@
}
mJoint->SetParameter(2 /*value of dParamVel in ODE*/, finalMotorVel);
-
- if ( mUseBacklash )
- {
- bool dead = false;
- float jointAngle = mJoint->GetAxisAngle(Joint::AI_FIRST);
-
- float maxDeadband = jointAngle + mHalfDeadband;
- float minDeadband = jointAngle - mHalfDeadband;
- if ( mGearPosition < maxDeadband
- && mGearPosition > minDeadband )
- {
- dead = true;
- }
- mGearPosition += finalMotorVel * 0.02;
- mGearPosition = salt::gClamp(mGearPosition, minDeadband-0.1f, maxDeadband+0.1f);
-
- mJoint->SetMaxMotorForce(Joint::AI_FIRST, dead?0.3:maxForce);
- }
- else
- {
- mJoint->SetMaxMotorForce(Joint::AI_FIRST, maxForce);
- }
-
if (motorAction->GetMotorVelocity() != 0)
{
boost::shared_ptr<RigidBody> body = mJoint->GetBody(Joint::BI_FIRST);
@@ -119,7 +90,7 @@
return true;
}
-boost::shared_ptr<ActionObject> AngularMotorEffector::GetActionObject(const Predicate& predicate)
+shared_ptr<ActionObject> AngularMotorEffector::GetActionObject(const Predicate& predicate)
{
for(;;)
{
@@ -146,16 +117,17 @@
break;
}
+ /*
float stiffness;
if (! predicate.AdvanceValue(iter, stiffness))
{
GetLog()->Error()
<< "ERROR: (HingeEffector) motor stiffness expected\n";
break;
- }
+ }*/
- return boost::shared_ptr<AngularMotorAction>(new AngularMotorAction(GetPredicate(),velocity,stiffness));
+ return shared_ptr<HingeAction>(new HingeAction(GetPredicate(),velocity));
}
- return boost::shared_ptr<ActionObject>();
+ return shared_ptr<ActionObject>();
}
Modified: branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.h
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.h 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.h 2012-09-13 08:05:11 UTC (rev 326)
@@ -38,18 +38,6 @@
/** constructs an Actionobject, describing a predicate */
virtual boost::shared_ptr<oxygen::ActionObject>
GetActionObject(const oxygen::Predicate& predicate);
-
- void SetUseBacklash(bool use) { mUseBacklash = use; }
-
- void SetDeadband(float deadband) { mHalfDeadband = deadband / 2;}
-
- void SetMaxForce(float force) { mMaxForce = force; }
-
-private:
- bool mUseBacklash;
- float mGearPosition;
- float mHalfDeadband;
- float mMaxForce;
};
DECLARE_CLASS(AngularMotorEffector);
Modified: branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector_c.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector_c.cpp 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector_c.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -20,61 +20,7 @@
#include "angularmotoreffector.h"
-FUNCTION(AngularMotorEffector,setUseBacklash)
-{
- bool use;
-
- if (
- (in.GetSize() != 1) ||
- (! in.GetValue(in.begin(), use))
- )
- {
- return false;
- }
-
-
- obj->SetUseBacklash(use);
- return true;
-}
-
-FUNCTION(AngularMotorEffector,setDeadband)
-{
- float deadband;
-
- if (
- (in.GetSize() != 1) ||
- (! in.GetValue(in.begin(), deadband))
- )
- {
- return false;
- }
-
-
- obj->SetDeadband(deadband);
- return true;
-}
-
-FUNCTION(AngularMotorEffector,setMaxForce)
-{
- float force;
-
- if (
- (in.GetSize() != 1) ||
- (! in.GetValue(in.begin(), force))
- )
- {
- return false;
- }
-
-
- obj->SetMaxForce(force);
- return true;
-}
-
void CLASS(AngularMotorEffector)::DefineClass()
{
DEFINE_BASECLASS(oxygen/Effector);
- DEFINE_FUNCTION(setUseBacklash);
- DEFINE_FUNCTION(setDeadband);
- DEFINE_FUNCTION(setMaxForce);
}
Modified: branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor.cpp 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -27,7 +27,12 @@
AngularMotorPerceptor::AngularMotorPerceptor()
: JointPerceptor<AngularMotor>::JointPerceptor(),
- mAngleRng(-0.005, 0.005)//-0.04395
+ mSenseAngle(true),
+ mSenseRate(false),
+ mSenseTorque(false),
+ mSenseCurrent(false),
+ mSenseTemperature(false),
+ mLastTemperature(std::numeric_limits<float>::min())
{
}
@@ -39,8 +44,7 @@
{
ParameterList& axisElement = predicate.parameter.AddList();
axisElement.AddValue(string("ax"));
- float noise = mAngleRng();
- float ang = mJoint->GetAxisAngle(Joint::AI_FIRST);// + noise;
+ float ang = mJoint->GetAxisAngle(Joint::AI_FIRST);
axisElement.AddValue(ang);
}
@@ -65,11 +69,16 @@
axisElement.AddValue(mJoint->GetCurrent() * 1000);
}
-void AngularMotorPerceptor::InsertTempeature(Predicate& predicate)
+void AngularMotorPerceptor::InsertTemperature(Predicate& predicate)
{
- ParameterList& axisElement = predicate.parameter.AddList();
- axisElement.AddValue(string("tp"));
- axisElement.AddValue(mJoint->GetTempeature());
+ float temp = mJoint->GetTemperature();
+ if (fabs(temp - mLastTemperature) > 1)
+ {
+ ParameterList& axisElement = predicate.parameter.AddList();
+ axisElement.AddValue(string("tp"));
+ axisElement.AddValue(temp);
+ mLastTemperature = temp;
+ }
}
bool AngularMotorPerceptor::Percept(boost::shared_ptr<oxygen::PredicateList> predList)
@@ -87,11 +96,10 @@
nameElement.AddValue(string("n"));
nameElement.AddValue(GetName());
- InsertAxisAngle(predicate);
- //InsertAxisRate(predicate);
- //InsertAxisTorque(predicate);
- InsertTempeature(predicate);
- InsertCurrent(predicate);
-
+ if (mSenseAngle) InsertAxisAngle(predicate);
+ if (mSenseRate) InsertAxisRate(predicate);
+ if (mSenseTorque) InsertAxisTorque(predicate);
+ if (mSenseTemperature) InsertTemperature(predicate);
+ if (mSenseCurrent) InsertCurrent(predicate);
return true;
}
Modified: branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor.h
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor.h 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor.h 2012-09-13 08:05:11 UTC (rev 326)
@@ -33,16 +33,26 @@
//! \return true, if valid data is available and false otherwise.
bool Percept(boost::shared_ptr<oxygen::PredicateList> predList);
+ void SetSenseAngle(bool v) { mSenseAngle = v; }
+ void SetSenseRate(bool v) { mSenseRate = v; }
+ void SetSenseTorque(bool v) { mSenseTorque = v; }
+ void SetSenseCurrent(bool v) { mSenseCurrent = v; }
+ void SetSenseTemperature(bool v) { mSenseTemperature = v; }
+
protected:
void InsertAxisAngle(oxygen::Predicate& predicate);
void InsertAxisRate(oxygen::Predicate& predicate);
void InsertAxisTorque(oxygen::Predicate& predicate);
void InsertCurrent(oxygen::Predicate& predicate);
- void InsertTempeature(oxygen::Predicate& predicate);
+ void InsertTemperature(oxygen::Predicate& predicate);
private:
- //! random number generator for distance errors
- salt::UniformRNG<> mAngleRng;
+ bool mSenseAngle;
+ bool mSenseRate;
+ bool mSenseTorque;
+ bool mSenseCurrent;
+ bool mSenseTemperature;
+ float mLastTemperature;
};
DECLARE_CLASS(AngularMotorPerceptor);
Modified: branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor_c.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor_c.cpp 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor_c.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -20,7 +20,92 @@
#include "angularmotorperceptor.h"
+FUNCTION(AngularMotorPerceptor,setSenseAngle)
+{
+ bool v;
+
+ if (
+ (in.GetSize() != 1) ||
+ (!in.GetValue(in.begin(), v))
+ )
+ {
+ return false;
+ }
+
+ obj->SetSenseAngle(v);
+ return true;
+}
+
+FUNCTION(AngularMotorPerceptor,setSenseRate)
+{
+ bool v;
+
+ if (
+ (in.GetSize() != 1) ||
+ (!in.GetValue(in.begin(), v))
+ )
+ {
+ return false;
+ }
+
+ obj->SetSenseRate(v);
+ return true;
+}
+
+FUNCTION(AngularMotorPerceptor,setSenseTorque)
+{
+ bool v;
+
+ if (
+ (in.GetSize() != 1) ||
+ (!in.GetValue(in.begin(), v))
+ )
+ {
+ return false;
+ }
+
+ obj->SetSenseTorque(v);
+ return true;
+}
+
+FUNCTION(AngularMotorPerceptor,setSenseCurrent)
+{
+ bool v;
+
+ if (
+ (in.GetSize() != 1) ||
+ (!in.GetValue(in.begin(), v))
+ )
+ {
+ return false;
+ }
+
+ obj->SetSenseCurrent(v);
+ return true;
+}
+
+FUNCTION(AngularMotorPerceptor,setSenseTemperature)
+{
+ bool v;
+
+ if (
+ (in.GetSize() != 1) ||
+ (!in.GetValue(in.begin(), v))
+ )
+ {
+ return false;
+ }
+
+ obj->SetSenseTemperature(v);
+ return true;
+}
+
void CLASS(AngularMotorPerceptor)::DefineClass()
{
DEFINE_BASECLASS(oxygen/Perceptor);
+ DEFINE_FUNCTION(setSenseAngle);
+ DEFINE_FUNCTION(setSenseRate);
+ DEFINE_FUNCTION(setSenseTorque);
+ DEFINE_FUNCTION(setSenseCurrent);
+ DEFINE_FUNCTION(setSenseTemperature);
}
Modified: branches/realistic_motor/spark/plugin/sparkagent/export.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/export.cpp 2012-09-10 12:57:23 UTC (rev 325)
+++ branches/realistic_motor/spark/plugin/sparkagent/export.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -29,6 +29,7 @@
#include "universaljointperceptor.h"
#include "angularmotoreffector.h"
#include "angularmotorperceptor.h"
+#include "stiffnesseffector.h"
ZEITGEIST_EXPORT_BEGIN()
ZEITGEIST_EXPORT(Hinge2Effector);
@@ -40,4 +41,5 @@
ZEITGEIST_EXPORT(UniversalJointPerceptor);
ZEITGEIST_EXPORT(AngularMotorEffector);
ZEITGEIST_EXPORT(AngularMotorPerceptor);
+ ZEITGEIST_EXPORT(StiffnessEffector);
ZEITGEIST_EXPORT_END()
Copied: branches/realistic_motor/spark/plugin/sparkagent/stiffnessaction.h (from rev 325, branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h)
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/stiffnessaction.h (rev 0)
+++ branches/realistic_motor/spark/plugin/sparkagent/stiffnessaction.h 2012-09-13 08:05:11 UTC (rev 326)
@@ -0,0 +1,41 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+
+ Copyright (C) 2012 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 STIFFNESSACTION_H
+#define STIFFNESSACTION_H
+
+#include <oxygen/gamecontrolserver/actionobject.h>
+
+class StiffnessAction : public oxygen::ActionObject
+{
+public:
+ StiffnessAction(const std::string& predicate, float stiffness)
+ : ActionObject(predicate), mStiffness(stiffness) {}
+
+ virtual ~StiffnessAction() {}
+
+ float GetStiffness() { return mStiffness; }
+
+protected:
+ float mStiffness;
+};
+
+
+#endif // STIFFNESSACTION_H
Added: branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector.cpp (rev 0)
+++ branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -0,0 +1,92 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+
+ Copyright (C) 2012 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 "stiffnesseffector.h"
+#include "stiffnessaction.h"
+
+using namespace std;
+using namespace boost;
+using namespace salt;
+using namespace zeitgeist;
+using namespace oxygen;
+
+
+StiffnessEffector::StiffnessEffector()
+ : JointEffector<AngularMotor>::JointEffector("amotorStiffness")
+{
+}
+
+StiffnessEffector::~StiffnessEffector()
+{
+}
+
+bool StiffnessEffector::Realize(boost::shared_ptr<ActionObject> action)
+{
+ if (mJoint.get() == 0) return false;
+
+ shared_ptr<StiffnessAction> stiffnessAction = shared_dynamic_cast<StiffnessAction>(action);
+
+ if (stiffnessAction.get() == 0)
+ {
+ GetLog()->Error()
+ << "ERROR: (StiffnessEffector) cannot realize an "
+ << "unknown ActionObject\n";
+ return false;
+ }
+
+ float stiffness = stiffnessAction->GetStiffness();
+ stiffness = gClamp(stiffness, 0.0f, 1.0f);
+ mJoint->SetStiffness(stiffness);
+ return true;
+}
+
+shared_ptr<ActionObject> StiffnessEffector::GetActionObject(const Predicate& predicate)
+{
+ for(;;)
+ {
+ if (mJoint.get() == 0)
+ {
+ break;
+ }
+
+ if (predicate.name != GetPredicate())
+ {
+ GetLog()->Error()
+ << "ERROR: (HingeEffector) invalid predicate"
+ << predicate.name << "\n";
+ break;
+ }
+
+ Predicate::Iterator iter = predicate.begin();
+
+
+ float stiffness;
+ if (! predicate.AdvanceValue(iter, stiffness))
+ {
+ GetLog()->Error()
+ << "ERROR: (HingeEffector) motor stiffness expected\n";
+ break;
+ }
+
+ return shared_ptr<StiffnessAction>(new StiffnessAction(GetPredicate(),stiffness));
+ }
+
+ return shared_ptr<ActionObject>();
+}
Copied: branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector.h (from rev 325, branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.h)
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector.h (rev 0)
+++ branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector.h 2012-09-13 08:05:11 UTC (rev 326)
@@ -0,0 +1,46 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+
+ Copyright (C) 2012 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 STIFFNESSEFFECTOR_H
+#define STIFFNESSEFFECTOR_H
+
+#include <oxygen/agentaspect/jointeffector.h>
+#include <oxygen/physicsserver/angularmotor.h>
+
+class StiffnessEffector: public oxygen::JointEffector<oxygen::AngularMotor>
+{
+public:
+ StiffnessEffector();
+ virtual ~StiffnessEffector();
+
+ /** 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);
+};
+
+DECLARE_CLASS(StiffnessEffector);
+
+#endif // STIFFNESSEFFECTOR_H
Copied: branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector_c.cpp (from rev 325, branches/realistic_motor/spark/plugin/sparkagent/angularmotorperceptor_c.cpp)
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector_c.cpp (rev 0)
+++ branches/realistic_motor/spark/plugin/sparkagent/stiffnesseffector_c.cpp 2012-09-13 08:05:11 UTC (rev 326)
@@ -0,0 +1,27 @@
+
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+
+ Copyright (C) 2012 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 "stiffnesseffector.h"
+
+void CLASS(StiffnessEffector)::DefineClass()
+{
+ DEFINE_BASECLASS(oxygen/Effector);
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
