From a3c1fec1713a5e258c051374c50f86d9efbc618c Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Fri, 17 Feb 2017 17:41:57 -0800 Subject: [PATCH] Add logging for generic robot and an example of logging state of kuka and cubes. --- .../PhysicsServerCommandProcessor.cpp | 168 ++++++++++++++++++ examples/pybullet/kuka_with_cube.py | 92 ++++++++++ examples/pybullet/kuka_with_cube_playback.py | 77 ++++++++ 3 files changed, 337 insertions(+) create mode 100644 examples/pybullet/kuka_with_cube.py create mode 100644 examples/pybullet/kuka_with_cube_playback.py diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0f7139257..99d33be1d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -558,6 +558,163 @@ struct MinitaurStateLogger : public InternalStateLogger } }; +struct GenericRobotStateLogger : public InternalStateLogger +{ + float m_loggingTimeStamp; + std::string m_fileName; + FILE* m_logFileHandle; + std::string m_structTypes; + btMultiBodyDynamicsWorld* m_dynamicsWorld; + + GenericRobotStateLogger(int loggingUniqueId, std::string fileName, btMultiBodyDynamicsWorld* dynamicsWorld) + :m_loggingTimeStamp(0), + m_logFileHandle(0), + m_dynamicsWorld(dynamicsWorld) + { + m_loggingType = STATE_LOGGING_GENERIC_ROBOT; + + btAlignedObjectArray structNames; + structNames.push_back("timeStamp"); + structNames.push_back("objectId"); + structNames.push_back("posX"); + structNames.push_back("posY"); + structNames.push_back("posZ"); + structNames.push_back("oriX"); + structNames.push_back("oriY"); + structNames.push_back("oriZ"); + structNames.push_back("oriW"); + structNames.push_back("velX"); + structNames.push_back("velY"); + structNames.push_back("velZ"); + structNames.push_back("omegaX"); + structNames.push_back("omegaY"); + structNames.push_back("omegaZ"); + structNames.push_back("qNum"); + structNames.push_back("q0"); + structNames.push_back("q1"); + structNames.push_back("q2"); + structNames.push_back("q3"); + structNames.push_back("q4"); + structNames.push_back("q5"); + structNames.push_back("q6"); + structNames.push_back("q7"); + structNames.push_back("q8"); + structNames.push_back("q9"); + structNames.push_back("q10"); + structNames.push_back("q11"); + structNames.push_back("u0"); + structNames.push_back("u1"); + structNames.push_back("u2"); + structNames.push_back("u3"); + structNames.push_back("u4"); + structNames.push_back("u5"); + structNames.push_back("u6"); + structNames.push_back("u7"); + structNames.push_back("u8"); + structNames.push_back("u9"); + structNames.push_back("u10"); + structNames.push_back("u11"); + + m_structTypes = "fIfffffffffffffIffffffffffffffffffffffff"; + const char* fileNameC = fileName.c_str(); + + m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes); + } + virtual void stop() + { + if (m_logFileHandle) + { + closeMinitaurLogFile(m_logFileHandle); + m_logFileHandle = 0; + } + } + + virtual void logState(btScalar timeStep) + { + if (m_logFileHandle) + { + for (int i=0;igetNumMultibodies();i++) + { + MinitaurLogRecord logData; + logData.m_values.push_back(m_loggingTimeStamp); + + btMultiBody* mb = m_dynamicsWorld->getMultiBody(i); + btVector3 pos = mb->getBasePos(); + btQuaternion ori = mb->getWorldToBaseRot(); + btVector3 vel = mb->getBaseVel(); + btVector3 omega = mb->getBaseOmega(); + + int objectUniqueId = mb->getUserIndex2(); + float posX = pos[0]; + float posY = pos[1]; + float posZ = pos[2]; + float oriX = ori.x(); + float oriY = ori.y(); + float oriZ = ori.z(); + float oriW = ori.w(); + float velX = vel[0]; + float velY = vel[1]; + float velZ = vel[2]; + float omegaX = omega[0]; + float omegaY = omega[1]; + float omegaZ = omega[2]; + + logData.m_values.push_back(objectUniqueId); + logData.m_values.push_back(posX); + logData.m_values.push_back(posY); + logData.m_values.push_back(posZ); + logData.m_values.push_back(oriX); + logData.m_values.push_back(oriY); + logData.m_values.push_back(oriZ); + logData.m_values.push_back(oriW); + logData.m_values.push_back(velX); + logData.m_values.push_back(velY); + logData.m_values.push_back(velZ); + logData.m_values.push_back(omegaX); + logData.m_values.push_back(omegaY); + logData.m_values.push_back(omegaZ); + + int numDofs = mb->getNumDofs(); + logData.m_values.push_back(numDofs); + + for (int j = 0; j < 12; ++j) + { + if (j < numDofs) + { + float q = mb->getJointPos(j); + logData.m_values.push_back(q); + } + else + { + float q = 0.0; + logData.m_values.push_back(q); + } + } + + for (int j = 0; j < 12; ++j) + { + if (j < numDofs) + { + float u = mb->getJointVel(j); + logData.m_values.push_back(u); + } + else + { + float u = 0.0; + logData.m_values.push_back(u); + } + } + + //at the moment, appendMinitaurLogData will directly write to disk (potential delay) + //better to fill a huge memory buffer and once in a while write it to disk + appendMinitaurLogData(m_logFileHandle, m_structTypes, logData); + fflush(m_logFileHandle); + } + + m_loggingTimeStamp++; + } + } +}; struct PhysicsServerCommandProcessorInternalData { @@ -1683,6 +1840,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } + if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_GENERIC_ROBOT) + { + + std::string fileName = clientCmd.m_stateLoggingArguments.m_fileName; + + int loggerUid = m_data->m_stateLoggersUniqueId++; + GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld); + m_data->m_stateLoggers.push_back(logger); + serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED; + serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid; + } } if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId>=0) { diff --git a/examples/pybullet/kuka_with_cube.py b/examples/pybullet/kuka_with_cube.py new file mode 100644 index 000000000..3820ca5a3 --- /dev/null +++ b/examples/pybullet/kuka_with_cube.py @@ -0,0 +1,92 @@ +import pybullet as p +import time +import math +from datetime import datetime + +#clid = p.connect(p.SHARED_MEMORY) +p.connect(p.GUI) +p.loadURDF("plane.urdf",[0,0,-0.3],useFixedBase=True) +kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0],useFixedBase=True) +p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1]) +kukaEndEffectorIndex = 6 +numJoints = p.getNumJoints(kukaId) +if (numJoints!=7): + exit() + +p.loadURDF("cube.urdf",[2,2,5]) +p.loadURDF("cube.urdf",[-2,-2,5]) +p.loadURDF("cube.urdf",[2,-2,5]) + +#lower limits for null space +ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] +#upper limits for null space +ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] +#joint ranges for null space +jr=[5.8,4,5.8,4,5.8,4,6] +#restposes for null space +rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] +#joint damping coefficents +jd=[0.1,0.1,0.1,0.1,0.1,0.1,0.1] + +for i in range (numJoints): + p.resetJointState(kukaId,i,rp[i]) + +p.setGravity(0,0,-10) +t=0. +prevPose=[0,0,0] +prevPose1=[0,0,0] +hasPrevPose = 0 +useNullSpace = 0 + +count = 0 +useOrientation = 1 +useSimulation = 1 +useRealTimeSimulation = 1 +p.setRealTimeSimulation(useRealTimeSimulation) +#trailDuration is duration (in seconds) after debug lines will be removed automatically +#use 0 for no-removal +trailDuration = 15 + +logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt") + +while 1: + if (useRealTimeSimulation): + dt = datetime.now() + t = (dt.second/60.)*2.*math.pi + else: + t=t+0.1 + + if (useSimulation and useRealTimeSimulation==0): + p.stepSimulation() + + for i in range (1): + pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)] + #end effector points down, not up (in case useOrientation==1) + orn = p.getQuaternionFromEuler([0,-math.pi,0]) + + if (useNullSpace==1): + if (useOrientation==1): + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp) + else: + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) + else: + if (useOrientation==1): + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd) + else: + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos) + + if (useSimulation): + for i in range (numJoints): + p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1) + else: + #reset the joint state (ignoring all dynamics, not recommended to use during simulation) + for i in range (numJoints): + p.resetJointState(kukaId,i,jointPoses[i]) + + ls = p.getLinkState(kukaId,kukaEndEffectorIndex) + if (hasPrevPose): + p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration) + p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) + prevPose=pos + prevPose1=ls[4] + hasPrevPose = 1 diff --git a/examples/pybullet/kuka_with_cube_playback.py b/examples/pybullet/kuka_with_cube_playback.py new file mode 100644 index 000000000..5b3aade9b --- /dev/null +++ b/examples/pybullet/kuka_with_cube_playback.py @@ -0,0 +1,77 @@ +import pybullet as p +import time +import math +from datetime import datetime +from numpy import * +from pylab import * +import struct +import sys +import os, fnmatch +import argparse +from time import sleep + +def readLogFile(filename, verbose = True): + f = open(filename, 'rb') + + print('Opened'), + print(filename) + + keys = f.readline().rstrip('\n').split(',') + fmt = f.readline().rstrip('\n') + + # The byte number of one record + sz = struct.calcsize(fmt) + # The type number of one record + ncols = len(fmt) + + if verbose: + print('Keys:'), + print(keys) + print('Format:'), + print(fmt) + print('Size:'), + print(sz) + print('Columns:'), + print(ncols) + + # Read data + wholeFile = f.read() + # split by alignment word + chunks = wholeFile.split('\xaa\xbb') + log = list() + for chunk in chunks: + if len(chunk) == sz: + values = struct.unpack(fmt, chunk) + record = list() + for i in range(ncols): + record.append(values[i]) + log.append(record) + + return log + +#clid = p.connect(p.SHARED_MEMORY) +p.connect(p.GUI) +p.loadURDF("plane.urdf",[0,0,-0.3]) +p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) +p.loadURDF("cube.urdf",[2,2,5]) +p.loadURDF("cube.urdf",[-2,-2,5]) +p.loadURDF("cube.urdf",[2,-2,5]) + +log = readLogFile("LOG0001.txt") + +recordNum = len(log) +itemNum = len(log[0]) +print('record num:'), +print(recordNum) +print('item num:'), +print(itemNum) + +for record in log: + Id = record[1] + pos = [record[2],record[3],record[4]] + orn = [record[5],record[6],record[7],record[8]] + p.resetBasePositionAndOrientation(Id,pos,orn) + numJoints = record[15] + for i in range (numJoints): + p.resetJointState(Id,i,record[i+16]) + sleep(0.0005) \ No newline at end of file