AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai 2017-02-03 12:32:08 -08:00
commit 3442b6524a
10 changed files with 170 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
data/sphere2_rolling_friction.urdf
View File

@ -3,6 +3,7 @@
<link name="base_link">
<contact>
<rolling_friction value="0.3"/>
<spinning_friction value="0.3"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>

BIN
docs/pybullet_quickstartguide.pdf
View File

Binary file not shown.

8
examples/Benchmarks/BenchmarkDemo.cpp
View File

@ -210,9 +210,13 @@ public:
{
for ( int i = iBegin; i < iEnd; ++i )
{
btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]);
cw->rayTest (source[i], dest[i], cb);
{
BT_PROFILE("cw->rayTest");
cw->rayTest(source[i], dest[i], cb);
}
if (cb.hasHit ())
{
hit[i] = cb.m_hitPointWorld;
@ -241,6 +245,8 @@ public:
void cast (btCollisionWorld* cw, bool multiThreading = false)
{
BT_PROFILE("cast");
#ifdef USE_BT_CLOCK
frame_timer.reset ();
#endif //USE_BT_CLOCK

47
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@ -295,8 +295,19 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
} else
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
return false;
if (ixx && iyy && izz)
{
inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
inertia.m_ixy = 0;
inertia.m_ixz = 0;
inertia.m_iyy = urdfLexicalCast<double>(iyy->GetText());
inertia.m_iyz = 0;
inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
} else
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
return false;
}
}
} else
{
@ -304,15 +315,29 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz")))
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
if ((inertia_xml->Attribute("ixx") && inertia_xml->Attribute("iyy") &&
inertia_xml->Attribute("izz")))
{
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = 0;
inertia.m_ixz = 0;
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = 0;
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
} else
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
} else
{
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
}
}
return true;

1
examples/MultiThreadedDemo/CommonRigidBodyMTBase.cpp
View File

