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

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
Erwin Coumans
2017-08-31 10:00:24 -07:00
parent 6d0e2cd527 ee30479a28
commit 6d940d7a02
8 changed files with 248 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -390,7 +390,7 @@ void ConvertURDF2BulletInternal(
{
//b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
if (enableConstraints)
world1->addConstraint(dof6,true);
@@ -417,7 +417,7 @@ void ConvertURDF2BulletInternal(
} else
{
btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
if (enableConstraints)
world1->addConstraint(dof6,true);
@@ -449,10 +449,8 @@ void ConvertURDF2BulletInternal(
} else
{
//btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
if (enableConstraints)
world1->addConstraint(dof6,true);

16
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3657,6 +3657,22 @@ int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int ma
return 0;
}
int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_LOG_FLAGS;
command->m_stateLoggingArguments.m_logFlags = logFlags;
}
return 0;
}
int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -471,6 +471,7 @@ int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int l
int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId);
int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId);
int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags);
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);

238
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -844,13 +844,15 @@ struct GenericRobotStateLogger : public InternalStateLogger
btAlignedObjectArray<int> m_bodyIdList;
bool m_filterObjectUniqueId;
int m_maxLogDof;
int m_logFlags;
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof)
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
:m_loggingTimeStamp(0),
m_logFileHandle(0),
m_dynamicsWorld(dynamicsWorld),
m_filterObjectUniqueId(false),
m_maxLogDof(maxLogDof)
m_maxLogDof(maxLogDof),
m_logFlags(logFlags)
{
m_loggingUniqueId = loggingUniqueId;
m_loggingType = STATE_LOGGING_GENERIC_ROBOT;
@@ -883,14 +885,26 @@ struct GenericRobotStateLogger : public InternalStateLogger
sprintf(jointName,"q%d",i);
structNames.push_back(jointName);
}
for (int i=0;i<m_maxLogDof;i++)
{
m_structTypes.append("f");
char jointName[256];
sprintf(jointName,"u%d",i);
sprintf(jointName,"v%d",i);
structNames.push_back(jointName);
}
if (m_logFlags & STATE_LOG_JOINT_TORQUES)
{
for (int i=0;i<m_maxLogDof;i++)
{
m_structTypes.append("f");
char jointName[256];
sprintf(jointName,"u%d",i);
structNames.push_back(jointName);
}
}
const char* fileNameC = fileName.c_str();
m_logFileHandle = createMinitaurLogFile(fileNameC, structNames, m_structTypes);
@@ -979,16 +993,50 @@ struct GenericRobotStateLogger : public InternalStateLogger
{
if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
{
float u = mb->getJointVel(j);
logData.m_values.push_back(u);
float v = mb->getJointVel(j);
logData.m_values.push_back(v);
}
}
for (int j = numDofs; j < m_maxLogDof; ++j)
{
float u = 0.0;
logData.m_values.push_back(u);
float v = 0.0;
logData.m_values.push_back(v);
}
if (m_logFlags & STATE_LOG_JOINT_TORQUES)
{
for (int j = 0; j < numJoints; ++j)
{
if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
{
float jointTorque = 0;
if (m_logFlags & STATE_LOG_JOINT_MOTOR_TORQUES)
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(j).m_userPtr;
if (motor)
{
jointTorque += motor->getAppliedImpulse(0)/timeStep;
}
}
if (m_logFlags & STATE_LOG_JOINT_USER_TORQUES)
{
if (mb->getLink(j).m_jointType == 0 || mb->getLink(j).m_jointType == 1)
{
jointTorque += mb->getJointTorque(j);//these are the 'user' applied external torques
}
}
logData.m_values.push_back(jointTorque);
}
}
for (int j = numDofs; j < m_maxLogDof; ++j)
{
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);
@@ -999,7 +1047,6 @@ struct GenericRobotStateLogger : public InternalStateLogger
}
}
};
struct ContactPointsStateLogger : public InternalStateLogger
{
int m_loggingTimeStamp;
@@ -2848,7 +2895,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
maxLogDof = clientCmd.m_stateLoggingArguments.m_maxLogDof;
}
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof);
int logFlags = 0;
if (clientCmd.m_updateFlags & STATE_LOGGING_LOG_FLAGS)
{
logFlags = clientCmd.m_stateLoggingArguments.m_logFlags;
}
GenericRobotStateLogger* logger = new GenericRobotStateLogger(loggerUid,fileName,m_data->m_dynamicsWorld,maxLogDof, logFlags);
if ((clientCmd.m_updateFlags & STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID) && (clientCmd.m_stateLoggingArguments.m_numBodyUniqueIds>0))
{
@@ -6716,36 +6769,151 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
if (parentBody->m_rigidBody)
{
if (clientCmd.m_userConstraintArguments.m_jointType == eGearType)
btRigidBody* parentRb = 0;
if (clientCmd.m_userConstraintArguments.m_parentJointIndex==-1)
{
btRigidBody* childRb = childBody->m_rigidBody;
if (childRb)
parentRb = parentBody->m_rigidBody;
} else
{
if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=0) &&
(clientCmd.m_userConstraintArguments.m_parentJointIndex<parentBody->m_rigidBodyJoints.size()))
{
btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
//for now we use the same local axis for both objects
btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
btScalar ratio=1;
btGearConstraint* gear = new btGearConstraint(*parentBody->m_rigidBody,*childRb, axisA,axisB,ratio);
m_data->m_dynamicsWorld->addConstraint(gear,true);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = gear;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
parentRb = &parentBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_parentJointIndex]->getRigidBodyB();
}
}
btRigidBody* childRb = 0;
if (childBody->m_rigidBody)
{
if (clientCmd.m_userConstraintArguments.m_childJointIndex==-1)
{
childRb = childBody->m_rigidBody;
}
else
{
if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=0)
&& (clientCmd.m_userConstraintArguments.m_childJointIndex<childBody->m_rigidBodyJoints.size()))
{
childRb = &childBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_childJointIndex]->getRigidBodyB();
}
}
}
switch (clientCmd.m_userConstraintArguments.m_jointType)
{
case eRevoluteType:
{
break;
}
case ePrismaticType:
{
break;
}
case eFixedType:
{
if (childRb && parentRb && (childRb!=parentRb))
{
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
btTransform offsetTrA,offsetTrB;
offsetTrA.setIdentity();
offsetTrA.setOrigin(pivotInParent);
offsetTrB.setIdentity();
offsetTrB.setOrigin(pivotInChild);
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRb, *childRb, offsetTrA, offsetTrB);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
m_data->m_dynamicsWorld->addConstraint(dof6);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = dof6;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
break;
}
case ePoint2PointType:
{
if (childRb && parentRb && (childRb!=parentRb))
{
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*parentRb,*childRb,pivotInParent,pivotInChild);
p2p->m_setting.m_impulseClamp = defaultMaxForce;
m_data->m_dynamicsWorld->addConstraint(p2p);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
break;
}
case eGearType:
{
if (childRb && parentRb && (childRb!=parentRb))
{
btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
//for now we use the same local axis for both objects
btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
btScalar ratio=1;
btGearConstraint* gear = new btGearConstraint(*parentRb,*childRb, axisA,axisB,ratio);
m_data->m_dynamicsWorld->addConstraint(gear,true);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = gear;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
break;
}
case eSphericalType:
{
b3Warning("constraint type not handled yet");
break;
}
case ePlanarType:
{
b3Warning("constraint type not handled yet");
break;
}
default:
{
b3Warning("unknown constraint type");
}
};
}
}
}

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -760,7 +760,8 @@ enum eStateLoggingEnums
STATE_LOGGING_FILTER_LINK_INDEX_B=32,
STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A=64,
STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B=128,
STATE_LOGGING_FILTER_DEVICE_TYPE=256
STATE_LOGGING_FILTER_DEVICE_TYPE=256,
STATE_LOGGING_LOG_FLAGS=512
};
struct VRCameraState
@@ -786,6 +787,7 @@ struct StateLoggingRequest
int m_bodyUniqueIdA; // only if STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A flag is set
int m_bodyUniqueIdB; // only if STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B flag is set
int m_deviceFilterType; //user to select (filter) which VR devices to log
int m_logFlags;
};
struct StateLoggingResultArgs

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -607,6 +607,12 @@ enum eUrdfCollisionFlags
GEOM_FORCE_CONCAVE_TRIMESH=1,
};
enum eStateLoggingFlags
{
STATE_LOG_JOINT_MOTOR_TORQUES=1,
STATE_LOG_JOINT_USER_TORQUES=2,
STATE_LOG_JOINT_TORQUES = STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES,
};

