Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add logging for generic robot and an example of logging state of kuka and cubes.

Browse Source
This commit is contained in:
yunfeibai
2017-02-17 17:41:57 -08:00
parent 45aa392a28
commit a3c1fec171
3 changed files with 337 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

168
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -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<std::string> 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;i<m_dynamicsWorld->getNumMultibodies();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)
{

92
examples/pybullet/kuka_with_cube.py Normal file
View File

@@ -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

77
examples/pybullet/kuka_with_cube_playback.py Normal file
View File

@@ -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)