diff --git a/data/gripper/wsg50_one_motor_gripper_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_free_base.sdf
index 7fc3ed2b2..552c36d28 100644
--- a/data/gripper/wsg50_one_motor_gripper_free_base.sdf
+++ b/data/gripper/wsg50_one_motor_gripper_free_base.sdf
@@ -303,6 +303,10 @@
+
+
+
+
0.042 0 0.145 0 0 1.5708
0.2
@@ -343,6 +347,10 @@
+
+
+
+
-0.042 0 0.145 0 0 4.71239
0.2
diff --git a/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
index 68e2a0a6e..0358f7a6a 100644
--- a/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
+++ b/data/gripper/wsg50_one_motor_gripper_new_free_base.sdf
@@ -300,6 +300,10 @@
+
+ 1.0
+ 1.5
+
0.062 0 0.145 0 0 1.5708
0.2
@@ -340,6 +344,10 @@
+
+ 1.0
+ 1.5
+
-0.062 0 0.145 0 0 4.71239
0.2
diff --git a/data/quadruped/quadruped.urdf b/data/quadruped/quadruped.urdf
index 843371c32..32e9c131c 100644
--- a/data/quadruped/quadruped.urdf
+++ b/data/quadruped/quadruped.urdf
@@ -5,8 +5,8 @@
-
-
+
+
@@ -26,9 +26,7 @@
-
-
-
+
@@ -55,9 +53,7 @@
-
-
-
+
@@ -84,9 +80,7 @@
-
-
-
+
@@ -113,9 +107,7 @@
-
-
-
+
@@ -142,9 +134,7 @@
-
-
-
+
@@ -171,9 +161,7 @@
-
-
-
+
@@ -200,9 +188,7 @@
-
-
-
+
@@ -229,9 +215,7 @@
-
-
-
+
@@ -258,8 +242,8 @@
-
-
+
+
@@ -286,8 +270,8 @@
-
-
+
+
@@ -315,8 +299,8 @@
-
-
+
+
@@ -345,8 +329,8 @@
-
-
+
+
@@ -374,8 +358,8 @@
-
-
+
+
@@ -402,8 +386,8 @@
-
-
+
+
@@ -431,8 +415,8 @@
-
-
+
+
@@ -461,8 +445,8 @@
-
-
+
+
@@ -493,8 +477,8 @@
-
-
+
+
@@ -521,8 +505,8 @@
-
-
+
+
@@ -550,8 +534,8 @@
-
-
+
+
@@ -580,8 +564,8 @@
-
-
+
+
@@ -609,8 +593,8 @@
-
-
+
+
@@ -637,8 +621,8 @@
-
-
+
+
@@ -666,8 +650,8 @@
-
-
+
+
@@ -696,8 +680,8 @@
-
-
+
+
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index 25c420522..b57304ca2 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -48,7 +48,7 @@ int gHuskyId = -1;
btTransform huskyTr = btTransform::getIdentity();
int gCreateObjectSimVR = -1;
-int gEnableKukaControl = 1;
+int gEnableKukaControl = 0;
btScalar simTimeScalingFactor = 1;
btScalar gRhsClamp = 1.f;
@@ -667,8 +667,6 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
m_data = new PhysicsServerCommandProcessorInternalData();
createEmptyDynamicsWorld();
- m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
- m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
}
@@ -717,6 +715,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
+ m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
+ m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+
}
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()
@@ -4030,7 +4031,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
}
-btVector3 gVRGripperPos(0,0,0.2);
+btVector3 gVRGripperPos(0.6, 0.4, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1);
@@ -4044,6 +4045,12 @@ int gDroppedSimulationSteps = 0;
int gNumSteps = 0;
double gDtInSec = 0.f;
double gSubStep = 0.f;
+
+void PhysicsServerCommandProcessor::enableRealTimeSimulation(bool enableRealTimeSim)
+{
+ m_data->m_allowRealTimeSimulation = enableRealTimeSim;
+}
+
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{
if (gResetSimulation)
@@ -4052,7 +4059,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
gResetSimulation = false;
}
- if ((gEnableRealTimeSimVR || m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
+ if ((m_data->m_allowRealTimeSimulation) && m_data->m_guiHelper)
{
///this hardcoded C++ scene creation is temporary for demo purposes. It will be done in Python later...
@@ -4170,7 +4177,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
{
int bodyId = 0;
- if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
+ if (loadUrdf("pr2_gripper.urdf", btVector3(-0.2, 0.15, 0.9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()))
{
InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody)
@@ -4200,6 +4207,30 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
loadUrdf("kuka_iiwa/model_vr_limits.urdf", btVector3(1.4, -0.2, 0.6), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_KukaId = bodyId;
+ InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
+ if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs() == 7)
+ {
+ btScalar q[7];
+ q[0] = 0;// -SIMD_HALF_PI;
+ q[1] = 0;
+ q[2] = 0;
+ q[3] = SIMD_HALF_PI;
+ q[4] = 0;
+ q[5] = -SIMD_HALF_PI*0.66;
+ q[6] = 0;
+
+ for (int i = 0; i < 7; i++)
+ {
+ kukaBody->m_multiBody->setJointPos(i, q[i]);
+ }
+ btAlignedObjectArray scratch_q;
+ btAlignedObjectArray scratch_m;
+ kukaBody->m_multiBody->forwardKinematics(scratch_q, scratch_m);
+ int nLinks = kukaBody->m_multiBody->getNumLinks();
+ scratch_q.resize(nLinks + 1);
+ scratch_m.resize(nLinks + 1);
+ kukaBody->m_multiBody->updateCollisionObjectWorldTransforms(scratch_q, scratch_m);
+ }
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .7), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .8), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("lego/lego.urdf", btVector3(1.0, -0.2, .9), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -4208,7 +4239,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Load one motor gripper for kuka
loadSdf("gripper/wsg50_one_motor_gripper_new_free_base.sdf", &gBufferServerToClient[0], gBufferServerToClient.size(), true);
m_data->m_gripperId = bodyId + 1;
- InteralBodyData* kukaBody = m_data->getHandle(m_data->m_KukaId);
+
InteralBodyData* gripperBody = m_data->getHandle(m_data->m_gripperId);
// Reset the default gripper motor maximum torque for damping to 0
@@ -4250,6 +4281,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute1);
m_data->m_dynamicsWorld->addMultiBodyConstraint(m_data->m_kukaGripperRevolute2);
+ kukaBody = m_data->getHandle(m_data->m_KukaId);
if (kukaBody->m_multiBody && kukaBody->m_multiBody->getNumDofs()==7)
{
gripperBody->m_multiBody->setHasSelfCollision(0);
@@ -4303,7 +4335,7 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
// Table area
loadUrdf("table/table.urdf", objectWorldTr[0].getOrigin(), objectWorldTr[0].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
- loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+ //loadUrdf("tray/tray_textured.urdf", objectWorldTr[1].getOrigin(), objectWorldTr[1].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("cup_small.urdf", objectWorldTr[2].getOrigin(), objectWorldTr[2].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
//loadUrdf("pitcher_small.urdf", objectWorldTr[3].getOrigin(), objectWorldTr[3].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
loadUrdf("teddy_vhacd.urdf", objectWorldTr[4].getOrigin(), objectWorldTr[4].getRotation(), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
@@ -4344,9 +4376,10 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets()
if (motor)
{
btScalar posTarget = (-0.048)*btMin(btScalar(0.75), gVRGripper2Analog) / 0.75;
- motor->setPositionTarget(posTarget, .2);
+ motor->setPositionTarget(posTarget, .8);
motor->setVelocityTarget(0.0, .5);
- motor->setMaxAppliedImpulse(5.0);
+ motor->setMaxAppliedImpulse(1.0);
+
}
}
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h
index dc7992980..c8a060069 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.h
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.h
@@ -85,6 +85,7 @@ public:
void replayFromLogFile(const char* fileName);
void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
void stepSimulationRealTime(double dtInSec);
+ void enableRealTimeSimulation(bool enableRealTimeSim);
void applyJointDamping(int bodyUniqueId);
};
diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp
index e5b4cc3e4..2bc823d3b 100644
--- a/examples/SharedMemory/PhysicsServerExample.cpp
+++ b/examples/SharedMemory/PhysicsServerExample.cpp
@@ -27,6 +27,7 @@
//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos1(0,0,0);
+btScalar gVRTeleportRotZ = 0;
btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn;
@@ -46,10 +47,47 @@ extern btScalar simTimeScalingFactor;
extern bool gVRGripperClosed;
-#if B3_USE_MIDI
+const char* startFileNameVR = "0_VRDemoSettings.txt";
#include
+//remember the settings (you don't want to re-tune again and again...)
+static void saveCurrentSettingsVR()
+{
+ FILE* f = fopen(startFileNameVR, "w");
+ if (f)
+ {
+ fprintf(f, "--camPosX= %f\n", gVRTeleportPos1[0]);
+ fprintf(f, "--camPosY= %f\n", gVRTeleportPos1[1]);
+ fprintf(f, "--camPosZ= %f\n", gVRTeleportPos1[2]);
+ fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
+ fclose(f);
+ }
+};
+
+static void loadCurrentSettingsVR(b3CommandLineArgs& args)
+{
+ int currentEntry = 0;
+ FILE* f = fopen(startFileNameVR, "r");
+ if (f)
+ {
+ char oneline[1024];
+ char* argv[] = { 0,&oneline[0] };
+
+ while (fgets(oneline, 1024, f) != NULL)
+ {
+ char *pos;
+ if ((pos = strchr(oneline, '\n')) != NULL)
+ *pos = '\0';
+ args.addArgs(2, argv);
+ }
+ fclose(f);
+ }
+
+};
+#if B3_USE_MIDI
+
+
static float getParamf(float rangeMin, float rangeMax, int midiVal)
{
float v = rangeMin + (rangeMax - rangeMin)* (float(midiVal / 127.));
@@ -70,9 +108,10 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
{
if (message->at(1) == 16)
{
- float rotZ = getParamf(-3.1415, 3.1415, message->at(2));
- gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), rotZ);
- b3Printf("gVRTeleportOrn rotZ = %f\n", rotZ);
+ gVRTeleportRotZ= getParamf(-3.1415, 3.1415, message->at(2));
+ gVRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
+ saveCurrentSettingsVR();
+ b3Printf("gVRTeleportOrn rotZ = %f\n", gVRTeleportRotZ);
}
if (message->at(1) == 32)
@@ -85,6 +124,7 @@ void midiCallback(double deltatime, std::vector< unsigned char > *message, void
if (message->at(1) == i)
{
gVRTeleportPos1[i] = getParamf(-2, 2, message->at(2));
+ saveCurrentSettingsVR();
b3Printf("gVRTeleportPos[%d] = %f\n", i,gVRTeleportPos1[i]);
}
@@ -927,7 +967,7 @@ public:
virtual void processCommandLineArgs(int argc, char* argv[])
{
b3CommandLineArgs args(argc,argv);
-
+ loadCurrentSettingsVR(args);
if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
{
printf("camPosX=%f\n", gVRTeleportPos1[0]);
@@ -1544,7 +1584,11 @@ void PhysicsServerExample::renderScene()
if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
{
- gEnableRealTimeSimVR = true;
+ if (!gEnableRealTimeSimVR)
+ {
+ gEnableRealTimeSimVR = true;
+ m_physicsServer.enableRealTimeSimulation(1);
+ }
}
@@ -1665,8 +1709,10 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
return;
if (gGraspingController < 0)
+ {
gGraspingController = controllerId;
-
+ gEnableKukaControl = true;
+ }
if (controllerId != gGraspingController)
{
if (button == 1 && state == 0)
@@ -1777,8 +1823,6 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
{
- gEnableRealTimeSimVR = true;
-
if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
{
printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.cpp b/examples/SharedMemory/PhysicsServerSharedMemory.cpp
index 111ac07f5..158bdac6b 100644
--- a/examples/SharedMemory/PhysicsServerSharedMemory.cpp
+++ b/examples/SharedMemory/PhysicsServerSharedMemory.cpp
@@ -241,6 +241,12 @@ void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
}
+void PhysicsServerSharedMemory::enableRealTimeSimulation(bool enableRealTimeSim)
+{
+ m_data->m_commandProcessor->enableRealTimeSimulation(enableRealTimeSim);
+}
+
+
void PhysicsServerSharedMemory::processClientCommands()
{
diff --git a/examples/SharedMemory/PhysicsServerSharedMemory.h b/examples/SharedMemory/PhysicsServerSharedMemory.h
index c330817b4..f29e844f2 100644
--- a/examples/SharedMemory/PhysicsServerSharedMemory.h
+++ b/examples/SharedMemory/PhysicsServerSharedMemory.h
@@ -28,6 +28,8 @@ public:
virtual void stepSimulationRealTime(double dtInSec);
+ virtual void enableRealTimeSimulation(bool enableRealTimeSim);
+
//bool supportsJointMotor(class btMultiBody* body, int linkIndex);
diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp
index d196c8ee8..89aa74204 100644
--- a/examples/StandaloneMain/hellovr_opengl_main.cpp
+++ b/examples/StandaloneMain/hellovr_opengl_main.cpp
@@ -855,7 +855,7 @@ void CMainApplication::RenderFrame()
// SwapWindow
{
B3_PROFILE("m_app->swapBuffer");
-// m_app->swapBuffer();
+ m_app->swapBuffer();
//SDL_GL_SwapWindow( m_pWindow );
}
diff --git a/examples/TinyRenderer/TinyRenderer.h b/examples/TinyRenderer/TinyRenderer.h
index 2ba811d80..c4df3c3e7 100644
--- a/examples/TinyRenderer/TinyRenderer.h
+++ b/examples/TinyRenderer/TinyRenderer.h
@@ -33,7 +33,7 @@ struct TinyRenderObjectData
TGAImage& m_rgbColorBuffer;
b3AlignedObjectArray& m_depthBuffer;//required, hence a reference
- b3AlignedObjectArray* m_shadowBuffer;
+ b3AlignedObjectArray* m_shadowBuffer;//optional, hence a pointer
b3AlignedObjectArray* m_segmentationMaskBufferPtr;//optional, hence a pointer
TinyRenderObjectData(TGAImage& rgbColorBuffer,b3AlignedObjectArray&depthBuffer);
diff --git a/examples/TinyRenderer/main.cpp b/examples/TinyRenderer/main.cpp
index 614b12d3a..32f5e7edd 100644
--- a/examples/TinyRenderer/main.cpp
+++ b/examples/TinyRenderer/main.cpp
@@ -147,7 +147,6 @@ int main(int argc, char* argv[])
renderData.m_rgbColorBuffer.set(x,y,color);
renderData.m_depthBuffer[x+y*textureWidth] = -1e30f;
- renderData.m_shadowBuffer[x+y*textureWidth] = -1e30f;
}
}
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index c2d6109f9..e5f78c1bc 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -432,7 +432,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
return NULL;
}
- static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold","numSubSteps", NULL };
+ static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps", NULL };
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps))
{
@@ -446,6 +446,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
{
b3PhysicsParamSetNumSolverIterations(command, numSolverIterations);
}
+ if (numSubSteps >= 0)
+ {
+ b3PhysicsParamSetNumSubSteps(command, numSubSteps);
+ }
if (fixedTimeStep >= 0)
{
b3PhysicsParamSetTimeStep(command, fixedTimeStep);
@@ -458,10 +462,6 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
{
b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold);
}
- if (numSubSteps>=0)
- {
- b3PhysicsParamSetNumSubSteps(command,numSubSteps);
- }
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
diff --git a/examples/pybullet/quadruped.py b/examples/pybullet/quadruped.py
index 6205191f1..31cffbce4 100644
--- a/examples/pybullet/quadruped.py
+++ b/examples/pybullet/quadruped.py
@@ -4,9 +4,10 @@ import math
p.connect(p.SHARED_MEMORY)
p.loadURDF("plane.urdf")
-quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
+p.setGravity(0,0,-1)
+p.setRealTimeSimulation(0)
+quadruped = p.loadURDF("quadruped/quadruped.urdf",10,-2,2)
#p.getNumJoints(1)
-
#right front leg
p.resetJointState(quadruped,0,1.57)
p.resetJointState(quadruped,2,-2.2)
@@ -64,16 +65,17 @@ p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)
-p.setGravity(0,0,-10)
+
p_gain = 2
speed = 10
amplitude = 1.3
#stand still
-t_end = time.time() + 5
+t_end = time.time() + 2
while time.time() < t_end:
p.stepSimulation()
+p.setGravity(0,0,-10)
jump_amp = 0.5
@@ -125,4 +127,4 @@ while time.time() < t_end:
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.stepSimulation()
-
+p.setRealTimeSimulation(1)
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index b85656875..3c6b6154c 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -570,7 +570,7 @@ void btMultiBody::clearForcesAndTorques()
void btMultiBody::clearVelocities()
{
- for (int i = 0; i < 6 + getNumLinks(); ++i)
+ for (int i = 0; i < 6 + getNumDofs(); ++i)
{
m_realBuf[i] = 0.f;
}