00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "universaljointeffector.h"
00021 #include "universaljointaction.h"
00022
00023 using namespace oxygen;
00024 using namespace zeitgeist;
00025 using namespace salt;
00026 using namespace boost;
00027 using namespace std;
00028
00029 UniversalJointEffector::UniversalJointEffector() : Effector()
00030 {
00031 SetName("universaljoint");
00032 }
00033
00034 UniversalJointEffector::~UniversalJointEffector()
00035 {
00036 }
00037
00038 bool UniversalJointEffector::Realize(shared_ptr<ActionObject> action)
00039 {
00040 if (mJoint.get() == 0)
00041 {
00042 return false;
00043 }
00044
00045 shared_ptr<UniversalJointAction> universalAction =
00046 shared_dynamic_cast<UniversalJointAction>(action);
00047
00048 if (universalAction.get() == 0)
00049 {
00050 GetLog()->Error()
00051 << "ERROR: (UniversalJointtEffector) cannot realize an "
00052 << "unknown ActionObject\n";
00053 return false;
00054 }
00055
00056 mJoint->SetParameter(dParamVel, universalAction->GetMotorVelocity(Joint::AI_FIRST));
00057 mJoint->SetParameter(dParamVel2, universalAction->GetMotorVelocity(Joint::AI_SECOND));
00058
00059 return true;
00060 }
00061
00062 shared_ptr<ActionObject> UniversalJointEffector::GetActionObject(const Predicate& predicate)
00063 {
00064 for(;;)
00065 {
00066 if (mJoint.get() == 0)
00067 {
00068 break;
00069 }
00070
00071 if (predicate.name != GetPredicate())
00072 {
00073 GetLog()->Error()
00074 << "ERROR: (UniversalJointEffector) invalid predicate"
00075 << predicate.name << "\n";
00076 break;
00077 }
00078
00079 Predicate::Iterator iter = predicate.begin();
00080
00081 float velocity1;
00082 float velocity2;
00083
00084 if (! predicate.AdvanceValue(iter, velocity1))
00085 {
00086 GetLog()->Error()
00087 << "ERROR: (UniversalJointEffector) motor velocity1 expected\n";
00088 break;
00089 }
00090 if (! predicate.AdvanceValue(iter, velocity2))
00091 {
00092 GetLog()->Error()
00093 << "ERROR: (UniversalJointEffector) motor velocity2 expected\n";
00094 break;
00095 }
00096
00097 return shared_ptr<UniversalJointAction>(new UniversalJointAction(GetPredicate(),velocity1,velocity2));
00098 }
00099
00100 return shared_ptr<ActionObject>();
00101 }
00102
00103 void UniversalJointEffector::OnLink()
00104 {
00105 mJoint = make_shared(FindParentSupportingClass<UniversalJoint>());
00106
00107 if (mJoint.get() == 0)
00108 {
00109 GetLog()->Error()
00110 << "(UniversalJointEffector) ERROR: found no UniversalJoint parent\n";
00111 }
00112 }
00113
00114 void UniversalJointEffector::OnUnlink()
00115 {
00116 mJoint.reset();
00117 }