43 throw std::invalid_argument(
"Failed to provide arguments to tgRBString::Config");
49 double minTotalLength) :
52 m_stringConfig(stringConf),
53 m_minTotalLength(minTotalLength)
58 tgRBString::tgRBString(
const tgTags& tags,
61 tgBaseString(tags, config.m_stringConfig, restLength, restLength),
66 tgRBString::tgRBString(std::string space_separated_tags,
70 config.m_stringConfig,
82 assert(allSegments.size() == m_config.m_segments);
84 allMuscles = this->find<tgLinearString> (
"muscle");
89 if (m_config.m_segments >= 2)
91 const double musclesPerSegment = (double) allMuscles.size() /
92 (double) (m_config.m_segments - 1);
97 m_effectiveStiffness = m_config.m_stringConfig.
stiffness *
99 (double) (m_config.m_segments - 1);
104 m_effectiveStiffness = 0;
121 throw std::invalid_argument(
"dt is not positive.");
130 std::cout <<
"Tension: " << getTension() <<
131 " Calculated Rest: " << getRestLength() <<
132 " Actual length: " << getCurrentLength() << std::endl;
139 void tgRBString::changeMuscles (
double lengthPercent,
double dt)
143 throw std::invalid_argument(
"dt is not positive.");
147 assert(lengthPercent > 0.0);
148 for(
int i = 0; i < allMuscles.size(); i++){
149 const double rl = allMuscles[i]->getRestLength();
153 std::cout <<
"Indiv Muscle " << rl * lengthPercent << std::endl;
156 allMuscles[i]->setRestLength(rl * lengthPercent, dt);
169 const double velChange = m_config.m_stringConfig.
maxAcc * dt;
170 const double actualLength = getCurrentLength();
175 const double fabsDiff = std::abs(diff);
181 if (abs(diff) > stepSize)
184 if (std::abs((diff/fabsDiff) * m_config.m_stringConfig.
targetVelocity -
185 mostRecentVelocity) >
189 stepSize = velChange * dt;
191 m_restLength += (diff/fabsDiff)*stepSize;
195 if (std::abs(diff/dt - mostRecentVelocity) > velChange)
200 diff = (diff/fabsDiff) * velChange * dt;
207 diff = -(mostRecentVelocity / std::abs(mostRecentVelocity)) *
211 m_restLength += diff;
218 changeMuscles(m_restLength / getRestLength(), dt);
223 void tgRBString::tensionMinLengthController(
const double targetTension,
226 const double stiffness = m_effectiveStiffness;
228 assert(stiffness > 0.0);
230 const double currentTension = getTension();
231 const double delta = targetTension - currentTension;
232 double diff = delta / stiffness;
241 std::cout <<
"m_preferred: " << m_preferredLength << std::endl;
246 const double tgRBString::getStartLength()
const
251 const double tgRBString::getCurrentLength()
const
253 double currLength = 0;
256 std::size_t n = allSegments.size() - 1;
257 for (std::size_t i = 0; i < n; i++)
259 const btVector3 rodCenterOfMass1 = allSegments[i]->centerOfMass();
260 const btVector3 rodCenterOfMass2 = allSegments[i + 1]->centerOfMass();
262 currLength += (rodCenterOfMass2 - rodCenterOfMass1).length();
266 currLength += allSegments[0]->length();
271 const double tgRBString::getTension()
const
273 const double tension = (getCurrentLength() - getRestLength())
274 * m_effectiveStiffness;
275 return (tension < 0) ? 0.0 : tension;
280 const double tgRBString::getRestLength()
const
282 double muscleRest = 0;
285 std::size_t segSize = allSegments.size();
286 for (std::size_t i = 0; i < segSize; i++)
288 rodRest += allSegments[i]->length();
291 std::size_t mSize = allMuscles.size();
292 for (std::size_t i = 0; i < mSize; i++)
294 muscleRest += allMuscles[i]->getRestLength();
299 muscleRest /= ((double) mSize / (
double) (segSize - 1));
301 return rodRest + muscleRest;
304 const double tgRBString::getVelocity()
const
309 const double tgRBString::computeVelocity(
const double dt)
const
326 throw std::invalid_argument(
"dt is negitive.");
332 void tgRBString::logHistory(
const double dt)
334 const double currentVelocity = computeVelocity(dt);
339 if (m_config.m_stringConfig.
hist)
virtual void setup(tgWorld &world)
virtual void moveMotors(double dt)
virtual void step(double dt)
Utility class for class casting and filtering collections by type.
std::deque< double > lastLengths
std::deque< double > tensionHistory
virtual void setup(tgWorld &world)
virtual void step(double dt)
Contains the definition of class tgLinearString.
Contains the definition of class tgRBString. A string with small rigid bodies to create contact dynam...
BaseStringHistory *const m_pHistory
std::deque< double > lastVelocities
std::deque< double > restLengths
std::vector< tgModel * > getDescendants() const