AnimatLab  2
Test
LinearHillMuscle.cpp
Go to the documentation of this file.
1 
7 #include "StdAfx.h"
8 #include "IMovableItemCallback.h"
9 #include "ISimGUICallback.h"
10 #include "AnimatBase.h"
11 
12 #include <math.h>
13 #include "Node.h"
14 #include "IPhysicsMovableItem.h"
15 #include "IPhysicsBody.h"
16 #include "BoundingBox.h"
17 #include "MovableItem.h"
18 #include "BodyPart.h"
19 #include "Joint.h"
20 #include "ReceptiveField.h"
21 #include "ContactSensor.h"
22 #include "RigidBody.h"
23 #include "Sensor.h"
24 #include "Attachment.h"
25 #include "Structure.h"
26 #include "Organism.h"
27 #include "ActivatedItem.h"
28 #include "ActivatedItemMgr.h"
29 #include "DataChartMgr.h"
30 #include "ExternalStimuliMgr.h"
31 #include "KeyFrame.h"
32 #include "SimulationRecorder.h"
33 #include "OdorType.h"
34 #include "Odor.h"
35 #include "Light.h"
36 #include "LightManager.h"
37 #include "Simulator.h"
38 
39 #include "ExternalStimulus.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 namespace AnimatSim
49 {
50  namespace Environment
51  {
52  namespace Bodies
53  {
61 {
62  m_fltKse = 0;
63  m_fltKpe = 0;
64  m_fltB = 0;
65  m_fltKseByB = 0;
66  m_fltKpeByKse = 0;
67  m_fltMaxTension = 0;
70  m_fltVmuscle = 0;
71  m_fltVm = (float) -0.15;
72  m_fltTL = 0;
73  m_fltTLPerc = 0;
74  m_fltAct = 0;
75  m_fltA = 0;
76  m_fltPrevA = 0;
77  m_fltTdot = 0;
78  m_fltTension = 0;
79  m_fltPrevTension = 0;
81  m_fltLength = 0;
82  m_fltPrevLength = 0;
84  m_fltIbRate = 0;
86 
87  m_fltSeLPrev = 0;
88  m_fltPeLPrev = 0;
89  m_fltVse = 0;
90  m_fltVpe = 0;
91 
92  m_aryMuscleVelocities = NULL;
94  m_iVelAvgIndex = 0;
95  m_fltAvgMuscleVel = 0;
96 }
97 
105 {
106  try
107  {
109  {delete[] m_aryMuscleVelocities; m_aryMuscleVelocities = NULL;}
110 
111  }
112  catch(...)
113  {Std_TraceMsg(0, "Caught Error in desctructor of LinearHillMuscle\r\n", "", -1, false, true);}
114 }
115 
125 
134 void LinearHillMuscle::Kse(float fltVal)
135 {
136  Std_InValidRange((float) 0.00001, (float) 1e11, fltVal, true, "Kse");
137  m_fltKse = fltVal;
139  m_fltKpeByKse = (1 + (m_fltKpe/m_fltKse));
140 }
141 
151 
160 void LinearHillMuscle::Kpe(float fltVal)
161 {
162  Std_InValidRange((float) 0, (float) 1e11, fltVal, true, "Kpe");
163  m_fltKpe = fltVal;
164  m_fltKpeByKse = (1 + (m_fltKpe/m_fltKse));
165 }
166 
175 float LinearHillMuscle::B() {return m_fltB;}
176 
185 void LinearHillMuscle::B(float fltVal)
186 {
187  Std_InValidRange((float) 0, (float) 1e11, fltVal, true, "B");
188  m_fltB = fltVal;
190 }
191 
200 float LinearHillMuscle::RestingLength() {return m_gainLengthTension.RestingLength();}
201 
210 void LinearHillMuscle::RestingLength(float fltVal) {m_gainLengthTension.RestingLength(fltVal);}
211 
221 
231 {
232  Std_InValidRange((float) 0, (float) 1e11, fltVal, true, "IbDischargeConstant");
233  m_fltIbDischargeConstant = fltVal;
234 }
235 
245 
255 
265 
275 
284 float LinearHillMuscle::TL() {return m_fltTL;}
285 
295 
304 float LinearHillMuscle::A() {return m_fltA;}
305 
315 
325 
327 {
328  MuscleBase::Enabled(bVal);
329 
330  if(!bVal)
331  {
333  m_fltTdot = 0;
334  m_fltTension = 0;
335  }
336 }
337 
348 inline float LinearHillMuscle::Ftl(float fltL)
349 {
350  float fltTl = m_gainLengthTension.CalculateGain(fltL);
351  if(fltTl<0) fltTl = 0;
352  return fltTl;
353 }
354 
365 inline float LinearHillMuscle::Fact(float fltStim)
366 {
367  float fltAct=m_gainStimTension.CalculateGain(fltStim);
368 
369  if(fltAct <0)
370  fltAct = 0;
371 
372  return fltAct;
373 }
374 
376 {
378 
379  m_fltDisplacement = 0;
381  m_fltVmuscle = 0;
382  m_fltPrevTension = 0;
383  m_fltTL = 0;
384  m_fltAct = 0;
385  m_fltA = 0;
386  m_fltPrevA = 0;
387  m_fltTLPerc = 0;
388  m_fltTdot = 0;
390  m_fltTension = 0;
391 
394 
397  if(m_fltPeLength<= 0)
399 
401 
402  m_fltVse = 0;
403  m_fltVpe = 0;
404 
405  m_fltVm = (float) -0.15;
406 
407  m_fltIbRate = 0;
408 
409  m_fltAvgMuscleVel = 0;
410  for(int i=0; i<m_iMuscleVelAvgCount; i++)
411  m_aryMuscleVelocities[i] = 0;
412 }
413 
415 {
418 }
419 
421 {
423  //int i=0;
424  //if(m_lpSim->Time() >= 0.5)
425  // i=6;
426 
427  //Store the previous muscle length
429 
430  //Calculate the current muscle length.
432 
433  //Calculate the displacement of this muscle d = (x-x*)
436 
437  //Calculate the instantaneous velocity of change of the muscle length.
439 
440  //Calculate averaged velocity
442  m_iVelAvgIndex++;
444 
445  m_fltAvgMuscleVel = 0;
446  for(int i=0; i<m_iMuscleVelAvgCount; i++)
447  m_fltAvgMuscleVel+= m_aryMuscleVelocities[i];
448  m_fltAvgMuscleVel = m_fltAvgMuscleVel/m_iMuscleVelAvgCount;
449 
450 
451  //Calculate the active force that is generated by neural stimulation
453  m_fltAct = Fact(m_fltVm);
454  m_fltA = m_fltTL * m_fltAct;
455 
456  m_fltTLPerc = m_fltTL*100;
457 
458  //Calculation of the derivitave of the tension
460 
461  //The new tension
462  m_fltInternalTension = m_fltInternalTension + m_fltTdot*m_lpSim->PhysicsTimeStep();
463 
464  //tension can never be negative, but we want to maintain the "internal" calculations so that the
465  //time constants are correct. If you shorten the muscle rapidly it will take it some time to
466  //rebuild its tension. This time will be determined by the internal tension, new length, and the params like viscosity.
467  //But the force seen on the muscle itself will still be 0 N because it is slack.
468  //If we did not do this then if your muscle is at rest and is pulled, stays steady, and then relaxed, then
469  //it would end up generating a negative tension. (See fig 4 of shadmehr web doc on muscle model).
470  //There is a problem in this figure in that you can see that the muscle produced negative tension and this is not
471  //possible in a real muscle.
472  if(m_fltInternalTension >= 0)
474  else
475  m_fltTension = 0;
476 
477  //Make certain that the tension never exceed the absolute maximum set by the user.
480 
483 
487 
490  {
491  m_fltSeLength = m_fltLength - m_gainLengthTension.MinPeLength();
493  }
494 
495  m_fltVse = (m_fltSeLength-m_fltSeLPrev)/m_lpSim->PhysicsTimeStep();
497 
499 
500  if(!m_bEnabled)
501  {
502  m_fltTension = 0;
503  m_fltInternalTension = 0;
504  }
505 }
506 
507 //Calculates the membrane voltage needed for the inverse dynamics of the muscle.
508 void LinearHillMuscle::CalculateInverseDynamics(float fltLength, float fltVelocity, float fltT, float &fltVm, float &fltA)
509 {
510  //Calculate inverse dynamics force needed
511  m_fltPrevA = fltA;
512  fltA = fltT - m_fltKpe*(fltLength-m_gainLengthTension.RestingLength()) - m_fltB*fltVelocity + (m_fltKpe/m_fltKse)*fltT;
513 
514  if(fltA<0)
515  fltA = m_fltPrevA;
516 
517  //Calculate tension length percentage
518  float fltTl = Ftl(fltLength);
519 
520  //Increase A to take Tension-length curve into account.
521  fltA = fltA/fltTl;
522 
523  //Use A to calculate voltage required.
524  if(fltA > 0)
525  fltVm = (float) (m_gainStimTension.A() - (1/m_gainStimTension.C())*log((m_gainStimTension.B()-fltA)/fltA));
526  else
527  fltVm = 0;
528 
529  //if(m_lpSim->Time() > 2.092 && m_strName == "Left Tricep Stretch Receptor")
530  // fltVm=fltVm;
531 }
532 
534 {
536 
539  if(m_fltPeLength<= 0)
541 
542  //Lets create the muscle velocity averaging array.
543  if(m_iMuscleVelAvgCount <= 0)
544  THROW_TEXT_ERROR(Al_Err_lInvalidMusc_Vel_Avg, Al_Err_strInvalidMusc_Vel_Avg, "Muscle Velocity Average: " + STR(m_iMuscleVelAvgCount));
545 
547 
548  //Zero out the average array
549  for(int i=0; i<m_iMuscleVelAvgCount; i++)
550  m_aryMuscleVelocities[i] = 0;
551 }
552 
553 float *LinearHillMuscle::GetDataPointer(const std::string &strDataType)
554 {
555  float *lpData=NULL;
556  std::string strType = Std_CheckString(strDataType);
557 
558  float *lpVal = NULL;
559 
560  if(strType == "VMUSCLE")
561  lpData = &m_fltVmuscle;
562  else if(strType == "VSE")
563  lpData = &m_fltVse;
564  else if(strType == "VPE")
565  lpData = &m_fltVpe;
566  else if(strType == "AVGVMUSCLE")
567  lpData = &m_fltAvgMuscleVel;
568  else if(strType == "DISPLACEMENT")
569  lpData = &m_fltDisplacement;
570  else if(strType == "DISPLACEMENTRATIO")
571  lpData = &m_fltDisplacementRatio;
572  else if(strType == "ACTIVATION")
573  lpData = &m_fltAct;
574  else if(strType == "A")
575  lpData = &m_fltA;
576  else if(strType == "SELENGTH")
577  lpData = &m_fltSeLength;
578  else if(strType == "PELENGTH")
579  lpData = &m_fltPeLength;
580  else if(strType == "SEDISPLACEMENT")
581  lpData = &m_fltSeDisplacement;
582  else if(strType == "IB")
583  lpData = &m_fltIbRate;
584  else if(strType == "TL")
585  lpData = &m_fltTLPerc;
586  else
587  lpData = MuscleBase::GetDataPointer(strDataType);
588 
589  return lpData;
590 }
591 
592 bool LinearHillMuscle::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
593 {
594  if(MuscleBase::SetData(strDataType, strValue, false))
595  return true;
596 
597  if(strDataType == "KSE")
598  {
599  Kse((float) atof(strValue.c_str()));
600  return true;
601  }
602 
603  if(strDataType == "KPE")
604  {
605  Kpe((float) atof(strValue.c_str()));
606  return true;
607  }
608 
609  if(strDataType == "B")
610  {
611  B((float) atof(strValue.c_str()));
612  return true;
613  }
614 
615  if(strDataType == "IBDISCHARGE")
616  {
617  IbDischargeConstant((float) atof(strValue.c_str()));
618  return true;
619  }
620 
621  //If it was not one of those above then we have a problem.
622  if(bThrowError)
623  THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);
624 
625  return false;
626 }
627 
628 void LinearHillMuscle::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
629 {
630  MuscleBase::QueryProperties(aryProperties);
631 
632  aryProperties.Add(new TypeProperty("Vmuscle", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
633  aryProperties.Add(new TypeProperty("Vse", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
634  aryProperties.Add(new TypeProperty("Vpe", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
635  aryProperties.Add(new TypeProperty("AvgVMuscle", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
636  aryProperties.Add(new TypeProperty("Displacement", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
637  aryProperties.Add(new TypeProperty("DisplacementRatio", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
638  aryProperties.Add(new TypeProperty("Activation", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
639  aryProperties.Add(new TypeProperty("A", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
640  aryProperties.Add(new TypeProperty("SeLength", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
641  aryProperties.Add(new TypeProperty("PeLength", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
642  aryProperties.Add(new TypeProperty("SeDisplacement", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
643  aryProperties.Add(new TypeProperty("Ib", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
644  aryProperties.Add(new TypeProperty("Tl", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
645 
646  aryProperties.Add(new TypeProperty("Kse", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
647  aryProperties.Add(new TypeProperty("Kpe", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
648  aryProperties.Add(new TypeProperty("B", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
649  aryProperties.Add(new TypeProperty("IbDischarge", AnimatPropertyType::Float, AnimatPropertyDirection::Set));
650 }
651 
652 void LinearHillMuscle::Load(CStdXml &oXml)
653 {
654  MuscleBase::Load(oXml);
655 
656  oXml.IntoElem(); //Into RigidBody Element
657 
658  Kse(oXml.GetChildFloat("Kse", m_fltKse));
659  Kpe(oXml.GetChildFloat("Kpe", m_fltKpe));
660  B(oXml.GetChildFloat("B", m_fltB));
662 
663  oXml.OutOfElem(); //OutOf RigidBody Element
664 }
665 
666  } //Bodies
667  } //Environment
668 } //AnimatSim
virtual float Vmuscle()
Gets the muscle membrane voltage.
virtual void AfterResetSimulation()
Called after a simulation reset for some objects.
Definition: LineBase.cpp:210
virtual void ResetSimulation()
Resets the simulation back to time 0.
Declares the external stimulus base class.
Base class file for all Animat simulation objects.
virtual float CalculateLength()
Calculates the length of the line.
Definition: LineBase.cpp:169
virtual void ResetSimulation()
Resets the simulation back to time 0.
Definition: MuscleBase.cpp:241
virtual void CalculateInverseDynamics(float fltLength, float fltVelocity, float fltT, float &fltVm, float &fltA)
Calculates the activation needed for a given tension value.
virtual void PhysicsTimeStep(float fltVal)
Sets the integration time step for the physics engine.
Definition: Simulator.cpp:1126
Declares the simulation recorder class.
virtual float MinPeLength()
Gets the minimum pe length.
Root namespace for the base simulation library for AnimatLab.
Declares the sigmoid gain class.
float m_fltPrevLength
Length of the line in the previous timestep.
Definition: LineBase.h:32
float m_fltTLPerc
The Tl percentage. This is primarily used for reporting purposes. It is m_fltTl*100. It ranges from 0-100 %.
Declares the body part class.
float m_fltVse
The velocity of change of the SE section of the muscle.
virtual float PeLength()
Gets the pe length.
float Fact(float fltStim)
Calculates the muscle activation.
float m_fltSeLPrev
The previous length of the se section of the muscle.
Simulator * m_lpSim
The pointer to a Simulation.
Definition: AnimatBase.h:43
float m_fltPrevA
Used to store the previous value of A when calculating the inverse muscle dynamics.
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
float m_fltPeLPrev
The previous length of the pe section of the muscle.
Declares the key frame class.
bool Std_InValidRange(int iMinVal, int iMaxVal, int iVal, bool bThrowError, std::string strParamName)
Tests whether a number is within a valid range.
virtual float Kse()
Gets the serial elastic spring constant.
Declares the muscle base class.
Declares the joint class.
Declares the organism class.
Declares a light object.
SigmoidGain m_gainStimTension
The stimulus-tension gain.
Definition: MuscleBase.h:45
virtual void AfterResetSimulation()
Called after a simulation reset for some objects.
float m_fltDisplacementRatio
This is the ratio of displacement of this muscle from its resting length.
float m_fltSeLength
The length of the SE section of the muscle. The resting length is one half the total muscle length...
Declares the activated item class.
float m_fltA
The total active force that is developed from both the current stimulus level and the tension-length ...
Declares a light manager object.
float m_fltIbDischargeConstant
Constant that relates length of muscle segment to discharge rate of type Ib fibers.
virtual float B()
Gets the damping value.
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.
float C()
Gets C parameter of sigmoidal eqation: Out = D+(B/(1+e^(C*(A-In))))
Definition: SigmoidGain.cpp:98
virtual float RestingLength()
Gets the resting length.
virtual float InternalTension()
Gets the internal tension.
Declares the node class.
float m_fltVpe
The velocity of change of the PE section of the muscle.
virtual void CreateJoints()
Allows the rigid body to create its joints using the chosen physics engine.
float m_fltPrevTension
Tension of the muscle in the last time slice.
Definition: MuscleBase.h:42
float m_fltMaxTension
The maximum tension that this muscle can ever generate. This is an upper limit to prevent unrealistic...
Definition: MuscleBase.h:28
float m_fltSeDisplacement
The displacement of the SE section of the muscle.
Declares the line base class.
float m_fltTL
The proportion of active force developed that can actually be used at this muscle length...
virtual float SeRestLength()
Gets the se rest length.
virtual float Displacement()
Gets the displacement.
float m_fltKse
Spring constant for the series spring.
float m_fltAct
The active force that is developed from the current stimulus level before the tension-length relation...
virtual float CalculateGain(float fltInput)
Calculates the gain.
float m_fltKseByB
Precalculation of Kse divided by B.
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 CreateJoints()
Allows the rigid body to create its joints using the chosen physics engine.
Definition: LineBase.cpp:317
float B()
Gets B parameter of sigmoidal eqation: Out = D+(B/(1+e^(C*(A-In))))
Definition: SigmoidGain.cpp:78
virtual float IbDischargeConstant()
Gets the ib discharge constant.
float m_fltVmuscle
The velocity of shortenting of the muscle.
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
float m_fltTension
Tension of the muscle.
Definition: MuscleBase.h:39
float A()
Gets A parameter of sigmoidal eqation: Out = D+(B/(1+e^(C*(A-In))))
Definition: SigmoidGain.cpp:58
virtual float SeLength()
Gets the se length.
int m_iMuscleVelAvgCount
The number of instantanious velocity readings to average to calculate the average velocity...
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
Declares the sensor class.
virtual bool Enabled()
Gets whether the muscle is enabled.
Definition: MuscleBase.cpp:174
float m_fltLength
Current length of the line.
Definition: LineBase.h:29
Declares the data chart manager class.
Declares the rigid body class.
virtual float Kpe()
Gets the parallel elastic spring constant.
std::string Std_CheckString(std::string strVal)
Converts a string to upper case and trims it.
virtual float DisplacementRatio()
Gets the displacement ratio.
LengthTensionGain m_gainLengthTension
The length-tension gain.
Definition: MuscleBase.h:48
Declares the structure class.
Declares the odor type class.
float Ftl(float fltLce)
Calculates the length tension relationship..
virtual float CalculateGain(float fltInput)
Calculates the gain.
Declares the odor class.
Declares the simulator class.
Declares the activated item manager class.
Declares the contact sensor class.
Declares the external stimuli manager class.
float m_fltTdot
The derivative of tension at the current time step.
Definition: MuscleBase.h:36
virtual float Act()
Gets the muscle activation value.
Declares the attachment class.
float m_fltDisplacement
This is the amount of displacement of this muscle from its resting length.
Declares the receptive field class.
float m_fltKpeByKse
Precalculation of (1+(Kpe/Kse))
virtual float GetChildFloat(std::string strElementName)
Gets a float value from the element with the specified name.
Definition: StdXml.cpp:617
float m_fltKpe
Spring constant for the parrallel spring.
float m_fltPeLength
The length of the PE section of the muscle. The resting length is one half the total muscle length...
virtual void CalculateTension()
Calculates the tension.