NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
tgBulletSpringCable.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 
25 // This module
26 #include "tgBulletSpringCable.h"
28 #include "tgCast.h"
29 // The BulletPhysics library
30 #include "BulletDynamics/Dynamics/btRigidBody.h"
31 
32 #include <iostream>
33 #include <stdexcept>
34 
35 tgBulletSpringCable::tgBulletSpringCable( const std::vector<tgBulletSpringCableAnchor*>& anchors,
36  double coefK,
37  double dampingCoefficient,
38  double pretension) :
40  coefK, dampingCoefficient, pretension),
41 m_anchors(anchors),
42 anchor1(anchors.front()),
43 anchor2(anchors.back())
44 {
45  assert(m_anchors.size() >= 2);
46  assert(invariant());
47  // tgSpringCable does heavy lifting as far as determining rest length
48 }
49 
50 
52 {
53  #if (0)
54  std::cout << "Destroying tgBulletSpringCable" << std::endl;
55  #endif
56 
57  std::size_t n = m_anchors.size();
58 
59  // Make absolutely sure these are deleted, in case we have a poorly timed reset
60  if (m_anchors[0] != anchor1)
61  {
62  delete anchor1;
63  }
64 
65  if (m_anchors[n-1] != anchor2)
66  {
67  delete anchor2;
68  }
69 
70  for (std::size_t i = 0; i < n; i++)
71  {
72  delete m_anchors[i];
73  }
74 
75 
76  m_anchors.clear();
77 }
78 
80 {
81  if (dt <= 0.0)
82  {
83  throw std::invalid_argument("dt is not positive!");
84  }
85 
86  calculateAndApplyForce(dt);
87  assert(invariant());
88 }
89 
90 void tgBulletSpringCable::calculateAndApplyForce(double dt)
91 {
92  btVector3 force(0.0, 0.0, 0.0);
93  double magnitude = 0.0;
94  const btVector3 dist =
96 
97  // These computations should occur for history regardless of motion
98  const double currLength = dist.length();
99  const btVector3 unitVector = dist / currLength;
100  const double stretch = currLength - m_restLength;
101 
102  magnitude = m_coefK * stretch;
103 
104  const double deltaStretch = currLength - m_prevLength;
105  m_velocity = deltaStretch / dt;
106 
108 
109  if (abs(magnitude) * 1.0 < abs(m_damping))
110  {
111  m_damping =
112  (m_damping > 0.0 ? magnitude * 1.0 : -magnitude * 1.0);
113  }
114 
115  magnitude += m_damping;
116 
117  #if (0)
118  std::cout << "Length: " << dist.length() << " rl: " << m_restLength <<std::endl;
119  #endif
120 
121  if (dist.length() > m_restLength)
122  {
123  force = unitVector * magnitude;
124  }
125  else
126  {
127  // Leave force as the zero vector
128  }
129 
130  // Finished calculating, so can store things
131  m_prevLength = currLength;
132 
133  //Now Apply it to the connected two bodies
134  btVector3 point1 = this->anchor1->getRelativePosition();
135  this->anchor1->attachedBody->activate();
136  this->anchor1->attachedBody->applyImpulse(force*dt,point1);
137 
138  btVector3 point2 = this->anchor2->getRelativePosition();
139  this->anchor2->attachedBody->activate();
140  this->anchor2->attachedBody->applyImpulse(-force*dt,point2);
141 }
142 
144 {
145  const btVector3 dist =
146  this->anchor2->getWorldPosition() - this->anchor1->getWorldPosition();
147  return dist.length();
148 }
149 
151 {
152  double tension = (getActualLength() - m_restLength) * m_coefK;
153  tension = (tension < 0.0) ? 0.0 : tension;
154  return tension;
155 }
156 
157 const std::vector<const tgSpringCableAnchor*> tgBulletSpringCable::getAnchors() const
158 {
159  return tgCast::constFilter<tgBulletSpringCableAnchor, const tgSpringCableAnchor>(m_anchors);
160 }
161 
162 bool tgBulletSpringCable::invariant(void) const
163 {
164  return (m_coefK > 0.0 &&
165  m_dampingCoefficient >= 0.0 &&
166  m_prevLength >= 0.0 &&
167  m_restLength >= 0.0 &&
168  anchor1 != NULL &&
169  anchor2 != NULL &&
170  m_anchors.size() >= 2);
171 }
virtual const double getActualLength() const
virtual void step(double dt)
tgBulletSpringCable(const std::vector< tgBulletSpringCableAnchor * > &anchors, double coefK, double dampingCoefficient, double pretension=0.0)
virtual const std::vector< const tgSpringCableAnchor * > getAnchors() const
Utility class for class casting and filtering collections by type.
const double m_dampingCoefficient
virtual btVector3 getWorldPosition() const
tgBulletSpringCableAnchor *const anchor2
Definition: tgCast.h:38
virtual const double getTension() const
std::vector< tgBulletSpringCableAnchor * > m_anchors
double m_restLength
Definitions of class tgBulletSpringCableAnchor, formerly muscleAnchor.
Definitions of classes tgBulletSpringCable $Id$.
tgBulletSpringCableAnchor *const anchor1
double m_prevLength
const double m_coefK
virtual btVector3 getRelativePosition() const