Menu
▾
▴
[simspark-svn] SF.net SVN: simspark:[325] branches/realistic_motor
|
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] |
