Merge pull request #2640 from erwincoumans/master

PyBullet: add option to merge fixed links in URDF file, capsule field init
This commit is contained in:
erwincoumans
2020-02-21 17:49:51 -08:00
committed by GitHub
10 changed files with 304 additions and 3 deletions
@@ -194,6 +194,37 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
if (xml_string.length())
{
result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (m_data->m_flags & CUF_PARSE_SENSORS));
if (m_data->m_flags & CUF_IGNORE_VISUAL_SHAPES)
{
for (int i=0; i < m_data->m_urdfParser.getModel().m_links.size(); i++)
{
UrdfLink* linkPtr = *m_data->m_urdfParser.getModel().m_links.getAtIndex(i);
linkPtr->m_visualArray.clear();
}
}
if (m_data->m_flags & CUF_IGNORE_COLLISION_SHAPES)
{
for (int i=0; i < m_data->m_urdfParser.getModel().m_links.size(); i++)
{
UrdfLink* linkPtr = *m_data->m_urdfParser.getModel().m_links.getAtIndex(i);
linkPtr->m_collisionArray.clear();
}
}
if (m_data->m_urdfParser.getModel().m_rootLinks.size())
{
if (m_data->m_flags & CUF_MERGE_FIXED_LINKS)
{
m_data->m_urdfParser.mergeFixedLinks(m_data->m_urdfParser.getModel(), m_data->m_urdfParser.getModel().m_rootLinks[0], &loggie, forceFixedBase, 0);
m_data->m_urdfParser.getModel().m_links.clear();
m_data->m_urdfParser.getModel().m_joints.clear();
m_data->m_urdfParser.recreateModel(m_data->m_urdfParser.getModel(), m_data->m_urdfParser.getModel().m_rootLinks[0], &loggie);
}
if (m_data->m_flags & CUF_PRINT_URDF_INFO)
{
m_data->m_urdfParser.printTree(m_data->m_urdfParser.getModel().m_rootLinks[0], &loggie, 0);
}
}
}
return result;
@@ -99,6 +99,12 @@ enum ConvertURDFFlags
CUF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
CUF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
CUF_MAINTAIN_LINK_ORDER = 131072,
CUF_ENABLE_WAKEUP = 1 << 18,
CUF_MERGE_FIXED_LINKS = 1 << 19,
CUF_IGNORE_VISUAL_SHAPES = 1 << 20,
CUF_IGNORE_COLLISION_SHAPES = 1 << 21,
CUF_PRINT_URDF_INFO = 1 << 22,
};
#endif //URDF_JOINT_TYPES_H
@@ -1544,6 +1544,238 @@ bool UrdfParser::parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint,
return true;
}
static void CalculatePrincipalAxisTransform(btScalar* masses, const btTransform* transforms, btMatrix3x3* inertiasIn, btTransform& principal, btVector3& inertiaOut)
{
int n = 2;
btScalar totalMass = 0;
btVector3 center(0, 0, 0);
int k;
for (k = 0; k < n; k++)
{
btAssert(masses[k] > 0);
center += transforms[k].getOrigin() * masses[k];
totalMass += masses[k];
}
btAssert(totalMass > 0);
center /= totalMass;
principal.setOrigin(center);
btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
for (k = 0; k < n; k++)
{
const btTransform& t = transforms[k];
btVector3 o = t.getOrigin() - center;
//compute inertia tensor in coordinate system of parent
btMatrix3x3 j = t.getBasis().transpose();
j *= inertiasIn[k];
j = t.getBasis() * j;
//add inertia tensor
tensor[0] += j[0];
tensor[1] += j[1];
tensor[2] += j[2];
//compute inertia tensor of pointmass at o
btScalar o2 = o.length2();
j[0].setValue(o2, 0, 0);
j[1].setValue(0, o2, 0);
j[2].setValue(0, 0, o2);
j[0] += o * -o.x();
j[1] += o * -o.y();
j[2] += o * -o.z();
//add inertia tensor of pointmass
tensor[0] += masses[k] * j[0];
tensor[1] += masses[k] * j[1];
tensor[2] += masses[k] * j[2];
}
tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
inertiaOut.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
}
bool UrdfParser::mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level)
{
for (int l = 0; l < level; l++)
{
printf("\t");
}
printf("processing %s\n", link->m_name.c_str());
for (int i = 0; i < link->m_childJoints.size();)
{
if (link->m_childJoints[i]->m_type == URDFFixedJoint)
{
UrdfLink* childLink = link->m_childLinks[i];
UrdfJoint* childJoint = link->m_childJoints[i];
for (int l = 0; l < level+1; l++)
{
printf("\t");
}
//mergeChildLink
printf("merge %s into %s!\n", childLink->m_name.c_str(), link->m_name.c_str());
for (int c = 0; c < childLink->m_collisionArray.size(); c++)
{
UrdfCollision col = childLink->m_collisionArray[c];
col.m_linkLocalFrame = childJoint->m_parentLinkToJointTransform * col.m_linkLocalFrame;
link->m_collisionArray.push_back(col);
}
for (int c = 0; c < childLink->m_visualArray.size(); c++)
{
UrdfVisual viz = childLink->m_visualArray[c];
viz.m_linkLocalFrame = childJoint->m_parentLinkToJointTransform * viz.m_linkLocalFrame;
link->m_visualArray.push_back(viz);
}
if (!link->m_inertia.m_hasLinkLocalFrame)
{
link->m_inertia.m_linkLocalFrame.setIdentity();
}
if (!childLink->m_inertia.m_hasLinkLocalFrame)
{
childLink->m_inertia.m_linkLocalFrame.setIdentity();
}
//for a 'forceFixedBase' don't merge
bool isStaticBase = false;
if (forceFixedBase && link->m_parentJoint == 0)
isStaticBase = true;
if (link->m_inertia.m_mass==0 && link->m_parentJoint == 0)
isStaticBase = true;
//skip the mass and inertia merge for a fixed base link
if (!isStaticBase)
{
btScalar masses[2] = { (btScalar)link->m_inertia.m_mass, (btScalar)childLink->m_inertia.m_mass };
btTransform transforms[2] = { link->m_inertia.m_linkLocalFrame, childJoint->m_parentLinkToJointTransform * childLink->m_inertia.m_linkLocalFrame };
btMatrix3x3 inertiaLink(
link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
btMatrix3x3 inertiaChild(
childLink->m_inertia.m_ixx, childLink->m_inertia.m_ixy, childLink->m_inertia.m_ixz,
childLink->m_inertia.m_ixy, childLink->m_inertia.m_iyy, childLink->m_inertia.m_iyz,
childLink->m_inertia.m_ixz, childLink->m_inertia.m_iyz, childLink->m_inertia.m_izz);
btMatrix3x3 inertiasIn[2] = { inertiaLink, inertiaChild };
btVector3 inertiaOut;
btTransform principal;
CalculatePrincipalAxisTransform(masses, transforms, inertiasIn, principal, inertiaOut);
link->m_inertia.m_hasLinkLocalFrame = true;
link->m_inertia.m_linkLocalFrame.setIdentity();
//link->m_inertia.m_linkLocalFrame = principal;
link->m_inertia.m_linkLocalFrame.setOrigin(principal.getOrigin());
link->m_inertia.m_ixx = inertiaOut[0];
link->m_inertia.m_ixy = 0;
link->m_inertia.m_ixz = 0;
link->m_inertia.m_iyy = inertiaOut[1];
link->m_inertia.m_iyz = 0;
link->m_inertia.m_izz = inertiaOut[2];
link->m_inertia.m_mass += childLink->m_inertia.m_mass;
}
link->m_childJoints.removeAtIndex(i);
link->m_childLinks.removeAtIndex(i);
for (int g = 0; g < childLink->m_childJoints.size(); g++)
{
UrdfLink* grandChildLink = childLink->m_childLinks[g];
UrdfJoint* grandChildJoint = childLink->m_childJoints[g];
for (int l = 0; l < level+2; l++)
{
printf("\t");
}
printf("relink %s from %s to %s!\n", grandChildLink->m_name.c_str(), childLink->m_name.c_str(), link->m_name.c_str());
grandChildJoint->m_parentLinkName = link->m_name;
grandChildJoint->m_parentLinkToJointTransform =
childJoint->m_parentLinkToJointTransform * grandChildJoint->m_parentLinkToJointTransform;
grandChildLink->m_parentLink = link;
grandChildLink->m_parentJoint->m_parentLinkName = link->m_name;
link->m_childJoints.push_back(grandChildJoint);
link->m_childLinks.push_back(grandChildLink);
}
model.m_links.remove(childLink->m_name.c_str());
model.m_joints.remove(childJoint->m_name.c_str());
}
else
{
//keep this link and recurse
mergeFixedLinks(model, link->m_childLinks[i], logger, forceFixedBase, level+1);
i++;
}
}
return true;
}
const std::string sJointNames[]={"unused",
"URDFRevoluteJoint",
"URDFPrismaticJoint",
"URDFContinuousJoint",
"URDFFloatingJoint",
"URDFPlanarJoint",
"URDFFixedJoint",
"URDFSphericalJoint",
};
bool UrdfParser::printTree(UrdfLink* link, ErrorLogger* logger, int level)
{
printf("\n");
for (int l = 0; l < level; l++)
{
printf("\t");
}
printf("%s (mass=%f) ", link->m_name.c_str(), link->m_inertia.m_mass);
if (link->m_parentJoint)
{
printf("(joint %s, joint type=%s\n", link->m_parentJoint->m_name.c_str(), sJointNames[link->m_parentJoint->m_type].c_str());
}
else
{
printf("\n");
}
for (int i = 0; i<link->m_childJoints.size(); i++)
{
printTree(link->m_childLinks[i], logger, level + 1);
btAssert(link->m_childJoints[i]->m_parentLinkName == link->m_name);
}
return true;
}
bool UrdfParser::recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger)
{
if (!link->m_parentJoint)
{
link->m_linkIndex = model.m_links.size();
model.m_links.insert(link->m_name.c_str(), link);
}
for (int i = 0; i < link->m_childJoints.size(); i++)
{
link->m_childLinks[i]->m_linkIndex = model.m_links.size();
const char* childName = link->m_childLinks[i]->m_name.c_str();
UrdfLink* childLink = link->m_childLinks[i];
model.m_links.insert(childName, childLink);
const char* jointName = link->m_childLinks[i]->m_parentJoint->m_name.c_str();
UrdfJoint* joint = link->m_childLinks[i]->m_parentJoint;
model.m_joints.insert(jointName, joint);
}
for (int i = 0; i < link->m_childJoints.size(); i++)
{
recreateModel(model, link->m_childLinks[i], logger);
}
return true;
}
bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
{
// every link has children links and joints, but no parents, so we create a
@@ -1629,6 +1861,9 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
logger->reportError("URDF without root link found");
return false;
}
return true;
}
@@ -357,6 +357,12 @@ public:
return m_urdf2Model;
}
bool mergeFixedLinks(UrdfModel& model, UrdfLink* link, ErrorLogger* logger, bool forceFixedBase, int level);
bool printTree(UrdfLink* link, ErrorLogger* logger, int level);
bool recreateModel(UrdfModel& model, UrdfLink* link, ErrorLogger* logger);
std::string sourceFileLocation(tinyxml2::XMLElement* e);
void setSourceFile(const std::string& sourceFile)
@@ -1438,6 +1438,7 @@ B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle c
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasFromTo = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
command->m_createUserShapeArgs.m_numUserShapes++;
@@ -3484,6 +3484,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
if (loadOk)
{
btTransform rootTrans;
rootTrans.setOrigin(pos);
rootTrans.setRotation(orn);
@@ -12009,6 +12010,17 @@ int PhysicsServerCommandProcessor::extractCollisionShapes(const btCollisionShape
switch (colShape->getShapeType())
{
case MULTI_SPHERE_SHAPE_PROXYTYPE:
{
btCapsuleShapeZ* capsule = (btCapsuleShapeZ*)colShape;
collisionShapeBuffer[0].m_collisionGeometryType = GEOM_CAPSULE;
collisionShapeBuffer[0].m_dimensions[0] = 2. * capsule->getHalfHeight();
collisionShapeBuffer[0].m_dimensions[1] = capsule->getRadius();
collisionShapeBuffer[0].m_dimensions[2] = 0;
numConverted++;
break;
break;
}
case STATIC_PLANE_PROXYTYPE:
{
btStaticPlaneShape* plane = (btStaticPlaneShape*)colShape;
+6 -2
View File
@@ -900,7 +900,7 @@ enum eCONNECT_METHOD
enum eURDF_Flags
{
URDF_USE_INERTIA_FROM_FILE = 2, //sync with URDF2Bullet.h 'ConvertURDFFlags'
URDF_USE_INERTIA_FROM_FILE = 2, //sync with URDFJointTypes.h 'ConvertURDFFlags'
URDF_USE_SELF_COLLISION = 8, //see CUF_USE_SELF_COLLISION
URDF_USE_SELF_COLLISION_EXCLUDE_PARENT = 16,
URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS = 32,
@@ -916,7 +916,11 @@ enum eURDF_Flags
URDF_USE_MATERIAL_COLORS_FROM_MTL = 32768,
URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL = 65536,
URDF_MAINTAIN_LINK_ORDER = 131072,
URDF_ENABLE_WAKEUP = 262144,
URDF_ENABLE_WAKEUP = 1 << 18,
URDF_MERGE_FIXED_LINKS = 1 << 19,
URDF_IGNORE_VISUAL_SHAPES = 1 << 20,
URDF_IGNORE_COLLISION_SHAPES = 1 << 21,
URDF_PRINT_URDF_INFO = 1 << 22,
};
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes
@@ -278,6 +278,7 @@ bool b3RobotSimulatorClientAPI_NoDirect::loadMJCF(const std::string& fileName, b
b3SharedMemoryCommandHandle command;
command = b3LoadMJCFCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
b3LoadMJCFCommandSetFlags(command, URDF_USE_IMPLICIT_CYLINDER);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_MJCF_LOADING_COMPLETED)
+5
View File
@@ -12605,6 +12605,11 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_COLORS_FROM_MTL", URDF_USE_MATERIAL_COLORS_FROM_MTL);
PyModule_AddIntConstant(m, "URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL", URDF_USE_MATERIAL_TRANSPARANCY_FROM_MTL);
PyModule_AddIntConstant(m, "URDF_MAINTAIN_LINK_ORDER", URDF_MAINTAIN_LINK_ORDER);
PyModule_AddIntConstant(m, "URDF_ENABLE_WAKEUP", URDF_ENABLE_WAKEUP);
PyModule_AddIntConstant(m, "URDF_MERGE_FIXED_LINKS", URDF_MERGE_FIXED_LINKS);
PyModule_AddIntConstant(m, "URDF_IGNORE_VISUAL_SHAPES", URDF_IGNORE_VISUAL_SHAPES);
PyModule_AddIntConstant(m, "URDF_IGNORE_COLLISION_SHAPES",URDF_IGNORE_COLLISION_SHAPES);
PyModule_AddIntConstant(m, "URDF_PRINT_URDF_INFO", URDF_PRINT_URDF_INFO);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);
+1 -1
View File
@@ -492,7 +492,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
setup(
name='pybullet',
version='2.6.5',
version='2.6.6',
description=
'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description=