NTRT Simulator  Version: Master
 All Classes Namespaces Files Functions Variables Typedefs Friends Pages
RibModelMixedContact.cpp
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 
29 #include "RibModelMixedContact.h"
30 #include "core/tgCast.h"
34 #include "core/tgString.h"
35 #include "tgcreator/tgBuildSpec.h"
38 #include "tgcreator/tgRodInfo.h"
39 #include "tgcreator/tgStructure.h"
41 #include "tgcreator/tgUtil.h"
42 
43 // Bullet Physics
44 #include "LinearMath/btVector3.h"
45 // The C++ Standard Library
46 #include <iostream>
47 
48 namespace
49 {
50  void addNodes(tgStructure& tetra, double v_size)
51  {
52  tetra.addNode(0,0,0); // center
53  tetra.addNode(0.0, v_size, v_size / 2.0); // top
54  tetra.addNode(-v_size, 0.0, -v_size); // left
55  tetra.addNode(v_size, 0.0, -v_size); // right
56  #if (0) // Attempt at compliant rib attachments
57  tetra.addNode(-v_size / 5.0, 0.0, 0.0); // right
58  tetra.addNode(v_size / 5.0, 0.0, 0.0); // right
59  #endif
60  }
61 
62  void ellipseNodes(tgStructure& tetra, double a, double b, double startT, double endT, size_t n)
63  {
64  double rodL = (endT - startT) / (double) n;
65 
66  for(size_t i = 0; i < n; i++)
67  {
68  double x = a * cos( startT + i * rodL);
69  double y = b * sin( startT + i * rodL);
70  // Just build it in xy, can rotate later
71  tgNode node( btVector3( x, y, 0.0));
72  tetra.addNode( node );
73  }
74  #if (0)
75  // Final Nodes for "feet"
76  double x = a * cos( startT + (n - 1) * rodL);
77  double y = b * sin( startT + (n - 1) * rodL) - 0.5;
78  tgNode node( btVector3( x, y, 0.0));
79  tetra.addNode( node );
80  #endif
81  }
82 
84  void addPairs(tgStructure& tetra)
85  {
86  tetra.addPair(0,1, "top rod");
87  tetra.addPair(0,2, "left rod");
88  tetra.addPair(0,3, "right rod");
89  #if (0) // extra connection points for muscles
90  tetra.addPair(0,4, "leftRib rod");
91  tetra.addPair(0,5, "rightRib rod");
92  #endif
93  }
94 
96  void makePairs(tgStructure& tetra)
97  {
98  size_t n = tetra.getNodes().size();
99  std::cout << "Nodes size is " << n << std::endl;
100  for(std::size_t i = 1; i < n; i++) {
101  tetra.addPair(i-1, i, "rod");
102  }
103  }
104 
105  void addSegments(tgStructure& snake, const tgStructure& tetra,
106  const tgStructure& ribs, double edge, size_t segmentCount)
107  {
108  const btVector3 offset(0, 0, -edge * 1.3);
109  const btVector3 rotAxis(0, 1.0, 0);
110  for (size_t i = 0; i < segmentCount; ++i)
111  {
112 
113  tgStructure* const t = new tgStructure(tetra);
114  // Need a new pointer for every rib
115  tgStructure* const r = new tgStructure(ribs);
116  tgStructure* const r2 = new tgStructure(ribs);
117  t->addChild(r);
118  t->addChild(r2);
119 
120  tgNodes r01 = r->getNodes();
121  tgNodes r02 = r2->getNodes();
122  tgNodes t01 = t->getNodes();
123 
124  size_t r02End = r02.getNodes().size();
125  #if (0)
126  r2->addRotation(t01[0], rotAxis, M_PI);
127  t->addPair(t01[0], r01[0], "multiMuscle connect");
128  t->addPair(t01[0], r02[0], "multiMuscle connect");
129  #else
130  t->addPair(t01[0], r01[0], "rod connect");
131  t->addPair(t01[0], r02[0], "rod connect");
132  r2->addRotation(r02[0], rotAxis, M_PI);
133  #endif
134 
135 
136  t->addTags(tgString("segment num", i + 1));
137  t->move((i + 1) * offset);
138  // Add a child to the snake
139  snake.addChild(t);
140  }
141  }
142 
143  // Add muscles that connect the segments
144  void addMuscles(tgStructure& snake)
145  {
146  const std::vector<tgStructure*> children = snake.getChildren();
147  for (size_t i = 1; i < children.size(); ++i)
148  {
149  tgNodes n0 = children[i-1]->getNodes();
150  tgNodes n1 = children[i ]->getNodes();
151 
152  const std::vector<tgStructure*> subChildren1 = children[i-1]->getChildren();
153  const std::vector<tgStructure*> subChildren2 = children[i]->getChildren();
154 
155  tgNodes r10 = subChildren1[0]->getNodes();
156  tgNodes r11 = subChildren1[1]->getNodes();
157  tgNodes r20 = subChildren2[0]->getNodes();
158  tgNodes r21 = subChildren2[1]->getNodes();
159 
160  if (i == 1)
161  {
162  snake.addPair(n0[1], n1[1], tgString("starting outer front muscleAct seg", i-1) + tgString(" seg", i));
163  snake.addPair(n0[2], n1[2], tgString("starting outer right muscleAct seg", i-1) + tgString(" seg", i));
164  snake.addPair(n0[3], n1[3], tgString("starting outer back muscleAct seg", i-1) + tgString(" seg", i));
165 
166  snake.addPair(n0[2], n1[1], tgString("starting inner front muscleAct seg", i-1) + tgString(" seg", i));
167  snake.addPair(n0[3], n1[1], tgString("starting inner right muscleAct seg", i-1) + tgString(" seg", i));
168 
169  snake.addPair(n0[2], r20[3], tgString("starting inner front muscleAct seg", i-1) + tgString(" seg", i));
170  snake.addPair(n0[3], r21[3], tgString("starting inner right muscleAct seg", i-1) + tgString(" seg", i));
171  }
172  else if (i == children.size() - 1)
173  {
174  snake.addPair(n0[1], n1[1], tgString("ending outer front muscleAct seg", i-1) + tgString(" seg", i));
175  snake.addPair(n0[2], n1[2], tgString("ending outer right muscleAct seg", i-1) + tgString(" seg", i));
176  snake.addPair(n0[3], n1[3], tgString("ending outer back muscleAct seg", i-1) + tgString(" seg", i));
177 
178  snake.addPair(n0[2], n1[1], tgString("ending inner front muscleAct seg", i-1) + tgString(" seg", i));
179  snake.addPair(n0[3], n1[1], tgString("ending inner right muscleAct seg", i-1) + tgString(" seg", i));
180 
181  snake.addPair(n0[2], r20[3], tgString("ending inner front muscleAct seg", i-1) + tgString(" seg", i));
182  snake.addPair(n0[3], r21[3], tgString("ending inner right muscleAct seg", i-1) + tgString(" seg", i));
183  }
184  else
185  {
186  snake.addPair(n0[1], n1[1], tgString("middle outer front muscleAct seg", i-1) + tgString(" seg", i));
187  snake.addPair(n0[2], n1[2], tgString("middle outer right muscleAct seg", i-1) + tgString(" seg", i));
188  snake.addPair(n0[3], n1[3], tgString("middle outer back muscleAct seg", i-1) + tgString(" seg", i));
189 
190  snake.addPair(n0[2], n1[1], tgString("middle inner front muscleAct seg", i-1) + tgString(" seg", i));
191  snake.addPair(n0[3], n1[1], tgString("middle inner right muscleAct seg", i-1) + tgString(" seg", i));
192 
193  snake.addPair(n0[2], r20[3], tgString("middle inner front muscleAct seg", i-1) + tgString(" seg", i));
194  snake.addPair(n0[3], r21[3], tgString("middle inner right muscleAct seg", i-1) + tgString(" seg", i));
195  }
196  snake.addPair(r10[5], r20[12], tgString("intercostal muscle seg", i-1) + tgString(" seg", i));
197  snake.addPair(r11[5], r21[12], tgString("intercostal muscle seg", i-1) + tgString(" seg", i));
198  snake.addPair(r10[12], r20[5], tgString("intercostal muscle seg", i-1) + tgString(" seg", i));
199  snake.addPair(r11[12], r21[5], tgString("intercostal muscle seg", i-1) + tgString(" seg", i));
200  }
201  }
202 
203  void trace(const tgStructureInfo& structureInfo, tgModel& model)
204  {
205  std::cout << "StructureInfo:" << std::endl
206  << structureInfo << std::endl
207  << "Model: " << std::endl
208  << model << std::endl;
209  // Showing the find function
210  const std::vector<tgSpringCableActuator*> outerMuscles =
211  model.find<tgSpringCableActuator>("outer");
212  for (size_t i = 0; i < outerMuscles.size(); ++i)
213  {
214  const tgSpringCableActuator* const pMuscle = outerMuscles[i];
215  assert(pMuscle != NULL);
216  std::cout << "Outer muscle: " << *pMuscle << std::endl;
217  }
218  }
219 
220 } // namespace
221 
222 RibModelMixedContact::RibModelMixedContact(int segments) :
223  BaseSpineModelLearning(segments)
224 {
225 }
226 
227 RibModelMixedContact::~RibModelMixedContact()
228 {
229 }
230 
232 {
233  double v_size = 3.0;
234 
235  // Create the spinal processes
236  tgStructure vertebrae;
237  addNodes(vertebrae, v_size);
238  addPairs(vertebrae);
239 
240  // Move the first one so we can create a longer snake.
241  // Or you could move the snake at the end, up to you.
242  vertebrae.move(btVector3(0.0, 2 * v_size, v_size * m_segments));
243 
244  // Create ribs and add them to the vertebrae
245  double majorAxis = 6.0;
246  double minorAxis = 4.0;
247  double startTheta = M_PI / 2.0;
248  double endTheta = 5.0 * M_PI / 4.0;
249  size_t segs = 15;
250 
251  tgStructure ribs;
252  ellipseNodes(ribs, majorAxis, minorAxis, startTheta, endTheta, segs);
253  makePairs(ribs);
254 
255  #if (0) // Attempt at compliant rib attachments
256  ribs.move(btVector3(v_size / 3.0, 2 * v_size - minorAxis, v_size * m_segments));
257  #else
258  ribs.move(btVector3(0.0, 2 * v_size - minorAxis -.3, v_size * m_segments));
259  #endif
260  // Create our snake segments
261  tgStructure snake;
262  addSegments(snake, vertebrae, ribs, v_size, m_segments);
263 
264  snake.move(btVector3(0.0, majorAxis, 0.0));
265 
266  addMuscles(snake);
267 
268  // Create the build spec that uses tags to turn the structure into a real model
269  // Note: This needs to be high enough or things fly apart...
270  const double density = 4.2 / 300.0;
271  const double radius = 0.5;
272  const double friction = 0.5; // Default is 0.5
273  const double rollFriction = 0.5; // Default is 0.0
274  const double restitution = 0.0; // Default
275 
276  const tgRod::Config rodConfig(radius, density, friction, rollFriction, restitution);
277  tgBuildSpec spec;
278  spec.addBuilder("rod", new tgRodInfo(rodConfig));
279 
280  const double elasticity = 500.0;
281  const double elasticityAct = 1000.0;
282  const double damping = 5.0;
283  const double dampingAct = 10.0;
284  const double pretension = 0.0;
285  const bool history = true;
286  const double maxTens = 1000.0;
287  const double maxTensAct = 7000.0;
288  const double maxSpeed = 100.0;
289  const double maxSpeedAct = 24.0;
290 
291  const double mRad = 1.0;
292  const double motorFriction = 10.0;
293  const double motorInertia = 1.0;
294  const bool backDrivable = false;
295 
296 
297  #if (0) //Replacing with tgKinematicActuator, leaving option to turn it off. 9/9/15.
298  tgKinematicActuator::Config muscleConfig(elasticity, damping, pretension,
299  mRad, motorFriction, motorInertia, backDrivable,
300  history, maxTens, maxSpeed);
301  spec.addBuilder("muscle", new tgKinematicContactCableInfo(muscleConfig));
302 
303  tgKinematicActuator::Config muscleConfigAct(elasticityAct, dampingAct, pretension,
304  mRad, motorFriction, motorInertia, backDrivable,
305  history, maxTensAct, maxSpeedAct);
306  spec.addBuilder("muscleAct", new tgKinematicContactCableInfo(muscleConfigAct));
307  #else
308  tgSpringCableActuator::Config muscleConfig(elasticity, damping, pretension, history);
309  spec.addBuilder("muscle", new tgBasicContactCableInfo(muscleConfig));
311  tgSpringCableActuator::Config muscleConfigAct(elasticityAct, dampingAct, pretension, history, 7000, 24);
312  spec.addBuilder("muscleAct", new tgBasicContactCableInfo(muscleConfigAct));
313  #endif
314 
315  #if (0) // Compliant Rib Attachments
316  const double stiffness = 1000;
317  const double damping = .01 * stiffness;
318 
319  tgSpringCableActuator::Config muscleConfig1(stiffness, damping, -M_PI / 2.0);
320  tgSpringCableActuator::Config muscleConfig2(stiffness, damping, M_PI / 2.0);
321  tgSpringCableActuator::Config muscleConfig3(stiffness, damping, M_PI);
322  tgSpringCableActuator::Config muscleConfig4(stiffness, damping, 0);
323 
324  spec.addBuilder("multiMuscle", new tgBasicActuatorInfo(muscleConfig1));
325  spec.addBuilder("multiMuscle", new tgBasicActuatorInfo(muscleConfig2));
326  spec.addBuilder("multiMuscle", new tgBasicActuatorInfo(muscleConfig3));
327  spec.addBuilder("multiMuscle", new tgBasicActuatorInfo(muscleConfig4));
328  #endif
329  // Create your structureInfo
330  tgStructureInfo structureInfo(snake, spec);
331 
332  // Use the structureInfo to build ourselves
333  structureInfo.buildInto(*this, world);
334 
335  // We could now use tgCast::filter or similar to pull out the models (e.g. muscles)
336  // that we want to control.
337  m_allMuscles = find<tgSpringCableActuator> ("muscleAct");
338  m_allSegments = find<tgModel> ("segment");
339 
340  #if (0)
341  trace(structureInfo, *this);
342  #endif
343 
344  // Actually setup the children
346 }
348 {
349 
351 
352 }
353 
355 {
356 
357  if (dt < 0.0)
358  {
359  throw std::invalid_argument("dt is not positive");
360  }
361  else
362  {
363  /* CPG update occurs in the controller so that we can decouple it
364  * from the physics update
365  */
366 
367  BaseSpineModelLearning::step(dt); // Step any children
368  }
369 }
const std::vector< tgStructure * > & getChildren() const
Definition: tgStructure.h:184
void addChild(tgStructure *child)
Definition of class tgRodInfo.
Convenience function for combining strings with ints, mostly for naming structures.
Utility class for class casting and filtering collections by type.
virtual void setup(tgWorld &world)
const tgNodes & getNodes() const
Definition: tgStructure.h:138
Definition of class tgBasicActuatorInfo.
virtual void step(double dt)
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Definition: tgStructure.cpp:80
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
void addRotation(const btVector3 &fixedPoint, const btVector3 &axis, double angle)
virtual void setup(tgWorld &world)
Definition of class tgBasicContactCableInfo.
std::string tgString(std::string s, int i)
Definition: tgString.h:33
Definition: tgNode.h:45
Definition of class tgStructure.
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition: tgModel.h:128
Definition of class tgStructureInfo.
Definition of class tgKinematicActuatorInfo.
Rand seeding simular to the evolution and terrain classes.
Implements a spine model with a rib cage, including contact cables and tags to facilitate different l...
Definition of class tgBuildSpec.
virtual void step(double dt)
Definition of class tgKinematicContactCableInfo.
void buildInto(tgModel &model, tgWorld &world)
void addNode(double x, double y, double z, std::string tags="")
Definition: tgStructure.cpp:70