37 #include "LinearMath/btVector3.h"
51 TetraSpineCollisions::TetraSpineCollisions(
size_t segments) :
56 TetraSpineCollisions::~TetraSpineCollisions()
61 void addNodes(
tgStructure& tetra,
double edge,
double height)
65 tetra.
addNode(-edge / 2.0, 0, 0,
"base");
67 tetra.
addNode( edge / 2.0, 0, 0,
"base");
69 tetra.
addNode(0, height, 0,
"base");
71 tetra.
addNode(0, edge / 2.0 * tan(M_PI / 6.0), 2.50 * 5.0,
"tip");
75 tgNode nn1 = (oldNodes[0] + oldNodes[2])/2.0;
78 tgNode nn2 = (oldNodes[1] + oldNodes[2])/2.0;
82 std::cout << (oldNodes[3] - oldNodes[2]).length() << std::endl;
83 std::cout << (oldNodes[3] - oldNodes[1]).length() << std::endl;
84 std::cout << (oldNodes[3] - oldNodes[0]).length() << std::endl;
90 tetra.
addPair(0, 1,
"back bottom rod");
91 tetra.
addPair(0, 4,
"back rightBottom rod");
92 tetra.
addPair(4, 2,
"back rightTop rod");
93 tetra.
addPair(0, 3,
"front right rod");
94 tetra.
addPair(1, 5,
"back leftBottom rod");
95 tetra.
addPair(5, 2,
"back leftTop rod");
96 tetra.
addPair(1, 3,
"front left rod");
97 tetra.
addPair(2, 3,
"front top rod");
104 const btVector3 offset(0, 0, -2.30 * 5.0);
105 for (
size_t i = 0; i < segmentCount; ++i)
110 t->addTags(
tgString(
"segment num", i ));
112 t->move((i + 1) * offset);
123 const std::vector<tgStructure*> children = snake.
getChildren();
124 for (
size_t i = 1; i < children.size(); ++i)
126 tgNodes n0 = children[i-1]->getNodes();
127 tgNodes n1 = children[i ]->getNodes();
139 void mapMuscles(TetraSpineCollisions::MuscleMap& muscleMap,
155 const std::vector<tgStructure*> children = structure.
getChildren();
156 tgNodes n0 = children[0]->getNodes();
159 btRigidBody* firstBody = model.getAllRigids()[0]->getPRigidBody();
161 std::vector<tgBaseRigid*> myRigids = model.getAllRigids();
163 for (
int i =0; i < myRigids.size(); i++)
165 std::cout << myRigids[i]->mass() <<
" " <<myRigids[i]->getPRigidBody() << std::endl;
169 abstractMarker marker1(firstBody, n0[3] - firstBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
171 model.addMarker(marker1);
173 tgNodes n1 = children[1]->getNodes();
175 btRigidBody* secondBody = model.getAllRigids()[15]->getPRigidBody();
177 abstractMarker marker2(secondBody, n1[3] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
179 model.addMarker(marker2);
181 abstractMarker marker3(secondBody, n1[1] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
183 model.addMarker(marker3);
185 abstractMarker marker4(secondBody, n1[0] - secondBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
187 model.addMarker(marker4);
189 tgNodes n2 = children[2]->getNodes();
191 btRigidBody* thirdBody = model.getAllRigids()[29]->getPRigidBody();
193 abstractMarker marker5(thirdBody, n2[3] - thirdBody->getCenterOfMassPosition (), btVector3(1, 0, 0), 0);
195 model.addMarker(marker5);
200 std::cout <<
"StructureInfo:" << std::endl
201 << structureInfo << std::endl
202 <<
"Model: " << std::endl
203 << model << std::endl;
205 const std::vector<tgSpringCableActuator*> outerMuscles =
207 for (
size_t i = 0; i < outerMuscles.size(); ++i)
210 assert(pMuscle != NULL);
211 std::cout <<
"Outer muscle: " << *pMuscle << std::endl;
221 const double edge = 3.8 * 5.0;
222 const double height = tgUtil::round(std::sqrt(3.0)/2 * edge);
223 std::cout <<
"edge: " << edge <<
"; height: " << height << std::endl;
227 addNodes(tetra, edge, height);
232 tetra.move(btVector3(0.0, 8.0, 10.0));
236 addSegments(snake, tetra, edge, m_segments);
244 const double radius = 0.635 / 2.0;
245 const double sphereRadius = 0.635 / 2.0;
246 const double density = .0201 / (pow(radius, 2) * M_PI * edge);
247 const double friction = 0.5;
250 spec.addBuilder(
"rod",
new tgRodInfo(rodConfig));
253 const double sphereVolume1 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3);
254 const double sphereVolume2 = 1000.0 * 4.0 / 3.0 * M_PI * pow(sphereRadius, 3);
256 const double baseCornerMidD = 180.0 / sphereVolume1;
257 const tgSphere::Config baseCornerMidConfig(sphereRadius, baseCornerMidD, friction);
258 spec.addBuilder(
"base",
new tgSphereInfo(baseCornerMidConfig));
260 const double tipCornerMidD = 120.0 / sphereVolume1;
261 const tgSphere::Config tipCornerMidConfig(sphereRadius, tipCornerMidD, friction);
262 spec.addBuilder(
"tip",
new tgSphereInfo(tipCornerMidConfig));
264 const double PCBD = 70.0 / sphereVolume2;
285 structureInfo.buildInto(*
this, world);
289 m_allMuscles = this->find<tgSpringCableActuator> (
"muscle");
290 m_allSegments = this->find<tgModel> (
"segment");
291 mapMuscles(m_muscleMap, *
this);
293 addMarkers(snake, *
this);
296 trace(structureInfo, *
this);
314 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...
Contains the definition of class tgUtil and overloaded operator<<() free functions.
Definition of class tgBuildSpec.
virtual void step(double dt)
void addNode(double x, double y, double z, std::string tags="")