4
examples/pybullet/examples/quadruped.py
View File

@@ -19,7 +19,7 @@ if (physId<0):
#p.resetSimulation()
p.loadURDF("plane.urdf",0,0,0)
p.setPhysicsEngineParameter(numSolverIterations=50)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT, "genericlogdata.bin", maxLogDof = 16, logFlags = p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4)
p.setGravity(0,0,0)
@@ -156,7 +156,7 @@ t_end = t + 100
i=0
ref_time = time.time()
while t < t_end:
for aa in range (100):
if (useRealTime):
t = time.time()-ref_time
else:

16
examples/pybullet/pybullet.c
View File

@@ -3573,12 +3573,13 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
int linkIndexA = -2;
int linkIndexB = -2;
int deviceTypeFilter = -1;
int logFlags = -1;
static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "deviceTypeFilter", "physicsClientId", NULL};
static char* kwlist[] = {"loggingType", "fileName", "objectUniqueIds", "maxLogDof", "bodyUniqueIdA", "bodyUniqueIdB", "linkIndexA", "linkIndexB", "deviceTypeFilter", "logFlags", "physicsClientId", NULL};
int physicsClientId = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiiii", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &deviceTypeFilter, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "is|Oiiiiiiii", kwlist,
&loggingType, &fileName, &objectUniqueIdsObj, &maxLogDof, &bodyUniqueIdA, &bodyUniqueIdB, &linkIndexA, &linkIndexB, &deviceTypeFilter, &logFlags, &physicsClientId))
return NULL;
sm = getPhysicsClient(physicsClientId);
@@ -3636,6 +3637,11 @@ static PyObject* pybullet_startStateLogging(PyObject* self, PyObject* args, PyOb
b3StateLoggingSetDeviceTypeFilter(commandHandle,deviceTypeFilter);
}
if (logFlags >0)
{
b3StateLoggingSetLogFlags(commandHandle, logFlags);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
@@ -7473,7 +7479,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "GEOM_FORCE_CONCAVE_TRIMESH", GEOM_FORCE_CONCAVE_TRIMESH);
PyModule_AddIntConstant(m, "STATE_LOG_JOINT_MOTOR_TORQUES", STATE_LOG_JOINT_MOTOR_TORQUES);
PyModule_AddIntConstant(m, "STATE_LOG_JOINT_USER_TORQUES", STATE_LOG_JOINT_USER_TORQUES);
PyModule_AddIntConstant(m, "STATE_LOG_JOINT_TORQUES", STATE_LOG_JOINT_USER_TORQUES+STATE_LOG_JOINT_MOTOR_TORQUES);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);