mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
Merge branch 'master' of https://github.com/erwincoumans/bullet3
This commit is contained in:
Erwin Coumans
commit
e381e84319
@ -49,30 +49,30 @@
|
||||
</visual>
|
||||
|
||||
<asset>
|
||||
<mesh name="index0" file="index0_collision.STL"/>
|
||||
<mesh name="index1" file="index1_collision.STL"/>
|
||||
<mesh name="index2" file="index2_collision.STL"/>
|
||||
<mesh name="index3" file="index3_collision.STL"/>
|
||||
<mesh name="middle0" file="middle0_collision.STL"/>
|
||||
<mesh name="middle1" file="middle1_collision.STL"/>
|
||||
<mesh name="middle2" file="middle2_collision.STL"/>
|
||||
<mesh name="middle3" file="middle3_collision.STL"/>
|
||||
<mesh name="palm" file="palm.STL"/>
|
||||
<mesh name="pinky0" file="pinky0_collision.STL"/>
|
||||
<mesh name="pinky1" file="pinky1_collision.STL"/>
|
||||
<mesh name="pinky2" file="pinky2_collision.STL"/>
|
||||
<mesh name="pinky3" file="pinky3_collision.STL"/>
|
||||
<mesh name="ring0" file="ring0_collision.STL"/>
|
||||
<mesh name="ring1" file="ring1_collision.STL"/>
|
||||
<mesh name="ring2" file="ring2_collision.STL"/>
|
||||
<mesh name="ring3" file="ring3_collision.STL"/>
|
||||
<mesh name="thumb0" file="thumb0_collision.STL"/>
|
||||
<mesh name="thumb1" file="thumb1_collision.STL"/>
|
||||
<mesh name="thumb2" file="thumb2_collision.STL"/>
|
||||
<mesh name="thumb3" file="thumb3_collision.STL"/>
|
||||
<mesh name="wristx" file="wristx_collision.STL"/>
|
||||
<mesh name="wristy" file="wristy_collision.STL"/>
|
||||
<mesh name="wristz" file="wristz_collision.STL"/>
|
||||
<mesh name="index0" file="index0_collision.stl"/>
|
||||
<mesh name="index1" file="index1_collision.stl"/>
|
||||
<mesh name="index2" file="index2_collision.stl"/>
|
||||
<mesh name="index3" file="index3_collision.stl"/>
|
||||
<mesh name="middle0" file="middle0_collision.stl"/>
|
||||
<mesh name="middle1" file="middle1_collision.stl"/>
|
||||
<mesh name="middle2" file="middle2_collision.stl"/>
|
||||
<mesh name="middle3" file="middle3_collision.stl"/>
|
||||
<mesh name="palm" file="palm.stl"/>
|
||||
<mesh name="pinky0" file="pinky0_collision.stl"/>
|
||||
<mesh name="pinky1" file="pinky1_collision.stl"/>
|
||||
<mesh name="pinky2" file="pinky2_collision.stl"/>
|
||||
<mesh name="pinky3" file="pinky3_collision.stl"/>
|
||||
<mesh name="ring0" file="ring0_collision.stl"/>
|
||||
<mesh name="ring1" file="ring1_collision.stl"/>
|
||||
<mesh name="ring2" file="ring2_collision.stl"/>
|
||||
<mesh name="ring3" file="ring3_collision.stl"/>
|
||||
<mesh name="thumb0" file="thumb0_collision.stl"/>
|
||||
<mesh name="thumb1" file="thumb1_collision.stl"/>
|
||||
<mesh name="thumb2" file="thumb2_collision.stl"/>
|
||||
<mesh name="thumb3" file="thumb3_collision.stl"/>
|
||||
<mesh name="wristx" file="wristx_collision.stl"/>
|
||||
<mesh name="wristy" file="wristy_collision.stl"/>
|
||||
<mesh name="wristz" file="wristz_collision.stl"/>
|
||||
|
||||
<texture type="skybox" builtin="gradient" rgb1=".4 .6 .8" rgb2="0 0 0"
|
||||
width="100" height="100"/>
|
||||
|
||||
@ -215,6 +215,10 @@
|
||||
</joint>
|
||||
|
||||
<link name='finger_right'>
|
||||
<contact>
|
||||
<lateral_friction>1.0</lateral_friction>
|
||||
<spinning_friction>1.5</spinning_friction>
|
||||
</contact>
|
||||
<pose frame=''>0.062 0 0.145 0 0 1.5708</pose>
|
||||
<inertial>
|
||||
<mass>0.2</mass>
|
||||
@ -255,6 +259,10 @@
|
||||
</joint>
|
||||
|
||||
<link name='finger_left'>
|
||||
<contact>
|
||||
<lateral_friction>1.0</lateral_friction>
|
||||
<spinning_friction>1.5</spinning_friction>
|
||||
</contact>
|
||||
<pose frame=''>-0.062 0 0.145 0 0 4.71239</pose>
|
||||
<inertial>
|
||||
<mass>0.2</mass>
|
||||
|
||||
@ -869,8 +869,8 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
|
||||
}
|
||||
|
||||
|
||||
int width = 1280;
|
||||
int height=640;
|
||||
int width = 1024;
|
||||
int height=768;
|
||||
#ifndef NO_OPENGL3
|
||||
SimpleOpenGL3App* simpleApp=0;
|
||||
sUseOpenGL2 =args.CheckCmdLineFlag("opengl2");
|
||||
|
||||
@ -30,12 +30,6 @@
|
||||
|
||||
#include <vector>
|
||||
|
||||
enum eMJCF_FILE_TYPE_ENUMS
|
||||
{
|
||||
MJCF_FILE_STL = 1,
|
||||
MJCF_FILE_OBJ = 2
|
||||
};
|
||||
|
||||
enum ePARENT_LINK_ENUMS
|
||||
{
|
||||
BASE_LINK_INDEX=-1,
|
||||
@ -137,9 +131,11 @@ struct MyMJCFAsset
|
||||
struct BulletMJCFImporterInternalData
|
||||
{
|
||||
GUIHelperInterface* m_guiHelper;
|
||||
struct LinkVisualShapesConverter* m_customVisualShapesConverter;
|
||||
char m_pathPrefix[1024];
|
||||
|
||||
std::string m_fileModelName;
|
||||
std::string m_sourceFileName; // with path
|
||||
std::string m_fileModelName; // without path
|
||||
btHashMap<btHashString,MyMJCFAsset> m_assets;
|
||||
|
||||
btAlignedObjectArray<UrdfModel*> m_models;
|
||||
@ -150,22 +146,37 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
|
||||
int m_activeModel;
|
||||
int m_activeBodyUniqueId;
|
||||
//todo: for full MJCF compatibility, we would need a stack of default values
|
||||
int m_defaultCollisionGroup;
|
||||
int m_defaultCollisionMask;
|
||||
btScalar m_defaultCollisionMargin;
|
||||
|
||||
// joint defaults
|
||||
std::string m_defaultJointLimited;
|
||||
|
||||
// geom defaults
|
||||
std::string m_defaultGeomRgba;
|
||||
|
||||
//those collision shapes are deleted by caller (todo: make sure this happens!)
|
||||
btAlignedObjectArray<btCollisionShape*> m_allocatedCollisionShapes;
|
||||
|
||||
BulletMJCFImporterInternalData()
|
||||
:m_activeModel(-1),
|
||||
m_activeBodyUniqueId(-1),
|
||||
m_defaultCollisionGroup(1),
|
||||
m_defaultCollisionMask(1),
|
||||
m_defaultCollisionMargin(0.001)//assume unit meters, margin is 1mm
|
||||
{
|
||||
m_pathPrefix[0] = 0;
|
||||
}
|
||||
|
||||
std::string sourceFileLocation(TiXmlElement* e)
|
||||
{
|
||||
char buf[1024];
|
||||
snprintf(buf, sizeof(buf), "%s:%i", m_sourceFileName.c_str(), e->Row());
|
||||
return buf;
|
||||
}
|
||||
|
||||
const UrdfLink* getLink(int modelIndex, int linkIndex) const
|
||||
{
|
||||
@ -238,6 +249,17 @@ struct BulletMJCFImporterInternalData
|
||||
{
|
||||
parseAssets(child_xml,logger);
|
||||
}
|
||||
if (n=="joint")
|
||||
{
|
||||
// Other attributes here:
|
||||
// armature="1"
|
||||
// damping="1"
|
||||
// limited="true"
|
||||
if (const char* conTypeStr = child_xml->Attribute("limited"))
|
||||
{
|
||||
m_defaultJointLimited = child_xml->Attribute("limited");
|
||||
}
|
||||
}
|
||||
if (n=="geom")
|
||||
{
|
||||
//contype, conaffinity
|
||||
@ -251,6 +273,11 @@ struct BulletMJCFImporterInternalData
|
||||
{
|
||||
m_defaultCollisionMask = urdfLexicalCast<int>(conAffinityStr);
|
||||
}
|
||||
const char* rgba = child_xml->Attribute("rgba");
|
||||
if (rgba)
|
||||
{
|
||||
m_defaultGeomRgba = rgba;
|
||||
}
|
||||
}
|
||||
}
|
||||
handled=true;
|
||||
@ -361,39 +388,57 @@ struct BulletMJCFImporterInternalData
|
||||
bool isLimited = false;
|
||||
double range[2] = {1,0};
|
||||
|
||||
std::string lim = m_defaultJointLimited;
|
||||
if (limitedStr)
|
||||
{
|
||||
std::string lim = limitedStr;
|
||||
if (lim=="true")
|
||||
{
|
||||
isLimited = true;
|
||||
//parse the 'range' field
|
||||
btArray<std::string> pieces;
|
||||
btArray<float> sizes;
|
||||
btAlignedObjectArray<std::string> strArray;
|
||||
urdfIsAnyOf(" ", strArray);
|
||||
urdfStringSplit(pieces, rangeStr, strArray);
|
||||
for (int i = 0; i < pieces.size(); ++i)
|
||||
{
|
||||
if (!pieces[i].empty())
|
||||
{
|
||||
sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
|
||||
}
|
||||
}
|
||||
if (sizes.size()==2)
|
||||
{
|
||||
range[0] = sizes[0];
|
||||
range[1] = sizes[1];
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("Expected range[2] in joint with limits");
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
// logger->reportWarning("joint without limited field");
|
||||
lim = limitedStr;
|
||||
}
|
||||
if (lim=="true")
|
||||
{
|
||||
isLimited = true;
|
||||
//parse the 'range' field
|
||||
btArray<std::string> pieces;
|
||||
btArray<float> sizes;
|
||||
btAlignedObjectArray<std::string> strArray;
|
||||
urdfIsAnyOf(" ", strArray);
|
||||
urdfStringSplit(pieces, rangeStr, strArray);
|
||||
for (int i = 0; i < pieces.size(); ++i)
|
||||
{
|
||||
if (!pieces[i].empty())
|
||||
{
|
||||
sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
|
||||
}
|
||||
}
|
||||
if (sizes.size()==2)
|
||||
{
|
||||
// TODO angle units are in "<compiler angle="degree" inertiafromgeom="true"/>
|
||||
range[0] = sizes[0] * B3_PI / 180;
|
||||
range[1] = sizes[1] * B3_PI / 180;
|
||||
} else
|
||||
{
|
||||
logger->reportWarning("Expected range[2] in joint with limits");
|
||||
}
|
||||
}
|
||||
|
||||
// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
|
||||
// degrees of freedom created by this joint. These are constants added to the
|
||||
// diagonal of the inertia matrix in generalized coordinates. They make the
|
||||
// simulation more stable, and often increase physical realism. This is because
|
||||
// when a motor is attached to the system with a transmission that amplifies
|
||||
// the motor force by c, the inertia of the rotor (i.e. the moving part of the
|
||||
// motor) is amplified by c*c. The same holds for gears in the early stages of
|
||||
// planetary gear boxes. These extra inertias often dominate the inertias of
|
||||
// the robot parts that are represented explicitly in the model, and the
|
||||
// armature attribute is the way to model them.
|
||||
|
||||
// TODO damping : real, "0" Damping applied to all degrees of
|
||||
// freedom created by this joint. Unlike friction loss
|
||||
// which is computed by the constraint solver, damping is
|
||||
// simply a force linear in velocity. It is included in
|
||||
// the passive forces. Despite this simplicity, larger
|
||||
// damping values can make numerical integrators unstable,
|
||||
// which is why our Euler integrator handles damping
|
||||
// implicitly. See Integration in the Computation chapter.
|
||||
|
||||
bool jointHandled = false;
|
||||
const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
|
||||
@ -443,11 +488,11 @@ struct BulletMJCFImporterInternalData
|
||||
jointPtr->m_parentLinkToJointTransform = parentLinkToJointTransform;
|
||||
jointPtr->m_type = ejtype;
|
||||
int numJoints = m_models[modelIndex]->m_joints.size();
|
||||
|
||||
|
||||
//range
|
||||
jointPtr->m_lowerLimit = range[0];
|
||||
jointPtr->m_upperLimit = range[1];
|
||||
|
||||
|
||||
if (nameStr)
|
||||
{
|
||||
jointPtr->m_name =nameStr;
|
||||
@ -489,10 +534,23 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
|
||||
|
||||
// const char* rgba = link_xml->Attribute("rgba");
|
||||
const char* gType = link_xml->Attribute("type");
|
||||
const char* sz = link_xml->Attribute("size");
|
||||
const char* posS = link_xml->Attribute("pos");
|
||||
|
||||
std::string rgba = m_defaultGeomRgba;
|
||||
if (const char* rgbattr = link_xml->Attribute("rgba"))
|
||||
{
|
||||
rgba = rgbattr;
|
||||
}
|
||||
if (!rgba.empty())
|
||||
{
|
||||
// "0 0.7 0.7 1"
|
||||
parseVector4(geom.m_localMaterial.m_rgbaColor, rgba);
|
||||
geom.m_hasLocalMaterial = true;
|
||||
geom.m_localMaterial.m_name = rgba;
|
||||
}
|
||||
|
||||
if (posS)
|
||||
{
|
||||
btVector3 pos(0,0,0);
|
||||
@ -560,10 +618,10 @@ struct BulletMJCFImporterInternalData
|
||||
handledGeomType = true;
|
||||
}
|
||||
|
||||
//todo: capsule, cylinder, meshes or heightfields etc
|
||||
if (geomType == "capsule")
|
||||
if (geomType == "capsule" || geomType == "cylinder")
|
||||
{
|
||||
geom.m_type = URDF_GEOM_CAPSULE;
|
||||
// <geom conaffinity="0" contype="0" fromto="0 0 0 0 0 0.02" name="root" rgba="0.9 0.4 0.6 1" size=".011" type="cylinder"/>
|
||||
geom.m_type = geomType=="cylinder" ? URDF_GEOM_CYLINDER : URDF_GEOM_CAPSULE;
|
||||
|
||||
btArray<std::string> pieces;
|
||||
btArray<float> sizes;
|
||||
@ -621,9 +679,14 @@ struct BulletMJCFImporterInternalData
|
||||
MyMJCFAsset* assetPtr = m_assets[meshStr];
|
||||
if (assetPtr)
|
||||
{
|
||||
handledGeomType = true;
|
||||
geom.m_type = URDF_GEOM_MESH;
|
||||
geom.m_meshFileName = assetPtr->m_fileName;
|
||||
bool exists = findExistingMeshFile(
|
||||
m_sourceFileName, assetPtr->m_fileName, sourceFileLocation(link_xml),
|
||||
&geom.m_meshFileName,
|
||||
&geom.m_meshFileType);
|
||||
handledGeomType = exists;
|
||||
|
||||
geom.m_meshScale.setValue(1,1,1);
|
||||
//todo: parse mesh scale
|
||||
if (sz)
|
||||
@ -632,13 +695,6 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
if (geomType == "cylinder")
|
||||
{
|
||||
geom.m_type = URDF_GEOM_CYLINDER;
|
||||
handledGeomType = true;
|
||||
}
|
||||
#endif
|
||||
if (handledGeomType)
|
||||
{
|
||||
|
||||
@ -803,6 +859,7 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
return orgChildLinkIndex;
|
||||
}
|
||||
|
||||
bool parseBody(TiXmlElement* link_xml, int modelIndex, int orgParentLinkIndex, MJCFErrorLogger* logger)
|
||||
{
|
||||
int newParentLinkIndex = orgParentLinkIndex;
|
||||
@ -964,10 +1021,6 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
|
||||
linkPtr->m_linkTransformInWorld = linkTransform;
|
||||
if (bodyN == "cart1")//front_left_leg")
|
||||
{
|
||||
printf("found!\n");
|
||||
}
|
||||
if ((newParentLinkIndex != INVALID_LINK_INDEX) && !skipFixedJoint)
|
||||
{
|
||||
//linkPtr->m_linkTransformInWorld.setIdentity();
|
||||
@ -1113,10 +1166,11 @@ struct BulletMJCFImporterInternalData
|
||||
|
||||
};
|
||||
|
||||
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper)
|
||||
BulletMJCFImporter::BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
|
||||
{
|
||||
m_data = new BulletMJCFImporterInternalData();
|
||||
m_data->m_guiHelper = helper;
|
||||
m_data->m_customVisualShapesConverter = customConverter;
|
||||
}
|
||||
|
||||
BulletMJCFImporter::~BulletMJCFImporter()
|
||||
@ -1135,7 +1189,8 @@ bool BulletMJCFImporter::loadMJCF(const char* fileName, MJCFErrorLogger* logger,
|
||||
b3FileUtils fu;
|
||||
|
||||
//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
|
||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
|
||||
bool fileFound = (b3ResourcePath::findResourcePath(fileName,relativeFileName,1024)>0);
|
||||
m_data->m_sourceFileName = relativeFileName;
|
||||
|
||||
std::string xml_string;
|
||||
m_data->m_pathPrefix[0] = 0;
|
||||
@ -1399,21 +1454,26 @@ bool BulletMJCFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
|
||||
void BulletMJCFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const
|
||||
{
|
||||
if (m_data->m_customVisualShapesConverter)
|
||||
{
|
||||
const UrdfLink* link = m_data->getLink(m_data->m_activeModel, urdfIndex);
|
||||
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,inertialFrame, link, 0, colObj, objectIndex);
|
||||
}
|
||||
}
|
||||
|
||||
void BulletMJCFImporter::setBodyUniqueId(int bodyId)
|
||||
{
|
||||
|
||||
m_data->m_activeBodyUniqueId = bodyId;
|
||||
}
|
||||
|
||||
int BulletMJCFImporter::getBodyUniqueId() const
|
||||
{
|
||||
return 0;
|
||||
b3Assert(m_data->m_activeBodyUniqueId != -1);
|
||||
return m_data->m_activeBodyUniqueId;
|
||||
}
|
||||
|
||||
|
||||
static btCollisionShape* MjcfCreateConvexHullFromShapes(std::vector<tinyobj::shape_t>& shapes, const btVector3& geomScale, btScalar collisionMargin)
|
||||
{
|
||||
btCompoundShape* compound = new btCompoundShape();
|
||||
@ -1496,132 +1556,87 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
}
|
||||
case URDF_GEOM_MESH:
|
||||
{
|
||||
//////////////////////
|
||||
if (1)
|
||||
{
|
||||
if (col->m_geometry.m_meshFileName.length())
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
switch (col->m_geometry.m_meshFileType)
|
||||
{
|
||||
const char* filename = col->m_geometry.m_meshFileName.c_str();
|
||||
//b3Printf("mesh->filename=%s\n",filename);
|
||||
char fullPath[1024];
|
||||
int fileType = 0;
|
||||
sprintf(fullPath,"%s%s",pathPrefix,filename);
|
||||
b3FileUtils::toLower(fullPath);
|
||||
char tmpPathPrefix[1024];
|
||||
int maxPathLen = 1024;
|
||||
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
|
||||
|
||||
char collisionPathPrefix[1024];
|
||||
sprintf(collisionPathPrefix,"%s%s",pathPrefix,tmpPathPrefix);
|
||||
|
||||
if (strstr(fullPath,".stl"))
|
||||
case UrdfGeometry::FILE_OBJ:
|
||||
{
|
||||
fileType = MJCF_FILE_STL;
|
||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
glmesh = LoadMeshFromObj(col->m_geometry.m_meshFileName.c_str(), 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, col->m_geometry.m_meshFileName.c_str());
|
||||
//create a convex hull for each shape, and store it in a btCompoundShape
|
||||
|
||||
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
if (strstr(fullPath,".obj"))
|
||||
{
|
||||
fileType = MJCF_FILE_OBJ;
|
||||
}
|
||||
|
||||
sprintf(fullPath,"%s%s",pathPrefix,filename);
|
||||
FILE* f = fopen(fullPath,"rb");
|
||||
if (f)
|
||||
case UrdfGeometry::FILE_STL:
|
||||
{
|
||||
fclose(f);
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
|
||||
|
||||
switch (fileType)
|
||||
{
|
||||
case MJCF_FILE_OBJ:
|
||||
{
|
||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
glmesh = LoadMeshFromObj(fullPath, collisionPathPrefix);
|
||||
}
|
||||
else
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes, fullPath, collisionPathPrefix);
|
||||
//create a convex hull for each shape, and store it in a btCompoundShape
|
||||
glmesh = LoadMeshFromSTL(col->m_geometry.m_meshFileName.c_str());
|
||||
break;
|
||||
}
|
||||
default:
|
||||
b3Warning("%s: Unsupported file type in Collision: %s (maybe .dae?)\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileType);
|
||||
}
|
||||
|
||||
childShape = MjcfCreateConvexHullFromShapes(shapes, col->m_geometry.m_meshScale, m_data->m_defaultCollisionMargin);
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
case MJCF_FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(fullPath);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
b3Warning("Unsupported file type in Collision: %s\n",fullPath);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
if (childShape)
|
||||
{
|
||||
// okay!
|
||||
}
|
||||
else if (!glmesh || glmesh->m_numvertices<=0)
|
||||
{
|
||||
b3Warning("%s: cannot extract anything useful from mesh '%s'\n", col->m_sourceFileLocation.c_str(), col->m_geometry.m_meshFileName.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
|
||||
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
|
||||
//convex->setUserIndex(shapeId);
|
||||
btAlignedObjectArray<btVector3> convertedVerts;
|
||||
convertedVerts.reserve(glmesh->m_numvertices);
|
||||
for (int i=0;i<glmesh->m_numvertices;i++)
|
||||
{
|
||||
convertedVerts.push_back(btVector3(
|
||||
glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
|
||||
glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
|
||||
glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
|
||||
}
|
||||
|
||||
if (!childShape && glmesh && (glmesh->m_numvertices>0))
|
||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||
{
|
||||
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
|
||||
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
|
||||
//convex->setUserIndex(shapeId);
|
||||
btAlignedObjectArray<btVector3> convertedVerts;
|
||||
convertedVerts.reserve(glmesh->m_numvertices);
|
||||
for (int i=0;i<glmesh->m_numvertices;i++)
|
||||
{
|
||||
convertedVerts.push_back(btVector3(
|
||||
glmesh->m_vertices->at(i).xyzw[0]*col->m_geometry.m_meshScale[0],
|
||||
glmesh->m_vertices->at(i).xyzw[1]*col->m_geometry.m_meshScale[1],
|
||||
glmesh->m_vertices->at(i).xyzw[2]*col->m_geometry.m_meshScale[2]));
|
||||
}
|
||||
|
||||
if (col->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
|
||||
btTriangleMesh* meshInterface = new btTriangleMesh();
|
||||
for (int i=0;i<glmesh->m_numIndices/3;i++)
|
||||
{
|
||||
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
|
||||
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
|
||||
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
|
||||
btVector3(v1[0],v1[1],v1[2]),
|
||||
btVector3(v2[0],v2[1],v2[2]));
|
||||
}
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||
childShape = trimesh;
|
||||
} else
|
||||
{
|
||||
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
convexHull->optimizeConvexHull();
|
||||
//convexHull->initializePolyhedralFeatures();
|
||||
convexHull->setMargin(m_data->m_defaultCollisionMargin);
|
||||
childShape = convexHull;
|
||||
}
|
||||
} else
|
||||
{
|
||||
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
|
||||
float* v0 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3)).xyzw;
|
||||
float* v1 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+1)).xyzw;
|
||||
float* v2 = glmesh->m_vertices->at(glmesh->m_indices->at(i*3+2)).xyzw;
|
||||
meshInterface->addTriangle(btVector3(v0[0],v0[1],v0[2]),
|
||||
btVector3(v1[0],v1[1],v1[2]),
|
||||
btVector3(v2[0],v2[1],v2[2]));
|
||||
}
|
||||
|
||||
delete glmesh;
|
||||
|
||||
btBvhTriangleMeshShape* trimesh = new btBvhTriangleMeshShape(meshInterface,true,true);
|
||||
childShape = trimesh;
|
||||
} else
|
||||
{
|
||||
b3Warning("mesh geometry not found %s\n",fullPath);
|
||||
btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
|
||||
convexHull->optimizeConvexHull();
|
||||
//convexHull->initializePolyhedralFeatures();
|
||||
convexHull->setMargin(m_data->m_defaultCollisionMargin);
|
||||
childShape = convexHull;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
//////////////////////
|
||||
delete glmesh;
|
||||
break;
|
||||
}
|
||||
|
||||
case URDF_GEOM_CAPSULE:
|
||||
{
|
||||
//todo: convert fromto to btCapsuleShape + local btTransform
|
||||
@ -1636,7 +1651,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
btVector3 fromto[2] = {f,t};
|
||||
btScalar radii[2] = {btScalar(col->m_geometry.m_capsuleRadius)
|
||||
,btScalar(col->m_geometry.m_capsuleRadius)};
|
||||
|
||||
|
||||
btMultiSphereShape* ms = new btMultiSphereShape(fromto,radii,2);
|
||||
childShape = ms;
|
||||
} else
|
||||
@ -1647,11 +1662,8 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
} // switch geom
|
||||
|
||||
}
|
||||
}
|
||||
if (childShape)
|
||||
{
|
||||
m_data->m_allocatedCollisionShapes.push_back(childShape);
|
||||
|
||||
@ -18,9 +18,8 @@ class BulletMJCFImporter : public URDFImporterInterface
|
||||
{
|
||||
struct BulletMJCFImporterInternalData* m_data;
|
||||
|
||||
|
||||
public:
|
||||
BulletMJCFImporter(struct GUIHelperInterface* helper);
|
||||
BulletMJCFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter);
|
||||
virtual ~BulletMJCFImporter();
|
||||
|
||||
virtual bool parseMJCFString(const char* xmlString, MJCFErrorLogger* logger);
|
||||
@ -66,7 +65,7 @@ public:
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const;
|
||||
virtual void setBodyUniqueId(int bodyId);
|
||||
virtual int getBodyUniqueId() const;
|
||||
|
||||
|
||||
@ -188,12 +188,12 @@ void ImportMJCFSetup::initPhysics()
|
||||
m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
|
||||
|
||||
//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||
btIDebugDraw::DBG_DrawConstraints
|
||||
+btIDebugDraw::DBG_DrawContactPoints
|
||||
+btIDebugDraw::DBG_DrawAabb
|
||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(
|
||||
btIDebugDraw::DBG_DrawConstraints
|
||||
+btIDebugDraw::DBG_DrawContactPoints
|
||||
+btIDebugDraw::DBG_DrawAabb
|
||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
|
||||
|
||||
if (m_guiHelper->getParameterInterface())
|
||||
@ -203,20 +203,23 @@ void ImportMJCFSetup::initPhysics()
|
||||
slider.m_maxVal = 10;
|
||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||
}
|
||||
|
||||
BulletMJCFImporter importer(m_guiHelper);
|
||||
|
||||
BulletMJCFImporter importer(m_guiHelper, 0);
|
||||
MyMJCFLogger logger;
|
||||
bool result = importer.loadMJCF(m_fileName,&logger);
|
||||
if (result)
|
||||
{
|
||||
btTransform rootTrans;
|
||||
rootTrans.setIdentity();
|
||||
|
||||
for (int m =0; m<importer.getNumModels();m++)
|
||||
{
|
||||
btTransform rootTrans;
|
||||
rootTrans.setIdentity();
|
||||
|
||||
for (int m =0; m<importer.getNumModels();m++)
|
||||
{
|
||||
importer.activateModel(m);
|
||||
|
||||
// normally used with PhysicsServerCommandProcessor that allocates unique ids to multibodies,
|
||||
// emulate this behavior here:
|
||||
importer.setBodyUniqueId(m);
|
||||
|
||||
importer.activateModel(m);
|
||||
|
||||
btMultiBody* mb = 0;
|
||||
|
||||
|
||||
@ -226,18 +229,17 @@ void ImportMJCFSetup::initPhysics()
|
||||
MyMultiBodyCreator creation(m_guiHelper);
|
||||
|
||||
rootTrans.setIdentity();
|
||||
importer.getRootTransformInWorld(rootTrans);
|
||||
importer.getRootTransformInWorld(rootTrans);
|
||||
|
||||
ConvertURDF2Bullet(importer,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,importer.getPathPrefix(),CUF_USE_MJCF);
|
||||
|
||||
mb = creation.getBulletMultiBody();
|
||||
if (mb)
|
||||
{
|
||||
printf("first MJCF file converted!\n");
|
||||
std::string* name =
|
||||
new std::string(importer.getLinkName(
|
||||
importer.getRootLinkIndex()));
|
||||
m_nameMemory.push_back(name);
|
||||
if (mb)
|
||||
{
|
||||
std::string* name =
|
||||
new std::string(importer.getLinkName(
|
||||
importer.getRootLinkIndex()));
|
||||
m_nameMemory.push_back(name);
|
||||
#ifdef TEST_MULTIBODY_SERIALIZATION
|
||||
s->registerNameForPointer(name->c_str(),name->c_str());
|
||||
#endif//TEST_MULTIBODY_SERIALIZATION
|
||||
@ -287,10 +289,11 @@ void ImportMJCFSetup::initPhysics()
|
||||
m_data->m_numMotors++;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
// not multibody
|
||||
if (1)
|
||||
{
|
||||
//create motors for each generic joint
|
||||
|
||||
@ -75,16 +75,6 @@ void BulletURDFImporter::printTree()
|
||||
// btAssert(0);
|
||||
}
|
||||
|
||||
|
||||
enum MyFileType
|
||||
{
|
||||
FILE_STL=1,
|
||||
FILE_COLLADA=2,
|
||||
FILE_OBJ=3,
|
||||
};
|
||||
|
||||
|
||||
|
||||
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
|
||||
{
|
||||
m_data = new BulletURDFInternalData;
|
||||
@ -572,8 +562,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
|
||||
case URDF_GEOM_CYLINDER:
|
||||
{
|
||||
btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
|
||||
btScalar cylLength = collision->m_geometry.m_cylinderLength;
|
||||
btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
|
||||
btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight;
|
||||
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
||||
@ -624,7 +614,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
switch (collision->m_geometry.m_meshFileType)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
case UrdfGeometry::FILE_OBJ:
|
||||
if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
|
||||
{
|
||||
glmesh = LoadMeshFromObj(collision->m_geometry.m_meshFileName.c_str(), 0);
|
||||
@ -640,11 +630,11 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
|
||||
}
|
||||
break;
|
||||
|
||||
case FILE_STL:
|
||||
case UrdfGeometry::FILE_STL:
|
||||
glmesh = LoadMeshFromSTL(collision->m_geometry.m_meshFileName.c_str());
|
||||
break;
|
||||
|
||||
case FILE_COLLADA:
|
||||
case UrdfGeometry::FILE_COLLADA:
|
||||
{
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
@ -795,8 +785,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
for (int i = 0; i<numSteps; i++)
|
||||
{
|
||||
|
||||
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
|
||||
btScalar cylLength = visual->m_geometry.m_cylinderLength;
|
||||
btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
|
||||
btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight;
|
||||
|
||||
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
|
||||
vertices.push_back(vert);
|
||||
@ -833,7 +823,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
{
|
||||
switch (visual->m_geometry.m_meshFileType)
|
||||
{
|
||||
case FILE_OBJ:
|
||||
case UrdfGeometry::FILE_OBJ:
|
||||
{
|
||||
b3ImportMeshData meshData;
|
||||
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(visual->m_geometry.m_meshFileName, meshData))
|
||||
@ -852,13 +842,13 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_STL:
|
||||
case UrdfGeometry::FILE_STL:
|
||||
{
|
||||
glmesh = LoadMeshFromSTL(visual->m_geometry.m_meshFileName.c_str());
|
||||
break;
|
||||
}
|
||||
|
||||
case FILE_COLLADA:
|
||||
case UrdfGeometry::FILE_COLLADA:
|
||||
{
|
||||
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
|
||||
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
|
||||
@ -1128,15 +1118,17 @@ bool BulletURDFImporter::getLinkContactInfo(int linkIndex, URDFLinkContactInfo&
|
||||
return false;
|
||||
}
|
||||
|
||||
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
|
||||
void BulletURDFImporter::convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& localInertiaFrame, class btCollisionObject* colObj, int bodyUniqueId) const
|
||||
{
|
||||
|
||||
if (m_data->m_customVisualShapesConverter)
|
||||
{
|
||||
const UrdfModel& model = m_data->m_urdfParser.getModel();
|
||||
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, model, colObj, bodyUniqueId);
|
||||
UrdfLink*const* linkPtr = model.m_links.getAtIndex(urdfIndex);
|
||||
if (linkPtr)
|
||||
{
|
||||
m_data->m_customVisualShapesConverter->convertVisualShapes(linkIndex,pathPrefix,localInertiaFrame, *linkPtr, &model, colObj, bodyUniqueId);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int BulletURDFImporter::getNumAllocatedCollisionShapes() const
|
||||
|
||||
@ -51,7 +51,7 @@ public:
|
||||
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const;
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int bodyUniqueId) const;
|
||||
|
||||
///todo(erwincoumans) refactor this convertLinkCollisionShapes/memory allocation
|
||||
|
||||
|
||||
@ -1,9 +1,14 @@
|
||||
#ifndef LINK_VISUAL_SHAPES_CONVERTER_H
|
||||
#define LINK_VISUAL_SHAPES_CONVERTER_H
|
||||
|
||||
struct UrdfLink;
|
||||
struct UrdfModel;
|
||||
class btTransform;
|
||||
class btCollisionObject;
|
||||
|
||||
struct LinkVisualShapesConverter
|
||||
{
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const class btTransform& localInertiaFrame, const struct UrdfModel& model, class btCollisionObject* colObj, int objectIndex)=0;
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex) =0;
|
||||
};
|
||||
|
||||
#endif //LINK_VISUAL_SHAPES_CONVERTER_H
|
||||
|
||||
@ -315,7 +315,7 @@ void ConvertURDF2BulletInternal(
|
||||
|
||||
|
||||
|
||||
//untested: u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,body);
|
||||
//untested: u2b.convertLinkVisualShapes2(linkIndex,urdfLinkIndex,pathPrefix,localInertialFrame,body);
|
||||
} else
|
||||
{
|
||||
if (cache.m_bulletMultiBody==0)
|
||||
@ -473,8 +473,8 @@ void ConvertURDF2BulletInternal(
|
||||
btVector4 color = selectColor2();//(0.0,0.0,0.5);
|
||||
u2b.getLinkColor(urdfLinkIndex,color);
|
||||
creation.createCollisionObjectGraphicsInstance(urdfLinkIndex,col,color);
|
||||
|
||||
u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,col, u2b.getBodyUniqueId());
|
||||
|
||||
u2b.convertLinkVisualShapes2(mbLinkIndex, urdfLinkIndex, pathPrefix, localInertialFrame,col, u2b.getBodyUniqueId());
|
||||
|
||||
URDFLinkContactInfo contactInfo;
|
||||
u2b.getLinkContactInfo(urdfLinkIndex,contactInfo);
|
||||
@ -491,7 +491,7 @@ void ConvertURDF2BulletInternal(
|
||||
}
|
||||
} else
|
||||
{
|
||||
//u2b.convertLinkVisualShapes2(urdfLinkIndex,pathPrefix,localInertialFrame,compoundShape);
|
||||
//u2b.convertLinkVisualShapes2(urdfLinkIndex,urdfIndex,pathPrefix,localInertialFrame,compoundShape);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -49,7 +49,7 @@ public:
|
||||
///quick hack: need to rethink the API/dependencies of this
|
||||
virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;}
|
||||
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
|
||||
virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const { }
|
||||
virtual void setBodyUniqueId(int bodyId) {}
|
||||
virtual int getBodyUniqueId() const { return 0;}
|
||||
|
||||
|
||||
@ -401,8 +401,9 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
||||
logger->reportError("Cylinder shape must have both length and radius attributes");
|
||||
return false;
|
||||
}
|
||||
geom.m_cylinderRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||
geom.m_cylinderLength = urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
geom.m_hasFromTo = false;
|
||||
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||
geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length"));
|
||||
|
||||
}
|
||||
else if (type_name == "capsule")
|
||||
@ -575,7 +576,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
||||
if (name_char)
|
||||
visual.m_name = name_char;
|
||||
|
||||
visual.m_hasLocalMaterial = false;
|
||||
visual.m_geometry.m_hasLocalMaterial = false;
|
||||
|
||||
// Material
|
||||
TiXmlElement *mat = config->FirstChildElement("material");
|
||||
@ -597,7 +598,7 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
||||
matPtr->m_rgbaColor = rgba;
|
||||
|
||||
visual.m_materialName = matPtr->m_name;
|
||||
visual.m_hasLocalMaterial = true;
|
||||
visual.m_geometry.m_hasLocalMaterial = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -616,11 +617,11 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
|
||||
TiXmlElement *c = mat->FirstChildElement("color");
|
||||
if (t||c)
|
||||
{
|
||||
if (parseMaterial(visual.m_localMaterial, mat,logger))
|
||||
if (parseMaterial(visual.m_geometry.m_localMaterial, mat,logger))
|
||||
{
|
||||
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_localMaterial);
|
||||
UrdfMaterial* matPtr = new UrdfMaterial(visual.m_geometry.m_localMaterial);
|
||||
model.m_materials.insert(matPtr->m_name.c_str(),matPtr);
|
||||
visual.m_hasLocalMaterial = true;
|
||||
visual.m_geometry.m_hasLocalMaterial = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -1415,12 +1416,12 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
for (int i=0;i<link->m_visualArray.size();i++)
|
||||
{
|
||||
UrdfVisual& vis = link->m_visualArray.at(i);
|
||||
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
|
||||
if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
|
||||
{
|
||||
UrdfMaterial** mat = m_urdf2Model.m_materials.find(vis.m_materialName.c_str());
|
||||
if (mat && *mat)
|
||||
{
|
||||
vis.m_localMaterial = **mat;
|
||||
vis.m_geometry.m_localMaterial = **mat;
|
||||
} else
|
||||
{
|
||||
//logger->reportError("Cannot find material with name:");
|
||||
@ -1610,12 +1611,12 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
|
||||
for (int i=0;i<link->m_visualArray.size();i++)
|
||||
{
|
||||
UrdfVisual& vis = link->m_visualArray.at(i);
|
||||
if (!vis.m_hasLocalMaterial && vis.m_materialName.c_str())
|
||||
if (!vis.m_geometry.m_hasLocalMaterial && vis.m_materialName.c_str())
|
||||
{
|
||||
UrdfMaterial** mat = localModel->m_materials.find(vis.m_materialName.c_str());
|
||||
if (mat && *mat)
|
||||
{
|
||||
vis.m_localMaterial = **mat;
|
||||
vis.m_geometry.m_localMaterial = **mat;
|
||||
} else
|
||||
{
|
||||
//logger->reportError("Cannot find material with name:");
|
||||
|
||||
@ -18,9 +18,13 @@ struct ErrorLogger
|
||||
|
||||
struct UrdfMaterial
|
||||
{
|
||||
std::string m_name;
|
||||
std::string m_name;
|
||||
std::string m_textureFilename;
|
||||
btVector4 m_rgbaColor;
|
||||
btVector4 m_rgbaColor; // [0]==r [1]==g [2]==b [3]==a
|
||||
UrdfMaterial():
|
||||
m_rgbaColor(0.8, 0.8, 0.8, 1)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct UrdfInertia
|
||||
@ -66,9 +70,6 @@ struct UrdfGeometry
|
||||
btVector3 m_capsuleFrom;
|
||||
btVector3 m_capsuleTo;
|
||||
|
||||
double m_cylinderRadius;
|
||||
double m_cylinderLength;
|
||||
|
||||
btVector3 m_planeNormal;
|
||||
|
||||
enum {
|
||||
@ -79,32 +80,30 @@ struct UrdfGeometry
|
||||
int m_meshFileType;
|
||||
std::string m_meshFileName;
|
||||
btVector3 m_meshScale;
|
||||
|
||||
UrdfMaterial m_localMaterial;
|
||||
bool m_hasLocalMaterial;
|
||||
};
|
||||
|
||||
bool findExistingMeshFile(const std::string& urdf_path, std::string fn,
|
||||
const std::string& error_message_prefix,
|
||||
std::string* out_found_filename, int* out_type); // intended to fill UrdfGeometry::m_meshFileName and Type, but can be used elsewhere
|
||||
|
||||
struct UrdfVisual
|
||||
struct UrdfShape
|
||||
{
|
||||
std::string m_sourceFileLocation;
|
||||
btTransform m_linkLocalFrame;
|
||||
UrdfGeometry m_geometry;
|
||||
std::string m_name;
|
||||
std::string m_materialName;
|
||||
bool m_hasLocalMaterial;
|
||||
UrdfMaterial m_localMaterial;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
struct UrdfCollision
|
||||
struct UrdfVisual: UrdfShape
|
||||
{
|
||||
std::string m_materialName;
|
||||
};
|
||||
|
||||
struct UrdfCollision: UrdfShape
|
||||
{
|
||||
std::string m_sourceFileLocation;
|
||||
btTransform m_linkLocalFrame;
|
||||
UrdfGeometry m_geometry;
|
||||
std::string m_name;
|
||||
int m_flags;
|
||||
int m_collisionGroup;
|
||||
int m_collisionMask;
|
||||
|
||||
@ -1067,4 +1067,4 @@ void PhysicsDirect::setTimeOut(double timeOutInSeconds)
|
||||
double PhysicsDirect::getTimeOut() const
|
||||
{
|
||||
return m_data->m_timeOutInSeconds;
|
||||
}
|
||||
}
|
||||
|
||||
@ -1499,7 +1499,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
|
||||
|
||||
m_data->m_sdfRecentLoadedBodies.clear();
|
||||
|
||||
BulletMJCFImporter u2b(m_data->m_guiHelper); //, &m_data->m_visualConverter
|
||||
BulletMJCFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
||||
|
||||
bool useFixedBase = false;
|
||||
MyMJCFLogger2 logger;
|
||||
@ -4612,20 +4612,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
int remain = totalNumVisualShapes - clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
int shapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
|
||||
m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
|
||||
int success = m_data->m_visualConverter.getVisualShapesData(clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId,
|
||||
shapeIndex,
|
||||
visualShapeStoragePtr);
|
||||
|
||||
|
||||
//m_visualConverter
|
||||
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
|
||||
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
|
||||
serverCmd.m_type =CMD_VISUAL_SHAPE_INFO_COMPLETED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
if (success) {
|
||||
serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes = remain-1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied = 1;
|
||||
serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex = clientCmd.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex;
|
||||
serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId = clientCmd.m_requestVisualShapeDataArguments.m_bodyUniqueId;
|
||||
serverCmd.m_numDataStreamBytes = sizeof(b3VisualShapeData)*serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
|
||||
serverCmd.m_type = CMD_VISUAL_SHAPE_INFO_COMPLETED;
|
||||
}
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_UPDATE_VISUAL_SHAPE:
|
||||
{
|
||||
|
||||
@ -170,7 +170,7 @@ void TinyRendererVisualShapeConverter::setLightSpecularCoeff(float specularCoeff
|
||||
m_data->m_hasLightSpecularCoeff = true;
|
||||
}
|
||||
|
||||
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
|
||||
void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut, b3VisualShapeData& visualShapeOut)
|
||||
{
|
||||
|
||||
visualShapeOut.m_visualGeometryType = visual->m_geometry.m_type;
|
||||
@ -178,6 +178,12 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
visualShapeOut.m_dimensions[1] = 0;
|
||||
visualShapeOut.m_dimensions[2] = 0;
|
||||
visualShapeOut.m_meshAssetFileName[0] = 0;
|
||||
if (visual->m_geometry.m_hasLocalMaterial) {
|
||||
visualShapeOut.m_rgbaColor[0] = visual->m_geometry.m_localMaterial.m_rgbaColor[0];
|
||||
visualShapeOut.m_rgbaColor[1] = visual->m_geometry.m_localMaterial.m_rgbaColor[1];
|
||||
visualShapeOut.m_rgbaColor[2] = visual->m_geometry.m_localMaterial.m_rgbaColor[2];
|
||||
visualShapeOut.m_rgbaColor[3] = visual->m_geometry.m_localMaterial.m_rgbaColor[3];
|
||||
}
|
||||
|
||||
GLInstanceGraphicsShape* glmesh = 0;
|
||||
|
||||
@ -186,25 +192,63 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
switch (visual->m_geometry.m_type)
|
||||
{
|
||||
case URDF_GEOM_CYLINDER:
|
||||
case URDF_GEOM_CAPSULE:
|
||||
{
|
||||
btVector3 p1 = visual->m_geometry.m_capsuleFrom;
|
||||
btVector3 p2 = visual->m_geometry.m_capsuleTo;
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
btScalar rad, len;
|
||||
if (visual->m_geometry.m_hasFromTo) {
|
||||
btVector3 v = p2 - p1;
|
||||
btVector3 center = (p2 + p1) * 0.5;
|
||||
btVector3 up_vector(0,0,1);
|
||||
btVector3 dir = v.normalized();
|
||||
btVector3 axis = dir.cross(up_vector);
|
||||
if (axis.fuzzyZero())
|
||||
{
|
||||
axis = btVector3(0,0,1);
|
||||
}
|
||||
else
|
||||
{
|
||||
axis.normalize();
|
||||
}
|
||||
btQuaternion q(axis, -acos(dir.dot(up_vector)));
|
||||
btTransform capsule_orient(q, center);
|
||||
tr = visual->m_linkLocalFrame * capsule_orient;
|
||||
len = v.length();
|
||||
rad = visual->m_geometry.m_capsuleRadius;
|
||||
} else {
|
||||
tr = visual->m_linkLocalFrame;
|
||||
len = visual->m_geometry.m_capsuleHalfHeight;
|
||||
rad = visual->m_geometry.m_capsuleRadius;
|
||||
}
|
||||
visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];
|
||||
visualShapeOut.m_localVisualFrame[1] = tr.getOrigin()[1];
|
||||
visualShapeOut.m_localVisualFrame[2] = tr.getOrigin()[2];
|
||||
visualShapeOut.m_localVisualFrame[3] = tr.getRotation()[0];
|
||||
visualShapeOut.m_localVisualFrame[4] = tr.getRotation()[1];
|
||||
visualShapeOut.m_localVisualFrame[5] = tr.getRotation()[2];
|
||||
visualShapeOut.m_localVisualFrame[6] = tr.getRotation()[3];
|
||||
visualShapeOut.m_dimensions[0] = len;
|
||||
visualShapeOut.m_dimensions[1] = rad;
|
||||
|
||||
btAlignedObjectArray<btVector3> vertices;
|
||||
|
||||
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_cylinderLength;
|
||||
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_cylinderRadius;
|
||||
|
||||
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
|
||||
int numSteps = 32;
|
||||
for (int i = 0; i<numSteps; i++)
|
||||
{
|
||||
|
||||
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
|
||||
btScalar cylLength = visual->m_geometry.m_cylinderLength;
|
||||
|
||||
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
|
||||
btVector3 vert(rad*btSin(SIMD_2_PI*(float(i) / numSteps)), rad*btCos(SIMD_2_PI*(float(i) / numSteps)), len / 2.);
|
||||
vertices.push_back(vert);
|
||||
vert[2] = -cylLength / 2.;
|
||||
vert[2] = -len / 2.;
|
||||
vertices.push_back(vert);
|
||||
}
|
||||
if (visual->m_geometry.m_type==URDF_GEOM_CAPSULE) {
|
||||
// TODO: check if tiny renderer works with that, didn't check -- Oleg
|
||||
btVector3 pole1(0, 0, + len / 2. + rad);
|
||||
btVector3 pole2(0, 0, - len / 2. - rad);
|
||||
vertices.push_back(pole1);
|
||||
vertices.push_back(pole2);
|
||||
}
|
||||
|
||||
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
|
||||
cylZShape->setMargin(0.001);
|
||||
@ -228,7 +272,7 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
case URDF_GEOM_SPHERE:
|
||||
{
|
||||
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_sphereRadius;
|
||||
|
||||
|
||||
btScalar radius = visual->m_geometry.m_sphereRadius;
|
||||
btSphereShape* sphereShape = new btSphereShape(radius);
|
||||
convexColShape = sphereShape;
|
||||
@ -244,14 +288,6 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
|
||||
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
|
||||
|
||||
visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0];
|
||||
visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1];
|
||||
visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2];
|
||||
visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0];
|
||||
visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1];
|
||||
visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2];
|
||||
visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3];
|
||||
|
||||
switch (visual->m_geometry.m_meshFileType)
|
||||
{
|
||||
case UrdfGeometry::FILE_OBJ:
|
||||
@ -467,17 +503,30 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
|
||||
|
||||
|
||||
|
||||
void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int bodyUniqueId)
|
||||
void TinyRendererVisualShapeConverter::convertVisualShapes(
|
||||
int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
|
||||
const UrdfLink* linkPtr, const UrdfModel* model,
|
||||
class btCollisionObject* colObj, int bodyUniqueId)
|
||||
{
|
||||
|
||||
|
||||
UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
|
||||
btAssert(linkPtr); // TODO: remove if (not doing it now, because diff will be 50+ lines)
|
||||
if (linkPtr)
|
||||
{
|
||||
const btArray<UrdfVisual>* shapeArray;
|
||||
bool useVisual;
|
||||
int cnt = 0;
|
||||
if (linkPtr->m_visualArray.size() > 0)
|
||||
{
|
||||
useVisual = true;
|
||||
cnt = linkPtr->m_visualArray.size();
|
||||
}
|
||||
else
|
||||
{
|
||||
// We have to see something, take collision shape. Useful for MuJoCo xml, where there is not visual shape.
|
||||
useVisual = false;
|
||||
cnt = linkPtr->m_collisionArray.size();
|
||||
}
|
||||
|
||||
const UrdfLink* link = *linkPtr;
|
||||
|
||||
for (int v1 = 0; v1 < link->m_visualArray.size();v1++)
|
||||
for (int v1=0; v1<cnt; v1++)
|
||||
{
|
||||
btAlignedObjectArray<MyTexture2> textures;
|
||||
btAlignedObjectArray<GLInstanceVertex> vertices;
|
||||
@ -485,20 +534,28 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
|
||||
btTransform startTrans; startTrans.setIdentity();
|
||||
//int graphicsIndex = -1;
|
||||
|
||||
const UrdfVisual& vis = link->m_visualArray[v1];
|
||||
btTransform childTrans = vis.m_linkLocalFrame;
|
||||
btHashString matName(vis.m_materialName.c_str());
|
||||
UrdfMaterial *const * matPtr = model.m_materials[matName];
|
||||
|
||||
float rgbaColor[4] = {1,1,1,1};
|
||||
|
||||
if (matPtr)
|
||||
const UrdfShape* vis;
|
||||
if (useVisual) {
|
||||
vis = &linkPtr->m_visualArray[v1];
|
||||
} else {
|
||||
vis = &linkPtr->m_collisionArray[v1];
|
||||
}
|
||||
btTransform childTrans = vis->m_linkLocalFrame;
|
||||
|
||||
float rgbaColor[4] = {1,1,1,1};
|
||||
if (model && useVisual)
|
||||
{
|
||||
UrdfMaterial *const mat = *matPtr;
|
||||
for (int i=0;i<4;i++)
|
||||
rgbaColor[i] = mat->m_rgbaColor[i];
|
||||
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
|
||||
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
|
||||
btHashString matName(linkPtr->m_visualArray[v1].m_materialName.c_str());
|
||||
UrdfMaterial*const* matPtr = model->m_materials[matName];
|
||||
if (matPtr)
|
||||
{
|
||||
for (int i=0; i<4; i++)
|
||||
{
|
||||
rgbaColor[i] = (*matPtr)->m_rgbaColor[i];
|
||||
}
|
||||
//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
|
||||
//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
|
||||
}
|
||||
}
|
||||
|
||||
TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
|
||||
@ -513,19 +570,19 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
|
||||
b3VisualShapeData visualShape;
|
||||
visualShape.m_objectUniqueId = bodyUniqueId;
|
||||
visualShape.m_linkIndex = linkIndex;
|
||||
visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0];
|
||||
visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1];
|
||||
visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2];
|
||||
visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0];
|
||||
visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1];
|
||||
visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2];
|
||||
visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3];
|
||||
visualShape.m_rgbaColor[0] = rgbaColor[0];
|
||||
visualShape.m_rgbaColor[1] = rgbaColor[1];
|
||||
visualShape.m_rgbaColor[2] = rgbaColor[2];
|
||||
visualShape.m_rgbaColor[3] = rgbaColor[3];
|
||||
visualShape.m_localVisualFrame[0] = vis->m_linkLocalFrame.getOrigin()[0];
|
||||
visualShape.m_localVisualFrame[1] = vis->m_linkLocalFrame.getOrigin()[1];
|
||||
visualShape.m_localVisualFrame[2] = vis->m_linkLocalFrame.getOrigin()[2];
|
||||
visualShape.m_localVisualFrame[3] = vis->m_linkLocalFrame.getRotation()[0];
|
||||
visualShape.m_localVisualFrame[4] = vis->m_linkLocalFrame.getRotation()[1];
|
||||
visualShape.m_localVisualFrame[5] = vis->m_linkLocalFrame.getRotation()[2];
|
||||
visualShape.m_localVisualFrame[6] = vis->m_linkLocalFrame.getRotation()[3];
|
||||
visualShape.m_rgbaColor[0] = rgbaColor[0];
|
||||
visualShape.m_rgbaColor[1] = rgbaColor[1];
|
||||
visualShape.m_rgbaColor[2] = rgbaColor[2];
|
||||
visualShape.m_rgbaColor[3] = rgbaColor[3];
|
||||
|
||||
convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
|
||||
convertURDFToVisualShape(vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
|
||||
m_data->m_visualShapes.push_back(visualShape);
|
||||
|
||||
if (vertices.size() && indices.size())
|
||||
|
||||
@ -16,7 +16,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
|
||||
|
||||
virtual ~TinyRendererVisualShapeConverter();
|
||||
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
|
||||
virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfLink* linkPtr, const UrdfModel* model, class btCollisionObject* colShape, int objectIndex);
|
||||
|
||||
virtual int getNumVisualShapes(int bodyUniqueId);
|
||||
|
||||
|
||||
@ -15,14 +15,8 @@ BUTTONS=6
|
||||
gripper_max_joint = 0.550569
|
||||
while True:
|
||||
events = p.getVREvents()
|
||||
|
||||
for e in (events):
|
||||
if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes
|
||||
p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
|
||||
p.setJointMotorControl2(pr2_gripper, 0, controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,
|
||||
force=1.0)
|
||||
p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL,
|
||||
targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,
|
||||
force=1.1)
|
||||
|
||||
for e in (events):
|
||||
if e[CONTROLLER_ID] == 3: # To make sure we only get the value for one of the remotes
|
||||
p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=500)
|
||||
p.setJointMotorControl2(pr2_gripper, 0, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.0)
|
||||
p.setJointMotorControl2(pr2_gripper, 2, controlMode=p.POSITION_CONTROL,targetPosition=gripper_max_joint - e[ANALOG] * gripper_max_joint,force=1.1)
|
||||
Loading…
Reference in New Issue
Block a user