AnimatLab  2
Test
InverseMuscleCurrent.cpp
1 // InverseMuscleCurrent.cpp: implementation of the InverseMuscleCurrent class.
2 //
4 #include "StdAfx.h"
5 #include <iostream>
6 #include "IMovableItemCallback.h"
7 #include "ISimGUICallback.h"
8 #include "AnimatBase.h"
9 
10 #include "Node.h"
11 #include "Link.h"
12 #include "IPhysicsMovableItem.h"
13 #include "IPhysicsBody.h"
14 #include "BoundingBox.h"
15 #include "MovableItem.h"
16 #include "BodyPart.h"
17 #include "Gain.h"
18 #include "Adapter.h"
19 #include "Joint.h"
20 #include "ReceptiveField.h"
21 #include "ContactSensor.h"
22 #include "RigidBody.h"
23 #include "Structure.h"
24 #include "NeuralModule.h"
25 #include "NervousSystem.h"
26 #include "Organism.h"
27 #include "ActivatedItem.h"
28 #include "ActivatedItemMgr.h"
29 #include "DataChartMgr.h"
30 #include "ExternalStimuliMgr.h"
31 #include "ExternalStimulus.h"
32 #include "ExternalInputStimulus.h"
33 #include "KeyFrame.h"
34 #include "SimulationRecorder.h"
35 #include "OdorType.h"
36 #include "Odor.h"
37 #include "Light.h"
38 #include "LightManager.h"
39 #include "Simulator.h"
40 
41 #include "LineBase.h"
42 #include "Gain.h"
43 #include "SigmoidGain.h"
44 #include "LengthTensionGain.h"
45 #include "MuscleBase.h"
46 #include "LinearHillMuscle.h"
47 
48 #include "InverseMuscleCurrent.h"
49 
50 namespace AnimatSim
51 {
52  namespace ExternalStimuli
53  {
54 
56 // Construction/Destruction
58 
59 InverseMuscleCurrent::InverseMuscleCurrent()
60 {
61  m_lpTargetNode = NULL;
62  m_lpExternalCurrent = NULL;
63  m_lpMuscle = NULL;
64  m_fltCurrent = 0;
65  m_fltPrevCurrent = 0;
66  m_fltOffset = 0;
67  m_fltT = 0;
68  m_iIndex = 0;
69  m_fltLength = 0;
70  m_fltVelocity = 0;
71  m_fltVm = 0;
72  m_fltA = 0;
73  m_fltConductance = 100e-9f;
74  m_fltRestPotential = -100e-3f;
75 }
76 
77 InverseMuscleCurrent::~InverseMuscleCurrent()
78 {
79 
80 try
81 {
82  m_lpExternalCurrent = NULL;
83  m_lpMuscle = NULL;
84  m_aryTime.RemoveAll();
85  m_aryLength.RemoveAll();
86  m_aryVelocity.RemoveAll();
87 }
88 catch(...)
89 {Std_TraceMsg(0, "Caught Error in desctructor of InverseMuscleCurrent\r\n", "", -1, false, true);}
90 }
91 
92 
93 void InverseMuscleCurrent::RestPotential(float fltV)
94 {
95  Std_IsAboveMin((float) 0, fltV, false, "RestPotential");
96 
97  m_fltRestPotential = fltV;
98 }
99 
100 float InverseMuscleCurrent::RestPotential() {return m_fltRestPotential;}
101 
102 void InverseMuscleCurrent::Conductance(float fltG)
103 {
104  Std_IsAboveMin((float) 0, fltG, false, "Conductance");
105 
106  m_fltConductance = fltG;
107 }
108 
109 float InverseMuscleCurrent::Conductance() {return m_fltConductance;}
110 
111 void InverseMuscleCurrent::MuscleID(std::string strID)
112 {
113  m_strMuscleID = strID;
114 
115  if(Std_IsBlank(strID))
116  m_lpMuscle = NULL;
117  else
118  m_lpMuscle = dynamic_cast<LinearHillMuscle *>( m_lpSim->FindByID(m_strMuscleID));
119 }
120 
121 std::string InverseMuscleCurrent::MuscleID() {return m_strMuscleID;}
122 
123 LinearHillMuscle *InverseMuscleCurrent::Muscle() {return m_lpMuscle;}
124 
125 void InverseMuscleCurrent::MuscleLengthData(std::string strFilename)
126 {
127  m_strMuscleLengthData = strFilename;
128 
129  m_aryTime.RemoveAll();
130  m_aryLength.RemoveAll();
131  m_aryVelocity.RemoveAll();
132 
133  if(!Std_IsBlank(m_strMuscleLengthData))
134  {
135  //Now load the muscle data
136  m_strMuscleLengthData = AnimatSim::GetFilePath(m_lpSim->ProjectPath(), m_strMuscleLengthData);
137  LoadMuscleData(m_strMuscleLengthData);
138 
139  if(m_aryTime.GetSize() < 2)
140  THROW_PARAM_ERROR(Al_Err_lMuscleLengthDataEmpty, Al_Err_strMuscleLengthDataEmpty, "File", m_strMuscleLengthData);
141 
142  //Get the time step used in the data
143  float fltStep = m_aryTime[1] - m_aryTime[0];
144 
145  if(fabs(fltStep - m_lpSim->PhysicsTimeStep()) > 1e-3)
146  THROW_TEXT_ERROR(Al_Err_lMuscleLengthTimeStep, Al_Err_strMuscleLengthTimeStep, " File Time Step: " + STR(fltStep) + " Physics Time Step: " + STR(m_lpSim->PhysicsTimeStep()) );
147 
148  //Set the start and end times using the data file
149  m_bLoadedTime = true;
150  m_fltStartTime = m_aryTime[0];
151  m_fltEndTime = m_aryTime[m_aryTime.GetSize()-1];
152 
153  m_lStartSlice = (long) (m_fltStartTime / m_lpSim->TimeStep() + 0.5);
154  m_lEndSlice = (long) (m_fltEndTime / m_lpSim->TimeStep() + 0.5);
155  }
156 }
157 
158 std::string InverseMuscleCurrent::MuscleLengthData() {return m_strMuscleLengthData;}
159 
160 void InverseMuscleCurrent::TargetNodeID(std::string strID)
161 {
162  if(Std_IsBlank(strID))
163  THROW_TEXT_ERROR(Al_Err_lBodyIDBlank, Al_Err_strBodyIDBlank, "Muscle ID is missing.");
164 
165  m_strTargetNodeID = strID;
166 }
167 
168 std::string InverseMuscleCurrent::TargetNodeID() {return m_strTargetNodeID;}
169 
170 Node *InverseMuscleCurrent::TargetNode() {return m_lpTargetNode;}
171 
173 {
175 
176  //Lets try and get the node we will dealing with.
177  m_lpTargetNode = dynamic_cast<Node *>(m_lpSim->FindByID(m_strTargetNodeID));
178  if(!m_lpTargetNode)
179  THROW_PARAM_ERROR(Al_Err_lNodeNotFound, Al_Err_strNodeNotFound, "ID: ", m_strTargetNodeID);
180 
181  m_lpExternalCurrent = m_lpTargetNode->GetDataPointer("ExternalCurrent");
182 
183  if(!m_lpExternalCurrent)
184  THROW_TEXT_ERROR(Al_Err_lDataPointNotFound, Al_Err_strDataPointNotFound,
185  ("Stimulus: " + m_strID + "Node: " + m_strTargetNodeID + " DataType: ExternalCurrent"));
186 
187 
188  //Tells how many time steps it takes before we do this stimulus.
189  //The neural stuff can have faster time steps than the physics engine.
190  //We only need to update this guy when the physics engine steps though.
192 
193  if(!Std_IsBlank(m_strMuscleID))
194  m_lpMuscle = dynamic_cast<LinearHillMuscle *>( m_lpSim->FindByID(m_strMuscleID));
195 
196  m_lStartSlice = (long) (m_fltStartTime / m_lpSim->TimeStep() + 0.5);
197  m_lEndSlice = (long) (m_fltEndTime / m_lpSim->TimeStep() + 0.5);
198 }
199 
201 {
202  m_fltCurrent = 0;
203  m_fltPrevCurrent = 0;
204  m_fltOffset = 0;
205  m_fltT = 0;
206  m_iIndex = 0;
207  m_fltLength = 0;
208  m_fltVelocity = 0;
209  m_fltVm = 0;
210  m_fltA = 0;
211 }
212 
214 {
216 
217  m_iIndex = 0;
218 
219  if(m_lpMuscle)
220  {
221  m_fltT = m_lpMuscle->Tension();
222  }
223 }
224 
226 {
227  if(m_lpMuscle && m_iIndex < m_aryTime.GetSize())
228  {
229  float fltTime = m_aryTime[m_iIndex];
230  float fltTime1 = m_lpSim->Time();
231 
232  //if(fltTime >= 2.03)
233  // fltTime = fltTime;
234 
235  m_fltLength = m_aryLength[m_iIndex];
236  m_fltVelocity = m_aryVelocity[m_iIndex];
237 
238  //First calculate the active tension required.
239  m_lpMuscle->CalculateInverseDynamics(m_fltLength, m_fltVelocity, m_fltT, m_fltVm, m_fltA);
240  m_fltVm = m_fltVm - m_fltRestPotential;
241 
242  m_fltPrevCurrent = m_fltCurrent;
243  m_fltCurrent = m_fltVm*m_fltConductance;
244  m_fltOffset = m_fltCurrent - m_fltPrevCurrent;
245 
246  *m_lpExternalCurrent = *m_lpExternalCurrent + m_fltOffset;
247 
248  m_iIndex++;
249  }
250 
251 }
252 
254 {
256 
257  if(m_lpMuscle)
258  *m_lpExternalCurrent = *m_lpExternalCurrent - m_fltCurrent;
259 }
260 
261 float *InverseMuscleCurrent::GetDataPointer(const std::string &strDataType)
262 {
263  float *lpData=NULL;
264  std::string strType = Std_CheckString(strDataType);
265 
266  if(strType == "A")
267  lpData = &m_fltA;
268  else if(strType == "VM")
269  lpData = &m_fltVm;
270  else if(strType == "CURRENT")
271  lpData = &m_fltCurrent;
272  else
273  THROW_TEXT_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "StimulusName: " + STR(m_strName) + " DataType: " + strDataType);
274 
275  return lpData;
276 }
277 
278 bool InverseMuscleCurrent::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
279 {
280  if(ExternalStimulus::SetData(strDataType, strValue, false))
281  return true;
282 
283  if(strDataType == "RESTPOTENTIAL")
284  {
285  RestPotential((float) atof(strValue.c_str()));
286  return true;
287  }
288 
289  if(strDataType == "CONDUCTANCE")
290  {
291  Conductance((float) atof(strValue.c_str()));
292  return true;
293  }
294 
295  if(strDataType == "MUSCLEID")
296  {
297  MuscleID(strValue);
298  return true;
299  }
300 
301  if(strDataType == "MUSCLELENGTHDATA")
302  {
303  MuscleLengthData(strValue);
304  return true;
305  }
306 
307  //If it was not one of those above then we have a problem.
308  if(bThrowError)
309  THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);
310 
311  return false;
312 }
313 
314 void InverseMuscleCurrent::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
315 {
316  ExternalStimulus::QueryProperties(aryProperties);
317 
318  aryProperties.Add(new TypeProperty("A", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
319  aryProperties.Add(new TypeProperty("Vm", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
320  aryProperties.Add(new TypeProperty("Current", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
321 
322  aryProperties.Add(new TypeProperty("RestPotential", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
323  aryProperties.Add(new TypeProperty("Conductance", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
324  aryProperties.Add(new TypeProperty("MuscleID", AnimatPropertyType::String, AnimatPropertyDirection::Set));
325  aryProperties.Add(new TypeProperty("MuscleLengthData", AnimatPropertyType::String, AnimatPropertyDirection::Set));
326 }
327 
328 void InverseMuscleCurrent::Load(CStdXml &oXml)
329 {
331 
332  oXml.IntoElem(); //Into Item Element
333 
334  m_strID = Std_CheckString(oXml.GetChildString("ID"));
335  if(Std_IsBlank(m_strID))
336  THROW_ERROR(Al_Err_lIDBlank, Al_Err_strIDBlank);
337 
338  m_strName = oXml.GetChildString("Name", "");
339 
340  //This will add this object to the object list of the simulation.
341  m_lpSim->AddToObjectList(this);
342 
343  TargetNodeID(oXml.GetChildString("TargetNodeID"));
344  AlwaysActive(oXml.GetChildBool("AlwaysActive", m_bAlwaysActive));
345  Enabled(oXml.GetChildBool("Enabled", m_bEnabled));
346  MuscleID(oXml.GetChildString("MuscleID", ""));
347  MuscleLengthData(oXml.GetChildString("LengthData", ""));
348  Conductance(oXml.GetChildFloat("Conductance", m_fltConductance));
349  RestPotential(oXml.GetChildFloat("RestPotential", m_fltRestPotential));
350 
351  oXml.OutOfElem(); //OutOf Simulus Element
352 }
353 
354 void InverseMuscleCurrent::LoadMuscleData(std::string strFilename)
355 {
356  std::ifstream fsFile(strFilename.c_str());
357 
358  if(fsFile.fail())
359  THROW_TEXT_ERROR(Al_Err_lOpenFile, Al_Err_strOpenFile, "File: " + strFilename);
360 
361  //Read off the top column name line
362  CStdArray<std::string> aryParts;
363  char sLine[300];
364  fsFile.getline(sLine, 300);
365 
366  Std_Split(sLine, "\t", aryParts);
367  if(aryParts.GetSize() != 3)
368  THROW_PARAM_ERROR(Al_Err_lInvalidMuscleLengthCols, Al_Err_strInvalidMuscleLengthCols, "Col Size", aryParts.GetSize());
369 
370  float fltTime=0, fltLength=0, fltVelocity=0;
371  while(!fsFile.eof())
372  {
373  fsFile.getline(sLine, 300);
374 
375  Std_Split(sLine, "\t", aryParts);
376 
377  if(aryParts.GetSize() == 3)
378  {
379  fltTime = atof(aryParts[0].c_str());
380  fltLength = atof(aryParts[1].c_str());
381  fltVelocity = atof(aryParts[2].c_str());
382 
383  m_aryTime.Add(fltTime);
384  m_aryLength.Add(fltLength);
385  m_aryVelocity.Add(fltVelocity);
386  }
387  }
388 }
389 
390  } //ExternalStimuli
391 } //VortexAnimatSim
392 
393 
394 
395 
Declares the external stimulus base class.
Base class file for all Animat simulation objects.
virtual void CalculateInverseDynamics(float fltLength, float fltVelocity, float fltT, float &fltVm, float &fltA)
Calculates the activation needed for a given tension value.
Declares the nervous system class.
virtual void PhysicsTimeStep(float fltVal)
Sets the integration time step for the physics engine.
Definition: Simulator.cpp:1126
virtual void Activate()
Activates this item.
Declares the simulation recorder class.
long m_lEndSlice
The time slice where this item is deactived.
Definition: ActivatedItem.h:39
Root namespace for the base simulation library for AnimatLab.
Declares the sigmoid gain class.
Declares the body part class.
virtual void Deactivate()
Deactivates this item.
Simulator * m_lpSim
The pointer to a Simulation.
Definition: AnimatBase.h:43
virtual bool Enabled()
Gets whether the item is enabled or not.
virtual bool IntoElem()
Goes into the next element where the cursor is located.
Definition: StdXml.cpp:42
virtual void ResetSimulation()
Resets the simulation back to time 0.
virtual float * GetDataPointer(const std::string &strDataType)
Returns a float pointer to a data item of interest in this object.
Definition: AnimatBase.cpp:340
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.
int Std_Split(const std::string &input, const std::string &delimiter, CStdArray< std::string > &results)
Splits a string into an array of subparts based on a delimiter.
Declares the key frame class.
Declares the muscle base class.
Declares the joint class.
Declares the organism class.
bool Std_IsAboveMin(int iMinVal, int iVal, bool bThrowError, std::string strParamName, bool bInclusiveLimit)
Tests if a number is above a minimum value.
std::string m_strID
The unique Id for this object.
Definition: AnimatBase.h:55
Declares a light object.
Declares the activated item class.
virtual float TimeStep()
Gets the smallest integration time step used within the simulation.
Definition: Simulator.cpp:951
float Tension()
Gets the tension of the muscle.
Definition: MuscleBase.cpp:92
Declares a light manager object.
Declares the bounding box class.
Declares the gain base class.
A standard xml manipulation class.
Definition: StdXml.h:19
Declares the linear hill muscle class.
Declares the node class.
Declares the line base class.
virtual std::string ProjectPath()
Gets the project path.
Definition: Simulator.cpp:287
virtual std::string GetChildString(std::string strElementName)
Gets a string value from the element with the specified name.
Definition: StdXml.cpp:307
virtual AnimatBase * FindByID(std::string strID, bool bThrowError=true)
Searches for the object with the specified ID.
Definition: Simulator.cpp:4008
virtual void StepSimulation()
Step the simulation for 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 bool GetChildBool(std::string strElementName)
Gets a bool value from the element with the specified name.
Definition: StdXml.cpp:699
bool m_bEnabled
Tells if this item is enabled or not. If it is not enabled then it is not run.
Definition: AnimatBase.h:40
Base class for body parts and neural network nodes.
Definition: Node.h:25
Declares an inverted quadratic gain class used to calculate length-tension relationship for muscle...
virtual bool OutOfElem()
Goes out of the element where the cursor is located.
Definition: StdXml.cpp:56
virtual void Deactivate()
Deactivates this item.
virtual bool SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError=true)
Set a variable based on a string data type name.
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.
A muscle that is connected between two attachment points.
Declares the structure class.
Declares the odor type class.
virtual void Initialize()
Initializes this object.
virtual void VerifySystemPointers()
Verify that system pointers have been set correctly.
Definition: AnimatBase.cpp:315
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.
virtual void QueryProperties(CStdPtrArray< TypeProperty > &aryProperties)
Queries this object for a list of properties that can be changed using SetData.
Declares the contact sensor class.
Declares the external stimuli manager class.
virtual void Initialize()
Initializes this object.
bool m_bAlwaysActive
Determines whether this item is always active during simulation.
Definition: ActivatedItem.h:59
Declares the external input stimulus class.
virtual void AddToObjectList(AnimatBase *lpItem)
Adds an object to the list of all simulation objects.
Definition: Simulator.cpp:4068
virtual bool AlwaysActive()
Gets whether this item is always active.
long m_lStartSlice
The time slice where this item becomes active.
Definition: ActivatedItem.h:36
Declares the receptive field class.
std::string m_strName
The name for this object.
Definition: AnimatBase.h:61
virtual float GetChildFloat(std::string strElementName)
Gets a float value from the element with the specified name.
Definition: StdXml.cpp:617
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.