37 #include "LinearMath/btVector3.h"
51 TetraSpineCollisions::TetraSpineCollisions(
size_t segments,
double scale) :
53 scaleFactor(scale / 10.0)
57 TetraSpineCollisions::~TetraSpineCollisions()
62 void addNodes(
tgStructure& tetra,
double edge,
double height,
double scale)
66 tetra.
addNode(-edge / 2.0, 0, 0,
"base");
68 tetra.
addNode( edge / 2.0, 0, 0,
"base");
70 tetra.
addNode(0, height, 0,
"base");
72 tetra.
addNode(0, edge / 2.0 * tan(M_PI / 6.0), 2.50 * scale,
"tip");
76 tgNode nn1 = (oldNodes[0] + oldNodes[2])/2.0;
79 tgNode nn2 = (oldNodes[1] + oldNodes[2])/2.0;
83 std::cout << (oldNodes[3] - oldNodes[2]).length() << std::endl;
84 std::cout << (oldNodes[3] - oldNodes[1]).length() << std::endl;
85 std::cout << (oldNodes[3] - oldNodes[0]).length() << std::endl;
91 tetra.
addPair(0, 1,
"back bottom rod");
92 tetra.
addPair(0, 4,
"back rightBottom rod");
93 tetra.
addPair(4, 2,
"back rightTop rod");
94 tetra.
addPair(0, 3,
"front right rod");
95 tetra.
addPair(1, 5,
"back leftBottom rod");
96 tetra.
addPair(5, 2,
"back leftTop rod");
97 tetra.
addPair(1, 3,
"front left rod");
98 tetra.
addPair(2, 3,
"front top rod");
105 const btVector3 offset(0, 0, edge);
106 for (
size_t i = 0; i < segmentCount; ++i)
111 t->addTags(
tgString(
"segment num", i ));
113 t->move((i + 1) * offset);
124 const std::vector<tgStructure*> children = snake.
getChildren();
125 for (
size_t i = 1; i < children.size(); ++i)
127 tgNodes n0 = children[i-1]->getNodes();
128 tgNodes n1 = children[i ]->getNodes();
140 void mapMuscles(TetraSpineCollisions::MuscleMap& muscleMap,
156 const std::vector<tgStructure*> children = structure.
getChildren();
157 tgNodes n0 = children[0]->getNodes();
160 btRigidBody* firstBody = model.getAllRigids()[0]->getPRigidBody();
162 std::vector<tgBaseRigid*> myRigids = model.getAllRigids();
164 for (
int i =0; i < myRigids.size(); i++)
166 std::cout << myRigids[i]->mass() <<
" " <<myRigids[i]->getPRigidBody() << std::endl;
170 abstractMarker marker1(firstBody, n0[3] - firstBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
172 model.addMarker(marker1);
174 tgNodes n1 = children[1]->getNodes();
176 btRigidBody* secondBody = model.getAllRigids()[15]->getPRigidBody();
178 abstractMarker marker2(secondBody, n1[3] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
180 model.addMarker(marker2);
182 abstractMarker marker3(secondBody, n1[1] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
184 model.addMarker(marker3);
186 abstractMarker marker4(secondBody, n1[0] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
188 model.addMarker(marker4);
190 tgNodes n2 = children[2]->getNodes();
192 btRigidBody* thirdBody = model.getAllRigids()[29]->getPRigidBody();
194 abstractMarker marker5(thirdBody, n2[3] - thirdBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
196 model.addMarker(marker5);
201 std::cout <<
"StructureInfo:" << std::endl
202 << structureInfo << std::endl
203 <<
"Model: " << std::endl
204 << model << std::endl;
206 const std::vector<tgSpringCableActuator*> outerMuscles =
208 for (
size_t i = 0; i < outerMuscles.size(); ++i)
211 assert(pMuscle != NULL);
212 std::cout <<
"Outer muscle: " << *pMuscle << std::endl;
222 const double edge = 3.8 * scaleFactor;
223 const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
224 std::cout <<
"edge: " << edge <<
"; height: " << height << std::endl;
228 addNodes(tetra, edge, height, scaleFactor);
233 tetra.move(btVector3(0.0, 8.0, 10.0));
237 addSegments(snake, tetra, -2.30 * scaleFactor, m_segments);
245 const double radius = 0.635 * scaleFactor / 10.0;
246 const double sphereRadius = 0.635 * scaleFactor / (10.0);
247 const double density = 2.0 *.0201 / (pow(radius, 2) * M_PI * edge);
248 const double friction = 0.5;
251 spec.addBuilder(
"rod",
new tgRodInfo(rodConfig));
254 const double sphereVolume1 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3);
255 const double sphereVolume2 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3);
257 const double baseCornerMidD = 180.0 / sphereVolume1;
258 const tgSphere::Config baseCornerMidConfig(sphereRadius, baseCornerMidD, friction);
259 spec.addBuilder(
"base",
new tgSphereInfo(baseCornerMidConfig));
261 const double tipCornerMidD = 120.0 / sphereVolume1;
262 const tgSphere::Config tipCornerMidConfig(sphereRadius, tipCornerMidD, friction);
263 spec.addBuilder(
"tip",
new tgSphereInfo(tipCornerMidConfig));
265 const double PCBD = 70.0 / sphereVolume2;
290 m_allMuscles = this->find<tgSpringCableActuator> (
"muscle");
291 m_allSegments = this->find<tgModel> (
"segment");
292 mapMuscles(m_muscleMap, *
this);
297 trace(structureInfo, *
this);
315 throw std::invalid_argument(
"dt is not positive");
const std::vector< tgStructure * > & getChildren() const
virtual void step(const double dt)
void addChild(tgStructure *child)
Definition of class tgRodInfo.
Definition of class tgSphereInfo.
Convenience function for combining strings with ints, mostly for naming structures.
Contains the definition of class tgSphere.
Utility class for class casting and filtering collections by type.
Markers for specific places on a tensegrity.
virtual void setup(tgWorld &world)
const tgNodes & getNodes() const
Definition of class tgBasicActuatorInfo.
void addPair(int fromNodeIdx, int toNodeIdx, std::string tags="")
Contains the definition of abstract base class tgSpringCableActuator. Assumes that the string is line...
virtual void setup(tgWorld &world)
std::string tgString(std::string s, int i)
Definition of class tgStructure.
std::vector< T * > find(const tgTagSearch &tagSearch)
Definition of class tgStructureInfo.
Middle segment of In Won's robot reconfigured with tgBulletContactSpringCable and more segments...
Rand seeding simular to the evolution and terrain classes.
Definition of class tgBuildSpec.
virtual void step(double dt)
void buildInto(tgModel &model, tgWorld &world)
void addNode(double x, double y, double z, std::string tags="")