NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgRBString.cpp
Go to the documentation of this file.
1 /*
2  * Copyright © 2012, United States Government, as represented by the
3  * Administrator of the National Aeronautics and Space Administration.
4  * All rights reserved.
5  *
6  * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
7  * under the Apache License, Version 2.0 (the "License");
8  * you may not use this file except in compliance with the License.
9  * You may obtain a copy of the License at
10  * http://www.apache.org/licenses/LICENSE-2.0.
11  *
12  * Unless required by applicable law or agreed to in writing,
13  * software distributed under the License is distributed on an
14  * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
15  * either express or implied. See the License for the specific language
16  * governing permissions and limitations under the License.
17 */
18 
28 #include "tgRBString.h"
29 #include "core/tgLinearString.h"
30 
31 #include "core/tgCast.h"
32 
33 // The C++ Standard Library
34 #include <cmath>
35 #include <stdexcept>
36 
42 {
43  throw std::invalid_argument("Failed to provide arguments to tgRBString::Config");
44 }
45 
46 tgRBString::Config::Config( std::size_t segments,
47  const tgRod::Config& rodConf,
48  const tgLinearString::Config& stringConf,
49  double minTotalLength) :
50 m_segments(segments),
51 m_rodConfig(rodConf),
52 m_stringConfig(stringConf),
53 m_minTotalLength(minTotalLength)
54 {
55 }
56 
57 // @todo consider storing other confing info as a member variable
58 tgRBString::tgRBString(const tgTags& tags,
59  tgRBString::Config& config,
60  double restLength) :
61 tgBaseString(tags, config.m_stringConfig, restLength, restLength),
62 m_config(config)
63 {
64 }
65 
67 {
68  // These occur after the build process
69  allSegments = tgCast::filter<tgModel, tgRod> (getDescendants());
70 
71  assert(allSegments.size() == m_config.m_segments);
72 
73  allMuscles = this->find<tgLinearString> ("muscle");
74  // Should we assert something here?
75 
76  // Consider making this an assert since tensionMinLength also
77  // Depends on it
78  if (m_config.m_segments >= 2)
79  {
80  const double musclesPerSegment = (double) allMuscles.size() /
81  (double) (m_config.m_segments - 1);
82  /* True if all muscles are homogenous, which should be true
83  * given tgRBStringInfo's use of configs. Should this include
84  * RB segment mass?
85  */
86  m_effectiveStiffness = m_config.m_stringConfig.stiffness *
87  musclesPerSegment /
88  (double) (m_config.m_segments - 1);
89  }
90  else
91  {
92  // Infinite stiffness means its a single RB, which has no tension
93  m_effectiveStiffness = 0;
94  }
95  // All the heavy lifting is done by info
96  tgModel::setup(world);
97  logHistory(0.0);
98 
99 }
100 
101 void tgRBString::teardown()
102 {
104 }
105 
106 void tgRBString::step(double dt)
107 {
108  if (dt <= 0.0)
109  {
110  throw std::invalid_argument("dt is not positive.");
111  }
112  else
113  {
114 
115  logHistory(dt);
116  tgModel::step(dt); // Step any children
117 
118 #if (1)
119  std::cout << "Tension: " << getTension() <<
120  " Calculated Rest: " << getRestLength() <<
121  " Actual length: " << getCurrentLength() << std::endl;
122 #endif
123  }
124 
125 }
126 
127 //Does this make myModel an observer as well??
128 void tgRBString::changeMuscles (double lengthPercent, double dt)
129 {
130  if (dt <= 0.0)
131  {
132  throw std::invalid_argument("dt is not positive.");
133  }
134  else
135  {
136  assert(lengthPercent > 0.0);
137  for( int i = 0; i < allMuscles.size(); i++){
138  const double rl = allMuscles[i]->getRestLength();
139 
140  assert(rl > 0.0);
141  #if (0)
142  std::cout << "Indiv Muscle " << rl * lengthPercent << std::endl;
143  #endif
144 
145  allMuscles[i]->setRestLength(rl * lengthPercent, dt);
146  }
147  }
148 }
149 
150 void tgRBString::moveMotors(double dt)
151 {
152  // @todo add functions from muscle2P Bounded
153 
154  // Reverse the sign if restLength >= preferredLength
155  // Velocity limiter
156  double stepSize = m_config.m_stringConfig.targetVelocity * dt;
157  // Acceleration limiter
158  const double velChange = m_config.m_stringConfig.maxAcc * dt;
159  const double actualLength = getCurrentLength();
160  const double mostRecentVelocity = m_pHistory->lastVelocities.back();
161  m_restLength = getRestLength();
162 
163  double diff = m_preferredLength - m_restLength;
164  const double fabsDiff = std::abs(diff);
165 
166  // If below actual length, don't shorten any more
167  if ((actualLength > m_config.m_stringConfig.minActualLength) ||
168  (diff > 0))
169  {
170  if (abs(diff) > stepSize)
171  {
172  //Cap Velocity
173  if (std::abs((diff/fabsDiff) * m_config.m_stringConfig.targetVelocity -
174  mostRecentVelocity) >
175  velChange)
176  {
177  // Cap Acceleration
178  stepSize = velChange * dt;
179  }
180  m_restLength += (diff/fabsDiff)*stepSize;
181  }
182  else
183  {
184  if (std::abs(diff/dt - mostRecentVelocity) > velChange)
185  {
186  // Cap Acceleration
187  if (diff != 0)
188  {
189  diff = (diff/fabsDiff) * velChange * dt;
190  }
191  else
192  {
193  // If m_prevVelocity was zero, it would be smaller than
194  // velChange. Therefore preVelocity is valid for figuring
195  // out direction
196  diff = -(mostRecentVelocity / std::abs(mostRecentVelocity)) *
197  velChange * dt;
198  }
199  }
200  m_restLength += diff;
201  }
202  }
203 
204  // Ensure we aren't going below min rest length
205  if(m_restLength >= m_config.m_stringConfig.minRestLength)
206  {
207  changeMuscles(m_restLength / getRestLength(), dt);
208  }
209 
210 }
211 
212 void tgRBString::tensionMinLengthController(const double targetTension,
213  float dt)
214 {
215  const double stiffness = m_effectiveStiffness;
216  // @todo: write invariant that checks this;
217  assert(stiffness > 0.0);
218 
219  const double currentTension = getTension();
220  const double delta = targetTension - currentTension;
221  double diff = delta / stiffness;
222  m_restLength = getRestLength();
223 
224  const double newLength = m_restLength - diff;
225 
226  m_preferredLength =
227  (newLength > m_config.m_stringConfig.minRestLength) ?
228  newLength : m_config.m_stringConfig.minRestLength;
229 #if (0)
230  std::cout << "m_preferred: " << m_preferredLength << std::endl;
231 #endif
232  moveMotors(dt);
233 }
234 
235 const double tgRBString::getStartLength() const
236 {
237  return m_pHistory->lastLengths.front();
238 }
239 
240 const double tgRBString::getCurrentLength() const
241 {
242  double currLength = 0;
243 
244  // This doesn't consider anchors
245  std::size_t n = allSegments.size() - 1;
246  for (std::size_t i = 0; i < n; i++)
247  {
248  const btVector3 rodCenterOfMass1 = allSegments[i]->centerOfMass();
249  const btVector3 rodCenterOfMass2 = allSegments[i + 1]->centerOfMass();
250 
251  currLength += (rodCenterOfMass2 - rodCenterOfMass1).length();
252  }
253 
254  // Add the distance from the COM to the ends of the rods
255  currLength += allSegments[0]->length();
256 
257  return currLength;
258 }
259 
260 const double tgRBString::getTension() const
261 {
262  const double tension = (getCurrentLength() - getRestLength())
263  * m_effectiveStiffness;
264  return (tension < 0) ? 0.0 : tension;
265 }
266 
267 
268 // How does this relate to m_restLength?
269 const double tgRBString::getRestLength() const
270 {
271  double muscleRest = 0;
272  double rodRest = 0;
273 
274  std::size_t segSize = allSegments.size();
275  for (std::size_t i = 0; i < segSize; i++)
276  {
277  rodRest += allSegments[i]->length();
278  }
279 
280  std::size_t mSize = allMuscles.size();
281  for (std::size_t i = 0; i < mSize; i++)
282  {
283  muscleRest += allMuscles[i]->getRestLength();
284  }
285 
286  // This assumes all four muscles for a given segment have the
287  // same RL, or that averaging is appropreate
288  muscleRest /= ((double) mSize / (double) (segSize - 1));
289 
290  return rodRest + muscleRest;
291 }
292 
293 const double tgRBString::getVelocity() const
294 {
295  return m_pHistory->lastVelocities.back();
296 }
297 
298 const double tgRBString::computeVelocity(const double dt) const
299 {
300  // Check to make sure we're calling this at the start of or after
301  // a log history step
302  assert (m_pHistory->lastLengths.size() == m_pHistory->lastVelocities.size());
303  double vel;
304  if (dt > 0.0)
305  {
306  vel = (getCurrentLength() - m_pHistory->lastLengths.back()) / dt;
307  }
308  else if (dt == 0.0)
309  {
310  // For zero velocity at startup.
311  vel = 0.0;
312  }
313  else
314  {
315  throw std::invalid_argument("dt is negitive.");
316  }
317  return vel;
318 }
319 
320 // Private function, dt should have already been checked
321 void tgRBString::logHistory(const double dt)
322 {
323  const double currentVelocity = computeVelocity(dt);
324  // Needed for rendering, so not optional
325  m_pHistory->lastLengths.push_back(getCurrentLength());
326  // Needed for moveMotors
327  m_pHistory->lastVelocities.push_back(currentVelocity);
328  if (m_config.m_stringConfig.hist)
329  {
330  // Damping history is difficult to compute, so we're leaving it out
331  m_pHistory->restLengths.push_back(getRestLength());
332  m_pHistory->tensionHistory.push_back(getTension());
333  }
334 }
virtual void teardown()
Definition: tgModel.cpp:68
virtual void setup(tgWorld &world)
Definition: tgModel.cpp:57
virtual void step(double dt)
Definition: tgModel.cpp:84
Utility class for class casting and filtering collections by type.
virtual void setup(tgWorld &world)
Definition: tgRBString.cpp:66
Contains the definition of class tgRBString. A string with small rigid bodies to create contact dynam...
Definition: tgTags.h:44