Skip to content
Browse files

Log Position and Forces

  • Loading branch information...
1 parent bf0e1e7 commit 9f68bef286228aac48b309d0b5a3cd2fb96c95d0 Henry Herman committed
View
2 AllBulletDemos.xcodeproj/project.pbxproj
@@ -841,7 +841,6 @@
05A6BC9815658B80002ECBE2 /* hexapodController.py */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.script.python; path = hexapodController.py; sourceTree = "<group>"; };
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>"; };
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; };
@@ -1654,7 +1653,6 @@
05B1FE84156D6110002FD23F /* democmds.py */,
05B1FE7F1569DDE4002FD23F /* demo.py */,
05B1FE801569DDE4002FD23F /* hexapod.py */,
- 05B1FE811569DDE4002FD23F /* hexapod.pyc */,
);
path = hexapodcontroller;
sourceTree = "<group>";
View
54 hexapod/Hexapod.cpp
@@ -59,8 +59,10 @@ void debugCtrlParams(HpodCtrlParams params) {
//################## BEGIN HEXAPOD ######################//
-Hexapod::Hexapod (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,
- btScalar scale_hexapod) : m_ownerWorld (ownerWorld),
+
+
+Hexapod::Hexapod ( btDynamicsWorld* ownerWorld, const btVector3& positionOffset,
+ btScalar scale_hexapod,unsigned int _podid) : m_ownerWorld (ownerWorld),
BodyPart(ownerWorld),
m_ctrlParams(),
m_legs(),
@@ -69,18 +71,25 @@ Hexapod::Hexapod (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,
m_bodies(),
m_joints(),
m_leftLegs(),
- m_forces()
+ m_forces(),
+ m_replys()
{
-
+ podid = _podid;
param_idx = 0;
- HpodCtrlParams empty;
- m_ctrlParams.push_back(empty);
- m_ctrlParams.clear();
+ stepping = false;
+ //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();
+ //std::vector<HpodCtrlParams> m_ctrlParams;
+ //m_forces.push_back(btVector3(0.f,0.f,0.f));
+ //m_forces.clear();
+ //HpodReply empty_reply;
+ //std::vector<HpodReply> m_replys;
+ //m_replys.push_back(empty_reply);
+ //m_replys.clear();
+
btVector3 xaxis;
xaxis.setValue(btScalar(1.), btScalar(0.), btScalar(0.));
@@ -273,7 +282,7 @@ void Hexapod::step() {
//std::cout << "Array sz: " << m_ctrlParams.size() << std::endl;
static bool showComplete = true;
- if (param_idx==1) {
+ if (param_idx==0) {
showComplete=true;
}
@@ -284,14 +293,20 @@ void Hexapod::step() {
std::cout << "Stepping: "<< param_idx << std::endl;
setCtrlParams(m_ctrlParams[param_idx]);
debugCtrlParams(m_ctrlParams[param_idx]);
+ storeReply();
+ stepping=true;
param_idx++;
} else {
if(showComplete) {
std::cout << "Complete" << std::endl;
showComplete=false;
+ stepping=false;
}
}
+}
+bool Hexapod::isStepping() {
+ return stepping;
}
void Hexapod::getForces() {
@@ -300,6 +315,7 @@ void Hexapod::getForces() {
}
void Hexapod::reset() {
param_idx = 0;
+ m_replys.clear();
}
btVector3 Hexapod::getPosition() {
@@ -308,6 +324,22 @@ btVector3 Hexapod::getPosition() {
return pos;
}
+void Hexapod::storeReply() {
+ HpodReply reply;
+ reply.podid = podid;
+ btVector3 pos = getPosition();
+ reply.xpos = pos.x();
+ reply.ypos = pos.y();
+ reply.zpos = pos.z();
+
+ for(int i = 0; i<NUMLEGS; i++) {
+ reply.upperlegforce[i] = m_legs[i]->hip->getAppliedImpulse();
+ reply.lowerlegforce[i] = m_legs[i]->knee->getAppliedImpulse();
+ }
+ m_replys.push_back(reply);
+ std::cout<<"Store reply:"<<m_replys.size()<<std::endl;
+}
+
Hexapod::~Hexapod()
{
View
25 hexapod/Hexapod.h
@@ -37,6 +37,16 @@ class HpodCtrlParams {
btScalar dtHip;
};
+class HpodReply {
+ public:
+ unsigned int podid;
+ btScalar xpos;
+ btScalar ypos;
+ btScalar zpos;
+ btScalar upperlegforce[NUMLEGS];
+ btScalar lowerlegforce[NUMLEGS];
+};
+
void debugPos(btVector3 pos);
void debugCtrlParams(HpodCtrlParams params);
@@ -76,12 +86,17 @@ class Hexapod : public BodyPart
btAlignedObjectArray<btTypedConstraint*> m_joints;
unsigned int param_idx;
+
+ unsigned int podid;
+
+ bool stepping;
public:
- Hexapod (btDynamicsWorld* ownerWorld,
- const btVector3& positionOffset,
- btScalar scale_hexapod = btScalar(1.0));
+ Hexapod (btDynamicsWorld* ownerWorld,
+ const btVector3& positionOffset,
+ btScalar scale_hexapod = btScalar(1.0), unsigned int _podid=0);
+
~Hexapod ();
inline uint64_t legCount() {
@@ -91,6 +106,8 @@ class Hexapod : public BodyPart
btAlignedObjectArray<class Leg*> m_leftLegs;
btAlignedObjectArray<class Leg*> m_rightlegs;
std::vector<HpodCtrlParams> m_ctrlParams;
+ std::vector<HpodReply> m_replys;
+
btAlignedObjectArray<btVector3> m_forces;
@@ -100,7 +117,9 @@ class Hexapod : public BodyPart
void step();
void reset();
+ void storeReply();
void getForces();
+ bool isStepping();
btRigidBody* body;
void wake();
View
44 hexapod/HexapodSimulation.cpp
@@ -69,6 +69,7 @@ HexapodSimulationDemo::HexapodSimulationDemo(): zcontext(1), zsocket(zcontext, Z
zsocket.bind("tcp://*:5555");
state = RUN;
isDirty=false;
+ isRunningExp=false;
}
HexapodSimulationDemo::~HexapodSimulationDemo() {
@@ -121,7 +122,7 @@ void HexapodSimulationDemo::initPhysics()
localCreateRigidBody(btScalar(0.),groundTransform,groundShape);
}
- // Spawn one ragdoll
+ // Spawn one Hexapod
spawnHexapod();
clientResetScene();
@@ -129,7 +130,8 @@ void HexapodSimulationDemo::initPhysics()
void HexapodSimulationDemo::spawnHexapod(bool random)
{
- Hexapod* hexapod = new Hexapod (m_dynamicsWorld, btVector3 (0,2,1),5.f);
+ unsigned int podid =m_hexapods.size();
+ Hexapod* hexapod = new Hexapod (m_dynamicsWorld, btVector3 (0,2,1),5.f, podid);
m_hexapods.push_back(hexapod);
}
@@ -172,11 +174,23 @@ void HexapodSimulationDemo::zmqRecv() {
debugCtrlParams(*params);
#endif
+
// Send reply back
try
{
- zmq::message_t reply(3);
- memcpy((void *) reply.data (), "ACK", 3);
+ zmq::message_t reply;
+ if (ctrlParam==SIMCHKBUSY) {
+ if(isBusySim()) {
+ reply.rebuild(4);
+ memcpy((void *) reply.data (), "YES", 4);
+ } else {
+ reply.rebuild(3);
+ memcpy((void *) reply.data (), "NO", 3);
+ }
+ } else {
+ reply.rebuild(4);
+ memcpy((void *) reply.data (), "ACK", 4);
+ }
zsocket.send(reply);
}
catch (zmq::error_t e)
@@ -362,7 +376,10 @@ void HexapodSimulationDemo::processCommand(HpodSimCtrl cmd,HpodCtrlParams *param
#endif
clientResetScene();
state=RESETEXP;
-
+ break;
+ case SIMCHKBUSY:
+ cout << "CMD:CHKBUSY"<<endl;
+ //Do NOTHING Dealt with in zmqRec
default:
break;
}
@@ -401,7 +418,7 @@ void HexapodSimulationDemo::setMotorTargets(btScalar deltaTime)
hpod->setCtrlParams(immParams);
}
break;
-
+ case CHKBUSY:
case RUN:
default:
break;
@@ -413,16 +430,17 @@ void HexapodSimulationDemo::setMotorTargets(btScalar deltaTime)
debugPos(hpod->getPosition());
}
- /*
- for (int r=0; r<m_hexapods.size(); r++)
- {
- Hexapod *hpod = m_hexapods[r];
- hpod->reset();
- }
- */
//Set motor shiznit here
}
+bool HexapodSimulationDemo::isBusySim() {
+ for (int r=0; r<m_hexapods.size(); r++) {
+ Hexapod* hpod=m_hexapods[r];
+ if(hpod->isStepping())
+ return true;
+ }
+ return false;
+}
void motorPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
{
View
9 hexapod/HexapodSimulation.h
@@ -43,6 +43,7 @@ using namespace std;
#define SIMLOADIMM 5
#define SIMRUNEXP 6
#define SIMRESETEXP 7
+#define SIMCHKBUSY 8
#define DEBUG_CMD
//#define DEBUG_HPOD_CTRL_PARAMS
@@ -79,9 +80,11 @@ class HexapodSimulationDemo : public PlatformDemoApplication
RUNEXP,
RUNIMM,
START,
- RESETEXP
- } state;
+ RESETEXP,
+ CHKBUSY
+ } state, previousState;
+ bool isRunningExp;
bool isDirty;
@@ -107,6 +110,8 @@ class HexapodSimulationDemo : public PlatformDemoApplication
void zmqRecv();
+ bool isBusySim();
+
btAlignedObjectArray<HpodCtrlParams> m_ctrlParams;
private:
View
7 hexapod/Leg.cpp
@@ -39,6 +39,7 @@ Leg::Leg (Hexapod *hexapod, btDynamicsWorld* ownerWorld, const btTransform& offs
// Setup the geometry
m_shapes[LEG_UPPER] = new btCapsuleShape(btScalar(scale_hexapod*UPPER_LEG_THICK), btScalar(scale_hexapod*UPPER_LEG_LENGTH));
m_shapes[LEG_LOWER] = new btCapsuleShape(btScalar(scale_hexapod*LOWER_LEG_THICK), btScalar(scale_hexapod*LOWER_LEG_LENGTH));
+
// Setup all the rigid bodies
@@ -49,11 +50,12 @@ Leg::Leg (Hexapod *hexapod, btDynamicsWorld* ownerWorld, const btTransform& offs
//transform.setOrigin(btVector3(btScalar(-0.18*scale_hexapod), btScalar(0.65*scale_hexapod), btScalar(0.)));
m_bodies[LEG_UPPER] = localCreateRigidBody(btScalar(1.), globalFrame*transform, m_shapes[LEG_UPPER]);
+ upperleg = m_bodies[LEG_UPPER];
transform.setIdentity();
transform.setOrigin(btVector3(btScalar(0*scale_hexapod), btScalar((-1)*LOWER_LEG_LENGTH*scale_hexapod), btScalar(0.)));
m_bodies[LEG_LOWER] = localCreateRigidBody(btScalar(1.), globalFrame*transform, m_shapes[LEG_LOWER]);
-
+ lowerleg = m_bodies[LEG_LOWER];
// Setup some damping on the m_bodies
for (int i = 0; i < LEG_COUNT; ++i)
@@ -92,7 +94,7 @@ Leg::Leg (Hexapod *hexapod, btDynamicsWorld* ownerWorld, const btTransform& offs
localA, localB);
hingeC->setLimit(-SIMD_EPSILON, SIMD_PI*0.7f);
-
+ hingeC->enableFeedback(true);
m_joints.push_back(hingeC);
knee = hingeC;
knee->setDbgDrawSize(btScalar(1.f));
@@ -127,6 +129,7 @@ Leg::Leg (Hexapod *hexapod, btDynamicsWorld* ownerWorld, const btTransform& offs
//m_joints[JOINT_HIP] = coneC;
m_ownerWorld->addConstraint(coneC, true);
coneC->setDbgDrawSize(btScalar(1.f));
+ coneC->enableFeedback(true);
hip = coneC;
}
View
3 hexapod/Leg.h
@@ -63,9 +63,12 @@ class Leg : public BodyPart
btHingeConstraint* knee;
btConeTwistConstraint* hip;
+ btRigidBody *upperleg;
+ btRigidBody *lowerleg;
//btTypedConstraint* m_joints[JOINT_COUNT];
btAlignedObjectArray<btTypedConstraint *> m_joints;
//btTypedConstraint* m_joints[JOINT_COUNT];
+
~Leg ();
void setKneeTarget(const btQuaternion& targetAngleQ, btScalar dt);
View
13 hexapod/hexapodcontroller/demoarr.py
@@ -13,7 +13,7 @@
pod.dtKnee = 0.1
pod.dtHip = 0.1
-t = np.arange(0,25,0.01)
+t = np.arange(0,5,0.01)
w = 1
xs = abs(np.sin(2*np.pi*t/5))
pod.setControl(HpodSimCtrlParam.RESETEXP)
@@ -41,9 +41,14 @@
pod.send()
pod.clearParamArray()
#time.sleep(10)
- #pod.setControl(HpodSimCtrlParam.PAUSE)
- #pod.send()
-
+ s = ""
+ time.sleep(1)
+ while s!="NO":
+ pod.setControl(HpodSimCtrlParam.CHKBUSY)
+ s = pod.send()
+ s = s[:-1]
+ time.sleep(1)
+ print "RECV: %s" % s
print "Sent %d packets" % len(xs)
except (KeyboardInterrupt,):
View
7 hexapod/hexapodcontroller/hexapod.py
@@ -16,10 +16,11 @@ class HpodSimCtrlParam(c_uint):
LOADIMM=5
RUNEXP=6
RESETEXP=7
+ CHKBUSY=8
OPTS = (PAUSE, RESET,
CONTINUE, START,
LOAD,LOADIMM,
- RUNEXP, RESETEXP)
+ RUNEXP, RESETEXP,CHKBUSY)
class HpodCtrlParams(Structure):
@@ -190,13 +191,17 @@ def send(self):
self.msg.append(self.sendString)
self.socket.send_multipart(self.msg)
message = self.socket.recv()
+ #print "Received: %s" % str(message)
self.msg = []
+ return message
def sendArray(self):
self.msg.append(self.sendStringArray)
self.socket.send_multipart(self.msg)
message = self.socket.recv()
+ #print "Received: %s" % str(message)
self.msg = []
+ return message
def addParam(self):
self.loadParam()

0 comments on commit 9f68bef

Please sign in to comment.
Something went wrong with that request. Please try again.