AnimatLab  2
Test
ForceStimulus.cpp
1 // ForceStimulus.cpp: implementation of the ForceStimulus class.
2 //
4 
5 #include "StdAfx.h"
6 #include "IMovableItemCallback.h"
7 #include "ISimGUICallback.h"
8 #include "AnimatBase.h"
9 
10 #include <sys/types.h>
11 #include <sys/stat.h>
12 #include "Gain.h"
13 #include "Node.h"
14 #include "Link.h"
15 #include "IPhysicsMovableItem.h"
16 #include "IPhysicsBody.h"
17 #include "BoundingBox.h"
18 #include "MovableItem.h"
19 #include "BodyPart.h"
20 #include "Joint.h"
21 #include "MotorizedJoint.h"
22 #include "ReceptiveField.h"
23 #include "ContactSensor.h"
24 #include "RigidBody.h"
25 #include "Structure.h"
26 #include "NeuralModule.h"
27 #include "Adapter.h"
28 #include "NervousSystem.h"
29 #include "Organism.h"
30 #include "ActivatedItem.h"
31 #include "ActivatedItemMgr.h"
32 #include "DataChartMgr.h"
33 #include "ExternalStimulus.h"
34 #include "ExternalStimuliMgr.h"
35 #include "KeyFrame.h"
36 #include "SimulationRecorder.h"
37 #include "OdorType.h"
38 #include "Odor.h"
39 #include "Light.h"
40 #include "LightManager.h"
41 #include "Simulator.h"
42 
43 #include "ForceStimulus.h"
44 
45 namespace AnimatSim
46 {
47  namespace ExternalStimuli
48  {
49 
51 // Construction/Destruction
53 
54 ForceStimulus::ForceStimulus()
55 {
56  m_lpStructure = NULL;
57  m_lpBody = NULL;
58 
59  m_lpForceXEval = NULL;
60  m_lpForceYEval = NULL;
61  m_lpForceZEval = NULL;
62 
63  m_fltForceX = 0;
64  m_fltForceY = 0;
65  m_fltForceZ = 0;
66 
67  m_fltForceReportX = 0;
68  m_fltForceReportY = 0;
69  m_fltForceReportZ = 0;
70 
71  m_lpTorqueXEval = NULL;
72  m_lpTorqueYEval = NULL;
73  m_lpTorqueZEval = NULL;
74 
75  m_fltTorqueX = 0;
76  m_fltTorqueY = 0;
77  m_fltTorqueZ = 0;
78 
79  m_fltTorqueReportX = 0;
80  m_fltTorqueReportY = 0;
81  m_fltTorqueReportZ = 0;
82 }
83 
84 ForceStimulus::~ForceStimulus()
85 {
86 
87 try
88 {
89  m_lpStructure = NULL;
90  m_lpBody = NULL;
91 
92  if(m_lpForceXEval) delete m_lpForceXEval;
93  if(m_lpForceYEval) delete m_lpForceYEval;
94  if(m_lpForceZEval) delete m_lpForceZEval;
95 
96  if(m_lpTorqueXEval) delete m_lpTorqueXEval;
97  if(m_lpTorqueYEval) delete m_lpTorqueYEval;
98  if(m_lpTorqueZEval) delete m_lpTorqueZEval;
99 }
100 catch(...)
101 {Std_TraceMsg(0, "Caught Error in desctructor of ForceStimulus\r\n", "", -1, false, true);}
102 }
103 
104 CStdPostFixEval *ForceStimulus::SetupEquation(std::string strEquation)
105 {
106  CStdPostFixEval *lpEquation = NULL;
107 
108  if(!Std_IsBlank(strEquation))
109  {
110  lpEquation = new CStdPostFixEval;
111  lpEquation->AddVariable("t");
112  lpEquation->Equation(strEquation);
113  }
114 
115  return lpEquation;
116 }
117 
118 
119 void ForceStimulus::RelativePositionX(float fltVal)
120 {
122  m_oRelativePosition.x = fltVal * m_lpSim->InverseDistanceUnits();
123 }
124 
125 void ForceStimulus::RelativePositionY(float fltVal)
126 {
127  Simulator *m_lpSim = GetSimulator();
128  m_oRelativePosition.y = fltVal * m_lpSim->InverseDistanceUnits();
129 }
130 
131 void ForceStimulus::RelativePositionZ(float fltVal)
132 {
133  Simulator *m_lpSim = GetSimulator();
134  m_oRelativePosition.z = fltVal * m_lpSim->InverseDistanceUnits();
135 }
136 
137 void ForceStimulus::ForceXEquation(std::string strVal)
138 {
139  if(m_lpForceXEval)
140  {delete m_lpForceXEval; m_lpForceXEval = NULL;}
141 
142  m_strForceXEquation = strVal;
143  m_lpForceXEval = SetupEquation(strVal);
144 }
145 
146 void ForceStimulus::ForceYEquation(std::string strVal)
147 {
148  if(m_lpForceYEval)
149  {delete m_lpForceYEval; m_lpForceYEval = NULL;}
150 
151  m_strForceYEquation = strVal;
152  m_lpForceYEval = SetupEquation(strVal);
153 }
154 
155 void ForceStimulus::ForceZEquation(std::string strVal)
156 {
157  if(m_lpForceZEval)
158  {delete m_lpForceZEval; m_lpForceZEval = NULL;}
159 
160  m_strForceZEquation = strVal;
161  m_lpForceZEval = SetupEquation(strVal);
162 }
163 
164 void ForceStimulus::TorqueXEquation(std::string strVal)
165 {
166  if(m_lpTorqueXEval)
167  {delete m_lpTorqueXEval; m_lpTorqueXEval = NULL;}
168 
169  m_strTorqueXEquation = strVal;
170  m_lpTorqueXEval = SetupEquation(strVal);
171 }
172 
173 void ForceStimulus::TorqueYEquation(std::string strVal)
174 {
175  if(m_lpTorqueYEval)
176  {delete m_lpTorqueYEval; m_lpTorqueYEval = NULL;}
177 
178  m_strTorqueYEquation = strVal;
179  m_lpTorqueYEval = SetupEquation(strVal);
180 }
181 
182 void ForceStimulus::TorqueZEquation(std::string strVal)
183 {
184  if(m_lpTorqueZEval)
185  {delete m_lpTorqueZEval; m_lpTorqueZEval = NULL;}
186 
187  m_strTorqueZEquation = strVal;
188  m_lpTorqueZEval = SetupEquation(strVal);
189 }
190 
192 {
194 
195  //Lets try and get the node we will dealing with.
196  m_lpStructure = m_lpSim->FindStructureFromAll(m_strStructureID);
197  m_lpBody = m_lpStructure->FindRigidBody(m_strBodyID);
198 }
199 
201 {
203 
204  m_fltForceX = m_fltForceY = m_fltForceZ = 0;
205  m_fltForceReportX = m_fltForceReportY = m_fltForceReportZ = 0;
206  m_fltTorqueX = m_fltTorqueY = m_fltTorqueZ = 0;
207  m_fltTorqueReportX = m_fltTorqueReportY = m_fltTorqueReportZ = 0;
208 }
209 
211 {
212  try
213  {
214  //IMPORTANT! This stimulus applies a force to the physics engine, so it should ONLY be called once for every time the physcis
215  //engine steps. If you do not do this then the you will accumulate forces being applied during the neural steps, and the total
216  //force you apply will be greater than what it should be. To get around this we will only call the code in step simulation if
217  //the physics step count is equal to the step interval.
218  if(m_lpSim->PhysicsStepCount() == m_lpSim->PhysicsStepInterval())
219  {
220 
221  //Why do we multiply by the mass units here? The reason is that we have to try and keep the
222  //length and mass values in a range around 1 for the simulator to be able to function appropriately.
223  //So say we are uing grams and centimeters. This means that if we have a 1cm^3 box that weights 1 gram
224  //it will come in with a density of 1 g.cm^3 and we will set its density to 1. But the simulator treats this
225  //as 1 Kg and not 1g. So forces/torques and so on are scaled incorrectly. We must scale the force to be applied
226  //so it is acting against kilograms instead of grams. So a 1N force would be 1000N to produce the same effect.
227  if(m_lpForceXEval || m_lpForceYEval || m_lpForceZEval)
228  {
229  m_fltForceX = m_fltForceY = m_fltForceZ = 0;
230  m_fltForceReportX = m_fltForceReportY = m_fltForceReportZ = 0;
231 
232  if(m_lpForceXEval)
233  {
234  m_lpForceXEval->SetVariable("t", m_lpSim->Time());
235  m_fltForceReportX = m_lpForceXEval->Solve();
236  m_fltForceX = m_fltForceReportX * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits();
237  }
238 
239  if(m_lpForceYEval)
240  {
241  m_lpForceYEval->SetVariable("t", m_lpSim->Time());
242  m_fltForceReportY = m_lpForceYEval->Solve();
243  m_fltForceY = m_fltForceReportY * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits();
244  }
245 
246  if(m_lpForceZEval)
247  {
248  m_lpForceZEval->SetVariable("t", m_lpSim->Time());
249  m_fltForceReportZ = m_lpForceZEval->Solve();
250  m_fltForceZ = m_fltForceReportZ * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits();
251  }
252 
253  if(m_lpBody && (m_fltForceX || m_fltForceY || m_fltForceZ))
254  m_lpBody->AddForceAtLocalPos(m_oRelativePosition.x, m_oRelativePosition.y, m_oRelativePosition.z,
255  m_fltForceX, m_fltForceY, m_fltForceZ, false);
256  }
257 
258  if(m_lpTorqueXEval || m_lpTorqueYEval || m_lpTorqueZEval)
259  {
260  m_fltTorqueX = m_fltTorqueY = m_fltTorqueZ = 0;
261  m_fltTorqueReportX = m_fltTorqueReportY = m_fltTorqueReportZ = 0;
262 
263  if(m_lpTorqueXEval)
264  {
265  m_lpTorqueXEval->SetVariable("t", m_lpSim->Time());
266  m_fltTorqueReportX = m_lpTorqueXEval->Solve();
267  m_fltTorqueX = m_fltTorqueReportX * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits() * m_lpSim->InverseDistanceUnits();
268  }
269 
270  if(m_lpTorqueYEval)
271  {
272  m_lpTorqueYEval->SetVariable("t", m_lpSim->Time());
273  m_fltTorqueReportY = m_lpTorqueYEval->Solve();
274  m_fltTorqueY = m_fltTorqueReportY * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits() * m_lpSim->InverseDistanceUnits();
275  }
276 
277  if(m_lpTorqueZEval)
278  {
279  m_lpTorqueZEval->SetVariable("t", m_lpSim->Time());
280  m_fltTorqueReportZ = m_lpTorqueZEval->Solve();
281  m_fltTorqueZ = m_fltTorqueReportZ * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits() * m_lpSim->InverseDistanceUnits();
282  }
283 
284  if(m_lpBody && (m_fltTorqueX || m_fltTorqueY || m_fltTorqueZ))
285  m_lpBody->AddTorque(m_fltTorqueX, m_fltTorqueY, m_fltTorqueZ, false);
286  }
287  }
288  }
289  catch(...)
290  {
291  LOG_ERROR("Error Occurred while setting Joint Velocity");
292  }
293 }
294 
296 {
298  if(m_lpBody)
299  m_lpBody->WakeDynamics();
300 }
301 
302 float *ForceStimulus::GetDataPointer(const std::string &strDataType)
303 {
304  float *lpData=NULL;
305  std::string strType = Std_CheckString(strDataType);
306 
307  if(strType == "FORCEX")
308  lpData = &m_fltForceReportX;
309  else if(strType == "FORCEY")
310  lpData = &m_fltForceReportY;
311  else if(strType == "FORCEZ")
312  lpData = &m_fltForceReportZ;
313  else if(strType == "TORQUEX")
314  lpData = &m_fltTorqueReportX;
315  else if(strType == "TORQUEY")
316  lpData = &m_fltTorqueReportY;
317  else if(strType == "TORQUEZ")
318  lpData = &m_fltTorqueReportZ;
319  else
320  THROW_TEXT_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "StimulusName: " + STR(m_strName) + " DataType: " + strDataType);
321 
322  return lpData;
323 }
324 
325 bool ForceStimulus::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
326 {
327  std::string strType = Std_CheckString(strDataType);
328 
329  if(ExternalStimulus::SetData(strDataType, strValue, false))
330  return true;
331 
332  if(strType == "POSITIONX")
333  {
334  RelativePositionX(atof(strValue.c_str()));
335  return true;
336  }
337 
338  if(strType == "POSITIONY")
339  {
340  RelativePositionY(atof(strValue.c_str()));
341  return true;
342  }
343 
344  if(strType == "POSITIONZ")
345  {
346  RelativePositionZ(atof(strValue.c_str()));
347  return true;
348  }
349 
350  if(strType == "FORCEX")
351  {
352  ForceXEquation(strValue);
353  return true;
354  }
355 
356  if(strType == "FORCEY")
357  {
358  ForceYEquation(strValue);
359  return true;
360  }
361 
362  if(strType == "FORCEZ")
363  {
364  ForceZEquation(strValue);
365  return true;
366  }
367 
368  if(strType == "TORQUEX")
369  {
370  TorqueXEquation(strValue);
371  return true;
372  }
373 
374  if(strType == "TORQUEY")
375  {
376  TorqueYEquation(strValue);
377  return true;
378  }
379 
380  if(strType == "TORQUEZ")
381  {
382  TorqueZEquation(strValue);
383  return true;
384  }
385 
386  //If it was not one of those above then we have a problem.
387  if(bThrowError)
388  THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);
389 
390  return false;
391 }
392 
393 void ForceStimulus::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
394 {
395  ExternalStimulus::QueryProperties(aryProperties);
396 
397  aryProperties.Add(new TypeProperty("PositionX", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
398  aryProperties.Add(new TypeProperty("PositionY", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
399  aryProperties.Add(new TypeProperty("PositionZ", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
400  aryProperties.Add(new TypeProperty("ForceX", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
401  aryProperties.Add(new TypeProperty("ForceY", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
402  aryProperties.Add(new TypeProperty("ForceZ", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
403  aryProperties.Add(new TypeProperty("TorqueX", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
404  aryProperties.Add(new TypeProperty("TorqueY", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
405  aryProperties.Add(new TypeProperty("TorqueZ", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
406 }
407 
408 void ForceStimulus::Load(CStdXml &oXml)
409 {
410  ActivatedItem::Load(oXml);
411 
412  oXml.IntoElem(); //Into Simulus Element
413 
414  m_strStructureID = oXml.GetChildString("StructureID");
415  if(Std_IsBlank(m_strStructureID))
416  THROW_ERROR(Al_Err_lIDBlank, Al_Err_strIDBlank);
417 
418  m_strBodyID = oXml.GetChildString("BodyID");
419  if(Std_IsBlank(m_strBodyID))
420  THROW_ERROR(Al_Err_lBodyIDBlank, Al_Err_strBodyIDBlank);
421 
422  ForceXEquation(oXml.GetChildString("ForceX", ""));
423  ForceYEquation(oXml.GetChildString("ForceY", ""));
424  ForceZEquation(oXml.GetChildString("ForceZ", ""));
425 
426  TorqueXEquation(oXml.GetChildString("TorqueX", ""));
427  TorqueYEquation(oXml.GetChildString("TorqueY", ""));
428  TorqueZEquation(oXml.GetChildString("TorqueZ", ""));
429 
430  CStdFPoint oPoint;
431  Std_LoadPoint(oXml, "RelativePosition", m_oRelativePosition);
432 
433  //We need to scale the distance values to be appropriate. They
434  //will be saved as centimeters or some such in the config file,
435  //but we need them to be in "unit" values.
436  m_oRelativePosition *= m_lpSim->InverseDistanceUnits();
437 
438  oXml.OutOfElem(); //OutOf Simulus Element
439 }
440 
441  } //ExternalStimuli
442 } //AnimatSim
443 
444 
445 
446 
Declares the external stimulus base class.
Base class file for all Animat simulation objects.
Declares the nervous system class.
void AddVariable(std::string strVarName)
Adds a variable.
Declares the simulation recorder class.
Simulates the entire environment.
Definition: Simulator.h:31
Root namespace for the base simulation library for AnimatLab.
virtual float InverseMassUnits()
Gets the inverse mass units.
Definition: Simulator.cpp:1797
Declares the body part class.
virtual void QueryProperties(CStdPtrArray< TypeProperty > &aryProperties)
Queries this object for a list of properties that can be changed using SetData.
virtual void ResetSimulation()
Resets the simulation back to time 0.
virtual void AddTorque(float fltTx, float fltTy, float fltTz, bool bScaleUnits)
Adds a torque to this body about its center.
Definition: RigidBody.cpp:2501
Simulator * m_lpSim
The pointer to a Simulation.
Definition: AnimatBase.h:43
virtual short PhysicsStepInterval()
Gets the physics step interval.
Definition: Simulator.cpp:1097
virtual bool IntoElem()
Goes into the next element where the cursor is located.
Definition: StdXml.cpp:42
Class that stores information about types for QueryProperty information.
Definition: TypeProperty.h:35
virtual void QueryProperties(CStdPtrArray< TypeProperty > &aryProperties)
Queries this object for a list of properties that can be changed using SetData.
Declares the key frame class.
virtual Simulator * GetSimulator()
Gets the simulator pointer.
Definition: AnimatBase.cpp:123
Declares the joint class.
Declares the organism class.
Declares a light object.
virtual long PhysicsStepCount()
Gets the physics step count.
Definition: Simulator.cpp:1176
Declares the activated item class.
Declares a light manager object.
Declares the bounding box class.
Declares the gain base class.
void Equation(std::string strVal)
Sets the post-fix Equation.
A standard xml manipulation class.
Definition: StdXml.h:19
virtual void ResetSimulation()
Resets the simulation back to time 0.
Declares the node class.
virtual std::string GetChildString(std::string strElementName)
Gets a string value from the element with the specified name.
Definition: StdXml.cpp:307
virtual void Deactivate()
Deactivates this item.
Standard post fix evaluation class.
void Std_TraceMsg(const int iLevel, std::string strMessage, std::string strSourceFile, int iSourceLine, bool bLogToFile, bool bPrintHeader)
Traces a message to the debugger window.
virtual void AddForceAtLocalPos(float fltPx, float fltPy, float fltPz, float fltFx, float fltFy, float fltFz, bool bScaleUnits)
Adds a world-coordinate based force vector to this body at a specified local body position...
Definition: RigidBody.cpp:2461
double Solve()
Solves the equation using the defined variable values.
void SetVariable(std::string strVarName, double dblVal)
Sets the value of a variable.
virtual bool SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError=true)
Set a variable based on a string data type name.
virtual float * GetDataPointer(const std::string &strDataType)
Returns a float pointer to a data item of interest in this object.
virtual bool OutOfElem()
Goes out of the element where the cursor is located.
Definition: StdXml.cpp:56
virtual void Deactivate()
Deactivates this item.
virtual Structure * FindStructureFromAll(std::string strStructureID, bool bThrowError=true)
Searches for the first structure with the specified ID.
Definition: Simulator.cpp:3920
bool Std_IsBlank(std::string strVal)
Trims a string and tests if a string is blank.
Declares the data chart manager class.
Declares the rigid body class.
std::string Std_CheckString(std::string strVal)
Converts a string to upper case and trims it.
bool Std_LoadPoint(CStdXml &oXml, std::string strName, CStdIPoint &oPoint, bool bThrowError)
Standard load point.
Declares the structure class.
Declares the odor type class.
virtual void Initialize()
Initializes this object.
virtual float InverseDistanceUnits()
Gets the inverse distance units.
Definition: Simulator.cpp:1742
Declares the odor class.
Declares the simulator class.
Declares the neural module class.
virtual float Time()
Gets the current simulation time in seconds.
Definition: Simulator.cpp:559
Declares the activated item manager class.
Declares the contact sensor class.
Declares the external stimuli manager class.
virtual RigidBody * FindRigidBody(std::string strBodyID, bool bThrowError=true)
Finds a rigid body with a specified ID within this structure.
Definition: Structure.cpp:489
Declares the receptive field class.
virtual void StepSimulation()
Step the simulation for this object.
std::string m_strName
The name for this object.
Definition: AnimatBase.h:61
virtual void Initialize()
Initializes this object.
virtual bool SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError=true)
Set a variable based on a string data type name.