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

Merge pull request #1038 from YunfeiBai/master

Browse Source 
Load body name from URDF and add API to get the body name.
This commit is contained in:
erwincoumans
2017-03-29 18:19:03 -07:00
committed by GitHub
parent 30c1923b00 f062847038
commit c4cd6d2fa6
15 changed files with 39 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/cube.urdf
View File

@@ -1,5 +1,5 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<robot name="cube">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>

2
data/plane.urdf
View File

@@ -1,5 +1,5 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<robot name="plane">
<link name="planeLink">
<contact>
<lateral_friction value="1"/>

5
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -1323,6 +1323,11 @@ int BulletMJCFImporter::getRootLinkIndex() const
return "";
}
std::string BulletMJCFImporter::getBodyName() const
{
return m_data->m_fileModelName;
}
bool BulletMJCFImporter::getLinkColor(int linkIndex, btVector4& colorRGBA) const
{
// UrdfLink* link = m_data->getLink(linkIndex);

2
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h
View File

@@ -41,6 +41,8 @@ public:
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const;
virtual std::string getBodyName() const;
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const;

5
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -269,6 +269,11 @@ std::string BulletURDFImporter::getLinkName(int linkIndex) const
}
return "";
}
std::string BulletURDFImporter::getBodyName() const
{
return m_data->m_urdfParser.getModel().m_name;
}
std::string BulletURDFImporter::getJointName(int linkIndex) const
{

2
examples/Importers/ImportURDFDemo/BulletUrdfImporter.h
View File

@@ -34,6 +34,8 @@ public:
virtual int getRootLinkIndex() const;
virtual void getLinkChildIndices(int linkIndex, btAlignedObjectArray<int>& childLinkIndices) const;
virtual std::string getBodyName() const;
virtual std::string getLinkName(int linkIndex) const;

3
examples/Importers/ImportURDFDemo/URDFImporterInterface.h
View File

@@ -27,6 +27,9 @@ public:
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const =0;
virtual std::string getBodyName() const = 0;
/// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise
virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const { return false;}

1
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -552,7 +552,6 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
return 0;
}
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

4
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -20,6 +20,7 @@ struct BodyJointInfoCache
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName;
};
struct PhysicsClientSharedMemoryInternalData {
@@ -106,6 +107,7 @@ bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo&
{
BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
info.m_baseName = bodyJoints->m_baseName.c_str();
info.m_bodyName = bodyJoints->m_bodyName.c_str();
return true;
}
@@ -306,6 +308,7 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
@@ -418,6 +421,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
for (int i = 0; i < bf.m_multiBodies.size(); i++) {

3
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -17,6 +17,7 @@ struct BodyJointInfoCache2
{
std::string m_baseName;
btAlignedObjectArray<b3JointInfo> m_jointInfo;
std::string m_bodyName;
};
@@ -605,6 +606,7 @@ void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemorySta
BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
@@ -931,6 +933,7 @@ bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
info.m_baseName = bodyJoints->m_baseName.c_str();
info.m_bodyName = bodyJoints->m_bodyName.c_str();
return true;
}

7
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -120,6 +120,7 @@ struct InteralBodyData
btMultiBody* m_multiBody;
btRigidBody* m_rigidBody;
int m_testData;
std::string m_bodyName;
btTransform m_rootLocalInertialFrame;
btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
@@ -1607,6 +1608,8 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
btMultiBody* mb = creation.getBulletMultiBody();
btRigidBody* rb = creation.getRigidBody();
bodyHandle->m_bodyName = u2b.getBodyName();
if (useMultiBody)
{
@@ -2650,6 +2653,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
}
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
hasStatus = true;
} else
@@ -5015,8 +5020,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
default:
{
BT_PROFILE("CMD_UNKNOWN");

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -103,6 +103,7 @@ struct BulletDataStreamArgs
{
char m_bulletFileName[MAX_FILENAME_LENGTH];
int m_bodyUniqueId;
char m_bodyName[MAX_FILENAME_LENGTH];
};
struct SetJointFeedbackArgs
@@ -364,9 +365,6 @@ struct RequestActualStateArgs
int m_bodyUniqueId;
};
struct SendActualStateArgs
{
int m_bodyUniqueId;

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -201,6 +201,7 @@ struct b3UserConstraint
struct b3BodyInfo
{
const char* m_baseName;
const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
};

3
examples/pybullet/examples/kuka_with_cube.py
View File

@@ -48,6 +48,9 @@ p.setRealTimeSimulation(useRealTimeSimulation)
trailDuration = 15
logId = p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,"LOG0001.txt",[0,1,2])
for i in xrange(5):
print "Body %d's name is %s." % (i, p.getBodyInfo(i)[1])
while 1:
if (useRealTimeSimulation):

5
examples/pybullet/pybullet.c
View File

@@ -1714,8 +1714,9 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject*
struct b3BodyInfo info;
if (b3GetBodyInfo(sm, bodyUniqueId, &info))
{
PyObject* pyListJointInfo = PyTuple_New(1);
PyObject* pyListJointInfo = PyTuple_New(2);
PyTuple_SetItem(pyListJointInfo, 0, PyString_FromString(info.m_baseName));
PyTuple_SetItem(pyListJointInfo, 1, PyString_FromString(info.m_bodyName));
return pyListJointInfo;
}
}
@@ -5145,7 +5146,7 @@ static PyMethodDef SpamMethods[] = {
{"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS,
"Get the state (position, velocity etc) for a joint on a body."},
{"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS,
"Provides extra information such as the Cartesian world coordinates"
" center of mass (COM) of the link, relative to the world reference"