diff --git a/examples/Utils/RobotLoggingUtil.cpp b/examples/Utils/RobotLoggingUtil.cpp new file mode 100644 index 000000000..0499a5aa9 --- /dev/null +++ b/examples/Utils/RobotLoggingUtil.cpp @@ -0,0 +1,253 @@ +#include "RobotLoggingUtil.h" +#include +#include "LinearMath/btAlignedObjectArray.h" + +#include "../Importers/ImportURDFDemo/urdfStringSplit.h" + + +static bool readLine(FILE* file, btAlignedObjectArray& line) +{ + int c = 0; + for (c=fgetc(file);(c != EOF && c != '\n');c=fgetc(file)) + { + line.push_back(c); + } + line.push_back(0); + return (c == EOF); +} + + +int readMinitaurLogFile(const char* fileName, btAlignedObjectArray& structNames, std::string& structTypes, btAlignedObjectArray& logRecords, bool verbose) +{ + + int retVal = 0; + + FILE* f = fopen(fileName,"rb"); + if (f) + { + if (verbose) + { + printf("Opened file %s\n", fileName); + } + btAlignedObjectArray line0Buf; + bool eof = readLine(f,line0Buf); + btAlignedObjectArray line1Buf; + eof |= readLine(f,line1Buf); + std::string line0 = &line0Buf[0]; + structTypes = &line1Buf[0]; + + btAlignedObjectArray separators; + separators.push_back(","); + + urdfStringSplit(structNames,line0,separators); + if (verbose) + { + printf("Num Fields = %d\n",structNames.size()); + } + btAssert(structTypes.size() == structNames.size()); + if (structTypes.size() != structNames.size()) + { + retVal = eCorruptHeader; + } + int numStructsRead = 0; + + if (structTypes.size() == structNames.size()) + { + while (!eof) + { + unsigned char blaat[1024]; + size_t s = fread(blaat,2,1,f); + if (s!=1) + { + eof=true; + retVal = eInvalidAABBAlignCheck; + break; + } + if ((blaat[0] != 0xaa) || (blaat[1] != 0xbb)) + { + if (verbose) + { + printf("Expected 0xaa0xbb, terminating\n"); + } + } + + if (verbose) + { + printf("Reading structure %d\n",numStructsRead); + } + MinitaurLogRecord record; + + for (int i=0;i& structNames, std::string& structTypes) +{ + FILE* f = fopen(fileName,"wb"); + if (f) + { + for (int i=0;i + +struct MinitaurLogValue +{ + MinitaurLogValue() + :m_intVal(0xcdcdcdcd) + { + } + MinitaurLogValue(int iv) + :m_intVal(iv) + { + } + MinitaurLogValue(float fv) + :m_floatVal(fv) + { + } + MinitaurLogValue(char fv) + :m_charVal(fv) + { + } + + union + { + char m_charVal; + int m_intVal; + float m_floatVal; + }; +}; + +struct MinitaurLogRecord +{ + btAlignedObjectArray m_values; +}; + +enum MINITAUR_LOG_ERROR +{ + eMinitaurFileNotFound = -1, + eCorruptHeader = -2, + eUnknownType = -3, + eCorruptValue = -4, + eInvalidAABBAlignCheck = -5, +}; + +int readMinitaurLogFile(const char* fileName, btAlignedObjectArray& structNames, std::string& structTypes, btAlignedObjectArray& logRecords, bool verbose); + +FILE* createMinitaurLogFile(const char* fileName, btAlignedObjectArray& structNames, std::string& structTypes); +void appendMinitaurLogData(FILE* f, std::string& structTypes, const MinitaurLogRecord& logData); +void closeMinitaurLogFile(FILE* f); + +#endif //ROBOT_LOGGING_UTIL_H diff --git a/examples/pybullet/rollPitchYaw.py b/examples/pybullet/rollPitchYaw.py new file mode 100644 index 000000000..d807fb845 --- /dev/null +++ b/examples/pybullet/rollPitchYaw.py @@ -0,0 +1,26 @@ +import pybullet as p +import time + +cid = p.connect(p.SHARED_MEMORY) +if (cid<0): + p.connect(p.GUI) +q = p.loadURDF("quadruped/quadruped.urdf",useFixedBase=True) +rollId = p.addUserDebugParameter("roll",-1.5,1.5,0) +pitchId = p.addUserDebugParameter("pitch",-1.5,1.5,0) +yawId = p.addUserDebugParameter("yaw",-1.5,1.5,0) +fwdxId = p.addUserDebugParameter("fwd_x",-1,1,0) +fwdyId = p.addUserDebugParameter("fwd_y",-1,1,0) +fwdzId = p.addUserDebugParameter("fwd_z",-1,1,0) + +while True: + roll = p.readUserDebugParameter(rollId) + pitch = p.readUserDebugParameter(pitchId) + yaw = p.readUserDebugParameter(yawId) + x = p.readUserDebugParameter(fwdxId) + y = p.readUserDebugParameter(fwdyId) + z = p.readUserDebugParameter(fwdzId) + + orn = p.getQuaternionFromEuler([roll,pitch,yaw]) + p.resetBasePositionAndOrientation(q,[x,y,z],orn) + #p.stepSimulation()#not really necessary for this demo, no physics used +