mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 13:20:07 +00:00
Merge remote-tracking branch 'origin/master' into KKT
This commit is contained in:
commit
78aebd44e2
@ -437,7 +437,7 @@ const char **InPlaceParser::GetArglist(char *line, int &count) // convert sourc
|
||||
{
|
||||
const char **ret = 0;
|
||||
|
||||
const char *argv[MAXARGS];
|
||||
static const char *argv[MAXARGS];
|
||||
int argc = 0;
|
||||
|
||||
char *foo = line;
|
||||
|
13
data/torus_deform.urdf
Normal file
13
data/torus_deform.urdf
Normal file
@ -0,0 +1,13 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="torus">
|
||||
<deformable name="torus">
|
||||
<inertial>
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
|
||||
</inertial>
|
||||
<collision_margin value="0.006"/>
|
||||
<friction value= "0.5"/>
|
||||
<neohookean mu= "60" lambda= "200" damping= "0.01" />
|
||||
<visual filename="torus.vtk"/>
|
||||
</deformable>
|
||||
</robot>
|
@ -51,7 +51,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler
|
||||
m_showValue(true)
|
||||
|
||||
{
|
||||
memcpy(m_variableName, varName, strlen(varName) + 1);
|
||||
strncpy(m_variableName, varName, sizeof(m_variableName));
|
||||
}
|
||||
|
||||
void SliderMoved(Gwen::Controls::Base* pControl)
|
||||
@ -85,7 +85,7 @@ struct MySliderEventHandler : public Gwen::Event::Handler
|
||||
if (m_showValue)
|
||||
{
|
||||
char txt[1024];
|
||||
sprintf(txt, "%s : %.3f", m_variableName, val);
|
||||
snprintf(txt, sizeof(txt), "%s : %.3f", m_variableName, val);
|
||||
m_label->SetText(txt);
|
||||
}
|
||||
}
|
||||
@ -231,7 +231,7 @@ void GwenParameterInterface::registerSliderFloatParameter(SliderParams& params)
|
||||
}
|
||||
pSlider->SetValue(*params.m_paramValuePointer); //dimensions[i] );
|
||||
char labelName[1024];
|
||||
sprintf(labelName, "%s", params.m_name); //axisNames[0]);
|
||||
snprintf(labelName, sizeof(labelName), "%s", params.m_name); //axisNames[0]);
|
||||
MySliderEventHandler<btScalar>* handler = new MySliderEventHandler<btScalar>(labelName, label, pSlider, params.m_paramValuePointer, params.m_callback, params.m_userPointer);
|
||||
handler->m_showValue = params.m_showValues;
|
||||
m_paramInternalData->m_sliderEventHandlers.push_back(handler);
|
||||
|
@ -796,7 +796,7 @@ struct BulletMJCFImporterInternalData
|
||||
bool lastThree = false;
|
||||
parseVector3(size, sizeStr, logger, lastThree);
|
||||
}
|
||||
geom.m_boxSize = size;
|
||||
geom.m_boxSize = 2*size;
|
||||
handledGeomType = true;
|
||||
}
|
||||
if (geomType == "box")
|
||||
@ -809,7 +809,7 @@ struct BulletMJCFImporterInternalData
|
||||
parseVector3(size, sizeStr, logger, lastThree);
|
||||
}
|
||||
geom.m_type = URDF_GEOM_BOX;
|
||||
geom.m_boxSize = size;
|
||||
geom.m_boxSize = 2*size;
|
||||
handledGeomType = true;
|
||||
}
|
||||
|
||||
@ -1012,7 +1012,7 @@ struct BulletMJCFImporterInternalData
|
||||
}
|
||||
case URDF_GEOM_BOX:
|
||||
{
|
||||
totalVolume += 8. * col->m_geometry.m_boxSize[0] *
|
||||
totalVolume += col->m_geometry.m_boxSize[0] *
|
||||
col->m_geometry.m_boxSize[1] *
|
||||
col->m_geometry.m_boxSize[2];
|
||||
break;
|
||||
@ -1890,7 +1890,7 @@ void BulletMJCFImporter::convertURDFToVisualShapeInternal(const UrdfVisual* visu
|
||||
|
||||
case URDF_GEOM_BOX:
|
||||
{
|
||||
btVector3 extents = visual->m_geometry.m_boxSize;
|
||||
btVector3 extents = 0.5*visual->m_geometry.m_boxSize;
|
||||
btBoxShape* boxShape = new btBoxShape(extents * 0.5f);
|
||||
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
|
||||
convexColShape = boxShape;
|
||||
@ -2336,7 +2336,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
|
||||
}
|
||||
case URDF_GEOM_BOX:
|
||||
{
|
||||
childShape = new btBoxShape(col->m_geometry.m_boxSize);
|
||||
childShape = new btBoxShape(0.5*col->m_geometry.m_boxSize);
|
||||
break;
|
||||
}
|
||||
case URDF_GEOM_CYLINDER:
|
||||
|
@ -1533,3 +1533,8 @@ class btCompoundShape* BulletURDFImporter::convertLinkCollisionShapes(int linkIn
|
||||
const struct UrdfModel* BulletURDFImporter::getUrdfModel() const {
|
||||
return &m_data->m_urdfParser.getModel();
|
||||
};
|
||||
|
||||
const struct UrdfDeformable& BulletURDFImporter::getDeformableModel() const
|
||||
{
|
||||
return m_data->m_urdfParser.getDeformable();
|
||||
}
|
||||
|
@ -2,8 +2,8 @@
|
||||
#define BULLET_URDF_IMPORTER_H
|
||||
|
||||
#include "URDFImporterInterface.h"
|
||||
|
||||
#include "UrdfRenderingInterface.h"
|
||||
#include "UrdfParser.h"
|
||||
|
||||
struct BulletURDFTexture
|
||||
{
|
||||
@ -91,6 +91,7 @@ public:
|
||||
|
||||
virtual void setEnableTinyRenderer(bool enable);
|
||||
void convertURDFToVisualShapeInternal(const struct UrdfVisual* visual, const char* urdfPathPrefix, const class btTransform& visualTransform, btAlignedObjectArray<struct GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<struct BulletURDFTexture>& texturesOut, struct b3ImportMeshData& meshData) const;
|
||||
const struct UrdfDeformable& getDeformableModel() const;
|
||||
};
|
||||
|
||||
#endif //BULLET_URDF_IMPORTER_H
|
||||
|
@ -1098,6 +1098,155 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, XMLElement* config,
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UrdfParser::parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger)
|
||||
{
|
||||
const char* mu = config->Attribute("mu");
|
||||
const char* lambda = config->Attribute("lambda");
|
||||
const char* damping = config->Attribute("damping");
|
||||
if (!mu || !lambda)
|
||||
{
|
||||
logger->reportError("expected mu lambda for LameCoefficients.");
|
||||
return false;
|
||||
}
|
||||
lameCoefficients.mu = urdfLexicalCast<double>(mu);
|
||||
lameCoefficients.lambda = urdfLexicalCast<double>(lambda);
|
||||
if (damping)
|
||||
lameCoefficients.damping = urdfLexicalCast<double>(damping);
|
||||
else
|
||||
lameCoefficients.damping = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UrdfParser::parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger)
|
||||
{
|
||||
UrdfDeformable& deformable = model.m_deformable;
|
||||
const char* deformableName = config->Attribute("name");
|
||||
if (!deformableName)
|
||||
{
|
||||
logger->reportError("Deformable with no name");
|
||||
return false;
|
||||
}
|
||||
deformable.m_name = deformableName;
|
||||
|
||||
XMLElement* i = config->FirstChildElement("inertial");
|
||||
if (!i)
|
||||
{
|
||||
logger->reportError("expected an inertial element");
|
||||
}
|
||||
UrdfInertia inertia;
|
||||
if (!parseInertia(inertia, i, logger))
|
||||
{
|
||||
logger->reportError("Could not parse inertial element for deformable:");
|
||||
logger->reportError(deformable.m_name.c_str());
|
||||
return false;
|
||||
}
|
||||
deformable.m_mass = inertia.m_mass;
|
||||
|
||||
XMLElement* collisionMargin_xml = config->FirstChildElement("collision_margin");
|
||||
if (collisionMargin_xml)
|
||||
{
|
||||
if (!collisionMargin_xml->Attribute("value"))
|
||||
{
|
||||
logger->reportError("collision_margin element must have value attribute");
|
||||
return false;
|
||||
}
|
||||
deformable.m_collisionMargin = urdfLexicalCast<double>(collisionMargin_xml->Attribute("value"));
|
||||
}
|
||||
|
||||
XMLElement* friction_xml = config->FirstChildElement("friction");
|
||||
if (friction_xml)
|
||||
{
|
||||
if (!friction_xml->Attribute("value"))
|
||||
{
|
||||
logger->reportError("friction element must have value attribute");
|
||||
return false;
|
||||
}
|
||||
deformable.m_friction = urdfLexicalCast<double>(friction_xml->Attribute("value"));
|
||||
}
|
||||
|
||||
XMLElement* spring_xml = config->FirstChildElement("spring");
|
||||
if (spring_xml)
|
||||
{
|
||||
if (!spring_xml->Attribute("elastic_stiffness") || !spring_xml->Attribute("damping_stiffness"))
|
||||
{
|
||||
logger->reportError("spring element expect elastic and damping stiffness");
|
||||
return false;
|
||||
}
|
||||
|
||||
deformable.m_springCoefficients.elastic_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("elastic_stiffness"));
|
||||
deformable.m_springCoefficients.damping_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("damping_stiffness"));
|
||||
|
||||
if (spring_xml->Attribute("bending_stiffness"))
|
||||
deformable.m_springCoefficients.bending_stiffness = urdfLexicalCast<double>(spring_xml->Attribute("bending_stiffness"));
|
||||
}
|
||||
|
||||
XMLElement* corotated_xml = config->FirstChildElement("corotated");
|
||||
if (corotated_xml)
|
||||
{
|
||||
if (!parseLameCoefficients(deformable.m_corotatedCoefficients, corotated_xml, logger))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
XMLElement* neohookean_xml = config->FirstChildElement("neohookean");
|
||||
if (neohookean_xml)
|
||||
{
|
||||
if (!parseLameCoefficients(deformable.m_neohookeanCoefficients, neohookean_xml, logger))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
XMLElement* vis_xml = config->FirstChildElement("visual");
|
||||
if (!vis_xml)
|
||||
{
|
||||
logger->reportError("expected an visual element");
|
||||
return false;
|
||||
}
|
||||
if (!vis_xml->Attribute("filename"))
|
||||
{
|
||||
logger->reportError("expected a filename for visual geometry");
|
||||
return false;
|
||||
}
|
||||
std::string fn = vis_xml->Attribute("filename");
|
||||
deformable.m_visualFileName = fn;
|
||||
|
||||
int out_type(0);
|
||||
bool success = UrdfFindMeshFile(m_fileIO,
|
||||
model.m_sourceFile, fn, sourceFileLocation(vis_xml),
|
||||
&deformable.m_visualFileName, &out_type);
|
||||
|
||||
if (!success)
|
||||
{
|
||||
// warning already printed
|
||||
return false;
|
||||
}
|
||||
|
||||
XMLElement* col_xml = config->FirstChildElement("collision");
|
||||
if (col_xml)
|
||||
{
|
||||
if (!col_xml->Attribute("filename"))
|
||||
{
|
||||
logger->reportError("expected a filename for collision geoemtry");
|
||||
return false;
|
||||
}
|
||||
fn = vis_xml->Attribute("filename");
|
||||
success = UrdfFindMeshFile(m_fileIO,
|
||||
model.m_sourceFile, fn, sourceFileLocation(vis_xml),
|
||||
&deformable.m_simFileName, &out_type);
|
||||
|
||||
if (!success)
|
||||
{
|
||||
// warning already printed
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
ParseUserData(config, deformable.m_userData, logger);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UrdfParser::parseJointLimits(UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
|
||||
{
|
||||
joint.m_lowerLimit = 0.f;
|
||||
@ -1898,6 +2047,8 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
}
|
||||
m_urdf2Model.m_name = name;
|
||||
|
||||
ParseUserData(robot_xml, m_urdf2Model.m_userData, logger);
|
||||
|
||||
// Get all Material elements
|
||||
for (XMLElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
|
||||
{
|
||||
@ -1921,6 +2072,11 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
// sprintf(msg,"Num materials=%d", m_model.m_materials.size());
|
||||
// logger->printMessage(msg);
|
||||
|
||||
|
||||
XMLElement* deformable_xml = robot_xml->FirstChildElement("deformable");
|
||||
if(deformable_xml)
|
||||
return parseDeformable(m_urdf2Model, deformable_xml, logger);
|
||||
|
||||
for (XMLElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
|
||||
{
|
||||
UrdfLink* link = new UrdfLink;
|
||||
@ -2040,8 +2196,6 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
|
||||
}
|
||||
}
|
||||
|
||||
ParseUserData(robot_xml, m_urdf2Model.m_userData, logger);
|
||||
|
||||
if (m_urdf2Model.m_links.size() == 0)
|
||||
{
|
||||
logger->reportWarning("No links found in URDF file");
|
||||
|
@ -200,6 +200,45 @@ struct UrdfJoint
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
struct SpringCoeffcients{
|
||||
double elastic_stiffness;
|
||||
double damping_stiffness;
|
||||
double bending_stiffness;
|
||||
SpringCoeffcients():
|
||||
elastic_stiffness(0.),
|
||||
damping_stiffness(0.),
|
||||
bending_stiffness(0.){}
|
||||
};
|
||||
|
||||
struct LameCoefficients
|
||||
{
|
||||
double mu;
|
||||
double lambda;
|
||||
double damping;
|
||||
LameCoefficients() : mu(0.), lambda(0.), damping(0.) {}
|
||||
};
|
||||
|
||||
struct UrdfDeformable
|
||||
{
|
||||
std::string m_name;
|
||||
double m_mass;
|
||||
double m_collisionMargin;
|
||||
double m_friction;
|
||||
|
||||
SpringCoeffcients m_springCoefficients;
|
||||
LameCoefficients m_corotatedCoefficients;
|
||||
LameCoefficients m_neohookeanCoefficients;
|
||||
|
||||
std::string m_visualFileName;
|
||||
std::string m_simFileName;
|
||||
btHashMap<btHashString, std::string> m_userData;
|
||||
|
||||
UrdfDeformable() : m_mass(1.), m_collisionMargin(0.02), m_friction(1.), m_visualFileName(""), m_simFileName("")
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
struct UrdfModel
|
||||
{
|
||||
std::string m_name;
|
||||
@ -208,6 +247,7 @@ struct UrdfModel
|
||||
btHashMap<btHashString, UrdfMaterial*> m_materials;
|
||||
btHashMap<btHashString, UrdfLink*> m_links;
|
||||
btHashMap<btHashString, UrdfJoint*> m_joints;
|
||||
UrdfDeformable m_deformable;
|
||||
// Maps user data keys to user data values.
|
||||
btHashMap<btHashString, std::string> m_userData;
|
||||
|
||||
@ -283,6 +323,9 @@ protected:
|
||||
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseLameCoefficients(LameCoefficients& lameCoefficients, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
bool parseDeformable(UrdfModel& model, tinyxml2::XMLElement* config, ErrorLogger* logger);
|
||||
|
||||
|
||||
public:
|
||||
UrdfParser(struct CommonFileIOInterface* fileIO);
|
||||
@ -357,11 +400,14 @@ public:
|
||||
return m_urdf2Model;
|
||||
}
|
||||
|
||||
const UrdfDeformable& getDeformable() const
|
||||
{
|
||||
return m_urdf2Model.m_deformable;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
|
@ -141,8 +141,13 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
|
||||
if (eglGetError() == EGL_SUCCESS && initialized == EGL_TRUE)
|
||||
{
|
||||
m_data->egl_display = display;
|
||||
break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(stderr, "GetDisplay %d failed with error: %x\n", i, eglGetError());
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
@ -166,6 +171,10 @@ void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
|
||||
m_data->egl_display = display;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(stderr, "GetDisplay %d failed with error: %x\n", m_data->m_renderDevice, eglGetError());
|
||||
}
|
||||
}
|
||||
|
||||
if (!eglInitialize(m_data->egl_display, NULL, NULL))
|
||||
|
@ -654,8 +654,8 @@ void PhysicsClientExample::createButtons()
|
||||
{
|
||||
if (m_numMotors < MAX_NUM_MOTORS)
|
||||
{
|
||||
char motorName[1024];
|
||||
sprintf(motorName, "%s q", info.m_jointName);
|
||||
char motorName[1026];
|
||||
snprintf(motorName, sizeof(motorName), "%s q", info.m_jointName);
|
||||
// MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
|
||||
MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors];
|
||||
motorInfo->m_velTarget = 0.f;
|
||||
|
@ -3489,6 +3489,12 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
||||
rootTrans.setOrigin(pos);
|
||||
rootTrans.setRotation(orn);
|
||||
u2b.setRootTransformInWorld(rootTrans);
|
||||
if (!(u2b.getDeformableModel().m_visualFileName.empty()))
|
||||
{
|
||||
bool use_self_collision = false;
|
||||
use_self_collision = (flags & CUF_USE_SELF_COLLISION);
|
||||
return processDeformable(u2b.getDeformableModel(), pos, orn, bodyUniqueIdPtr, bufferServerToClient, bufferSizeInBytes, globalScaling, use_self_collision);
|
||||
}
|
||||
bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
|
||||
if (ok)
|
||||
{
|
||||
@ -8169,274 +8175,276 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe
|
||||
return hasStatus;
|
||||
}
|
||||
|
||||
bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
void constructUrdfDeformable(const struct SharedMemoryCommand& clientCmd, UrdfDeformable& deformable, bool verbose)
|
||||
{
|
||||
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
|
||||
bool hasStatus = true;
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
double scale = 1;
|
||||
double mass = 1;
|
||||
double collisionMargin = 0.02;
|
||||
const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
|
||||
if (m_data->m_verboseOutput)
|
||||
if (verbose)
|
||||
{
|
||||
b3Printf("Processed CMD_LOAD_SOFT_BODY:%s", loadSoftBodyArgs.m_fileName);
|
||||
}
|
||||
btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) != 0);
|
||||
btAssert(loadSoftBodyArgs.m_fileName);
|
||||
|
||||
btVector3 initialPos(0, 0, 0);
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION)
|
||||
{
|
||||
initialPos[0] = loadSoftBodyArgs.m_initialPosition[0];
|
||||
initialPos[1] = loadSoftBodyArgs.m_initialPosition[1];
|
||||
initialPos[2] = loadSoftBodyArgs.m_initialPosition[2];
|
||||
}
|
||||
btQuaternion initialOrn(0, 0, 0, 1);
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION)
|
||||
{
|
||||
initialOrn[0] = loadSoftBodyArgs.m_initialOrientation[0];
|
||||
initialOrn[1] = loadSoftBodyArgs.m_initialOrientation[1];
|
||||
initialOrn[2] = loadSoftBodyArgs.m_initialOrientation[2];
|
||||
initialOrn[3] = loadSoftBodyArgs.m_initialOrientation[3];
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE)
|
||||
{
|
||||
scale = clientCmd.m_loadSoftBodyArguments.m_scale;
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS)
|
||||
{
|
||||
mass = clientCmd.m_loadSoftBodyArguments.m_mass;
|
||||
deformable.m_mass = loadSoftBodyArgs.m_mass;
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN)
|
||||
{
|
||||
collisionMargin = clientCmd.m_loadSoftBodyArguments.m_collisionMargin;
|
||||
deformable.m_collisionMargin = loadSoftBodyArgs.m_collisionMargin;
|
||||
}
|
||||
btScalar spring_bending_stiffness = 0;
|
||||
deformable.m_visualFileName = loadSoftBodyArgs.m_fileName;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
|
||||
{
|
||||
btSoftBody* psb = NULL;
|
||||
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
|
||||
char relativeFileName[1024];
|
||||
char pathPrefix[1024];
|
||||
pathPrefix[0] = 0;
|
||||
if (fileIO->findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
|
||||
{
|
||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||
}
|
||||
const std::string& error_message_prefix = "";
|
||||
std::string out_found_filename, out_found_sim_filename;
|
||||
int out_type(0), out_sim_type(0);
|
||||
deformable.m_simFileName = loadSoftBodyArgs.m_simFileName;
|
||||
}
|
||||
else
|
||||
{
|
||||
deformable.m_simFileName = "";
|
||||
}
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
deformable.m_springCoefficients.elastic_stiffness = loadSoftBodyArgs.m_springElasticStiffness;
|
||||
deformable.m_springCoefficients.damping_stiffness = loadSoftBodyArgs.m_springDampingStiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
|
||||
{
|
||||
deformable.m_springCoefficients.bending_stiffness = loadSoftBodyArgs.m_springBendingStiffness;
|
||||
}
|
||||
|
||||
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SIM_MESH)
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
|
||||
{
|
||||
deformable.m_corotatedCoefficients.mu = loadSoftBodyArgs.m_corotatedMu;
|
||||
deformable.m_corotatedCoefficients.lambda = loadSoftBodyArgs.m_corotatedLambda;
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
|
||||
{
|
||||
deformable.m_neohookeanCoefficients.mu = loadSoftBodyArgs.m_NeoHookeanMu;
|
||||
deformable.m_neohookeanCoefficients.lambda = loadSoftBodyArgs.m_NeoHookeanLambda;
|
||||
deformable.m_neohookeanCoefficients.damping = loadSoftBodyArgs.m_NeoHookeanDamping;
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
|
||||
{
|
||||
deformable.m_friction = loadSoftBodyArgs.m_frictionCoeff;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
bool PhysicsServerCommandProcessor::processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision)
|
||||
{
|
||||
btSoftBody* psb = NULL;
|
||||
CommonFileIOInterface* fileIO(m_data->m_pluginManager.getFileIOInterface());
|
||||
char relativeFileName[1024];
|
||||
char pathPrefix[1024];
|
||||
pathPrefix[0] = 0;
|
||||
if (fileIO->findResourcePath(deformable.m_visualFileName.c_str(), relativeFileName, 1024))
|
||||
{
|
||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||
}
|
||||
const std::string& error_message_prefix = "";
|
||||
std::string out_found_filename, out_found_sim_filename;
|
||||
int out_type(0), out_sim_type(0);
|
||||
|
||||
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
|
||||
if (!deformable.m_simFileName.empty())
|
||||
{
|
||||
bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, deformable.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
|
||||
}
|
||||
else
|
||||
{
|
||||
out_sim_type = out_type;
|
||||
out_found_sim_filename = out_found_filename;
|
||||
}
|
||||
if (out_sim_type == UrdfGeometry::FILE_OBJ)
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
|
||||
if (!shapes.empty())
|
||||
{
|
||||
bool foundSimMesh = UrdfFindMeshFile(fileIO, pathPrefix, loadSoftBodyArgs.m_simFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
|
||||
}
|
||||
else
|
||||
{
|
||||
out_sim_type = out_type;
|
||||
out_found_sim_filename = out_found_filename;
|
||||
}
|
||||
if (out_sim_type == UrdfGeometry::FILE_OBJ)
|
||||
{
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
tinyobj::attrib_t attribute;
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_sim_filename.c_str(), "", fileIO);
|
||||
if (!shapes.empty())
|
||||
const tinyobj::shape_t& shape = shapes[0];
|
||||
btAlignedObjectArray<btScalar> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
for (int i = 0; i < attribute.vertices.size(); i++)
|
||||
{
|
||||
vertices.push_back(attribute.vertices[i]);
|
||||
}
|
||||
for (int i = 0; i < shape.mesh.indices.size(); i++)
|
||||
{
|
||||
indices.push_back(shape.mesh.indices[i].vertex_index);
|
||||
}
|
||||
int numTris = shape.mesh.indices.size() / 3;
|
||||
if (numTris > 0)
|
||||
{
|
||||
const tinyobj::shape_t& shape = shapes[0];
|
||||
btAlignedObjectArray<btScalar> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
for (int i = 0; i < attribute.vertices.size(); i++)
|
||||
{
|
||||
vertices.push_back(attribute.vertices[i]);
|
||||
}
|
||||
for (int i = 0; i < shape.mesh.indices.size(); i++)
|
||||
{
|
||||
indices.push_back(shape.mesh.indices[i].vertex_index);
|
||||
}
|
||||
int numTris = shape.mesh.indices.size() / 3;
|
||||
if (numTris > 0)
|
||||
{
|
||||
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
|
||||
if (softWorld)
|
||||
{
|
||||
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
|
||||
if (softWorld)
|
||||
psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
|
||||
if (!psb)
|
||||
{
|
||||
psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
|
||||
printf("Load deformable failed\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
{
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
|
||||
if (!psb)
|
||||
{
|
||||
psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
|
||||
printf("Load deformable failed\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btScalar spring_elastic_stiffness, spring_damping_stiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
|
||||
{
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
|
||||
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
|
||||
{
|
||||
spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness;
|
||||
}
|
||||
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness);
|
||||
deformWorld->addForce(psb, springForce);
|
||||
m_data->m_lf.push_back(springForce);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else if (out_sim_type == UrdfGeometry::FILE_VTK)
|
||||
{
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
|
||||
btScalar corotated_mu, corotated_lambda;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
|
||||
{
|
||||
corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
|
||||
corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
|
||||
btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda);
|
||||
deformWorld->addForce(psb, corotatedForce);
|
||||
m_data->m_lf.push_back(corotatedForce);
|
||||
}
|
||||
btScalar neohookean_mu, neohookean_lambda, neohookean_damping;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
|
||||
{
|
||||
neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
|
||||
neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
|
||||
neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping;
|
||||
btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping);
|
||||
deformWorld->addForce(psb, neohookeanForce);
|
||||
m_data->m_lf.push_back(neohookeanForce);
|
||||
}
|
||||
btScalar spring_elastic_stiffness, spring_damping_stiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
|
||||
{
|
||||
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
|
||||
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
|
||||
{
|
||||
spring_bending_stiffness = clientCmd.m_loadSoftBodyArguments.m_springBendingStiffness;
|
||||
}
|
||||
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness);
|
||||
deformWorld->addForce(psb, springForce);
|
||||
m_data->m_lf.push_back(springForce);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld && deformable.m_springCoefficients.elastic_stiffness > 0.)
|
||||
{
|
||||
btDeformableLagrangianForce* springForce =
|
||||
new btDeformableMassSpringForce(deformable.m_springCoefficients.elastic_stiffness,
|
||||
deformable.m_springCoefficients.damping_stiffness,
|
||||
true, deformable.m_springCoefficients.bending_stiffness);
|
||||
deformWorld->addForce(psb, springForce);
|
||||
m_data->m_lf.push_back(springForce);
|
||||
}
|
||||
if (psb != NULL)
|
||||
{
|
||||
#endif
|
||||
}
|
||||
else if (out_sim_type == UrdfGeometry::FILE_VTK)
|
||||
{
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
|
||||
if (!psb)
|
||||
{
|
||||
printf("Load deformable failed\n");
|
||||
return false;
|
||||
}
|
||||
btScalar corotated_mu(0.), corotated_lambda(0.);
|
||||
corotated_mu = deformable.m_corotatedCoefficients.mu;
|
||||
corotated_lambda = deformable.m_corotatedCoefficients.lambda;
|
||||
if (corotated_mu > 0 || corotated_lambda > 0)
|
||||
{
|
||||
btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda);
|
||||
deformWorld->addForce(psb, corotatedForce);
|
||||
m_data->m_lf.push_back(corotatedForce);
|
||||
}
|
||||
btScalar neohookean_mu, neohookean_lambda, neohookean_damping;
|
||||
neohookean_mu = deformable.m_neohookeanCoefficients.mu;
|
||||
neohookean_lambda = deformable.m_neohookeanCoefficients.lambda;
|
||||
neohookean_damping = deformable.m_neohookeanCoefficients.damping;
|
||||
if (neohookean_mu > 0 || neohookean_lambda > 0)
|
||||
{
|
||||
btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping);
|
||||
deformWorld->addForce(psb, neohookeanForce);
|
||||
m_data->m_lf.push_back(neohookeanForce);
|
||||
}
|
||||
|
||||
btScalar spring_elastic_stiffness, spring_damping_stiffness, spring_bending_stiffness;
|
||||
spring_elastic_stiffness = deformable.m_springCoefficients.elastic_stiffness;
|
||||
spring_damping_stiffness = deformable.m_springCoefficients.damping_stiffness;
|
||||
spring_bending_stiffness = deformable.m_springCoefficients.bending_stiffness;
|
||||
if (spring_elastic_stiffness > 0.)
|
||||
{
|
||||
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true, spring_bending_stiffness);
|
||||
deformWorld->addForce(psb, springForce);
|
||||
m_data->m_lf.push_back(springForce);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
if (psb != NULL)
|
||||
{
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
|
||||
if (deformWorld)
|
||||
{
|
||||
// load render mesh
|
||||
if (out_found_sim_filename != out_found_filename)
|
||||
{
|
||||
// load render mesh
|
||||
if (out_found_sim_filename != out_found_filename)
|
||||
{
|
||||
// load render mesh
|
||||
tinyobj::attrib_t attribute;
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
|
||||
|
||||
for (int s = 0; s < (int)shapes.size(); s++)
|
||||
{
|
||||
tinyobj::attrib_t attribute;
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
|
||||
std::string err = tinyobj::LoadObj(attribute, shapes, out_found_filename.c_str(), pathPrefix, m_data->m_pluginManager.getFileIOInterface());
|
||||
|
||||
for (int s = 0; s < (int)shapes.size(); s++)
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
int faceCount = shape.mesh.indices.size();
|
||||
int vertexCount = attribute.vertices.size() / 3;
|
||||
for (int v = 0; v < vertexCount; v++)
|
||||
{
|
||||
tinyobj::shape_t& shape = shapes[s];
|
||||
int faceCount = shape.mesh.indices.size();
|
||||
int vertexCount = attribute.vertices.size()/3;
|
||||
for (int v=0;v<vertexCount;v++)
|
||||
btSoftBody::Node n;
|
||||
n.m_x = btVector3(attribute.vertices[3 * v], attribute.vertices[3 * v + 1], attribute.vertices[3 * v + 2]);
|
||||
psb->m_renderNodes.push_back(n);
|
||||
}
|
||||
for (int f = 0; f < faceCount; f += 3)
|
||||
{
|
||||
if (f < 0 && f >= int(shape.mesh.indices.size()))
|
||||
{
|
||||
btSoftBody::Node n;
|
||||
n.m_x = btVector3(attribute.vertices[3*v],attribute.vertices[3*v+1],attribute.vertices[3*v+2]);
|
||||
psb->m_renderNodes.push_back(n);
|
||||
}
|
||||
for (int f = 0; f < faceCount; f += 3)
|
||||
{
|
||||
if (f < 0 && f >= int(shape.mesh.indices.size()))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
tinyobj::index_t v_0 = shape.mesh.indices[f];
|
||||
tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
|
||||
tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
|
||||
btSoftBody::Face ff;
|
||||
ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index];
|
||||
ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index];
|
||||
ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index];
|
||||
psb->m_renderFaces.push_back(ff);
|
||||
continue;
|
||||
}
|
||||
tinyobj::index_t v_0 = shape.mesh.indices[f];
|
||||
tinyobj::index_t v_1 = shape.mesh.indices[f + 1];
|
||||
tinyobj::index_t v_2 = shape.mesh.indices[f + 2];
|
||||
btSoftBody::Face ff;
|
||||
ff.m_n[0] = &psb->m_renderNodes[v_0.vertex_index];
|
||||
ff.m_n[1] = &psb->m_renderNodes[v_1.vertex_index];
|
||||
ff.m_n[2] = &psb->m_renderNodes[v_2.vertex_index];
|
||||
psb->m_renderFaces.push_back(ff);
|
||||
}
|
||||
}
|
||||
if (out_sim_type == UrdfGeometry::FILE_VTK)
|
||||
{
|
||||
btSoftBodyHelpers::interpolateBarycentricWeights(psb);
|
||||
}
|
||||
else if (out_sim_type == UrdfGeometry::FILE_OBJ)
|
||||
{
|
||||
btSoftBodyHelpers::extrapolateBarycentricWeights(psb);
|
||||
}
|
||||
}
|
||||
else
|
||||
if (out_sim_type == UrdfGeometry::FILE_VTK)
|
||||
{
|
||||
psb->m_renderNodes.resize(0);
|
||||
btSoftBodyHelpers::interpolateBarycentricWeights(psb);
|
||||
}
|
||||
btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
|
||||
btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
|
||||
deformWorld->addForce(psb, gravityForce);
|
||||
m_data->m_lf.push_back(gravityForce);
|
||||
btScalar collision_hardness = 1;
|
||||
psb->m_cfg.kKHR = collision_hardness;
|
||||
psb->m_cfg.kCHR = collision_hardness;
|
||||
|
||||
btScalar friction_coeff = 0;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT)
|
||||
else if (out_sim_type == UrdfGeometry::FILE_OBJ)
|
||||
{
|
||||
friction_coeff = loadSoftBodyArgs.m_frictionCoeff;
|
||||
btSoftBodyHelpers::extrapolateBarycentricWeights(psb);
|
||||
}
|
||||
psb->m_cfg.kDF = friction_coeff;
|
||||
bool use_bending_spring = false;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_BENDING_SPRINGS)
|
||||
{
|
||||
use_bending_spring = loadSoftBodyArgs.m_useBendingSprings;
|
||||
if (use_bending_spring)
|
||||
{
|
||||
psb->generateBendingConstraints(2);
|
||||
}
|
||||
}
|
||||
btSoftBody::Material* pm = psb->appendMaterial();
|
||||
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
|
||||
|
||||
// turn on the collision flag for deformable
|
||||
// collision between deformable and rigid
|
||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||
// turn on face contact only for multibodies
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
|
||||
// collion between deformable and deformable and self-collision
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
|
||||
psb->setCollisionFlags(0);
|
||||
psb->setTotalMass(mass);
|
||||
bool use_self_collision = false;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION)
|
||||
{
|
||||
use_self_collision = loadSoftBodyArgs.m_useSelfCollision;
|
||||
}
|
||||
psb->setSelfCollision(use_self_collision);
|
||||
}
|
||||
#endif//SKIP_DEFORMABLE_BODY
|
||||
else
|
||||
{
|
||||
psb->m_renderNodes.resize(0);
|
||||
}
|
||||
btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
|
||||
btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
|
||||
deformWorld->addForce(psb, gravityForce);
|
||||
m_data->m_lf.push_back(gravityForce);
|
||||
btScalar collision_hardness = 1;
|
||||
psb->m_cfg.kKHR = collision_hardness;
|
||||
psb->m_cfg.kCHR = collision_hardness;
|
||||
|
||||
psb->m_cfg.kDF = deformable.m_friction;
|
||||
if (deformable.m_springCoefficients.bending_stiffness)
|
||||
{
|
||||
psb->generateBendingConstraints(2);
|
||||
}
|
||||
btSoftBody::Material* pm = psb->appendMaterial();
|
||||
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
|
||||
|
||||
// turn on the collision flag for deformable
|
||||
// collision between deformable and rigid
|
||||
psb->m_cfg.collisions = btSoftBody::fCollision::SDF_RD;
|
||||
// turn on face contact only for multibodies
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::SDF_MDF;
|
||||
// collion between deformable and deformable and self-collision
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_DD;
|
||||
psb->setCollisionFlags(0);
|
||||
psb->setTotalMass(deformable.m_mass);
|
||||
psb->setSelfCollision(useSelfCollision);
|
||||
}
|
||||
#endif //SKIP_DEFORMABLE_BODY
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
|
||||
if (softWorld)
|
||||
@ -8450,21 +8458,22 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
|
||||
//turn on softbody vs softbody collision
|
||||
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
|
||||
psb->randomizeConstraints();
|
||||
psb->setTotalMass(mass, true);
|
||||
psb->setTotalMass(deformable.m_mass, true);
|
||||
}
|
||||
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
psb->scale(btVector3(scale, scale, scale));
|
||||
psb->rotate(initialOrn);
|
||||
psb->translate(initialPos);
|
||||
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
psb->scale(btVector3(scale, scale, scale));
|
||||
psb->rotate(orn);
|
||||
psb->translate(pos);
|
||||
|
||||
psb->getCollisionShape()->setMargin(collisionMargin);
|
||||
psb->getCollisionShape()->setUserPointer(psb);
|
||||
psb->getCollisionShape()->setMargin(deformable.m_collisionMargin);
|
||||
psb->getCollisionShape()->setUserPointer(psb);
|
||||
#ifndef SKIP_DEFORMABLE_BODY
|
||||
if (deformWorld)
|
||||
{
|
||||
deformWorld->addSoftBody(psb);
|
||||
} else
|
||||
#endif//SKIP_DEFORMABLE_BODY
|
||||
}
|
||||
else
|
||||
#endif //SKIP_DEFORMABLE_BODY
|
||||
{
|
||||
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
|
||||
if (softWorld)
|
||||
@ -8472,71 +8481,123 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
|
||||
softWorld->addSoftBody(psb);
|
||||
}
|
||||
}
|
||||
m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
|
||||
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
|
||||
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
|
||||
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
|
||||
bodyHandle->m_softBody = psb;
|
||||
m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
|
||||
*bodyUniqueId = m_data->m_bodyHandles.allocHandle();
|
||||
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(*bodyUniqueId);
|
||||
bodyHandle->m_softBody = psb;
|
||||
|
||||
b3VisualShapeData visualShape;
|
||||
b3VisualShapeData visualShape;
|
||||
|
||||
visualShape.m_objectUniqueId = bodyUniqueId;
|
||||
visualShape.m_linkIndex = -1;
|
||||
visualShape.m_visualGeometryType = URDF_GEOM_MESH;
|
||||
//dimensions just contains the scale
|
||||
visualShape.m_dimensions[0] = scale;
|
||||
visualShape.m_dimensions[1] = scale;
|
||||
visualShape.m_dimensions[2] = scale;
|
||||
//filename
|
||||
strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
|
||||
visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
|
||||
//position and orientation
|
||||
visualShape.m_localVisualFrame[0] = initialPos[0];
|
||||
visualShape.m_localVisualFrame[1] = initialPos[1];
|
||||
visualShape.m_localVisualFrame[2] = initialPos[2];
|
||||
visualShape.m_localVisualFrame[3] = initialOrn[0];
|
||||
visualShape.m_localVisualFrame[4] = initialOrn[1];
|
||||
visualShape.m_localVisualFrame[5] = initialOrn[2];
|
||||
visualShape.m_localVisualFrame[6] = initialOrn[3];
|
||||
//color and ids to be set by the renderer
|
||||
visualShape.m_rgbaColor[0] = 0;
|
||||
visualShape.m_rgbaColor[1] = 0;
|
||||
visualShape.m_rgbaColor[2] = 0;
|
||||
visualShape.m_rgbaColor[3] = 1;
|
||||
visualShape.m_tinyRendererTextureId = -1;
|
||||
visualShape.m_textureUniqueId =-1;
|
||||
visualShape.m_openglTextureId = -1;
|
||||
visualShape.m_objectUniqueId = *bodyUniqueId;
|
||||
visualShape.m_linkIndex = -1;
|
||||
visualShape.m_visualGeometryType = URDF_GEOM_MESH;
|
||||
//dimensions just contains the scale
|
||||
visualShape.m_dimensions[0] = scale;
|
||||
visualShape.m_dimensions[1] = scale;
|
||||
visualShape.m_dimensions[2] = scale;
|
||||
//filename
|
||||
strncpy(visualShape.m_meshAssetFileName, relativeFileName, VISUAL_SHAPE_MAX_PATH_LEN);
|
||||
visualShape.m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN - 1] = 0;
|
||||
//position and orientation
|
||||
visualShape.m_localVisualFrame[0] = pos[0];
|
||||
visualShape.m_localVisualFrame[1] = pos[1];
|
||||
visualShape.m_localVisualFrame[2] = pos[2];
|
||||
visualShape.m_localVisualFrame[3] = orn[0];
|
||||
visualShape.m_localVisualFrame[4] = orn[1];
|
||||
visualShape.m_localVisualFrame[5] = orn[2];
|
||||
visualShape.m_localVisualFrame[6] = orn[3];
|
||||
//color and ids to be set by the renderer
|
||||
visualShape.m_rgbaColor[0] = 0;
|
||||
visualShape.m_rgbaColor[1] = 0;
|
||||
visualShape.m_rgbaColor[2] = 0;
|
||||
visualShape.m_rgbaColor[3] = 1;
|
||||
visualShape.m_tinyRendererTextureId = -1;
|
||||
visualShape.m_textureUniqueId = -1;
|
||||
visualShape.m_openglTextureId = -1;
|
||||
|
||||
m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO);
|
||||
m_data->m_pluginManager.getRenderInterface()->addVisualShape(&visualShape, fileIO);
|
||||
|
||||
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
|
||||
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
|
||||
int pos = strlen(relativeFileName)-1;
|
||||
while(pos>=0 && relativeFileName[pos]!='/') { pos--;}
|
||||
btAssert(strlen(relativeFileName)-pos-5>0);
|
||||
std::string object_name (std::string(relativeFileName).substr(pos+1, strlen(relativeFileName)- 5 - pos));
|
||||
if (!deformable.m_name.empty())
|
||||
{
|
||||
bodyHandle->m_bodyName = deformable.m_name;
|
||||
}
|
||||
else
|
||||
{
|
||||
int pos = strlen(relativeFileName) - 1;
|
||||
while (pos >= 0 && relativeFileName[pos] != '/')
|
||||
{
|
||||
pos--;
|
||||
}
|
||||
btAssert(strlen(relativeFileName) - pos - 5 > 0);
|
||||
std::string object_name(std::string(relativeFileName).substr(pos + 1, strlen(relativeFileName) - 5 - pos));
|
||||
bodyHandle->m_bodyName = object_name;
|
||||
}
|
||||
b3Notification notification;
|
||||
notification.m_notificationType = BODY_ADDED;
|
||||
notification.m_bodyArgs.m_bodyUniqueId = *bodyUniqueId;
|
||||
m_data->m_pluginManager.addNotification(notification);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
|
||||
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
|
||||
bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
{
|
||||
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_FAILED;
|
||||
bool hasStatus = true;
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
UrdfDeformable deformable;
|
||||
|
||||
constructUrdfDeformable(clientCmd, deformable, m_data->m_verboseOutput);
|
||||
// const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
|
||||
btVector3 initialPos(0, 0, 0);
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_POSITION)
|
||||
{
|
||||
initialPos[0] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[0];
|
||||
initialPos[1] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[1];
|
||||
initialPos[2] = clientCmd.m_loadSoftBodyArguments.m_initialPosition[2];
|
||||
}
|
||||
btQuaternion initialOrn(0, 0, 0, 1);
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_INITIAL_ORIENTATION)
|
||||
{
|
||||
initialOrn[0] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[0];
|
||||
initialOrn[1] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[1];
|
||||
initialOrn[2] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[2];
|
||||
initialOrn[3] = clientCmd.m_loadSoftBodyArguments.m_initialOrientation[3];
|
||||
}
|
||||
|
||||
double scale = 1;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE)
|
||||
{
|
||||
scale = clientCmd.m_loadSoftBodyArguments.m_scale;
|
||||
}
|
||||
bool use_self_collision = false;
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION)
|
||||
{
|
||||
use_self_collision = clientCmd.m_loadSoftBodyArguments.m_useSelfCollision;
|
||||
}
|
||||
|
||||
int bodyUniqueId = -1;
|
||||
bool completedOk = processDeformable(deformable, initialPos, initialOrn, &bodyUniqueId, bufferServerToClient, bufferSizeInBytes, scale, use_self_collision);
|
||||
if (completedOk && bodyUniqueId >= 0)
|
||||
{
|
||||
m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld);
|
||||
serverStatusOut.m_type = CMD_LOAD_SOFT_BODY_COMPLETED;
|
||||
|
||||
int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
|
||||
serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
|
||||
|
||||
#ifdef ENABLE_LINK_MAPPER
|
||||
if (m_data->m_urdfLinkNameMapper.size())
|
||||
{
|
||||
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
|
||||
}
|
||||
#endif
|
||||
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
|
||||
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
|
||||
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
|
||||
|
||||
b3Notification notification;
|
||||
notification.m_notificationType = BODY_ADDED;
|
||||
notification.m_bodyArgs.m_bodyUniqueId = bodyUniqueId;
|
||||
m_data->m_pluginManager.addNotification(notification);
|
||||
if (m_data->m_urdfLinkNameMapper.size())
|
||||
{
|
||||
serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size() - 1)->m_memSerializer->getCurrentBufferSize();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
|
||||
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
|
||||
strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
|
||||
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
|
||||
#endif
|
||||
}
|
||||
return hasStatus;
|
||||
}
|
||||
|
||||
|
@ -5,6 +5,7 @@
|
||||
#include "LinearMath/btVector3.h"
|
||||
|
||||
#include "PhysicsCommandProcessorInterface.h"
|
||||
#include "../Importers/ImportURDFDemo/UrdfParser.h"
|
||||
|
||||
struct SharedMemLines
|
||||
{
|
||||
@ -104,6 +105,7 @@ protected:
|
||||
bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);
|
||||
|
||||
bool processImportedObjects(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, class URDFImporterInterface& u2b);
|
||||
bool processDeformable(const UrdfDeformable& deformable, const btVector3& pos, const btQuaternion& orn, int* bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes, btScalar scale, bool useSelfCollision);
|
||||
|
||||
bool supportsJointMotor(class btMultiBody* body, int linkIndex);
|
||||
|
||||
|
@ -1350,9 +1350,6 @@ void EGLRendererVisualShapeConverter::copyCameraImageDataGL(
|
||||
m_data->m_instancingRenderer->updateCamera(m_data->m_upAxis);
|
||||
|
||||
m_data->m_instancingRenderer->renderScene();
|
||||
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(1, 0, 0), b3MakeVector3(1, 0, 0), 3);
|
||||
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 1, 0), b3MakeVector3(0, 1, 0), 3);
|
||||
m_data->m_instancingRenderer->drawLine(b3MakeVector3(0, 0, 0), b3MakeVector3(0, 0, 1), b3MakeVector3(0, 0, 1), 3);
|
||||
|
||||
int numBytesPerPixel = 4; //RGBA
|
||||
|
||||
|
@ -72,7 +72,6 @@ struct InMemoryFileIO : public CommonFileIOInterface
|
||||
{
|
||||
for (int i=0;i<m_fileCache.size();i++)
|
||||
{
|
||||
b3HashString name = m_fileCache.getKeyAtIndex(i);
|
||||
InMemoryFile** memPtr = m_fileCache.getAtIndex(i);
|
||||
if (memPtr && *memPtr)
|
||||
{
|
||||
@ -81,9 +80,9 @@ struct InMemoryFileIO : public CommonFileIOInterface
|
||||
m_numFrees++;
|
||||
delete (mem);
|
||||
m_numFrees++;
|
||||
m_fileCache.remove(name);
|
||||
}
|
||||
}
|
||||
m_fileCache.clear();
|
||||
}
|
||||
|
||||
char* allocateBuffer(int len)
|
||||
|
@ -12,6 +12,9 @@ planeId = p.loadURDF("plane.urdf", [0,0,-2])
|
||||
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
|
||||
|
||||
bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5)
|
||||
|
||||
bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION)
|
||||
|
||||
p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
|
@ -0,0 +1,596 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="plane">
|
||||
<link name="chassis">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0.03 0.043794"/>
|
||||
<mass value="13.715"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.043794"/>
|
||||
<geometry>
|
||||
<mesh filename="chassis.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="-1.57 0 0" xyz="0 0 0.043794"/>
|
||||
<geometry>
|
||||
<mesh filename="chassis_vhacd_mod.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="FR_hip_motor">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.02 0 0"/>
|
||||
<mass value="1.095"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="hip_motor_mirror.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="hip_motor_mirror.stl" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="FR_hip_motor_2_chassis_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<parent link="chassis"/>
|
||||
<child link="FR_hip_motor"/>
|
||||
<origin rpy="0 0 0" xyz="-0.0817145 0 0.242889"/>
|
||||
<limit effort="100" velocity="100" lower="-0.873" upper="1.0472"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
<link name="FR_upper_leg">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
|
||||
<mass value="1.527"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="upper_leg_mirror.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="upper_leg_right_vhacd.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="FR_upper_leg_2_hip_motor_joint" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<parent link="FR_hip_motor"/>
|
||||
<child link="FR_upper_leg"/>
|
||||
<origin rpy="0 0 0" xyz="-0.053565 0 0"/>
|
||||
<limit effort="100" velocity="100" lower="-1.3" upper="3.4"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
<link name="FR_lower_leg">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
|
||||
<mass value="0.241"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 1.57079 0" xyz="0 0.0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="lower_leg3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 1.57079 0" xyz="0 0.0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="FR_lower_leg_2_upper_leg_joint" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<parent link="FR_upper_leg"/>
|
||||
<child link="FR_lower_leg"/>
|
||||
|
||||
<origin rpy="0 0 0" xyz="0.02069 -0.20833 -0.1422"/>
|
||||
<limit effort="100" velocity="100" lower="-2.164" upper="0"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="FL_hip_motor">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-.02 0 0"/>
|
||||
<mass value="1.095"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="hip_motor.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="hip_motor.stl" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="FL_hip_motor_2_chassis_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="chassis"/>
|
||||
<child link="FL_hip_motor"/>
|
||||
<origin rpy="0 0 0" xyz="0.0817145 0 0.242889"/>
|
||||
<limit effort="100" velocity="100" lower="-0.873" upper="1.0472"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
<link name="FL_upper_leg">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
|
||||
<mass value="1.527"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
|
||||
<mesh filename="upper_leg_left.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="upper_leg_left_vhacd.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="FL_upper_leg_2_hip_motor_joint" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<parent link="FL_hip_motor"/>
|
||||
<child link="FL_upper_leg"/>
|
||||
<origin rpy="0 0 0" xyz="0.055855 0 0"/>
|
||||
<limit effort="100" velocity="100" lower="-1.3" upper="3.4"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="FL_lower_leg">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
|
||||
<mass value="0.241"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="lower_leg3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white">
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="FL_lower_leg_2_upper_leg_joint" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<parent link="FL_upper_leg"/>
|
||||
<child link="FL_lower_leg"/>
|
||||
<origin rpy="0 0 0" xyz="-0.02069 -0.20833 -0.1422"/>
|
||||
<limit effort="100" velocity="100" lower="-2.164" upper="0"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
<link name="RR_hip_motor">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0.02 0 0"/>
|
||||
<mass value="1.095"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="hip_motor_mirror.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="hip_motor_mirror.stl" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="RR_hip_motor_2_chassis_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<parent link="chassis"/>
|
||||
<child link="RR_hip_motor"/>
|
||||
<origin rpy="0 0 0" xyz="-0.0817145 0 -0.194401"/>
|
||||
<limit effort="100" velocity="100" lower="-0.873" upper="1.0472"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
<link name="RR_upper_leg">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
|
||||
<mass value="1.527"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="upper_leg_mirror2.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="upper_leg_right_vhacd.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="RR_upper_leg_2_hip_motor_joint" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<parent link="RR_hip_motor"/>
|
||||
<child link="RR_upper_leg"/>
|
||||
<origin rpy="0 0 0" xyz="-0.053565 0 0"/>
|
||||
<limit effort="100" velocity="100" lower="-1.3" upper="3.4"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
<link name="RR_lower_leg">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
|
||||
<mass value="0.241"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="lower_leg3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="RR_lower_leg_2_upper_leg_joint" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<parent link="RR_upper_leg"/>
|
||||
<child link="RR_lower_leg"/>
|
||||
<origin rpy="0 0 0" xyz="0.02069 -0.20833 -0.1422"/>
|
||||
<limit effort="100" velocity="100" lower="-2.164" upper="0"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="RL_hip_motor">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="-.02 0 0"/>
|
||||
<mass value="1.095"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="hip_motor.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="hip_motor.stl" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="RL_hip_motor_2_chassis_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="chassis"/>
|
||||
<child link="RL_hip_motor"/>
|
||||
|
||||
<origin rpy="0 0 0" xyz="0.0817145 0 -0.194401"/>
|
||||
<limit effort="100" velocity="100" lower="-0.873" upper="1.0472"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
<link name="RL_upper_leg">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.04 -0.04"/>
|
||||
<mass value="1.527"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="upper_leg_left2.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="upper_leg_left_vhacd.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
|
||||
<joint name="RL_upper_leg_2_hip_motor_joint" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<parent link="RL_hip_motor"/>
|
||||
<child link="RL_upper_leg"/>
|
||||
<origin rpy="0 0 0" xyz="0.055855 0 0"/>
|
||||
<limit effort="100" velocity="100" lower="-1.3" upper="3.4"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="RL_lower_leg">
|
||||
<contact>
|
||||
<lateral_friction value="1"/>
|
||||
</contact>
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 -0.16 -0.02"/>
|
||||
<mass value="0.241"/>
|
||||
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="lower_leg3.obj" scale="1 1 1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin rpy="0 1.57079 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="lower_leg_3.stl" scale="1 1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
<joint name="RL_lower_leg_2_upper_leg_joint" type="revolute">
|
||||
<axis xyz="1 0 0"/>
|
||||
<parent link="RL_upper_leg"/>
|
||||
<child link="RL_lower_leg"/>
|
||||
<origin rpy="0 0 0" xyz="-0.02069 -0.20833 -0.1422"/>
|
||||
<limit effort="100" velocity="100" lower="-2.164" upper="0"/>
|
||||
<joint_properties damping=".0" friction=".0"/>
|
||||
</joint>
|
||||
|
||||
<link name="toeRL">
|
||||
<contact>
|
||||
<friction_anchor/>
|
||||
<stiffness value="30000.0"/>
|
||||
<damping value="1000.0"/>
|
||||
<spinning_friction value="0.1"/>
|
||||
<lateral_friction value="1.0"/>
|
||||
</contact>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.0"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.03"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="jtoeRL" type="fixed">
|
||||
<parent link="RL_lower_leg"/>
|
||||
<child link="toeRL"/>
|
||||
<origin xyz="0 -0.25 -0.022"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="toeRR">
|
||||
<contact>
|
||||
<friction_anchor/>
|
||||
<stiffness value="30000.0"/>
|
||||
<damping value="1000.0"/>
|
||||
<spinning_friction value="0.1"/>
|
||||
<lateral_friction value="1.0"/>
|
||||
</contact>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.0"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.03"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="jtoeRR" type="fixed">
|
||||
<parent link="RR_lower_leg"/>
|
||||
<child link="toeRR"/>
|
||||
<origin xyz="0 -0.25 -0.022"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="toeFL">
|
||||
<contact>
|
||||
<friction_anchor/>
|
||||
<stiffness value="30000.0"/>
|
||||
<damping value="1000.0"/>
|
||||
<spinning_friction value="0.1"/>
|
||||
<lateral_friction value="1.0"/>
|
||||
</contact>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.0"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.03"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="jtoeFL" type="fixed">
|
||||
<parent link="FL_lower_leg"/>
|
||||
<child link="toeFL"/>
|
||||
<origin xyz="0 -0.25 -0.022"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="toeFR">
|
||||
<contact>
|
||||
<friction_anchor/>
|
||||
<stiffness value="30000.0"/>
|
||||
<damping value="1000.0"/>
|
||||
<spinning_friction value="0.1"/>
|
||||
<lateral_friction value="1.0"/>
|
||||
</contact>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.0"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<sphere radius="0.03"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.1"/>
|
||||
<inertia ixx="0.000025" ixy="0" ixz="0" iyy="0.000025" iyz="0" izz="0.000025"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="jtoeFR" type="fixed">
|
||||
<parent link="FR_lower_leg"/>
|
||||
<child link="toeFR"/>
|
||||
<origin xyz="0 -0.25 -0.022"/>
|
||||
<dynamics damping="0.0" friction="0.0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
@ -30,9 +30,18 @@
|
||||
#if defined(__APPLE__) && (!defined(B3_NO_PYTHON_FRAMEWORK))
|
||||
#include <Python/Python.h>
|
||||
#else
|
||||
#ifdef _WIN32
|
||||
#ifdef _DEBUG
|
||||
#define BT_REMOVED_DEBUG
|
||||
//always use the release build of Python
|
||||
#undef _DEBUG
|
||||
#endif //_DEBUG
|
||||
#endif
|
||||
#include <Python.h>
|
||||
#endif
|
||||
|
||||
#ifdef BT_REMOVED_DEBUG
|
||||
#define _DEBUG
|
||||
#endif
|
||||
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"
|
||||
|
||||
#ifdef B3_DUMP_PYTHON_VERSION
|
||||
|
2
setup.py
2
setup.py
@ -501,7 +501,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS:
|
||||
|
||||
setup(
|
||||
name='pybullet',
|
||||
version='2.7.1',
|
||||
version='2.7.3',
|
||||
description=
|
||||
'Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
|
||||
long_description=
|
||||
|
@ -46,8 +46,6 @@ protected:
|
||||
|
||||
btAlignedObjectArray<btPersistentManifold*> m_manifoldsPtr;
|
||||
|
||||
btManifoldResult m_defaultManifoldResult;
|
||||
|
||||
btNearCallback m_nearCallback;
|
||||
|
||||
btPoolAllocator* m_collisionAlgorithmPoolAllocator;
|
||||
@ -95,11 +93,15 @@ public:
|
||||
|
||||
btPersistentManifold* getManifoldByIndexInternal(int index)
|
||||
{
|
||||
btAssert(index>=0);
|
||||
btAssert(index<m_manifoldsPtr.size());
|
||||
return m_manifoldsPtr[index];
|
||||
}
|
||||
|
||||
const btPersistentManifold* getManifoldByIndexInternal(int index) const
|
||||
{
|
||||
btAssert(index>=0);
|
||||
btAssert(index<m_manifoldsPtr.size());
|
||||
return m_manifoldsPtr[index];
|
||||
}
|
||||
|
||||
|
@ -876,7 +876,10 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
|
||||
// will we not request a velocity with the wrong direction ?
|
||||
// and the answer is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
|
||||
// so the sign of the force that is really matters
|
||||
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
|
||||
if (m_flags & BT_6DOF_FLAGS_USE_INFINITE_ERROR)
|
||||
info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
|
||||
else
|
||||
info->m_constraintError[srow] = vel + f / m * (rotational ? -1 : 1);
|
||||
|
||||
btScalar minf = f < fd ? f : fd;
|
||||
btScalar maxf = f < fd ? fd : f;
|
||||
|
@ -265,6 +265,7 @@ enum bt6DofFlags2
|
||||
BT_6DOF_FLAGS_ERP_STOP2 = 2,
|
||||
BT_6DOF_FLAGS_CFM_MOTO2 = 4,
|
||||
BT_6DOF_FLAGS_ERP_MOTO2 = 8,
|
||||
BT_6DOF_FLAGS_USE_INFINITE_ERROR = (1<<16)
|
||||
};
|
||||
#define BT_6DOF_FLAGS_AXIS_SHIFT2 4 // bits per axis
|
||||
|
||||
|
@ -14,7 +14,9 @@ subject to the following restrictions:
|
||||
*/
|
||||
|
||||
//#define COMPUTE_IMPULSE_DENOM 1
|
||||
//#define BT_ADDITIONAL_DEBUG
|
||||
#ifdef BT_DEBUG
|
||||
# define BT_ADDITIONAL_DEBUG
|
||||
#endif
|
||||
|
||||
//It is not necessary (redundant) to refresh contact manifolds, this refresh has been moved to the collision algorithms.
|
||||
|
||||
@ -690,8 +692,10 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
|
||||
{
|
||||
#if BT_THREADSAFE
|
||||
int solverBodyId = -1;
|
||||
bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
|
||||
if (isRigidBodyType && !body.isStaticOrKinematicObject())
|
||||
const bool isRigidBodyType = btRigidBody::upcast(&body) != NULL;
|
||||
const bool isStaticOrKinematic = body.isStaticOrKinematicObject();
|
||||
const bool isKinematic = body.isKinematicObject();
|
||||
if (isRigidBodyType && !isStaticOrKinematic)
|
||||
{
|
||||
// dynamic body
|
||||
// Dynamic bodies can only be in one island, so it's safe to write to the companionId
|
||||
@ -704,7 +708,7 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
|
||||
body.setCompanionId(solverBodyId);
|
||||
}
|
||||
}
|
||||
else if (isRigidBodyType && body.isKinematicObject())
|
||||
else if (isRigidBodyType && isKinematic)
|
||||
{
|
||||
//
|
||||
// NOTE: must test for kinematic before static because some kinematic objects also
|
||||
|
@ -136,8 +136,13 @@ void btRigidBody::setGravity(const btVector3& acceleration)
|
||||
|
||||
void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
|
||||
{
|
||||
m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
|
||||
m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
|
||||
#ifdef BT_USE_OLD_DAMPING_METHOD
|
||||
m_linearDamping = btMax(lin_damping, btScalar(0.0));
|
||||
m_angularDamping = btMax(ang_damping, btScalar(0.0));
|
||||
#else
|
||||
m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
|
||||
m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
|
||||
#endif
|
||||
}
|
||||
|
||||
///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
|
||||
@ -146,10 +151,9 @@ void btRigidBody::applyDamping(btScalar timeStep)
|
||||
//On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
|
||||
//todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
|
||||
|
||||
//#define USE_OLD_DAMPING_METHOD 1
|
||||
#ifdef USE_OLD_DAMPING_METHOD
|
||||
m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
|
||||
m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
|
||||
#ifdef BT_USE_OLD_DAMPING_METHOD
|
||||
m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
|
||||
m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
|
||||
#else
|
||||
m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
|
||||
m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
|
||||
|
@ -171,6 +171,8 @@ void btSimulationIslandManagerMt::initIslandPools()
|
||||
|
||||
btSimulationIslandManagerMt::Island* btSimulationIslandManagerMt::getIsland(int id)
|
||||
{
|
||||
btAssert(id >= 0);
|
||||
btAssert(id < m_lookupIslandFromId.size());
|
||||
Island* island = m_lookupIslandFromId[id];
|
||||
if (island == NULL)
|
||||
{
|
||||
|
@ -583,52 +583,6 @@ void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
|
||||
}
|
||||
}
|
||||
|
||||
btScalar btMultiBody::getKineticEnergy() const
|
||||
{
|
||||
int num_links = getNumLinks();
|
||||
// TODO: would be better not to allocate memory here
|
||||
btAlignedObjectArray<btVector3> omega;
|
||||
omega.resize(num_links + 1);
|
||||
btAlignedObjectArray<btVector3> vel;
|
||||
vel.resize(num_links + 1);
|
||||
compTreeLinkVelocities(&omega[0], &vel[0]);
|
||||
|
||||
// we will do the factor of 0.5 at the end
|
||||
btScalar result = m_baseMass * vel[0].dot(vel[0]);
|
||||
result += omega[0].dot(m_baseInertia * omega[0]);
|
||||
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
result += m_links[i].m_mass * vel[i + 1].dot(vel[i + 1]);
|
||||
result += omega[i + 1].dot(m_links[i].m_inertiaLocal * omega[i + 1]);
|
||||
}
|
||||
|
||||
return 0.5f * result;
|
||||
}
|
||||
|
||||
btVector3 btMultiBody::getAngularMomentum() const
|
||||
{
|
||||
int num_links = getNumLinks();
|
||||
// TODO: would be better not to allocate memory here
|
||||
btAlignedObjectArray<btVector3> omega;
|
||||
omega.resize(num_links + 1);
|
||||
btAlignedObjectArray<btVector3> vel;
|
||||
vel.resize(num_links + 1);
|
||||
btAlignedObjectArray<btQuaternion> rot_from_world;
|
||||
rot_from_world.resize(num_links + 1);
|
||||
compTreeLinkVelocities(&omega[0], &vel[0]);
|
||||
|
||||
rot_from_world[0] = m_baseQuat;
|
||||
btVector3 result = quatRotate(rot_from_world[0].inverse(), (m_baseInertia * omega[0]));
|
||||
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
rot_from_world[i + 1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent + 1];
|
||||
result += (quatRotate(rot_from_world[i + 1].inverse(), (m_links[i].m_inertiaLocal * omega[i + 1])));
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
void btMultiBody::clearConstraintForces()
|
||||
{
|
||||
|
@ -307,13 +307,6 @@ public:
|
||||
//
|
||||
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const;
|
||||
|
||||
//
|
||||
// calculate kinetic energy and angular momentum
|
||||
// useful for debugging.
|
||||
//
|
||||
|
||||
btScalar getKineticEnergy() const;
|
||||
btVector3 getAngularMomentum() const;
|
||||
|
||||
//
|
||||
// set external forces and torques. Note all external forces/torques are given in the WORLD frame.
|
||||
|
@ -30,10 +30,12 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, infoGlobal, debugDrawer);
|
||||
|
||||
//solve featherstone non-contact constraints
|
||||
|
||||
btScalar nonContactResidual = 0;
|
||||
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
|
||||
for (int i = 0; i < infoGlobal.m_numNonContactInnerIterations; ++i)
|
||||
{
|
||||
// reset the nonContactResdual to 0 at start of each inner iteration
|
||||
nonContactResidual = 0;
|
||||
for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
|
||||
{
|
||||
int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
|
||||
@ -41,7 +43,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
|
||||
|
||||
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
|
||||
leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
|
||||
nonContactResidual = btMax(nonContactResidual, residual * residual);
|
||||
|
||||
if (constraint.m_multiBodyA)
|
||||
constraint.m_multiBodyA->setPosUpdated(false);
|
||||
@ -49,6 +51,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
|
||||
constraint.m_multiBodyB->setPosUpdated(false);
|
||||
}
|
||||
}
|
||||
leastSquaredResidual = btMax(leastSquaredResidual, nonContactResidual);
|
||||
|
||||
//solve featherstone normal contact
|
||||
for (int j0 = 0; j0 < m_multiBodyNormalContactConstraints.size(); j0++)
|
||||
|
@ -222,6 +222,9 @@ void btSoftBody::initDefaults()
|
||||
m_useSelfCollision = false;
|
||||
m_collisionFlags = 0;
|
||||
m_softSoftCollision = false;
|
||||
m_maxSpeedSquared = 0;
|
||||
m_repulsionStiffness = 0.5;
|
||||
m_fdbvnt = 0;
|
||||
}
|
||||
|
||||
//
|
||||
@ -2602,7 +2605,7 @@ void btSoftBody::initializeFaceTree()
|
||||
for (int i = 0; i < m_faces.size(); ++i)
|
||||
{
|
||||
Face& f = m_faces[i];
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, m_sst.radmrg);
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = VolumeOf(f, 0);
|
||||
btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
|
||||
node->parent = NULL;
|
||||
node->data = &f;
|
||||
@ -2638,10 +2641,10 @@ void btSoftBody::initializeFaceTree()
|
||||
}
|
||||
}
|
||||
m_fdbvt.m_root = buildTreeBottomUp(leafNodes, adj);
|
||||
updateFaceTree(false, true);
|
||||
if (m_fdbvnt)
|
||||
delete m_fdbvnt;
|
||||
m_fdbvnt = copyToDbvnt(m_fdbvt.m_root);
|
||||
updateFaceTree(false, false);
|
||||
rebuildNodeTree();
|
||||
}
|
||||
|
||||
@ -2654,7 +2657,7 @@ void btSoftBody::rebuildNodeTree()
|
||||
for (int i = 0; i < m_nodes.size(); ++i)
|
||||
{
|
||||
Node& n = m_nodes[i];
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, m_sst.radmrg);
|
||||
ATTRIBUTE_ALIGNED16(btDbvtVolume) vol = btDbvtVolume::FromCR(n.m_x, 0);
|
||||
btDbvtNode* node = new (btAlignedAlloc(sizeof(btDbvtNode), 16)) btDbvtNode();
|
||||
node->parent = NULL;
|
||||
node->data = &n;
|
||||
@ -3451,7 +3454,7 @@ void btSoftBody::setSpringStiffness(btScalar k)
|
||||
{
|
||||
m_links[i].Feature::m_material->m_kLST = k;
|
||||
}
|
||||
repulsionStiffness = k;
|
||||
m_repulsionStiffness = k;
|
||||
}
|
||||
|
||||
void btSoftBody::initializeDmInverse()
|
||||
|
@ -724,6 +724,15 @@ public:
|
||||
/* SolverState */
|
||||
struct SolverState
|
||||
{
|
||||
//if you add new variables, always initialize them!
|
||||
SolverState()
|
||||
:sdt(0),
|
||||
isdt(0),
|
||||
velmrg(0),
|
||||
radmrg(0),
|
||||
updmrg(0)
|
||||
{
|
||||
}
|
||||
btScalar sdt; // dt*timescale
|
||||
btScalar isdt; // 1/sdt
|
||||
btScalar velmrg; // velocity margin
|
||||
@ -810,7 +819,7 @@ public:
|
||||
btScalar m_sleepingThreshold;
|
||||
btScalar m_maxSpeedSquared;
|
||||
btAlignedObjectArray<btVector3> m_quads; // quadrature points for collision detection
|
||||
btScalar repulsionStiffness;
|
||||
btScalar m_repulsionStiffness;
|
||||
btAlignedObjectArray<btVector3> m_X; // initial positions
|
||||
|
||||
btAlignedObjectArray<btVector4> m_renderNodesInterpolationWeights;
|
||||
@ -1300,7 +1309,7 @@ public:
|
||||
btScalar I = 0;
|
||||
btScalar mass = node->m_im == 0 ? 0 : btScalar(1)/node->m_im;
|
||||
if (applySpringForce)
|
||||
I = -btMin(repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
|
||||
I = -btMin(m_repulsionStiffness * timeStep * d, mass * (OVERLAP_REDUCTION_FACTOR * d / timeStep - vn));
|
||||
if (vn < 0)
|
||||
I += 0.5 * mass * vn;
|
||||
btScalar face_penetration = 0, node_penetration = node->m_penetration;
|
||||
|
@ -1300,13 +1300,15 @@ btSoftBody* btSoftBodyHelpers::CreateFromVtkFile(btSoftBodyWorldInfo& worldInfo,
|
||||
}
|
||||
else if (reading_tets)
|
||||
{
|
||||
int d;
|
||||
ss >> d;
|
||||
if (d != 4)
|
||||
{
|
||||
printf("Only Tetrahedra are supported in vtk file\n");
|
||||
return 0;
|
||||
}
|
||||
int d;
|
||||
ss >> d;
|
||||
if (d != 4)
|
||||
{
|
||||
printf("Load deformable failed: Only Tetrahedra are supported in VTK file.\n");
|
||||
fs.close();
|
||||
return 0;
|
||||
}
|
||||
ss.ignore(128, ' '); // ignore "4"
|
||||
Index tet;
|
||||
tet.resize(4);
|
||||
for (size_t i = 0; i < 4; i++)
|
||||
@ -1597,7 +1599,13 @@ void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
|
||||
{
|
||||
new_min_bary_weight = btMin(new_min_bary_weight, bary[k]);
|
||||
}
|
||||
if (new_min_bary_weight > min_bary_weight)
|
||||
|
||||
// p is out of the current best triangle, we found a traingle that's better
|
||||
bool better_than_closest_outisde = (new_min_bary_weight > min_bary_weight && min_bary_weight<0.);
|
||||
// p is inside of the current best triangle, we found a triangle that's better
|
||||
bool better_than_best_inside = (new_min_bary_weight>=0 && min_bary_weight>=0 && btFabs(dist)<btFabs(optimal_dist));
|
||||
|
||||
if (better_than_closest_outisde || better_than_best_inside)
|
||||
{
|
||||
btAlignedObjectArray<const btSoftBody::Node*> parents;
|
||||
parents.push_back(f.m_n[0]);
|
||||
@ -1607,11 +1615,6 @@ void btSoftBodyHelpers::extrapolateBarycentricWeights(btSoftBody* psb)
|
||||
optimal_bary = bary;
|
||||
optimal_dist = dist;
|
||||
min_bary_weight = new_min_bary_weight;
|
||||
// stop searching if the projected p is inside the triangle at hand
|
||||
if (bary[0]>=0. && bary[1]>=0. && bary[2]>=0.)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
psb->m_renderNodesInterpolationWeights[i] = optimal_bary;
|
||||
|
@ -346,10 +346,9 @@ struct btMatrixX
|
||||
T dotProd = 0;
|
||||
{
|
||||
{
|
||||
int r = rows();
|
||||
int c = cols();
|
||||
|
||||
for (int k = 0; k < cols(); k++)
|
||||
for (int k = 0; k < c; k++)
|
||||
{
|
||||
T w = (*this)(i, k);
|
||||
if (other(k, j) != 0.f)
|
||||
|
Loading…
Reference in New Issue
Block a user