@ -611,6 +611,7 @@ btConstraintSolver* createSolverByType( SolverType t )
case SOLVER_TYPE_MLCP_LEMKE:
mlcpSolver = new btLemkeSolver();
break;
default: {}
}
if (mlcpSolver)
{

1
examples/MultiThreadedDemo/CommonRigidBodyMTBase.h
View File

@ -31,6 +31,7 @@ inline const char* getSolverTypeName( SolverType t )
case SOLVER_TYPE_MLCP_PGS: return "MLCP ProjectedGaussSeidel";
case SOLVER_TYPE_MLCP_DANTZIG: return "MLCP Dantzig";
case SOLVER_TYPE_MLCP_LEMKE: return "MLCP Lemke";
default:{}
}
btAssert( !"unhandled solver type in switch" );
return "???";

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -189,6 +189,11 @@ struct b3BodyInfo
};
// copied from btMultiBodyLink.h
enum SensorType {
eSensorForceTorqueType = 1,
};
struct b3JointSensorState
{

2
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@ -211,7 +211,7 @@ updateVertex(
positions.push_back(in_positions[3*i.v_idx+1]);
positions.push_back(in_positions[3*i.v_idx+2]);
if (i.vn_idx >= 0) {
if (i.vn_idx >= 0 && ((3*i.vn_idx+2)<in_normals.size())) {
normals.push_back(in_normals[3*i.vn_idx+0]);
normals.push_back(in_normals[3*i.vn_idx+1]);
normals.push_back(in_normals[3*i.vn_idx+2]);

26
examples/pybullet/forcetorquesensor.py Normal file
View File

@ -0,0 +1,26 @@
import pybullet as p
p.connect(p.DIRECT)
hinge = p.loadURDF("hinge.urdf")
print("mass of linkA = 1kg, linkB = 1kg, total mass = 2kg")
hingeJointIndex = 0
#by default, joint motors are enabled, maintaining zero velocity
p.setJointMotorControl2(hinge,hingeJointIndex,p.VELOCITY_CONTROL,0,0,0)
p.setGravity(0,0,-10)
p.stepSimulation()
print("joint state without sensor")
print(p.getJointState(0,0))
p.enableJointForceTorqueSensor(hinge,hingeJointIndex)
p.stepSimulation()
print("joint state with force/torque sensor, gravity [0,0,-10]")
print(p.getJointState(0,0))
p.setGravity(0,0,0)
p.stepSimulation()
print("joint state with force/torque sensor, no gravity")
print(p.getJointState(0,0))
p.disconnect()

111
examples/pybullet/pybullet.c
View File

@ -2668,7 +2668,7 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keyw
return Py_None;
}
static PyObject* pybullet_getMatrixFromQuaterion(PyObject* self, PyObject* args)
static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args)
{
PyObject* quatObj;
double quat[4];
@ -2907,7 +2907,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes);
for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++)
{
PyObject* visualShapeObList = PyTuple_New(7);
PyObject* visualShapeObList = PyTuple_New(8);
PyObject* item;
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId);
PyTuple_SetItem(visualShapeObList, 0, item);
@ -2920,11 +2920,11 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
{
PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 3, vec);
}
@ -2934,28 +2934,41 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
{
PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[0]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[0]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[1]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[1]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[2]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 5, vec);
}
{
PyObject* vec = PyTuple_New(4);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[3]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[3]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[4]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[4]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[5]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[5]);
PyTuple_SetItem(vec, 2, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[6]);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[6]);
PyTuple_SetItem(vec, 3, item);
PyTuple_SetItem(visualShapeObList, 6, vec);
}
{
PyObject* rgba = PyTuple_New(4);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[0]);
PyTuple_SetItem(rgba, 0, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[1]);
PyTuple_SetItem(rgba, 1, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[2]);
PyTuple_SetItem(rgba, 2, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[3]);
PyTuple_SetItem(rgba, 3, item);
PyTuple_SetItem(visualShapeObList, 7, rgba);
}
PyTuple_SetItem(pyResultList, i, visualShapeObList);
}
@ -3362,6 +3375,63 @@ static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, P
}
*/
static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* args, PyObject *keywds)
{
int bodyUniqueId = -1;
int jointIndex = -1;
int enableSensor = 1;
int physicsClientId = 0;
b3PhysicsClientHandle sm = 0;
int numJoints = -1;
static char *kwlist[] = {"bodyUniqueId", "jointIndex" ,"enableSensor", "physicsClientId" };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &jointIndex, &enableSensor,&physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (bodyUniqueId < 0)
{
PyErr_SetString(SpamError, "Error: invalid bodyUniqueId");
return NULL;
}
numJoints = b3GetNumJoints(sm, bodyUniqueId);
if ((jointIndex < 0) || (jointIndex >= numJoints))
{
PyErr_SetString(SpamError, "Error: invalid jointIndex.");
return NULL;
}
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
commandHandle = b3CreateSensorCommandInit(sm, bodyUniqueId);
b3CreateSensorEnable6DofJointForceTorqueSensor(commandHandle, jointIndex, enableSensor);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_CLIENT_COMMAND_COMPLETED)
{
Py_INCREF(Py_None);
return Py_None;
}
}
PyErr_SetString(SpamError, "Error creating sensor.");
return NULL;
}
static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
{
@ -3455,15 +3525,11 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P
int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
PyObject* ob = PyLong_FromLong(userConstraintUid);
return ob;
} else
{
PyErr_SetString(SpamError, "createConstraint failed.");
return NULL;
}
Py_INCREF(Py_None);
return Py_None;
PyErr_SetString(SpamError, "createConstraint failed.");
return NULL;
}
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) {
@ -4690,6 +4756,10 @@ static PyMethodDef SpamMethods[] = {
"Remove a constraint using its unique id."
},
{ "enableJointForceTorqueSensor", (PyCFunction)pybullet_enableJointForceTorqueSensor, METH_VARARGS | METH_KEYWORDS,
"Enable or disable a joint force/torque sensor measuring the joint reaction forces."
},
{"saveWorld", (PyCFunction)pybullet_saveWorld, METH_VARARGS| METH_KEYWORDS,
"Save a approximate Python file to reproduce the current state of the world: saveWorld"
"(filename). (very preliminary and approximately)"},
@ -4868,7 +4938,7 @@ static PyMethodDef SpamMethods[] = {
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF "
"convention"},
{"getMatrixFromQuaterion", pybullet_getMatrixFromQuaterion,METH_VARARGS,
{"getMatrixFromQuaternion", pybullet_getMatrixFromQuaternion,METH_VARARGS,
"Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},
{"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS| METH_KEYWORDS,
@ -4954,6 +5024,9 @@ initpybullet(void)
PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read
PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read
PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
PyModule_AddIntConstant(m, "VELOCITY_CONTROL",
CONTROL_MODE_VELOCITY); // user read