Permalink
Browse files

Add output global pos output

  • Loading branch information...
1 parent 2656619 commit bf0e1e7361a7f13163350f1dc628a3caacd7c078 Henry Herman committed May 23, 2012
@@ -842,7 +842,8 @@
05B1FE7F1569DDE4002FD23F /* demo.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = demo.py; sourceTree = "<group>"; };
05B1FE801569DDE4002FD23F /* hexapod.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = hexapod.py; sourceTree = "<group>"; };
05B1FE811569DDE4002FD23F /* hexapod.pyc */ = {isa = PBXFileReference; lastKnownFileType = file; path = hexapod.pyc; sourceTree = "<group>"; };
- 05B1FE821569DDE4002FD23F /* test.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = test.py; sourceTree = "<group>"; };
+ 05B1FE83156D6108002FD23F /* demoarr.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = demoarr.py; sourceTree = "<group>"; };
+ 05B1FE84156D6110002FD23F /* democmds.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = democmds.py; sourceTree = "<group>"; };
05BFD5EC15531B3400D38F23 /* DynamicControlDemo */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = DynamicControlDemo; sourceTree = BUILT_PRODUCTS_DIR; };
05BFD5F815531B6400D38F23 /* DynamicControlDemo.1 */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.man; path = DynamicControlDemo.1; sourceTree = "<group>"; };
05BFD5F915531B6400D38F23 /* main.cpp */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.cpp; path = main.cpp; sourceTree = "<group>"; };
@@ -1649,10 +1650,11 @@
05B1FE7E1569DDE4002FD23F /* hexapodcontroller */ = {
isa = PBXGroup;
children = (
+ 05B1FE83156D6108002FD23F /* demoarr.py */,
+ 05B1FE84156D6110002FD23F /* democmds.py */,
05B1FE7F1569DDE4002FD23F /* demo.py */,
05B1FE801569DDE4002FD23F /* hexapod.py */,
05B1FE811569DDE4002FD23F /* hexapod.pyc */,
- 05B1FE821569DDE4002FD23F /* test.py */,
);
path = hexapodcontroller;
sourceTree = "<group>";
View
@@ -24,6 +24,9 @@
#define SIMD_PI_2 ((SIMD_PI)*0.5f)
#define SIMD_PI_4 ((SIMD_PI)*0.25f)
+void debugPos(btVector3 pos) {
+ std::cout << "Pos:X<" << pos.x() << ">, Y<" << pos.y() << ">, Z<" << pos.z() << ">" << std::endl;
+}
void debugCtrlParams(HpodCtrlParams params) {
std::cout << "Knee Angles: ";
@@ -65,15 +68,20 @@ Hexapod::Hexapod (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,
m_shapes(),
m_bodies(),
m_joints(),
- m_leftLegs()
+ m_leftLegs(),
+ m_forces()
{
param_idx = 0;
- HpodCtrlParams empty;
- std::vector<HpodCtrlParams> m_ctrlParams;
+ HpodCtrlParams empty;
m_ctrlParams.push_back(empty);
m_ctrlParams.clear();
+ std::vector<HpodCtrlParams> m_ctrlParams;
+ m_forces.push_back(btVector3(0.f,0.f,0.f));
+ m_forces.clear();
+
+
btVector3 xaxis;
xaxis.setValue(btScalar(1.), btScalar(0.), btScalar(0.));
@@ -286,10 +294,20 @@ void Hexapod::step() {
}
+void Hexapod::getForces() {
+
+
+}
void Hexapod::reset() {
param_idx = 0;
}
+btVector3 Hexapod::getPosition() {
+ btTransform temp = body->getWorldTransform();
+ btVector3 pos = temp.getOrigin();
+ return pos;
+}
+
Hexapod::~Hexapod()
{
View
@@ -37,6 +37,7 @@ class HpodCtrlParams {
btScalar dtHip;
};
+void debugPos(btVector3 pos);
void debugCtrlParams(HpodCtrlParams params);
class Hexapod : public BodyPart
@@ -72,7 +73,7 @@ class Hexapod : public BodyPart
btRigidBody* m_bodies[BODYPART_COUNT];
- btTypedConstraint* m_joints[JOINT_COUNT];
+ btAlignedObjectArray<btTypedConstraint*> m_joints;
unsigned int param_idx;
@@ -91,19 +92,22 @@ class Hexapod : public BodyPart
btAlignedObjectArray<class Leg*> m_rightlegs;
std::vector<HpodCtrlParams> m_ctrlParams;
+ btAlignedObjectArray<btVector3> m_forces;
+
void loadCtrlParams(HpodCtrlParams *params, unsigned long size);
void clearCtrlParams();
void step();
void reset();
+ void getForces();
+
btRigidBody* body;
void wake();
void setCtrlParams(const HpodCtrlParams params);
void getCtrlParams(HpodCtrlParams &params);
+ btVector3 getPosition();
-
-
};
#endif // HEXAPOD_H_INCLUDED
@@ -407,6 +407,12 @@ void HexapodSimulationDemo::setMotorTargets(btScalar deltaTime)
break;
}
isDirty=false;
+
+ for (int r=0; r<m_hexapods.size(); r++) {
+ Hexapod* hpod=m_hexapods[r];
+ debugPos(hpod->getPosition());
+ }
+
/*
for (int r=0; r<m_hexapods.size(); r++)
{
View
@@ -93,10 +93,10 @@ Leg::Leg (Hexapod *hexapod, btDynamicsWorld* ownerWorld, const btTransform& offs
hingeC->setLimit(-SIMD_EPSILON, SIMD_PI*0.7f);
- m_joints[JOINT_KNEE] = hingeC;
+ m_joints.push_back(hingeC);
knee = hingeC;
knee->setDbgDrawSize(btScalar(1.f));
- m_ownerWorld->addConstraint(m_joints[JOINT_KNEE], true);
+ m_ownerWorld->addConstraint(knee, true);
}
/// *************************** ///
@@ -123,9 +123,10 @@ Leg::Leg (Hexapod *hexapod, btDynamicsWorld* ownerWorld, const btTransform& offs
coneC = new btConeTwistConstraint(*m_bodies[LEG_UPPER], *parentBody, localA, localB);
coneC->setLimit(btScalar(SIMD_PI*0.4f), btScalar(SIMD_PI*0.3f), btScalar(0), 0.3f);
- m_joints[JOINT_HIP] = coneC;
- m_ownerWorld->addConstraint(m_joints[JOINT_HIP], true);
- m_joints[JOINT_HIP]->setDbgDrawSize(btScalar(1.f));
+ m_joints.push_back(coneC);
+ //m_joints[JOINT_HIP] = coneC;
+ m_ownerWorld->addConstraint(coneC, true);
+ coneC->setDbgDrawSize(btScalar(1.f));
hip = coneC;
}
@@ -193,16 +194,18 @@ void Leg::wake() {
m_bodies[LEG_LOWER]->activate();
}
+
Leg::~Leg()
{
int i;
- // Remove all constraints
- for (i = 0; i < JOINT_COUNT; ++i)
+ //Remove all constraints
+ for (i = 0; i < m_joints.size(); ++i)
{
m_ownerWorld->removeConstraint(m_joints[i]);
- delete m_joints[i]; m_joints[i] = 0;
+ delete m_joints[i];
}
+ //m_joints.clear();
// Remove all bodies and shapes
for (i = 0; i < LEG_COUNT; ++i)
View
@@ -63,7 +63,10 @@ class Leg : public BodyPart
btHingeConstraint* knee;
btConeTwistConstraint* hip;
- btTypedConstraint* m_joints[JOINT_COUNT];
+ //btTypedConstraint* m_joints[JOINT_COUNT];
+ btAlignedObjectArray<btTypedConstraint *> m_joints;
+ //btTypedConstraint* m_joints[JOINT_COUNT];
+
~Leg ();
void setKneeTarget(const btQuaternion& targetAngleQ, btScalar dt);
void setKneeTarget(const btScalar targetAngle, btScalar dt);
@@ -20,6 +20,7 @@
pod.send()
pod.setControl(HpodSimCtrlParam.CONTINUE)
pod.send()
+
pod.setControl(HpodSimCtrlParam.LOAD)
@@ -31,7 +32,6 @@
leg.knee.angle=x*2
leg.hip.yangle=2*x-1
leg.hip.xangle=2*x-1
- print pod.rightRearLeg.knee.angle
pod.addParam()
print "Add Param %f" % x
print "Array Built... sending"

0 comments on commit bf0e1e7

Please sign in to comment.