mirror of
https://github.com/bulletphysics/bullet3.git
synced 2026-11-17 21:26:06 +00:00
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:
@@ -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;
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user