AnimatLab  2
Test
VsForceStimulus.cpp
1 // VsForceStimulus.cpp: implementation of the VsForceStimulus class.
2 //
4 
5 #include "stdafx.h"
6 
7 #include "VsMovableItem.h"
8 #include "VsBody.h"
9 #include "VsJoint.h"
10 #include "VsMotorizedJoint.h"
11 #include "VsRigidBody.h"
12 #include "VsSimulator.h"
13 
14 #include "VsForceStimulus.h"
15  #include "VsDragger.h"
16 
17 namespace VortexAnimatSim
18 {
19  namespace ExternalStimuli
20  {
21 
23 // Construction/Destruction
25 
26 VsForceStimulus::VsForceStimulus()
27 {
28  m_lpStructure = NULL;
29  m_lpBody = NULL;
30  m_lpVsBody = NULL;
31 
32  m_lpForceXEval = NULL;
33  m_lpForceYEval = NULL;
34  m_lpForceZEval = NULL;
35 
36  m_fltForceX = 0;
37  m_fltForceY = 0;
38  m_fltForceZ = 0;
39 
40  m_fltForceReportX = 0;
41  m_fltForceReportY = 0;
42  m_fltForceReportZ = 0;
43 
44  m_lpTorqueXEval = NULL;
45  m_lpTorqueYEval = NULL;
46  m_lpTorqueZEval = NULL;
47 
48  m_fltTorqueX = 0;
49  m_fltTorqueY = 0;
50  m_fltTorqueZ = 0;
51 
52  m_fltTorqueReportX = 0;
53  m_fltTorqueReportY = 0;
54  m_fltTorqueReportZ = 0;
55 }
56 
57 VsForceStimulus::~VsForceStimulus()
58 {
59 
60 try
61 {
62  m_lpStructure = NULL;
63  m_lpBody = NULL;
64  m_lpVsBody = NULL;
65 
66  if(m_lpForceXEval) delete m_lpForceXEval;
67  if(m_lpForceYEval) delete m_lpForceYEval;
68  if(m_lpForceZEval) delete m_lpForceZEval;
69 
70  if(m_lpTorqueXEval) delete m_lpTorqueXEval;
71  if(m_lpTorqueYEval) delete m_lpTorqueYEval;
72  if(m_lpTorqueZEval) delete m_lpTorqueZEval;
73 }
74 catch(...)
75 {Std_TraceMsg(0, "Caught Error in desctructor of VsForceStimulus\r\n", "", -1, false, true);}
76 }
77 
78 CStdPostFixEval *VsForceStimulus::SetupEquation(std::string strEquation)
79 {
80  CStdPostFixEval *lpEquation = NULL;
81 
82  if(!Std_IsBlank(strEquation))
83  {
84  lpEquation = new CStdPostFixEval;
85  lpEquation->AddVariable("t");
86  lpEquation->Equation(strEquation);
87  }
88 
89  return lpEquation;
90 }
91 
92 
93 void VsForceStimulus::RelativePositionX(float fltVal)
94 {
95  Simulator *m_lpSim = GetSimulator();
96  m_oRelativePosition.x = fltVal * m_lpSim->InverseDistanceUnits();
97 }
98 
99 void VsForceStimulus::RelativePositionY(float fltVal)
100 {
101  Simulator *m_lpSim = GetSimulator();
102  m_oRelativePosition.y = fltVal * m_lpSim->InverseDistanceUnits();
103 }
104 
105 void VsForceStimulus::RelativePositionZ(float fltVal)
106 {
107  Simulator *m_lpSim = GetSimulator();
108  m_oRelativePosition.z = fltVal * m_lpSim->InverseDistanceUnits();
109 }
110 
111 void VsForceStimulus::ForceXEquation(std::string strVal)
112 {
113  if(m_lpForceXEval)
114  {delete m_lpForceXEval; m_lpForceXEval = NULL;}
115 
116  m_strForceXEquation = strVal;
117  m_lpForceXEval = SetupEquation(strVal);
118 }
119 
120 void VsForceStimulus::ForceYEquation(std::string strVal)
121 {
122  if(m_lpForceYEval)
123  {delete m_lpForceYEval; m_lpForceYEval = NULL;}
124 
125  m_strForceYEquation = strVal;
126  m_lpForceYEval = SetupEquation(strVal);
127 }
128 
129 void VsForceStimulus::ForceZEquation(std::string strVal)
130 {
131  if(m_lpForceZEval)
132  {delete m_lpForceZEval; m_lpForceZEval = NULL;}
133 
134  m_strForceZEquation = strVal;
135  m_lpForceZEval = SetupEquation(strVal);
136 }
137 
138 void VsForceStimulus::TorqueXEquation(std::string strVal)
139 {
140  if(m_lpTorqueXEval)
141  {delete m_lpTorqueXEval; m_lpTorqueXEval = NULL;}
142 
143  m_strTorqueXEquation = strVal;
144  m_lpTorqueXEval = SetupEquation(strVal);
145 }
146 
147 void VsForceStimulus::TorqueYEquation(std::string strVal)
148 {
149  if(m_lpTorqueYEval)
150  {delete m_lpTorqueYEval; m_lpTorqueYEval = NULL;}
151 
152  m_strTorqueYEquation = strVal;
153  m_lpTorqueYEval = SetupEquation(strVal);
154 }
155 
156 void VsForceStimulus::TorqueZEquation(std::string strVal)
157 {
158  if(m_lpTorqueZEval)
159  {delete m_lpTorqueZEval; m_lpTorqueZEval = NULL;}
160 
161  m_strTorqueZEquation = strVal;
162  m_lpTorqueZEval = SetupEquation(strVal);
163 }
164 
166 {
167  ExternalStimulus::Initialize();
168 
169  //Lets try and get the node we will dealing with.
170  m_lpStructure = m_lpSim->FindStructureFromAll(m_strStructureID);
171  m_lpBody = m_lpStructure->FindRigidBody(m_strBodyID);
172  m_lpVsBody = dynamic_cast<VsRigidBody *>(m_lpBody);
173 }
174 
176 {
177  ExternalStimulus::ResetSimulation();
178 
179  m_fltForceX = m_fltForceY = m_fltForceZ = 0;
180  m_fltForceReportX = m_fltForceReportY = m_fltForceReportZ = 0;
181  m_fltTorqueX = m_fltTorqueY = m_fltTorqueZ = 0;
182  m_fltTorqueReportX = m_fltTorqueReportY = m_fltTorqueReportZ = 0;
183 }
184 
186 {
187  try
188  {
189  //IMPORTANT! This stimulus applies a force to the physics engine, so it should ONLY be called once for every time the physcis
190  //engine steps. If you do not do this then the you will accumulate forces being applied during the neural steps, and the total
191  //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
192  //the physics step count is equal to the step interval.
193  if(m_lpSim->PhysicsStepCount() == m_lpSim->PhysicsStepInterval())
194  {
195  VxReal3 fltF, fltP;
196 
197  //Why do we multiply by the mass units here? The reason is that we have to try and keep the
198  //length and mass values in a range around 1 for the simulator to be able to function appropriately.
199  //So say we are uing grams and centimeters. This means that if we have a 1cm^3 box that weights 1 gram
200  //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
201  //as 1 Kg and not 1g. So forces/torques and so on are scaled incorrectly. We must scale the force to be applied
202  //so it is acting against kilograms instead of grams. So a 1N force would be 1000N to produce the same effect.
203  if(m_lpForceXEval || m_lpForceYEval || m_lpForceZEval)
204  {
205  m_fltForceX = m_fltForceY = m_fltForceZ = 0;
206  m_fltForceReportX = m_fltForceReportY = m_fltForceReportZ = 0;
207 
208  if(m_lpForceXEval)
209  {
210  m_lpForceXEval->SetVariable("t", m_lpSim->Time());
211  m_fltForceReportX = m_lpForceXEval->Solve();
212  m_fltForceX = m_fltForceReportX * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits();
213  }
214 
215  if(m_lpForceYEval)
216  {
217  m_lpForceYEval->SetVariable("t", m_lpSim->Time());
218  m_fltForceReportY = m_lpForceYEval->Solve();
219  m_fltForceY = m_fltForceReportY * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits();
220  }
221 
222  if(m_lpForceZEval)
223  {
224  m_lpForceZEval->SetVariable("t", m_lpSim->Time());
225  m_fltForceReportZ = m_lpForceZEval->Solve();
226  m_fltForceZ = m_fltForceReportZ * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits();
227  }
228 
229  fltF[0] = m_fltForceX; fltF[1] = m_fltForceY; fltF[2] = m_fltForceZ;
230  fltP[0] = m_oRelativePosition.x; fltP[1] = m_oRelativePosition.y; fltP[2] = m_oRelativePosition.z;
231 
232  if(m_lpVsBody->Part() && (m_fltForceX || m_fltForceY || m_fltForceZ))
233  m_lpVsBody->Part()->addForceAtLocalPosition(fltF, fltP);
234  }
235 
236  if(m_lpTorqueXEval || m_lpTorqueYEval || m_lpTorqueZEval)
237  {
238  m_fltTorqueX = m_fltTorqueY = m_fltTorqueZ = 0;
239  m_fltTorqueReportX = m_fltTorqueReportY = m_fltTorqueReportZ = 0;
240 
241  if(m_lpTorqueXEval)
242  {
243  m_lpTorqueXEval->SetVariable("t", m_lpSim->Time());
244  m_fltTorqueReportX = m_lpTorqueXEval->Solve();
245  m_fltTorqueX = m_fltTorqueReportX * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits() * m_lpSim->InverseDistanceUnits();
246  }
247 
248  if(m_lpTorqueYEval)
249  {
250  m_lpTorqueYEval->SetVariable("t", m_lpSim->Time());
251  m_fltTorqueReportY = m_lpTorqueYEval->Solve();
252  m_fltTorqueY = m_fltTorqueReportY * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits() * m_lpSim->InverseDistanceUnits();
253  }
254 
255  if(m_lpTorqueZEval)
256  {
257  m_lpTorqueZEval->SetVariable("t", m_lpSim->Time());
258  m_fltTorqueReportZ = m_lpTorqueZEval->Solve();
259  m_fltTorqueZ = m_fltTorqueReportZ * m_lpSim->InverseMassUnits() * m_lpSim->InverseDistanceUnits() * m_lpSim->InverseDistanceUnits();
260  }
261 
262  fltF[0] = m_fltTorqueX; fltF[1] = m_fltTorqueY; fltF[2] = m_fltTorqueZ;
263 
264  if(m_lpVsBody->Part() && (m_fltTorqueX || m_fltTorqueY || m_fltTorqueZ))
265  m_lpVsBody->Part()->addTorque (fltF);
266  }
267  }
268  }
269  catch(...)
270  {
271  LOG_ERROR("Error Occurred while setting Joint Velocity");
272  }
273 }
274 
276 {
278  if(m_lpVsBody->Part())
279  m_lpVsBody->Part()->wakeDynamics();
280 }
281 
282 float *VsForceStimulus::GetDataPointer(const std::string &strDataType)
283 {
284  float *lpData=NULL;
285  std::string strType = Std_CheckString(strDataType);
286 
287  if(strType == "FORCEX")
288  lpData = &m_fltForceReportX;
289  else if(strType == "FORCEY")
290  lpData = &m_fltForceReportY;
291  else if(strType == "FORCEZ")
292  lpData = &m_fltForceReportZ;
293  else if(strType == "TORQUEX")
294  lpData = &m_fltTorqueReportX;
295  else if(strType == "TORQUEY")
296  lpData = &m_fltTorqueReportY;
297  else if(strType == "TORQUEZ")
298  lpData = &m_fltTorqueReportZ;
299  else
300  THROW_TEXT_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "StimulusName: " + STR(m_strName) + " DataType: " + strDataType);
301 
302  return lpData;
303 }
304 
305 bool VsForceStimulus::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
306 {
307  std::string strType = Std_CheckString(strDataType);
308 
309  if(ExternalStimulus::SetData(strDataType, strValue, false))
310  return true;
311 
312  if(strType == "POSITIONX")
313  {
314  RelativePositionX(atof(strValue.c_str()));
315  return true;
316  }
317 
318  if(strType == "POSITIONY")
319  {
320  RelativePositionY(atof(strValue.c_str()));
321  return true;
322  }
323 
324  if(strType == "POSITIONZ")
325  {
326  RelativePositionZ(atof(strValue.c_str()));
327  return true;
328  }
329 
330  if(strType == "FORCEX")
331  {
332  ForceXEquation(strValue);
333  return true;
334  }
335 
336  if(strType == "FORCEY")
337  {
338  ForceYEquation(strValue);
339  return true;
340  }
341 
342  if(strType == "FORCEZ")
343  {
344  ForceZEquation(strValue);
345  return true;
346  }
347 
348  if(strType == "TORQUEX")
349  {
350  TorqueXEquation(strValue);
351  return true;
352  }
353 
354  if(strType == "TORQUEY")
355  {
356  TorqueYEquation(strValue);
357  return true;
358  }
359 
360  if(strType == "TORQUEZ")
361  {
362  TorqueZEquation(strValue);
363  return true;
364  }
365 
366  //If it was not one of those above then we have a problem.
367  if(bThrowError)
368  THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);
369 
370  return false;
371 }
372 
373 void VsForceStimulus::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
374 {
375  ExternalStimulus::QueryProperties(aryProperties);
376 
377  aryProperties.Add(new TypeProperty("PositionX", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
378  aryProperties.Add(new TypeProperty("PositionY", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
379  aryProperties.Add(new TypeProperty("PositionZ", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
380  aryProperties.Add(new TypeProperty("ForceX", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
381  aryProperties.Add(new TypeProperty("ForceY", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
382  aryProperties.Add(new TypeProperty("ForceZ", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
383  aryProperties.Add(new TypeProperty("TorqueX", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
384  aryProperties.Add(new TypeProperty("TorqueY", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
385  aryProperties.Add(new TypeProperty("TorqueZ", AnimatPropertyType::Float, AnimatPropertyDirection::Both));
386 }
387 
388 void VsForceStimulus::Load(CStdXml &oXml)
389 {
390  ActivatedItem::Load(oXml);
391 
392  oXml.IntoElem(); //Into Simulus Element
393 
394  m_strStructureID = oXml.GetChildString("StructureID");
395  if(Std_IsBlank(m_strStructureID))
396  THROW_ERROR(Al_Err_lIDBlank, Al_Err_strIDBlank);
397 
398  m_strBodyID = oXml.GetChildString("BodyID");
399  if(Std_IsBlank(m_strBodyID))
400  THROW_ERROR(Al_Err_lBodyIDBlank, Al_Err_strBodyIDBlank);
401 
402  ForceXEquation(oXml.GetChildString("ForceX", ""));
403  ForceYEquation(oXml.GetChildString("ForceY", ""));
404  ForceZEquation(oXml.GetChildString("ForceZ", ""));
405 
406  TorqueXEquation(oXml.GetChildString("TorqueX", ""));
407  TorqueYEquation(oXml.GetChildString("TorqueY", ""));
408  TorqueZEquation(oXml.GetChildString("TorqueZ", ""));
409 
410  CStdFPoint oPoint;
411  Std_LoadPoint(oXml, "RelativePosition", m_oRelativePosition);
412 
413  //We need to scale the distance values to be appropriate. They
414  //will be saved as centimeters or some such in the config file,
415  //but we need them to be in "unit" values.
416  m_oRelativePosition *= m_lpSim->InverseDistanceUnits();
417 
418  oXml.OutOfElem(); //OutOf Simulus Element
419 }
420 
421  } //ExternalStimuli
422 } //VortexAnimatSim
423 
424 
425 
426 
virtual void ResetSimulation()
Resets the simulation back to time 0.
A common class for all rigid body data specific to vortex.
Definition: VsRigidBody.h:55
virtual bool SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError=true)
Set a variable based on a string data type name.
Simulator * m_lpSim
The pointer to a Simulation.
Definition: AnimatBase.h:43
virtual void Deactivate()
Deactivates this item.
virtual Simulator * GetSimulator()
Gets the simulator pointer.
Definition: AnimatBase.cpp:123
Classes for implementing the cm-labs vortex physics engine for AnimatLab.
virtual void StepSimulation()
Step the simulation for this object.
virtual void Initialize()
Initializes this object.
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 Deactivate()
Deactivates this item.
bool Std_IsBlank(std::string strVal)
Trims a string and tests if a string is blank.
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.
virtual float * GetDataPointer(const std::string &strDataType)
Returns a float pointer to a data item of interest in this object.
std::string m_strName
The name for this object.
Definition: AnimatBase.h:61