AnimatLab  2
Test
VsMotorizedJoint.cpp
Go to the documentation of this file.
1 
7 #include "StdAfx.h"
8 #include "VsMovableItem.h"
9 #include "VsBody.h"
10 #include "VsJoint.h"
11 #include "VsMotorizedJoint.h"
12 #include "VsRigidBody.h"
13 #include "VsSimulator.h"
14 #include "VsDragger.h"
15 
16 namespace VortexAnimatSim
17 {
18  namespace Environment
19  {
20 
21 VsMotorizedJoint::VsMotorizedJoint()
22 {
23  m_lpThisMotorJoint = NULL;
24  m_bMotorOn = false;
25 }
26 
27 VsMotorizedJoint::~VsMotorizedJoint()
28 {
29 }
30 
31 void VsMotorizedJoint::SetThisPointers()
32 {
33  VsJoint::SetThisPointers();
34 
35  m_lpThisMotorJoint = dynamic_cast<MotorizedJoint *>(this);
36  if(!m_lpThisMotorJoint)
37  THROW_TEXT_ERROR(Vs_Err_lThisPointerNotDefined, Vs_Err_strThisPointerNotDefined, "m_lpThisMotorJoint, " + m_lpThisAB->Name());
38 
39  m_lpThisMotorJoint->PhysicsMotorJoint(this);
40 }
41 
42 //If this is a servo motor then the "velocity" signal is not really a velocity signal in this case.
43 //It is the desired position and we must convert it to the velocity needed to reach and maintian that position.
44 void VsMotorizedJoint::CalculateServoVelocity()
45 {
46  if(!m_vxJoint)
47  return;
48 
49  float fltTargetPos = m_lpThisJoint->GetPositionWithinLimits(m_lpThisMotorJoint->DesiredPosition());
50  //For calculating the error we need to get the actual current position of the joint, not the value that was obtained last
51  //time it called CollectData. If the motor is being synched with the robot for sim then this could be delayed. However, in the
52  //real system this type of error correction would be taking place within the actual servo's motor loop.
53  float fltError = fltTargetPos - GetCurrentVxJointPos();
54  m_lpThisMotorJoint->SetPosition(fltTargetPos);
55 
57  //int i=5;
58  //if(m_lpThisMotorJoint->ID() == "0085EE18-89F7-4039-8648-EC51114BEEFF" && GetSimulator()->Time() >= 0.5 && fabs(fltTargetPos) > 0)
59  // i=6;
60 
61  AnimatSim::Environment::eJointMotorType MotorType = m_lpThisMotorJoint->MotorType();
62  if((MotorType == eJointMotorType::PositionControl || MotorType == eJointMotorType::PositionVelocityControl) && m_lpThisMotorJoint->ReachedSetPosition() )
63  {
64  //Lock this joint position.
65  m_lpThisMotorJoint->DesiredVelocity(0);
66  }
67  else if(MotorType == eJointMotorType::PositionVelocityControl)
68  {
69  //If we set the desired velocity and position then make sure the desired velocity is in the right direction
70  float fltDesiredVel = fabs(m_lpThisMotorJoint->DesiredVelocity()) * Std_Sign(fltError);
71 
72  float fltPosError = fabs(fltError);
73  if(fltPosError > 0 && fltPosError < 0.05)
74  {
75  float fltDesiredVel2 = fltDesiredVel * (fabs(fltError)*m_lpThisMotorJoint->ServoGain());
76  //Only do this if the new desired velocity is less than the original one to slow it down. Never speed it up.
77  if(fabs(fltDesiredVel2) <= fabs(fltDesiredVel))
78  fltDesiredVel = fltDesiredVel2;
79  }
80 
81  m_lpThisMotorJoint->DesiredVelocity(fltDesiredVel);
82 
83  if(fabs(fltError) < 1e-4)
84  m_lpThisMotorJoint->ReachedSetPosition(true);
85  }
86  else if(MotorType == eJointMotorType::PositionControl)
87  {
88  if(m_lpThisJoint->EnableLimits())
89  {
90  float fltProp = fltError / m_lpThisJoint->GetLimitRange();
91  m_lpThisMotorJoint->DesiredVelocity(fltProp * m_lpThisMotorJoint->ServoGain());
92  }
93  else
94  m_lpThisMotorJoint->DesiredVelocity(fltError * m_lpThisMotorJoint->MaxVelocity());
95  }
96 }
97 
98 
99 void VsMotorizedJoint::Physics_SetVelocityToDesired()
100 {
101  if(m_lpThisMotorJoint->EnableMotor())
102  {
103  if(m_lpThisMotorJoint->MotorType() == eJointMotorType::PositionControl || m_lpThisMotorJoint->MotorType() == eJointMotorType::PositionVelocityControl)
104  CalculateServoVelocity();
105 
106  float fltDesiredVel = m_lpThisMotorJoint->DesiredVelocity();
107  float fltMaxVel = m_lpThisMotorJoint->MaxVelocity();
108  float fltMaxForce = m_lpThisMotorJoint->MaxForce();
109 
110  if(fltDesiredVel>fltMaxVel)
111  fltDesiredVel = fltMaxVel;
112 
113  if(fltDesiredVel < -fltMaxVel)
114  fltDesiredVel = -fltMaxVel;
115 
116  float fltSetVelocity = fltDesiredVel;
117 
118  m_lpThisMotorJoint->SetVelocity(fltSetVelocity);
119  m_lpThisMotorJoint->DesiredVelocity(0);
120  m_lpThisMotorJoint->DesiredPosition(0);
121 
122  //Only do anything if the velocity value has changed
123  if(m_vxJoint && fabs(m_lpThisJoint->JointVelocity() - fltSetVelocity) > 1e-4)
124  {
125  if(fabs(fltSetVelocity) > 1e-4 && m_vxJoint)
126  {
127  if(m_vxJoint->getControl(m_iCoordID) == Vx::VxConstraint::CoordinateControlEnum::kControlLocked)
128  Physics_EnableMotor(true, fltSetVelocity, fltMaxForce, false);
129  else
130  m_vxJoint->setMotorDesiredVelocity(m_iCoordID, fltSetVelocity);
131  }
132  else
133  {
134  if(m_vxJoint->getControl(m_iCoordID) != Vx::VxConstraint::CoordinateControlEnum::kControlLocked)
135  m_lpThisMotorJoint->EnableLock(true, m_vxJoint->getCoordinateCurrentPosition(m_iCoordID), fltMaxForce);
136  }
137  }
138 
139  m_lpThisMotorJoint->PrevVelocity(fltSetVelocity);
140  }
141 }
142 
143 void VsMotorizedJoint::Physics_EnableLock(bool bOn, float fltPosition, float fltMaxLockForce)
144 {
145  if (m_vxJoint)
146  {
147  if(bOn)
148  {
149  //set the lock parameters
150  m_vxJoint->setLockParameters(m_iCoordID, fltPosition, fltMaxLockForce);
151  //turn on the lock (disabling motorized or free mode)
152  m_vxJoint->setControl(m_iCoordID, VxConstraint::CoordinateControlEnum::kControlLocked);
153  }
154  else if (m_bMotorOn)
155  Physics_EnableMotor(true, 0, fltMaxLockForce, false);
156  else
157  m_vxJoint->setControl(m_iCoordID, VxConstraint::CoordinateControlEnum::kControlFree);
158  }
159 }
160 
161 void VsMotorizedJoint::Physics_EnableMotor(bool bOn, float fltDesiredVelocity, float fltMaxForce, bool bForceWakeup)
162 {
163  if (m_vxJoint)
164  {
165  if(bOn)
166  {
167  if(m_vxJoint->getControl(m_iCoordID) != Vx::VxConstraint::CoordinateControlEnum::kControlMotorized)
168  m_vxJoint->setControl(m_iCoordID, Vx::VxConstraint::CoordinateControlEnum::kControlMotorized);
169 
170  m_vxJoint->setMotorMaximumForce(m_iCoordID, fltMaxForce);
171  m_vxJoint->setMotorDesiredVelocity(m_iCoordID, fltDesiredVelocity);
172  }
173  else
174  m_vxJoint->setControl(m_iCoordID, Vx::VxConstraint::CoordinateControlEnum::kControlFree);
175 
176  m_bMotorOn = bOn;
177  }
178 }
179 
180 void VsMotorizedJoint::Physics_MaxForce(float fltVal)
181 {
182  if(m_vxJoint)
183  m_vxJoint->setMotorMaximumForce(m_iCoordID, fltVal);
184 }
185 
186  } // Environment
187 } //VortexAnimatSim
int Std_Sign(float fltVal)
Determines the sign of a number.
Classes for implementing the cm-labs vortex physics engine for AnimatLab.
eJointMotorType
The motor control type for this joint.