Menu
▾
▴
simspark-svn
|
From: <yx...@us...> - 2012-09-10 12:57:32
|
Revision: 325
http://simspark.svn.sourceforge.net/simspark/?rev=325&view=rev
Author: yxu
Date: 2012-09-10 12:57:23 +0000 (Mon, 10 Sep 2012)
Log Message:
-----------
merge changes in my local git
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/data/rsg/agent/nao/naoarm.rsg
branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoleg.rsg
branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg
branches/realistic_motor/spark/lib/oxygen/CMakeLists.txt
branches/realistic_motor/spark/lib/oxygen/oxygen.cpp
branches/realistic_motor/spark/lib/oxygen/oxygen.h
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/lib/oxygen/physicsserver/hingejoint.cpp
branches/realistic_motor/spark/lib/oxygen/physicsserver/int/angularmotorint.h
branches/realistic_motor/spark/lib/oxygen/physicsserver/int/jointint.h
branches/realistic_motor/spark/lib/oxygen/physicsserver/joint.h
branches/realistic_motor/spark/lib/salt/sharedlibrary.cpp
branches/realistic_motor/spark/plugin/CMakeLists.txt
branches/realistic_motor/spark/plugin/odeimps/odeangularmotor.cpp
branches/realistic_motor/spark/plugin/odeimps/odeangularmotor.h
branches/realistic_motor/spark/plugin/odeimps/odejoint.cpp
branches/realistic_motor/spark/plugin/odeimps/odejoint.h
branches/realistic_motor/spark/plugin/sparkagent/CMakeLists.txt
branches/realistic_motor/spark/plugin/sparkagent/export.cpp
branches/realistic_motor/spark/plugin/sparkagent/hingeaction.h
branches/realistic_motor/spark/plugin/sparkagent/hingeeffector.cpp
branches/realistic_motor/spark/plugin/sparkagent/hingeeffector.h
branches/realistic_motor/spark/spark/spark.rb
Added Paths:
-----------
branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint1.rsg
branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint2.rsg
branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint3.rsg
branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint4.rsg
branches/realistic_motor/spark/lib/oxygen/physicsserver/battery.cpp
branches/realistic_motor/spark/lib/oxygen/physicsserver/battery.h
branches/realistic_motor/spark/lib/oxygen/physicsserver/battery_c.cpp
branches/realistic_motor/spark/plugin/batteryperceptor/
branches/realistic_motor/spark/plugin/batteryperceptor/CMakeLists.txt
branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor.cpp
branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor.h
branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor_c.cpp
branches/realistic_motor/spark/plugin/batteryperceptor/export.cpp
branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h
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
Modified: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/hingejoint.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -2,54 +2,63 @@
(RSG 0 1)
(
- (templ $PerName $EffName
- $Path_Body1 $Path_Body2
- $Anchor_X $Anchor_Y $Anchor_Z
- $Axis_X $Axis_Y $Axis_Z
- $Min $Max
+ (templ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ $Ke $Kt $R
+ $Te $Ct $Ch
+ $FMax $MaxAbsSpeed
)
- (def $PI 3.14159265)
- ;(def $MinDeg (eval $Min * (eval 180.0 / $PI)))
- ;(def $MaxDeg (eval $Max * (eval 180.0 / $PI)))
- (def $MinDeg $Min)
- (def $MaxDeg $Max)
-
- ; There are 2 types of motor installed on the real Nao, each type has
- ; 2 types of "Reduction ratio".
- ; So, there are all 4 types of joint speed.
- ; They are: 188.67 deg/s, 290.25 deg/s, 305.16 deg/s, 351.77 deg/s
- ; The maximum of them is 351.77.
- ; We don't need to follow them totally that different hingeeffector is using
- ; different types of motor.
- ; All the hingeeffector uses the 351.77
- (def $MaxAbsSpeed (eval $PI * (eval 351.77 / 180.0)))
-
;
;Hinge Joint Parameters
;
- (def $FMax 10)
(def $CFM 1e-8)
(def $Stop_CFM 1e-8)
(def $Stop_ERP 0.8)
(def $FudgeFactor 1e-5)
(def $Bounce 1)
+ (def $PathBody1 (join ../ $Path_Body1))
+ (def $PathBody2 (join ../ $Path_Body2))
+
(nd HingeJoint
- (attach $Path_Body1 $Path_Body2)
- (setAnchor $Anchor_X $Anchor_Y $Anchor_Z)
- (setAxis $Axis_X $Axis_Y $Axis_Z)
- (setMaxMotorForce 0 $FMax)
- (setCFM 0 $CFM)
- (setStopCFM 0 $Stop_CFM)
- (setStopERP 0 $Stop_ERP)
- (setFudgeFactor 0 $FudgeFactor)
- (setBounce 0 $Bounce)
- (setLowStopDeg 0 $MinDeg)
- (setHighStopDeg 0 $MaxDeg)
- (setJointMaxSpeed1 $MaxAbsSpeed)
-
- (nd HingePerceptor (setName $PerName))
- (nd HingeEffector (setName $EffName))
- )
+ (attach $Path_Body1 $Path_Body2)
+ (setAnchor $Anchor_X $Anchor_Y $Anchor_Z)
+ (setAxis $Axis_X $Axis_Y $Axis_Z)
+ (setCFM 0 $CFM)
+ (setStopCFM 0 $Stop_CFM)
+ (setStopERP 0 $Stop_ERP)
+ (setFudgeFactor 0 $FudgeFactor)
+ (setBounce 0 $Bounce)
+ (setLowStopDeg 0 $MinDeg)
+ (setHighStopDeg 0 $MaxDeg)
+ (setMaxMotorForce 0 0.1) ;; as friction
+ (nd AngularMotor
+ (setNumAxes 1)
+ (attach $PathBody1 $PathBody2)
+ (setMotorAxis 0 1 $Axis_X $Axis_Y $Axis_Z)
+ (setMaxMotorForce 0 $FMax)
+ (setJointMaxSpeed1 $MaxAbsSpeed)
+ ;; constant of motor
+ (setSpeedConstant $Ke)
+ (setTorqueConstant $Kt)
+ (setResistance $R)
+ (setEnvironmentTempeature $Te)
+ (setThermalConductivity $Ct)
+ (setHeatCapacity $Ch)
+ (setProtectionTempeature 80)
+
+ (enableFeedback true)
+ (setBattery ../../../torso/Battery)
+
+ (nd AngularMotorPerceptor (setName $PerName))
+ (nd AngularMotorEffector
+ (setName $EffName)
+ (setMaxForce $FMax)
+ )
+ )
+ )
)
Added: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint1.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint1.rsg (rev 0)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint1.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,33 @@
+; -*- mode: lisp; -*-
+
+(RSG 0 1)
+(
+ (templ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ $Te
+ )
+
+ (def $PI 3.14159265)
+ (def $RMP2RPS (eval $PI / 30))
+
+ (def $MaxAbsSpeed (eval 39.7 * $RMP2RPS))
+ (def $Ke (eval 2.2 * $RMP2RPS))
+ (def $Kt 4.3)
+ (def $R 6.44)
+ (def $Ct 0.0158)
+ (def $Ch 27.2)
+ (def $FMax 12)
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ $Ke $Kt $R
+ $Te $Ct $Ch
+ $FMax $MaxAbsSpeed)
+ )
Added: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint2.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint2.rsg (rev 0)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint2.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,33 @@
+; -*- mode: lisp; -*-
+
+(RSG 0 1)
+(
+ (templ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ )
+
+ (def $PI 3.14159265)
+ (def $RMP2RPS (eval $PI / 30))
+
+ (def $MaxAbsSpeed (eval 79.2 * $RMP2RPS))
+ (def $Ke (eval 3.8 * $RMP2RPS))
+ (def $Kt 2.5)
+ (def $R 23.1)
+ (def $Ct 0.0158)
+ (def $Ch 27.2)
+ (def $Te 38)
+ (def $FMax 2.3)
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ $Ke $Kt $R
+ $Te $Ct $Ch
+ $FMax $MaxAbsSpeed)
+ )
Added: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint3.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint3.rsg (rev 0)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint3.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,33 @@
+; -*- mode: lisp; -*-
+
+(RSG 0 1)
+(
+ (templ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ )
+
+ (def $PI 3.14159265)
+ (def $RMP2RPS (eval $PI / 30))
+
+ (def $MaxAbsSpeed (eval 61.1 * $RMP2RPS))
+ (def $Ke (eval 3.4 * $RMP2RPS))
+ (def $Kt 2.8)
+ (def $R 6.44)
+ (def $Ct 0.0198)
+ (def $Ch 43.6)
+ (def $Te 27)
+ (def $FMax 7.8)
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ $Ke $Kt $R
+ $Te $Ct $Ch
+ $FMax $MaxAbsSpeed)
+ )
Added: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint4.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint4.rsg (rev 0)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/joint4.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,33 @@
+; -*- mode: lisp; -*-
+
+(RSG 0 1)
+(
+ (templ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ )
+
+ (def $PI 3.14159265)
+ (def $RMP2RPS (eval $PI / 30))
+
+ (def $MaxAbsSpeed (eval 68.7 * $RMP2RPS))
+ (def $Ke (eval 3.3 * $RMP2RPS))
+ (def $Kt 2.9)
+ (def $R 32.1)
+ (def $Ct 0.0198)
+ (def $Ch 43.6)
+ (def $Te 38)
+ (def $FMax 2.6)
+
+ (importScene rsg/agent/nao/hingejoint.rsg
+ $PerName $EffName
+ $Path_Body1 $Path_Body2
+ $Anchor_X $Anchor_Y $Anchor_Z
+ $Axis_X $Axis_Y $Axis_Z
+ $MinDeg $MaxDeg
+ $Ke $Kt $R
+ $Te $Ct $Ch
+ $FMax $MaxAbsSpeed)
+ )
Modified: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/nao.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/nao.rsg 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/nao.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -59,6 +59,15 @@
(importScene rsg/agent/nao/contactjointhandler.rsg)
)
+ (nd Battery
+ (setName Battery)
+ (setCapacity 167184)
+ (setPower 167184)
+ (nd BatteryPerceptor
+ (setName BAT)
+ )
+ )
+
;Install effectors and perceptors
(nd StaticMeshInitEffector)
Modified: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoarm.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoarm.rsg 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoarm.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -147,7 +147,7 @@
;(importScene rsg/agent/nao/sphere_physics.rsg $ShoulderRadius $ShoulderMass)
(importScene rsg/agent/nao/sphere_physics_nocollider.rsg $ShoulderRadius $ShoulderMass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint2.rsg
$Joint1PerName $Joint1EffName
../sphereBody ../../body/boxBody
0 0 0
@@ -174,7 +174,7 @@
(importScene rsg/agent/nao/box_physics.rsg $UpperArmSizeX $UpperArmSizeY $UpperArmSizeZ $UpperArmMass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint4.rsg
$Joint2PerName $Joint2EffName
../boxBody $ShoulderBodyPath
$aj2_Anchor_X $aj2_Anchor_Y $aj2_Anchor_Z
@@ -194,7 +194,7 @@
;(importScene rsg/agent/nao/sphere_physics.rsg $ElbowRadius $ElbowMass)
(importScene rsg/agent/nao/sphere_physics_nocollider.rsg $ElbowRadius $ElbowMass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint2.rsg
$Joint3PerName $Joint3EffName
../sphereBody $UpperarmBodyPath
0 0 0
@@ -220,7 +220,7 @@
(importScene rsg/agent/nao/box_physics.rsg $LowerArmSizeX $LowerArmSizeY $LowerArmSizeZ $LowerArmMass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint4.rsg
$Joint4PerName $Joint4EffName
../boxBody $ElbowBodyPath
$aj4_Anchor_X $aj4_Anchor_Y $aj4_Anchor_Z
Modified: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoleg.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoleg.rsg 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoleg.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -188,12 +188,13 @@
(importScene rsg/agent/nao/sphere_physics_nocollider.rsg $Hip1Radius $Hip1Mass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint1.rsg
$Joint1PerName $Joint1EffName
../sphereBody ../../body/boxBody
0 0 0
$lj1_axis_x $lj1_axis_y $lj1_axis_z
- $lj1_min $lj1_max)
+ $lj1_min $lj1_max
+ 33)
)
;
@@ -207,12 +208,13 @@
;(importScene rsg/agent/nao/sphere_physics.rsg $Hip2Radius $Hip2Mass)
(importScene rsg/agent/nao/sphere_physics_nocollider.rsg $Hip2Radius $Hip2Mass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint1.rsg
$Joint2PerName $Joint2EffName
../sphereBody $Hip1BodyPath
0 0 0 ;anchor
0 1 0 ;axis
- $lj2_min $lj2_max)
+ $lj2_min $lj2_max
+ 27)
)
;
@@ -242,7 +244,7 @@
)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint3.rsg
$Joint3PerName $Joint3EffName
../boxBody $Hip2BodyPath
$lj3_Anchor_X $lj3_Anchor_Y $lj3_Anchor_Z
@@ -276,7 +278,7 @@
(importScene rsg/agent/nao/contactjointhandler.rsg)
)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint3.rsg
$Joint4PerName $Joint4EffName
../boxBody $ThighBodyPath
0 -0.01 0.045
@@ -296,7 +298,7 @@
;(importScene rsg/agent/nao/sphere_physics.rsg $AnkleRadius $AnkleMass)
(importScene rsg/agent/nao/sphere_physics_nocollider.rsg $AnkleRadius $AnkleMass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint3.rsg
$Joint5PerName $Joint5EffName
../sphereBody $ShankBodyPath
0 0 0
@@ -335,12 +337,13 @@
(importScene rsg/agent/nao/touchperceptorhandler.rsg)
)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint1.rsg
$Joint6PerName $Joint6EffName
../boxBody $AnkleBodyPath
0 -0.03 0.035
0 1 0 ;1 -1 -1
- $lj6_min $lj6_max)
+ $lj6_min $lj6_max
+ 27)
(nd ObjectState
(setID $FootName)
Modified: branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg
===================================================================
--- branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/rcssserver3d/data/rsg/agent/nao/naoneckhead.rsg 2012-09-10 12:57:23 UTC (rev 325)
@@ -46,7 +46,7 @@
(importScene rsg/agent/nao/capsule_appearance.rsg $Neck_Radius $Neck_Length matDarkGrey)
(importScene rsg/agent/nao/capsule_physics.rsg $Neck_Radius $Neck_Length $Neck_Mass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint2.rsg
hj1 he1
../capsuleBody ../../body/boxBody
0 0 0
@@ -78,7 +78,7 @@
)
(importScene rsg/agent/nao/sphere_physics.rsg $Head_Radius $Head_Mass)
- (importScene rsg/agent/nao/hingejoint.rsg
+ (importScene rsg/agent/nao/joint4.rsg
hj2 he2
../sphereBody ../../neck/capsuleBody
;../boxBody ../../body/boxBody
Modified: branches/realistic_motor/spark/lib/oxygen/CMakeLists.txt
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/CMakeLists.txt 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/CMakeLists.txt 2012-09-10 12:57:23 UTC (rev 325)
@@ -53,6 +53,7 @@
physicsserver/universaljoint.h
physicsserver/hinge2joint.h
physicsserver/angularmotor.h
+ physicsserver/battery.h
#interfaces
physicsserver/int/angularmotorint.h
@@ -227,6 +228,8 @@
physicsserver/hinge2joint_c.cpp
physicsserver/angularmotor.cpp
physicsserver/angularmotor_c.cpp
+ physicsserver/battery.cpp
+ physicsserver/battery_c.cpp
sceneserver/basenode.cpp
sceneserver/basenode_c.cpp
Modified: branches/realistic_motor/spark/lib/oxygen/oxygen.cpp
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/oxygen.cpp 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/oxygen.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -75,6 +75,7 @@
zg.GetCore()->RegisterClassObject(new CLASS(SliderJoint), "oxygen/");
zg.GetCore()->RegisterClassObject(new CLASS(UniversalJoint), "oxygen/");
zg.GetCore()->RegisterClassObject(new CLASS(Hinge2Joint), "oxygen/");
+ zg.GetCore()->RegisterClassObject(new CLASS(Battery), "oxygen/");
// collider
zg.GetCore()->RegisterClassObject(new CLASS(Collider), "oxygen/");
Modified: branches/realistic_motor/spark/lib/oxygen/oxygen.h
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/oxygen.h 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/oxygen.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -73,6 +73,7 @@
#include <oxygen/physicsserver/sliderjoint.h>
#include <oxygen/physicsserver/universaljoint.h>
#include <oxygen/physicsserver/hinge2joint.h>
+#include <oxygen/physicsserver/battery.h>
#include <oxygen/sceneserver/basenode.h>
#include <oxygen/sceneserver/camera.h>
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.cpp
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.cpp 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -29,7 +29,16 @@
boost::shared_ptr<AngularMotorInt> AngularMotor::mAngularMotorImp;
-AngularMotor::AngularMotor() : Joint()
+AngularMotor::AngularMotor() : Joint(),
+ mKe(4.3),
+ mKt(4.3),
+ mR(6.44),
+ mI(0),
+ mTempEnvironment(20),
+ mTempMotor(mTempEnvironment),
+ mThermalConductivity(0.025),
+ mHeatCapacity(26),
+ mTempProtection(80)
{
}
@@ -53,6 +62,17 @@
}
mJointID = mAngularMotorImp->CreateAngularMotor(world);
+
+ mHingeJoint = FindParentSupportingClass<HingeJoint>().lock();
+
+ if (mHingeJoint.get() == 0)
+ {
+ GetLog()->Error()
+ << "(" << GetClass()->GetName()
+ << ") ERROR: found no Joint parent\n";
+ }
+
+ mTempMotor = mTempEnvironment;
}
void AngularMotor::SetMode(int mode)
@@ -88,7 +108,7 @@
void AngularMotor::SetMotorAxis(EAxisIndex idx, EAxisAnchor anchor,
const salt::Vector3f& axis)
{
- Vector3f globalAxis = GetWorldTransform() * axis;
+ Vector3f globalAxis = axis;//GetWorldTransform() * axis;
mAngularMotorImp->SetMotorAxis(idx, anchor, globalAxis, mJointID);
}
@@ -109,6 +129,7 @@
float AngularMotor::GetAxisAngle(EAxisIndex idx)
{
+ return mHingeJoint->GetAngle(); // HACK
return mAngularMotorImp->GetAxisAngle(idx, mJointID);
}
@@ -126,3 +147,78 @@
{
return mJointImp->GetParameter(parameter, mJointID);
}
+
+float AngularMotor::GetTorque() const
+{
+ return mAngularMotorImp->GetTorque(mJointID);
+}
+
+void AngularMotor::PrePhysicsUpdateInternal(float deltaTime)
+{
+ const float v = gAbs(gDegToRad(GetAngularMotorVelocity(Joint::AI_FIRST)));
+ const float t = GetTorque();
+
+ mI = t / mKt;
+ float Pr = mI * mI * mR;
+ float P = mKe * v * mI + Pr;
+ float E = P * deltaTime;
+
+ if ( mBattery.get() != 0 )
+ {
+ if (!mBattery->Consume(E))
+ {
+ SetMaxMotorForce(Joint::AI_FIRST, 0);
+ }
+ }
+
+ if (mTempMotor > mTempProtection)
+ {
+ SetMaxMotorForce(Joint::AI_FIRST, 0);
+ }
+
+ mTempMotor += (Pr - mThermalConductivity*(mTempMotor-mTempEnvironment)) * deltaTime / mHeatCapacity;
+}
+
+void AngularMotor::SetBattery(const std::string& batteryPath)
+{
+ mBattery.reset();
+ if (batteryPath.empty())
+ {
+ return;
+ }
+
+ boost::shared_ptr<Leaf> mySelf = shared_static_cast<Leaf>
+ (GetSelf().lock());
+
+ boost::shared_ptr<Leaf> leaf = GetCore()->Get(batteryPath,mySelf);
+
+ if (leaf.get() == 0)
+ {
+ GetLog()->Error()
+ << "(HingeJoint) ERROR: cannot find node '"
+ << batteryPath << "'\n";
+ return;
+ }
+
+ boost::shared_ptr<Battery> battery = shared_dynamic_cast<Battery>(leaf);
+
+ if (battery.get() == 0)
+ {
+ GetLog()->Error()
+ << "(HingeJoint) ERROR: node '"
+ << batteryPath << "' is not a Battery node \n";
+ return;
+ }
+
+ mBattery = battery;
+}
+
+bool AngularMotor::Disabled()
+{
+ if ( mBattery.get() != 0 && mBattery->IsEmpty() )
+ {
+ return true;
+ }
+
+ return (mTempMotor > mTempProtection);
+}
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.h
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.h 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -22,6 +22,8 @@
#include <oxygen/oxygen_defines.h>
#include <oxygen/physicsserver/joint.h>
+#include <oxygen/physicsserver/battery.h>
+#include <oxygen/physicsserver/hingejoint.h>
namespace oxygen
{
@@ -114,18 +116,59 @@
*/
float GetAxisAngleRate(EAxisIndex idx);
-protected:
- /** creates a new angularmotor joint */
- virtual void OnLink();
-
/** sets a joint parameter value */
virtual void SetParameter(int parameter, float value);
/** returns a joint parameter value */
virtual float GetParameter(int parameter) const;
+
+ void SetBattery(const std::string& batteryPath);
+
+ bool Disabled();
+
+ /** get the current of joint */
+ float GetCurrent() const { return mI; }
+
+ /** get the tempeature of joint */
+ float GetTempeature() const { return mTempMotor; }
+
+ /** constant setters */
+ void setSpeedConstant(float v) { mKe = v; }
+ void setTorqueConstant(float v) { mKt = v; }
+ void setResistance(float v) { mR = v; }
+ void setEnvironmentTempeature(float v) { mTempEnvironment = v; mTempMotor = v;}
+ void setThermalConductivity(float v) { mThermalConductivity = v; }
+ void setHeatCapacity(float v) { mHeatCapacity = v; }
+ void setProtectionTempeature(float v) { mTempProtection = v; }
+
+ /** returns the torque on the joint */
+ float GetTorque() const;
+
+protected:
+ /** creates a new angularmotor joint */
+ virtual void OnLink();
+
+ /** update energe comsuption and tempeture */
+ virtual void PrePhysicsUpdateInternal(float deltaTime);
private:
static boost::shared_ptr<AngularMotorInt> mAngularMotorImp;
+
+ boost::shared_ptr<Battery> mBattery;
+
+ /** parameters of motor */
+ float mKe;
+ float mKt;
+ float mR;
+ float mI; // current
+ /** parameters of tempeature */
+ float mTempEnvironment;
+ float mTempMotor;
+ float mThermalConductivity;
+ float mHeatCapacity;
+ float mTempProtection;
+
+ boost::shared_ptr<HingeJoint> mHingeJoint; // HACK: assume connected with hinge joint
};
DECLARE_CLASS(AngularMotor);
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor_c.cpp
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor_c.cpp 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/angularmotor_c.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -149,6 +149,38 @@
return obj->GetAxisAngleRate(static_cast<Joint::EAxisIndex>(inAxisIdx));
}
+FUNCTION(AngularMotor,setBattery)
+{
+ std::string inPath;
+
+ if (in.GetSize() != 1
+ || (!in.GetValue(in[0], inPath)))
+ {
+ return false;
+ }
+
+ obj->SetBattery(inPath);
+ return true;
+}
+
+#define SET_ANGULAR_MOTOR_CONSTANT(functionName) \
+ static zeitgeist::GCValue functionName(AngularMotor *obj, \
+ __attribute__((unused)) const zeitgeist::ParameterList &in)\
+{\
+ float inValue;\
+ if (in.GetSize() != 1 || (!in.GetValue(in[0], inValue))) { return false; }\
+ obj->functionName(inValue);\
+ return true;\
+}
+
+SET_ANGULAR_MOTOR_CONSTANT(setSpeedConstant)
+SET_ANGULAR_MOTOR_CONSTANT(setTorqueConstant)
+SET_ANGULAR_MOTOR_CONSTANT(setResistance)
+SET_ANGULAR_MOTOR_CONSTANT(setEnvironmentTempeature)
+SET_ANGULAR_MOTOR_CONSTANT(setThermalConductivity)
+SET_ANGULAR_MOTOR_CONSTANT(setHeatCapacity)
+SET_ANGULAR_MOTOR_CONSTANT(setProtectionTempeature)
+
void CLASS(AngularMotor)::DefineClass()
{
DEFINE_BASECLASS(oxygen/Joint);
@@ -162,4 +194,12 @@
DEFINE_FUNCTION(setAxisAngle);
DEFINE_FUNCTION(getAxisAngle);
DEFINE_FUNCTION(getAxisAngleRate);
+ DEFINE_FUNCTION(setBattery);
+ DEFINE_FUNCTION(setSpeedConstant);
+ DEFINE_FUNCTION(setTorqueConstant);
+ DEFINE_FUNCTION(setResistance);
+ DEFINE_FUNCTION(setEnvironmentTempeature);
+ DEFINE_FUNCTION(setThermalConductivity);
+ DEFINE_FUNCTION(setHeatCapacity);
+ DEFINE_FUNCTION(setProtectionTempeature);
}
Copied: branches/realistic_motor/spark/lib/oxygen/physicsserver/battery.cpp (from rev 324, branches/realistic_motor/spark/plugin/sparkagent/hingeaction.h)
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/battery.cpp (rev 0)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/battery.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,54 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+ Copyright (C) 2011 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/battery.h>
+
+using namespace oxygen;
+using namespace boost;
+
+
+Battery::Battery() : PhysicsObject(),
+ mCapacity(0),
+ mPower(0)
+{
+}
+
+Battery::~Battery()
+{
+}
+
+void Battery::OnLink()
+{
+ PhysicsObject::OnLink();
+}
+
+bool Battery::Consume(float v)
+{
+ if (mPower > v)
+ {
+ mPower -= v;
+ return true;
+ }
+ else
+ {
+ mPower = 0;
+ return false;
+ }
+}
Copied: branches/realistic_motor/spark/lib/oxygen/physicsserver/battery.h (from rev 324, branches/realistic_motor/spark/plugin/sparkagent/hingeaction.h)
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/battery.h (rev 0)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/battery.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,54 @@
+/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+ this file is part of rcssserver3D
+ Copyright (C) 2011 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 OXYGEN_BATTERY_H
+#define OXYGEN_BATTERY_H
+
+#include <oxygen/physicsserver/physicsobject.h>
+
+namespace oxygen
+{
+
+class OXYGEN_API Battery : public PhysicsObject
+{
+public:
+ Battery();
+ virtual ~Battery();
+ virtual void OnLink();
+
+ bool Consume(float v);
+
+ void SetCapacity(float v) {mCapacity = v;}
+
+ void SetPower(float v) { mPower = salt::gMin(mCapacity, v); }
+
+ bool IsEmpty() const { return mPower <= 0; }
+
+ float State() const { return mPower / mCapacity; }
+
+protected:
+ float mCapacity;
+ float mPower;
+};
+
+DECLARE_CLASS(Battery);
+
+} //namespace oxygen
+
+#endif //OXYGEN_BATTERY_H
Added: branches/realistic_motor/spark/lib/oxygen/physicsserver/battery_c.cpp
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/battery_c.cpp (rev 0)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/battery_c.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,58 @@
+/* -*- mode: c++ -*-
+
+ this file is part of rcssserver3D
+ Copyright (C) 2011 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/battery.h>
+
+using namespace oxygen;
+
+FUNCTION(Battery,setCapacity)
+{
+ float inValue;
+
+ if (in.GetSize() != 1
+ || (!in.GetValue(in[0], inValue)))
+ {
+ return false;
+ }
+
+ obj->SetCapacity(inValue);
+ return true;
+}
+
+FUNCTION(Battery,setPower)
+{
+ float inValue;
+
+ if (in.GetSize() != 1
+ || (!in.GetValue(in[0], inValue)))
+ {
+ return false;
+ }
+
+ obj->SetPower(inValue);
+ return true;
+}
+
+void CLASS(Battery)::DefineClass()
+{
+ DEFINE_BASECLASS(oxygen/PhysicsObject);
+ DEFINE_FUNCTION(setCapacity);
+ DEFINE_FUNCTION(setPower);
+}
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/hingejoint.cpp
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/hingejoint.cpp 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/hingejoint.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -30,7 +30,6 @@
HingeJoint::HingeJoint() : Generic6DOFJoint()
{
-
}
HingeJoint::~HingeJoint()
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/int/angularmotorint.h
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/int/angularmotorint.h 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/int/angularmotorint.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -106,6 +106,9 @@
euler angle rate.
*/
virtual float GetAxisAngleRate(int idx, long jointID) = 0;
+
+ /** returns the torque on the joint */
+ virtual float GetTorque(long jointID) const = 0;
};
} //namespace oxygen
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/int/jointint.h
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/int/jointint.h 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/int/jointint.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -211,9 +211,12 @@
/** sets the angular motor velocity in degrees */
virtual void SetAngularMotorVelocity(int idx, float deg, long jointID) = 0;
- /** returns the angular motor velocity in degrees */
+ /** returns the angular motor velocity in degrees / s */
virtual float GetAngularMotorVelocity(int idx, long jointID) const = 0;
+ /** returns the angular motor angle in degrees */
+ virtual float GetAngularMotorAngle(int idx, long jointID) const = 0;
+
/** sets the maximum force or torque that the motor will use to
achieve the desired velocity. This must always be greater than
or equal to zero. Setting this to zero (the default value)
Modified: branches/realistic_motor/spark/lib/oxygen/physicsserver/joint.h
===================================================================
--- branches/realistic_motor/spark/lib/oxygen/physicsserver/joint.h 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/oxygen/physicsserver/joint.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -274,7 +274,7 @@
/** This has to be declared here because rsgedit upcasts to Joint
at one point and then calls this.
*/
- virtual void SetParameter(int parameter, float value){};
+ virtual void SetParameter(int parameter, float value){}
protected:
/** associates the created joint with this node */
Modified: branches/realistic_motor/spark/lib/salt/sharedlibrary.cpp
===================================================================
--- branches/realistic_motor/spark/lib/salt/sharedlibrary.cpp 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/lib/salt/sharedlibrary.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -80,6 +80,7 @@
#endif
mLibHandle = ::dlopen((libName + ".so").c_str(), RTLD_LAZY);
+#ifdef __APPLE__
if (mLibHandle == 0)
{ // we didn't find the plugin, so we try again...
/* mainly to work with MacOS bundles, so that plugins can be located like this:
@@ -93,6 +94,7 @@
*/
mLibHandle = ::dlopen((RFile::BundlePath() + "Contents/plugins/" + libName + ".so").c_str(), RTLD_LAZY);
}
+#endif
if (mLibHandle == 0)
{
std::cerr << "(SharedLibrary) ERROR: dlopen failed for " << libName
Modified: branches/realistic_motor/spark/plugin/CMakeLists.txt
===================================================================
--- branches/realistic_motor/spark/plugin/CMakeLists.txt 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/plugin/CMakeLists.txt 2012-09-10 12:57:23 UTC (rev 325)
@@ -32,6 +32,7 @@
add_subdirectory(soundsystemfmod)
add_subdirectory(timersystemboost)
add_subdirectory(timersystemsdl)
+add_subdirectory(batteryperceptor)
if (APPLE)
if (DEVEL)
add_subdirectory(imageperceptor)
Added: branches/realistic_motor/spark/plugin/batteryperceptor/CMakeLists.txt
===================================================================
--- branches/realistic_motor/spark/plugin/batteryperceptor/CMakeLists.txt (rev 0)
+++ branches/realistic_motor/spark/plugin/batteryperceptor/CMakeLists.txt 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,21 @@
+
+########### next target ###############
+
+set(batteryperceptor_LIB_SRCS
+ export.cpp
+ batteryperceptor.h
+ batteryperceptor.cpp
+ batteryperceptor_c.cpp
+)
+
+add_library(batteryperceptor MODULE ${batteryperceptor_LIB_SRCS})
+
+target_link_libraries(batteryperceptor ${spark_libs} ${Boost_LIBRARIES})
+
+if (NOT APPLE)
+ set_target_properties(batteryperceptor PROPERTIES VERSION 0.0.0 SOVERSION 0)
+endif (NOT APPLE)
+
+if (NOT APPLE)
+ install(TARGETS batteryperceptor DESTINATION ${LIBDIR}/${CMAKE_PROJECT_NAME})
+endif (NOT APPLE)
Added: branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor.cpp (rev 0)
+++ branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,67 @@
+/* -*- 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 "batteryperceptor.h"
+#include <zeitgeist/logserver/logserver.h>
+
+using namespace oxygen;
+using namespace boost;
+using namespace zeitgeist;
+using namespace salt;
+using namespace std;
+
+BatteryPerceptor::BatteryPerceptor() : oxygen::Perceptor()
+{
+}
+
+BatteryPerceptor::~BatteryPerceptor()
+{
+}
+
+void BatteryPerceptor::OnLink()
+{
+ mBattery = shared_static_cast<Battery>(FindParentSupportingClass<Battery>().lock());
+ if (0 == mBattery.get())
+ {
+ GetLog()->Error()<< "(BatteryPerceptor) ERROR: can not link to battery\n";
+ }
+}
+
+void BatteryPerceptor::OnUnLink()
+{
+ mBattery.reset();
+}
+
+bool BatteryPerceptor::Percept(boost::shared_ptr<PredicateList> predList)
+{
+ Predicate &predicate = predList->AddPredicate();
+ predicate.name = GetName();
+ predicate.parameter.Clear();
+
+ float v = 0;
+ if(0 != mBattery.get())
+ {
+ v = mBattery->State();
+ }
+
+ predicate.parameter.AddValue(v);
+
+ return true;
+}
Copied: branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor.h (from rev 324, branches/realistic_motor/spark/plugin/sparkagent/hingeaction.h)
===================================================================
--- branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor.h (rev 0)
+++ branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,47 @@
+/* -*- 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 BATTERY_PERCEPTOR_H
+#define BATTERY_PERCEPTOR_H
+
+#include <oxygen/agentaspect/perceptor.h>
+#include <oxygen/physicsserver/battery.h>
+
+class BatteryPerceptor : public oxygen::Perceptor
+{
+public:
+ BatteryPerceptor();
+ virtual ~BatteryPerceptor();
+
+ //! \return true, if valid data is available and false otherwise.
+ bool Percept(boost::shared_ptr<oxygen::PredicateList> predList);
+
+ virtual void OnLink();
+
+ virtual void OnUnLink();
+
+private:
+
+ boost::shared_ptr<oxygen::Battery> mBattery;
+};
+
+DECLARE_CLASS(BatteryPerceptor);
+
+#endif //BATTERY_PERCEPTOR_H
Copied: branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor_c.cpp (from rev 324, branches/realistic_motor/spark/plugin/sparkagent/hingeaction.h)
===================================================================
--- branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor_c.cpp (rev 0)
+++ branches/realistic_motor/spark/plugin/batteryperceptor/batteryperceptor_c.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -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 "batteryperceptor.h"
+
+using namespace boost;
+using namespace oxygen;
+
+void CLASS(BatteryPerceptor)::DefineClass()
+{
+ DEFINE_BASECLASS(oxygen/Perceptor);
+}
Copied: branches/realistic_motor/spark/plugin/batteryperceptor/export.cpp (from rev 324, branches/realistic_motor/spark/plugin/sparkagent/hingeaction.h)
===================================================================
--- branches/realistic_motor/spark/plugin/batteryperceptor/export.cpp (rev 0)
+++ branches/realistic_motor/spark/plugin/batteryperceptor/export.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,25 @@
+/* -*- 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 "batteryperceptor.h"
+
+ZEITGEIST_EXPORT_BEGIN()
+ ZEITGEIST_EXPORT(BatteryPerceptor);
+ZEITGEIST_EXPORT_END()
Modified: branches/realistic_motor/spark/plugin/odeimps/odeangularmotor.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/odeimps/odeangularmotor.cpp 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/plugin/odeimps/odeangularmotor.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -98,3 +98,13 @@
dJointID ODEJoint = (dJointID) jointID;
return gRadToDeg(dJointGetAMotorAngleRate(ODEJoint,idx));
}
+
+float AngularMotorImp::GetTorque(long jointID) const
+{
+ dJointID ODEJoint = (dJointID) jointID;
+ dJointFeedback* fb = dJointGetFeedback(ODEJoint);
+ if(fb)
+ return dLENGTH(fb->t1);
+ else
+ return 0;
+}
Modified: branches/realistic_motor/spark/plugin/odeimps/odeangularmotor.h
===================================================================
--- branches/realistic_motor/spark/plugin/odeimps/odeangularmotor.h 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/plugin/odeimps/odeangularmotor.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -50,6 +50,7 @@
void SetAxisAngle(int idx, float degAngle, long jointID);
float GetAxisAngle(int idx, long jointID);
float GetAxisAngleRate(int idx, long jointID);
+ virtual float GetTorque(long jointID) const;
};
DECLARE_CLASS(AngularMotorImp);
Modified: branches/realistic_motor/spark/plugin/odeimps/odejoint.cpp
===================================================================
--- branches/realistic_motor/spark/plugin/odeimps/odejoint.cpp 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/plugin/odeimps/odejoint.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -295,6 +295,12 @@
return gRadToDeg(GetParameter(dParamVel + (idx * dParamGroup), jointID));
}
+float JointImp::GetAngularMotorAngle(int idx, long jointID) const
+{
+ dJointID JointImp = (dJointID) jointID;
+ return gRadToDeg(dJointGetAMotorAngle(JointImp, idx));
+}
+
void JointImp::SetMaxMotorForce(int idx, float f, long jointID)
{
SetParameter(dParamFMax + (idx * dParamGroup), f, jointID);
Modified: branches/realistic_motor/spark/plugin/odeimps/odejoint.h
===================================================================
--- branches/realistic_motor/spark/plugin/odeimps/odejoint.h 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/plugin/odeimps/odejoint.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -74,6 +74,7 @@
float GetLinearMotorVelocity(int idx, long jointID) const;
void SetAngularMotorVelocity(int idx, float deg, long jointID);
float GetAngularMotorVelocity(int idx, long jointID) const;
+ float GetAngularMotorAngle(int idx, long jointID) const;
void SetMaxMotorForce(int idx, float f, long jointID);
float GetMaxMotorForce(int idx, long jointID) const;
void SetParameter(int parameter, float value, long jointID);
Modified: branches/realistic_motor/spark/plugin/sparkagent/CMakeLists.txt
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/CMakeLists.txt 2012-09-10 11:08:54 UTC (rev 324)
+++ branches/realistic_motor/spark/plugin/sparkagent/CMakeLists.txt 2012-09-10 12:57:23 UTC (rev 325)
@@ -27,6 +27,13 @@
universaljointperceptor.h
universaljointperceptor.cpp
universaljointperceptor_c.cpp
+ angularmotoraction.h
+ angularmotoreffector.h
+ angularmotoreffector.cpp
+ angularmotoreffector_c.cpp
+ angularmotorperceptor.h
+ angularmotorperceptor.cpp
+ angularmotorperceptor_c.cpp
)
add_library(sparkagent MODULE ${sparkagent_LIB_SRCS})
Copied: branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h (from rev 324, branches/realistic_motor/spark/plugin/sparkagent/hingeaction.h)
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h (rev 0)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotoraction.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -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 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
Copied: branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.cpp (from rev 324, branches/realistic_motor/spark/plugin/sparkagent/hingeeffector.cpp)
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.cpp (rev 0)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.cpp 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,161 @@
+/* -*- 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 "angularmotoreffector.h"
+#include "angularmotoraction.h"
+#include <oxygen/physicsserver/rigidbody.h>
+
+using namespace oxygen;
+using namespace zeitgeist;
+using namespace salt;
+using namespace boost;
+using namespace std;
+
+AngularMotorEffector::AngularMotorEffector()
+ : JointEffector<AngularMotor>::JointEffector("amotor"),
+ mUseBacklash(false),
+ mGearPosition(0),
+ mHalfDeadband(1),
+ mMaxForce(10)
+{
+}
+
+AngularMotorEffector::~AngularMotorEffector()
+{
+}
+
+bool AngularMotorEffector::Realize(boost::shared_ptr<ActionObject> action)
+{
+ if (mJoint.get() == 0)
+ {
+ return false;
+ }
+
+ // disabled joint?
+ if ( mJoint->Disabled() )
+ {
+ boost::shared_ptr<RigidBody> body = mJoint->GetBody(Joint::BI_FIRST);
+ if (body && !body->IsEnabled() )
+ {
+ body->Enable();
+ }
+ mJoint->SetParameter(2 /*value of dParamVel in ODE*/, 0);
+ return false;
+ }
+
+ boost::shared_ptr<AngularMotorAction> motorAction =
+ shared_dynamic_cast<AngularMotorAction>(action);
+
+ if (motorAction.get() == 0)
+ {
+ GetLog()->Error()
+ << "ERROR: (HingeEffector) cannot realize an "
+ << "unknown ActionObject\n";
+ return false;
+ }
+
+ 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) ?
+ gMin(finalMotorVel, mJoint->GetJointMaxSpeed1())
+ :
+ gMax(finalMotorVel, - mJoint->GetJointMaxSpeed1());
+ }
+
+ 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);
+ if (body && !body->IsEnabled())
+ {
+ body->Enable();
+ }
+ }
+ return true;
+}
+
+boost::shared_ptr<ActionObject> AngularMotorEffector::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 velocity;
+ if (! predicate.AdvanceValue(iter, velocity))
+ {
+ GetLog()->Error()
+ << "ERROR: (HingeEffector) motor velocity expected\n";
+ 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 boost::shared_ptr<ActionObject>();
+}
Copied: branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.h (from rev 324, branches/realistic_motor/spark/plugin/sparkagent/hingeeffector.h)
===================================================================
--- branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.h (rev 0)
+++ branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector.h 2012-09-10 12:57:23 UTC (rev 325)
@@ -0,0 +1,57 @@
+/* -*- 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_EFFECTOR_H
+#define ANGULAR_MOTOR_EFFECTOR_H
+
+#include <oxygen/agentaspect/jointeffector.h>
+#include <oxygen/physicsserver/angularmotor.h>
+
+class AngularMotorEffector : public oxygen::JointEffector<oxygen::AngularMotor>
+{
+public:
+ AngularMotorEffector();
+ virtual ~AngularMotorEffector();
+
+ /** 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);
+
+ 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);
+
+#endif // ANGULAR_MOTOR_EFFECTOR_H
Added: branches/realistic_motor/spark/plugin/sparkagent/angularmotoreffector_c.cpp
===================================================================
--- bra...
[truncated message content] |
|
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.
|
×
Want the latest updates on software, tech news, and AI?
Get latest updates about software, tech news, and AI from SourceForge directly in your inbox once a month.
Thanks for helping keep SourceForge clean.
X