From 0b41fe1a491097cfd9ce102679108f64a7e76b38 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 7 Mar 2019 17:55:45 -0800 Subject: [PATCH 01/13] use b3RobotJointInfo instead of b3JointInfo, so it is initialized. don't reset the camera in SharedMemoryInProcessPhysicsC_API --- examples/BulletRobotics/FixJointBoxes.cpp | 13 ++++++------- .../SharedMemoryInProcessPhysicsC_API.cpp | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp index 875a76b1d..fa29d4f93 100644 --- a/examples/BulletRobotics/FixJointBoxes.cpp +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -57,7 +57,7 @@ public: args.m_startPosition.setValue(0, i * 0.05, 1); cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args); - b3JointInfo jointInfo; + b3RobotJointInfo jointInfo; jointInfo.m_parentFrame[1] = -0.025; jointInfo.m_childFrame[1] = 0.025; @@ -102,12 +102,11 @@ public: virtual void resetCamera() { - // float dist = 1; - // float pitch = -20; - // float yaw = -30; - // float targetPos[3] = {0, 0.2, 0.5}; - - // m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + float dist = 1; + float pitch = -20; + float yaw = -30; + float targetPos[3] = {0, 0.2, 0.5}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } }; diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index d4215dc01..5df2d31d7 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -163,7 +163,7 @@ public: options.m_skipGraphicsUpdate = skipGraphicsUpdate; m_physicsServerExample = PhysicsServerCreateFuncBullet2(options); m_physicsServerExample->initPhysics(); - m_physicsServerExample->resetCamera(); + //m_physicsServerExample->resetCamera(); setSharedMemoryInterface(m_sharedMem); m_clock.reset(); m_prevTime = m_clock.getTimeMicroseconds(); From a1f15ae01a6b885a0ad5ee40e42e462ead2ec373 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 7 Mar 2019 21:13:00 -0800 Subject: [PATCH 02/13] Expose anisotropic friction, add snake demo. Simple snake slither locomotion from > 15 years ago, thanks to Michael Ewert @ Havok! Visit http://www.snakerobots.com to see one of these in the wild --- examples/SharedMemory/PhysicsClientC_API.cpp | 15 +++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 19 ++- examples/SharedMemory/SharedMemoryCommands.h | 2 + examples/pybullet/examples/snake.py | 123 ++++++++++++++++++ examples/pybullet/pybullet.c | 11 +- 6 files changed, 169 insertions(+), 3 deletions(-) create mode 100644 examples/pybullet/examples/snake.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index cc2187ed5..12ef3975a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2780,6 +2780,21 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle comman return 0; } +B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double anisotropicFriction[]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex; + command->m_changeDynamicsInfoArgs.m_anisotropicFriction[0] = anisotropicFriction[0]; + command->m_changeDynamicsInfoArgs.m_anisotropicFriction[1] = anisotropicFriction[1]; + command->m_changeDynamicsInfoArgs.m_anisotropicFriction[2] = anisotropicFriction[2]; + + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION; + return 0; +} + + B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 01529410b..3499a8c88 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -148,7 +148,9 @@ extern "C" B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double localInertiaDiagonal[]); + B3_SHARED_API int b3ChangeDynamicsInfoSetAnisotropicFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, const double anisotropicFriction[]); + B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction); B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0c21c325e..f0f8dc6d2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7664,6 +7664,9 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc btVector3 newLocalInertiaDiagonal(clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0], clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1], clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2]); + btVector3 anisotropicFriction(clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[0], + clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[1], + clientCmd.m_changeDynamicsInfoArgs.m_anisotropicFriction[2]); btAssert(bodyUniqueId >= 0); @@ -7796,6 +7799,12 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc { mb->setBaseInertia(newLocalInertiaDiagonal); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) + { + mb->getBaseCollider()->setAnisotropicFriction(anisotropicFriction); + } + + } else { @@ -7858,6 +7867,10 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc { mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal; } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) + { + mb->getLinkCollider(linkIndex)->setAnisotropicFriction(anisotropicFriction); + } } } } @@ -7946,7 +7959,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal); } } - + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) + { + body->m_rigidBody->setAnisotropicFriction(anisotropicFriction); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) { body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 8b4dfcb4c..7c25bb3f1 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -164,6 +164,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD = 4096, CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE = 8192, CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING = 16384, + CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION = 32768, }; struct ChangeDynamicsInfoArgs @@ -186,6 +187,7 @@ struct ChangeDynamicsInfoArgs double m_contactProcessingThreshold; int m_activationState; double m_jointDamping; + double m_anisotropicFriction[3]; }; struct GetDynamicsInfoArgs diff --git a/examples/pybullet/examples/snake.py b/examples/pybullet/examples/snake.py new file mode 100644 index 000000000..52d876aeb --- /dev/null +++ b/examples/pybullet/examples/snake.py @@ -0,0 +1,123 @@ +import pybullet as p +import time +import math + + +# This simple snake logic is from some 15 year old Havok C++ demo +# Thanks to Michael Ewert! + + +p.connect(p.GUI) +plane = p.createCollisionShape(p.GEOM_PLANE) + +p.createMultiBody(0,plane) + +useMaximalCoordinates = False +sphereRadius = 0.25 +colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]]) + +mass = 1 +visualShapeId = -1 + +link_Masses=[] +linkCollisionShapeIndices=[] +linkVisualShapeIndices=[] +linkPositions=[] +linkOrientations=[] +linkInertialFramePositions=[] +linkInertialFrameOrientations=[] +indices=[] +jointTypes=[] +axis=[] + +for i in range (36): + link_Masses.append(1) + linkCollisionShapeIndices.append(colBoxId) + linkVisualShapeIndices.append(-1) + linkPositions.append([0,sphereRadius*2.0+0.01,0]) + linkOrientations.append([0,0,0,1]) + linkInertialFramePositions.append([0,0,0]) + linkInertialFrameOrientations.append([0,0,0,1]) + indices.append(i) + jointTypes.append(p.JOINT_REVOLUTE) + axis.append([0,0,1]) + +basePosition = [0,0,1] +baseOrientation = [0,0,0,1] +sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrientation,linkMasses=link_Masses,linkCollisionShapeIndices=linkCollisionShapeIndices,linkVisualShapeIndices=linkVisualShapeIndices,linkPositions=linkPositions,linkOrientations=linkOrientations,linkInertialFramePositions=linkInertialFramePositions, linkInertialFrameOrientations=linkInertialFrameOrientations,linkParentIndices=indices,linkJointTypes=jointTypes,linkJointAxis=axis, useMaximalCoordinates=useMaximalCoordinates) + +p.setGravity(0,0,-10) +p.setRealTimeSimulation(0) + +p.getNumJoints(sphereUid) +for i in range (p.getNumJoints(sphereUid)): + p.getJointInfo(sphereUid,i) + p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=[1,0.01,0.01])#0,0,0])#1,0.01,0.01]) + +dt = 1./240. +SNAKE_NORMAL_PERIOD=0.1#1.5 +m_wavePeriod = SNAKE_NORMAL_PERIOD + +m_waveLength=4 +m_wavePeriod=1.5 +m_waveAmplitude=0.4 +m_waveFront = 0.0 +#our steering value +m_steering = 0.0 +m_segmentLength = sphereRadius*2.0 +forward = 0 + +while (1): + keys = p.getKeyboardEvents() + for k,v in keys.items(): + + if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_TRIGGERED)): + m_steering = -.2 + if (k == p.B3G_RIGHT_ARROW and (v&p.KEY_WAS_RELEASED)): + m_steering = 0 + if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_TRIGGERED)): + m_steering = .2 + if (k == p.B3G_LEFT_ARROW and (v&p.KEY_WAS_RELEASED)): + m_steering = 0 + + amp = 0.2 + offset = 0.6 + numMuscles = p.getNumJoints(sphereUid) + scaleStart = 1.0 + + #start of the snake with smaller waves. + #I think starting the wave at the tail would work better ( while it still goes from head to tail ) + if( m_waveFront < m_segmentLength*4.0 ): + scaleStart = m_waveFront/(m_segmentLength*4.0) + + segment = numMuscles-1 + + #we simply move a sin wave down the body of the snake. + #this snake may be going backwards, but who can tell ;) + for joint in range (p.getNumJoints(sphereUid)): + segment = joint#numMuscles-1-joint + #map segment to phase + phase = (m_waveFront - (segment+1)*m_segmentLength)/ m_waveLength + phase -= math.floor(phase) + phase *= math.pi * 2.0 + + #map phase to curvature + targetPos = math.sin( phase ) * scaleStart * m_waveAmplitude + + #// steer snake by squashing +ve or -ve side of sin curve + if( m_steering > 0 and targetPos < 0 ): + targetPos *= 1.0/( 1.0 + m_steering ) + + if( m_steering < 0 and targetPos > 0 ): + targetPos *= 1.0/( 1.0 - m_steering ) + + #set our motor + p.setJointMotorControl2(sphereUid,joint,p.POSITION_CONTROL,targetPosition=targetPos+m_steering,force=30) + + #wave keeps track of where the wave is in time + m_waveFront += dt/m_wavePeriod * m_waveLength; + p.stepSimulation() + + + time.sleep(dt) + \ No newline at end of file diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 7f800d40c..755fcea13 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1242,12 +1242,13 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO int activationState = -1; double jointDamping = -1; PyObject* localInertiaDiagonalObj = 0; + PyObject* anisotropicFrictionObj = 0; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &physicsClientId)) { return NULL; } @@ -1273,6 +1274,12 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, mass); } + if (anisotropicFrictionObj) + { + double anisotropicFriction[3]; + pybullet_internalSetVectord(anisotropicFrictionObj, anisotropicFriction); + b3ChangeDynamicsInfoSetAnisotropicFriction(command, bodyUniqueId, linkIndex, anisotropicFriction); + } if (localInertiaDiagonalObj) { double localInertiaDiagonal[3]; From 84a7ceb41672588ea09c3be5a5298232443f912d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 7 Mar 2019 22:05:15 -0800 Subject: [PATCH 03/13] bump up PyBullet version to 2.4.7 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 169872e23..2eb03542d 100644 --- a/setup.py +++ b/setup.py @@ -466,7 +466,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.4.5', + version='2.4.7', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 53979b1b7bc75e8289fc8bbdfbc8f954db1df1e5 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 8 Mar 2019 07:43:50 -0800 Subject: [PATCH 04/13] tweak mouse wheel multiplier --- examples/CommonInterfaces/CommonGraphicsAppInterface.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/CommonInterfaces/CommonGraphicsAppInterface.h b/examples/CommonInterfaces/CommonGraphicsAppInterface.h index 959e51a40..acfaf478c 100644 --- a/examples/CommonInterfaces/CommonGraphicsAppInterface.h +++ b/examples/CommonInterfaces/CommonGraphicsAppInterface.h @@ -251,7 +251,7 @@ struct CommonGraphicsApp float cameraDistance = camera->getCameraDistance(); if (deltay < 0 || cameraDistance > 1) { - cameraDistance -= deltay*m_wheelMultiplier * 0.01f; + cameraDistance -= deltay*m_wheelMultiplier; if (cameraDistance < 1) cameraDistance = 1; camera->setCameraDistance(cameraDistance); From ce531e6015162e3b51b13d5aa1751fff9f0b44a8 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Fri, 8 Mar 2019 09:20:32 -0800 Subject: [PATCH 05/13] allow to PyBullet.changeDynamics for all links in maximal coordinate rigid bodies change snake.py to use useMaximalCoordinate = True by default --- .../ImportURDFDemo/MyMultiBodyCreator.cpp | 10 ++- .../PhysicsServerCommandProcessor.cpp | 68 ++++++++++++------- examples/pybullet/examples/snake.py | 9 ++- 3 files changed, 57 insertions(+), 30 deletions(-) diff --git a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp index 905a6cc00..e41ef7881 100644 --- a/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp +++ b/examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp @@ -33,9 +33,13 @@ class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btSc { btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal); rbci.m_startWorldTransform = initialWorldTrans; - m_rigidBody = new btRigidBody(rbci); - - return m_rigidBody; + btRigidBody* body = new btRigidBody(rbci); + if (m_rigidBody == 0) + { + //only store the root of the multi body + m_rigidBody = body; + } + return body; } class btMultiBodyLinkCollider* MyMultiBodyCreator::allocateMultiBodyLinkCollider(int /*urdfLinkIndex*/, int mbLinkIndex, btMultiBody* multiBody) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f0f8dc6d2..9de30169f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7876,104 +7876,124 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc } else { + + btRigidBody* rb = 0; if (body && body->m_rigidBody) + { + if (linkIndex == -1) + { + rb = body->m_rigidBody; + } + else + { + if (linkIndex >= 0 && linkIndex < body->m_rigidBodyJoints.size()) + { + btRigidBody* parentRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyA(); + btRigidBody* childRb = &body->m_rigidBodyJoints[linkIndex]->getRigidBodyB(); + rb = childRb; + } + + } + } + + if (rb) { if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE) { if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping) { - body->m_rigidBody->forceActivationState(ACTIVE_TAG); + rb->forceActivationState(ACTIVE_TAG); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping) { - body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION); + rb->forceActivationState(DISABLE_DEACTIVATION); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp) { - body->m_rigidBody->forceActivationState(ACTIVE_TAG); + rb->forceActivationState(ACTIVE_TAG); } if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep) { - body->m_rigidBody->forceActivationState(ISLAND_SLEEPING); + rb->forceActivationState(ISLAND_SLEEPING); } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) { - btScalar angDamping = body->m_rigidBody->getAngularDamping(); - body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping); + btScalar angDamping = rb->getAngularDamping(); + rb->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) { - btScalar linDamping = body->m_rigidBody->getLinearDamping(); - body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + btScalar linDamping = rb->getLinearDamping(); + rb->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) { - body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); + rb->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) { - body->m_rigidBody->setRestitution(restitution); + rb->setRestitution(restitution); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) { - body->m_rigidBody->setFriction(lateralFriction); + rb->setFriction(lateralFriction); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION) { - body->m_rigidBody->setSpinningFriction(spinningFriction); + rb->setSpinningFriction(spinningFriction); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION) { - body->m_rigidBody->setRollingFriction(rollingFriction); + rb->setRollingFriction(rollingFriction); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR) { if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor) { - body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); + rb->setCollisionFlags(rb->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR); } else { - body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); + rb->setCollisionFlags(rb->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR); } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS) { btVector3 localInertia; - if (body->m_rigidBody->getCollisionShape()) + if (rb->getCollisionShape()) { - body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass, localInertia); + rb->getCollisionShape()->calculateLocalInertia(mass, localInertia); } - body->m_rigidBody->setMassProps(mass, localInertia); + rb->setMassProps(mass, localInertia); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL) { - btScalar orgMass = body->m_rigidBody->getInvMass(); + btScalar orgMass = rb->getInvMass(); if (orgMass > 0) { - body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal); + rb->setMassProps(mass, newLocalInertiaDiagonal); } } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION) { - body->m_rigidBody->setAnisotropicFriction(anisotropicFriction); + rb->setAnisotropicFriction(anisotropicFriction); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD) { - body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); + rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); } if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) { - body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); + rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); //for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled - body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.); + rb->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.); } } } diff --git a/examples/pybullet/examples/snake.py b/examples/pybullet/examples/snake.py index 52d876aeb..af2c6d512 100644 --- a/examples/pybullet/examples/snake.py +++ b/examples/pybullet/examples/snake.py @@ -12,9 +12,10 @@ plane = p.createCollisionShape(p.GEOM_PLANE) p.createMultiBody(0,plane) -useMaximalCoordinates = False +useMaximalCoordinates = True sphereRadius = 0.25 -colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]]) +#colBoxId = p.createCollisionShapeArray([p.GEOM_BOX, p.GEOM_SPHERE],radii=[sphereRadius+0.03,sphereRadius+0.03], halfExtents=[[sphereRadius,sphereRadius,sphereRadius],[sphereRadius,sphereRadius,sphereRadius]]) +colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius]) mass = 1 visualShapeId = -1 @@ -49,10 +50,12 @@ sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,basePosition,baseOrien p.setGravity(0,0,-10) p.setRealTimeSimulation(0) +anistropicFriction = [1,0.01,0.01] +p.changeDynamics(sphereUid,-1,lateralFriction=2,anisotropicFriction=anistropicFriction) p.getNumJoints(sphereUid) for i in range (p.getNumJoints(sphereUid)): p.getJointInfo(sphereUid,i) - p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=[1,0.01,0.01])#0,0,0])#1,0.01,0.01]) + p.changeDynamics(sphereUid,i,lateralFriction=2,anisotropicFriction=anistropicFriction) dt = 1./240. SNAKE_NORMAL_PERIOD=0.1#1.5 From 550f4c478534ac98b3df657867117052afa151d9 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 9 Mar 2019 09:23:16 -0800 Subject: [PATCH 06/13] expose maxJointVelocity through PyBullet.changeDynamics, this Fixes Issue #1890 bump up PyBullet to version 2.4.8 --- examples/SharedMemory/PhysicsClientC_API.cpp | 13 +++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 3 ++- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 8 +++++++- examples/SharedMemory/SharedMemoryCommands.h | 2 ++ examples/pybullet/pybullet.c | 12 +++++++++--- setup.py | 2 +- 6 files changed, 34 insertions(+), 6 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 12ef3975a..8eaac0a6f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2928,6 +2928,19 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemo return 0; } +B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double maxJointVelocity) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linkIndex = -1; + command->m_changeDynamicsInfoArgs.m_maxJointVelocity = maxJointVelocity; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY; + return 0; +} + + + B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 3499a8c88..2b67887f0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -164,7 +164,8 @@ extern "C" B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double ccdSweptSphereRadius); B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold); B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState); - + B3_SHARED_API int b3ChangeDynamicsInfoSetMaxJointVelocity(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, double maxJointVelocity); + B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info); B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand2(b3SharedMemoryCommandHandle commandHandle, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9de30169f..e04686e81 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7804,7 +7804,11 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc mb->getBaseCollider()->setAnisotropicFriction(anisotropicFriction); } - + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY) + { + mb->setMaxCoordinateVelocity(clientCmd.m_changeDynamicsInfoArgs.m_maxJointVelocity); + } + } else { @@ -7989,6 +7993,8 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc rb->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold); } + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS) { rb->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 7c25bb3f1..5be07b05a 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -165,6 +165,7 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE = 8192, CHANGE_DYNAMICS_INFO_SET_JOINT_DAMPING = 16384, CHANGE_DYNAMICS_INFO_SET_ANISOTROPIC_FRICTION = 32768, + CHANGE_DYNAMICS_INFO_SET_MAX_JOINT_VELOCITY = 1<<16, }; struct ChangeDynamicsInfoArgs @@ -188,6 +189,7 @@ struct ChangeDynamicsInfoArgs int m_activationState; double m_jointDamping; double m_anisotropicFriction[3]; + double m_maxJointVelocity; }; struct GetDynamicsInfoArgs diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 755fcea13..dfd95e524 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1243,12 +1243,13 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO double jointDamping = -1; PyObject* localInertiaDiagonalObj = 0; PyObject* anisotropicFrictionObj = 0; - + double maxJointVelocity = -1; + b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction", "restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "jointDamping", "anisotropicFriction", "maxJointVelocity", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddidOdi", kwlist, &bodyUniqueId, &linkIndex, &mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution, &linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &activationState, &jointDamping, &anisotropicFrictionObj, &maxJointVelocity, &physicsClientId)) { return NULL; } @@ -1336,6 +1337,11 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO { b3ChangeDynamicsInfoSetContactProcessingThreshold(command, bodyUniqueId, linkIndex, contactProcessingThreshold); } + if (maxJointVelocity >= 0) + { + b3ChangeDynamicsInfoSetMaxJointVelocity(command, bodyUniqueId, maxJointVelocity); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } diff --git a/setup.py b/setup.py index 2eb03542d..adb6f3526 100644 --- a/setup.py +++ b/setup.py @@ -466,7 +466,7 @@ if 'BT_USE_EGL' in EGL_CXX_FLAGS: setup( name = 'pybullet', - version='2.4.7', + version='2.4.8', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 2ca6172f1ab957b0123f07cfd84b9e7443019213 Mon Sep 17 00:00:00 2001 From: Damian Bemben Date: Sun, 10 Mar 2019 22:41:40 +0000 Subject: [PATCH 07/13] Fixing issue where mass of motors is uneven Mass of motors was being tilted to the left, due to FL hip motor and RL hip motor having a mass of 1.095 and FR hip motor and BR hip motor having a mass of 0.241. This led to issues with the laikago tilting. May require further investigation to see if the laikago is at the proper center of mass! --- examples/pybullet/gym/pybullet_data/laikago/laikago.urdf | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/gym/pybullet_data/laikago/laikago.urdf b/examples/pybullet/gym/pybullet_data/laikago/laikago.urdf index 62e7e47b6..57b0a59b3 100644 --- a/examples/pybullet/gym/pybullet_data/laikago/laikago.urdf +++ b/examples/pybullet/gym/pybullet_data/laikago/laikago.urdf @@ -36,7 +36,7 @@ - + @@ -262,7 +262,7 @@ - + From 7aba1f9e8a546b4a5c7feee10f863736a36bb099 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 25 Mar 2019 14:41:58 -0700 Subject: [PATCH 08/13] Use two SI solvers as blocks in block solver. In the btBlockSolver we are experimenting with, we have SI for both multibody and rigid body. I'm currently replacing rigid body SI solver with two smaller SI solvers. The two examples provided by RigidBodyBoxes.h should have the same behavior. --- examples/BlockSolver/BlockSolverExample.cpp | 34 +- examples/BlockSolver/BlockSolverExample.h | 12 - examples/BlockSolver/btBlockSolver.cpp | 334 ++++++++++++++---- examples/BlockSolver/btBlockSolver.h | 70 +++- examples/BulletRobotics/FixJointBoxes.cpp | 23 +- examples/ExampleBrowser/CMakeLists.txt | 2 + .../CollisionShape2TriangleMesh.cpp | 10 +- examples/ExampleBrowser/ExampleEntries.cpp | 11 +- .../ExampleBrowser/OpenGLExampleBrowser.cpp | 1 - 9 files changed, 369 insertions(+), 128 deletions(-) diff --git a/examples/BlockSolver/BlockSolverExample.cpp b/examples/BlockSolver/BlockSolverExample.cpp index f3ab0cfed..ba699ee64 100644 --- a/examples/BlockSolver/BlockSolverExample.cpp +++ b/examples/BlockSolver/BlockSolverExample.cpp @@ -13,6 +13,7 @@ class BlockSolverExample : public CommonMultiBodyBase { int m_option; + public: BlockSolverExample(GUIHelperInterface* helper, int option); virtual ~BlockSolverExample(); @@ -35,10 +36,9 @@ public: btMultiBody* loadRobot(std::string filepath); }; - BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option) : CommonMultiBodyBase(helper), - m_option(option) + m_option(option) { m_guiHelper->setUpAxis(2); } @@ -51,13 +51,12 @@ BlockSolverExample::~BlockSolverExample() void BlockSolverExample::stepSimulation(float deltaTime) { //use a smaller internal timestep, there are stability issues - btScalar internalTimeStep = 1./240.f; + btScalar internalTimeStep = 1. / 240.f; m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep); } void BlockSolverExample::initPhysics() { - ///collision configuration contains default setup for memory, collision setup m_collisionConfiguration = new btDefaultCollisionConfiguration(); @@ -66,37 +65,34 @@ void BlockSolverExample::initPhysics() m_broadphase = new btDbvtBroadphase(); - btMLCPSolverInterface* mlcp; - if (m_option&BLOCK_SOLVER_SI) + if (m_option & BLOCK_SOLVER_SI) { btAssert(!m_solver); m_solver = new btMultiBodyConstraintSolver; b3Printf("Constraint Solver: Sequential Impulse"); } - if (m_option&BLOCK_SOLVER_MLCP_PGS) + if (m_option & BLOCK_SOLVER_MLCP_PGS) { btAssert(!m_solver); mlcp = new btSolveProjectedGaussSeidel(); m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); b3Printf("Constraint Solver: MLCP + PGS"); } - if (m_option&BLOCK_SOLVER_MLCP_DANTZIG) + if (m_option & BLOCK_SOLVER_MLCP_DANTZIG) { btAssert(!m_solver); mlcp = new btDantzigSolver(); m_solver = new btMultiBodyMLCPConstraintSolver(mlcp); b3Printf("Constraint Solver: MLCP + Dantzig"); } - if (m_option&BLOCK_SOLVER_BLOCK) + if (m_option & BLOCK_SOLVER_BLOCK) { m_solver = new btBlockSolver(); } btAssert(m_solver); - - btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); m_dynamicsWorld = world; m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); @@ -104,16 +100,14 @@ void BlockSolverExample::initPhysics() m_dynamicsWorld->getSolverInfo().m_numIterations = 50; m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6); //todo: what value is good? - if (m_option&BLOCK_SOLVER_SCENE_MB_STACK) + if (m_option & BLOCK_SOLVER_SCENE_MB_STACK) { createMultiBodyStack(); } - m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); } - void BlockSolverExample::createMultiBodyStack() { ///create a few basic rigid bodies @@ -134,7 +128,7 @@ void BlockSolverExample::createMultiBodyStack() btMultiBody* body = createMultiBody(mass, tr, groundShape); } - for (int i=0;i<10;i++) + for (int i = 0; i < 10; i++) { btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1))); m_collisionShapes.push_back(boxShape); @@ -143,10 +137,10 @@ void BlockSolverExample::createMultiBodyStack() mass = 100; btTransform tr; tr.setIdentity(); - tr.setOrigin(btVector3(0, 0, 0.1+i*0.2)); + tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2)); btMultiBody* body = createMultiBody(mass, tr, boxShape); } - if(0) + if (0) { btMultiBody* mb = loadRobot("cube_small.urdf"); btTransform tr; @@ -173,25 +167,21 @@ btMultiBody* BlockSolverExample::createMultiBody(btScalar mass, const btTransfor int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter); int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); - this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask); mb->setBaseCollider(collider); mb->finalizeMultiDof(); - this->m_dynamicsWorld->addMultiBody(mb); m_dynamicsWorld->forwardKinematics(); return mb; } - - btMultiBody* BlockSolverExample::loadRobot(std::string filepath) { btMultiBody* m_multiBody = 0; BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0); - bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf"); + bool loadOk = u2b.loadURDF(filepath.c_str()); // lwr / kuka.urdf"); if (loadOk) { int rootLinkIndex = u2b.getRootLinkIndex(); diff --git a/examples/BlockSolver/BlockSolverExample.h b/examples/BlockSolver/BlockSolverExample.h index 9d2c137be..faa11d885 100644 --- a/examples/BlockSolver/BlockSolverExample.h +++ b/examples/BlockSolver/BlockSolverExample.h @@ -2,18 +2,6 @@ #ifndef BLOCK_SOLVER_EXAMPLE_H #define BLOCK_SOLVER_EXAMPLE_H -enum BlockSolverOptions -{ - BLOCK_SOLVER_SI=1<<0, - BLOCK_SOLVER_MLCP_PGS = 1 << 1, - BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2, - BLOCK_SOLVER_BLOCK = 1 << 3, - - BLOCK_SOLVER_SCENE_MB_STACK= 1 << 5, - BLOCK_SOLVER_SCENE_CHAIN = 1<< 6, - -}; - class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options); #endif //BLOCK_SOLVER_EXAMPLE_H diff --git a/examples/BlockSolver/btBlockSolver.cpp b/examples/BlockSolver/btBlockSolver.cpp index f908cc487..fa7b0116b 100644 --- a/examples/BlockSolver/btBlockSolver.cpp +++ b/examples/BlockSolver/btBlockSolver.cpp @@ -1,7 +1,17 @@ #include "btBlockSolver.h" #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" + #include "LinearMath/btQuickprof.h" +void setupHelper(btSISolverSingleIterationData& siData, + btCollisionObject** bodies, int numBodies, + const btContactSolverInfo& info, + btTypedConstraint** constraintStart, int constrainNums, + btPersistentManifold** manifoldPtr, int numManifolds); + struct btBlockSolverInternalData { btAlignedObjectArray m_tmpSolverBodyPool; @@ -13,86 +23,273 @@ struct btBlockSolverInternalData btAlignedObjectArray m_orderTmpConstraintPool; btAlignedObjectArray m_orderNonContactConstraintPool; btAlignedObjectArray m_orderFrictionConstraintPool; - btAlignedObjectArray m_tmpConstraintSizesPool; - - + btAlignedObjectArray + m_tmpConstraintSizesPool; + unsigned long m_btSeed2; int m_fixedBodyId; int m_maxOverrideNumSolverIterations; - btAlignedObjectArray m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading + btAlignedObjectArray + m_kinematicBodyUniqueIdToSolverBodyTable; // only used for multithreading btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric; btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit; btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse; btBlockSolverInternalData() - :m_btSeed2(0), - m_fixedBodyId(-1), - m_maxOverrideNumSolverIterations(0), - m_resolveSingleConstraintRowGeneric(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()), - m_resolveSingleConstraintRowLowerLimit(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()), - m_resolveSplitPenetrationImpulse(btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric()) - { - } + : m_btSeed2(0), + m_fixedBodyId(-1), + m_maxOverrideNumSolverIterations(0), + m_resolveSingleConstraintRowGeneric( + btSequentialImpulseConstraintSolver:: + getScalarConstraintRowSolverGeneric()), + m_resolveSingleConstraintRowLowerLimit( + btSequentialImpulseConstraintSolver:: + getScalarConstraintRowSolverLowerLimit()), + m_resolveSplitPenetrationImpulse( + btSequentialImpulseConstraintSolver:: + getScalarSplitPenetrationImpulseGeneric()) {} }; - - - btBlockSolver::btBlockSolver() { - m_data2 = new btBlockSolverInternalData; + m_data21 = new btBlockSolverInternalData; + m_data22 = new btBlockSolverInternalData; } btBlockSolver::~btBlockSolver() { - delete m_data2; + delete m_data21; + delete m_data22; } - -btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +btScalar btBlockSolver::solveGroupInternalBlock( + btCollisionObject** bodies, int numBodies, + btPersistentManifold** manifoldPtr, int numManifolds, + btTypedConstraint** constraints, int numConstraints, + const btContactSolverInfo& info, btIDebugDraw* debugDrawer, + btDispatcher* dispatcher) { + // initialize data for two children solvers + btSISolverSingleIterationData siData1( + m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool, + m_data21->m_tmpSolverNonContactConstraintPool, + m_data21->m_tmpSolverContactFrictionConstraintPool, + m_data21->m_tmpSolverContactRollingFrictionConstraintPool, + m_data21->m_orderTmpConstraintPool, + m_data21->m_orderNonContactConstraintPool, + m_data21->m_orderFrictionConstraintPool, + m_data21->m_tmpConstraintSizesPool, + m_data21->m_resolveSingleConstraintRowGeneric, + m_data21->m_resolveSingleConstraintRowLowerLimit, + m_data21->m_resolveSplitPenetrationImpulse, + m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2, + m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations); - btSISolverSingleIterationData siData(m_data2->m_tmpSolverBodyPool, - m_data2->m_tmpSolverContactConstraintPool, - m_data2->m_tmpSolverNonContactConstraintPool, - m_data2->m_tmpSolverContactFrictionConstraintPool, - m_data2->m_tmpSolverContactRollingFrictionConstraintPool, - m_data2->m_orderTmpConstraintPool, - m_data2->m_orderNonContactConstraintPool, - m_data2->m_orderFrictionConstraintPool, - m_data2->m_tmpConstraintSizesPool, - m_data2->m_resolveSingleConstraintRowGeneric, - m_data2->m_resolveSingleConstraintRowLowerLimit, - m_data2->m_resolveSplitPenetrationImpulse, - m_data2->m_kinematicBodyUniqueIdToSolverBodyTable, - m_data2->m_btSeed2, - m_data2->m_fixedBodyId, - m_data2->m_maxOverrideNumSolverIterations); - - m_data2->m_fixedBodyId = -1; - //todo: setup sse2/4 constraint row methods + btSISolverSingleIterationData siData2( + m_data22->m_tmpSolverBodyPool, m_data22->m_tmpSolverContactConstraintPool, + m_data22->m_tmpSolverNonContactConstraintPool, + m_data22->m_tmpSolverContactFrictionConstraintPool, + m_data22->m_tmpSolverContactRollingFrictionConstraintPool, + m_data22->m_orderTmpConstraintPool, + m_data22->m_orderNonContactConstraintPool, + m_data22->m_orderFrictionConstraintPool, + m_data22->m_tmpConstraintSizesPool, + m_data22->m_resolveSingleConstraintRowGeneric, + m_data22->m_resolveSingleConstraintRowLowerLimit, + m_data22->m_resolveSplitPenetrationImpulse, + m_data22->m_kinematicBodyUniqueIdToSolverBodyTable, m_data22->m_btSeed2, + m_data22->m_fixedBodyId, m_data22->m_maxOverrideNumSolverIterations); + + m_data21->m_fixedBodyId = -1; + m_data22->m_fixedBodyId = -1; + + // set up + int halfNumConstraints1 = numConstraints / 2; + int halfNumConstraints2 = numConstraints - halfNumConstraints1; + + int halfNumManifolds1 = numConstraints / 2; + int halfNumManifolds2 = numManifolds - halfNumManifolds1; + + setupHelper(siData1, bodies, numBodies, info, constraints, + halfNumConstraints1, manifoldPtr, halfNumManifolds1); + + setupHelper(siData2, bodies, numBodies, info, + constraints + halfNumConstraints1, halfNumConstraints2, + manifoldPtr + halfNumManifolds1, halfNumManifolds2); + // set up complete + + // begin solve + btScalar leastSquaresResidual = 0; + { + BT_PROFILE("solveGroupCacheFriendlyIterations"); + /// this is a special step to resolve penetrations (just for contacts) + btSequentialImpulseConstraintSolver:: + solveGroupCacheFriendlySplitImpulseIterationsInternal( + siData1, bodies, numBodies, manifoldPtr, halfNumManifolds1, + constraints, halfNumConstraints1, info, debugDrawer); + + btSequentialImpulseConstraintSolver:: + solveGroupCacheFriendlySplitImpulseIterationsInternal( + siData2, bodies, numBodies, manifoldPtr + halfNumManifolds1, + halfNumManifolds2, constraints + halfNumConstraints1, + halfNumConstraints2, info, debugDrawer); + + int maxIterations = + siData1.m_maxOverrideNumSolverIterations > info.m_numIterations + ? siData1.m_maxOverrideNumSolverIterations + : info.m_numIterations; + + for (int iteration = 0; iteration < maxIterations; iteration++) + { + auto res1 = + btSequentialImpulseConstraintSolver::solveSingleIterationInternal( + siData1, iteration, constraints, halfNumConstraints1, info); + + auto res2 = + btSequentialImpulseConstraintSolver::solveSingleIterationInternal( + siData2, iteration, constraints + halfNumConstraints1, + halfNumConstraints2, info); + leastSquaresResidual = btMax(res1, res2); + + if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || + (iteration >= (maxIterations - 1))) + { +#ifdef VERBOSE_RESIDUAL_PRINTF + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, + iteration); +#endif + break; + } + } + } + + btScalar res = btSequentialImpulseConstraintSolver:: + solveGroupCacheFriendlyFinishInternal(siData1, bodies, numBodies, info); + +btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal( + siData2, bodies, numBodies, info); + + return res; +} + +void setupHelper(btSISolverSingleIterationData& siData, + btCollisionObject** bodies, int numBodies, + const btContactSolverInfo& info, + btTypedConstraint** constraintStart, int constrainNums, + btPersistentManifold** manifoldPtr, int numManifolds) +{ + btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies, + numBodies, info); + btSequentialImpulseConstraintSolver::convertJointsInternal( + siData, constraintStart, constrainNums, info); - btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies, numBodies, info); - btSequentialImpulseConstraintSolver::convertJointsInternal(siData, constraints, numConstraints, info); - int i; btPersistentManifold* manifold = 0; - // btCollisionObject* colObj0=0,*colObj1=0; for (i = 0; i < numManifolds; i++) { manifold = manifoldPtr[i]; - btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, info); + btSequentialImpulseConstraintSolver::convertContactInternal(siData, + manifold, info); + + int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size(); + int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size(); + int numFrictionPool = + siData.m_tmpSolverContactFrictionConstraintPool.size(); + + siData.m_orderNonContactConstraintPool.resizeNoInitialize( + numNonContactPool); + if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) + siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2); + else + siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool); + + siData.m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool); + { + int i; + for (i = 0; i < numNonContactPool; i++) + { + siData.m_orderNonContactConstraintPool[i] = i; + } + for (i = 0; i < numConstraintPool; i++) + { + siData.m_orderTmpConstraintPool[i] = i; + } + for (i = 0; i < numFrictionPool; i++) + { + siData.m_orderFrictionConstraintPool[i] = i; + } + } } +} +btScalar btBlockSolver::solveGroup(btCollisionObject** bodies, int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher) +{ + // if (m_childSolvers.size()) + // hard code to use block solver for now + return solveGroupInternalBlock(bodies, numBodies, manifoldPtr, numManifolds, + constraints, numConstraints, info, debugDrawer, + dispatcher); + // else + // return solveGroupInternal(bodies, numBodies, manifoldPtr, numManifolds, + // constraints, numConstraints, info, debugDrawer, + // dispatcher); +} +btScalar btBlockSolver::solveGroupInternal( + btCollisionObject** bodies, int numBodies, + btPersistentManifold** manifoldPtr, int numManifolds, + btTypedConstraint** constraints, int numConstraints, + const btContactSolverInfo& info, btIDebugDraw* debugDrawer, + btDispatcher* dispatcher) +{ + btSISolverSingleIterationData siData( + m_data21->m_tmpSolverBodyPool, m_data21->m_tmpSolverContactConstraintPool, + m_data21->m_tmpSolverNonContactConstraintPool, + m_data21->m_tmpSolverContactFrictionConstraintPool, + m_data21->m_tmpSolverContactRollingFrictionConstraintPool, + m_data21->m_orderTmpConstraintPool, + m_data21->m_orderNonContactConstraintPool, + m_data21->m_orderFrictionConstraintPool, + m_data21->m_tmpConstraintSizesPool, + m_data21->m_resolveSingleConstraintRowGeneric, + m_data21->m_resolveSingleConstraintRowLowerLimit, + m_data21->m_resolveSplitPenetrationImpulse, + m_data21->m_kinematicBodyUniqueIdToSolverBodyTable, m_data21->m_btSeed2, + m_data21->m_fixedBodyId, m_data21->m_maxOverrideNumSolverIterations); + + m_data21->m_fixedBodyId = -1; + // todo: setup sse2/4 constraint row methods + + btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies, + numBodies, info); + btSequentialImpulseConstraintSolver::convertJointsInternal( + siData, constraints, numConstraints, info); + + int i; + btPersistentManifold* manifold = 0; + // btCollisionObject* colObj0=0,*colObj1=0; + + for (i = 0; i < numManifolds; i++) + { + manifold = manifoldPtr[i]; + btSequentialImpulseConstraintSolver::convertContactInternal(siData, + manifold, info); + } int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size(); int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size(); int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size(); - ///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints + // @todo: use stack allocator for such temporarily memory, same for solver + // bodies/constraints siData.m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool); if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2); @@ -118,43 +315,60 @@ btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, b btScalar leastSquaresResidual = 0; - - { BT_PROFILE("solveGroupCacheFriendlyIterations"); - ///this is a special step to resolve penetrations (just for contacts) - btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, info, debugDrawer); + /// this is a special step to resolve penetrations (just for contacts) + btSequentialImpulseConstraintSolver:: + solveGroupCacheFriendlySplitImpulseIterationsInternal( + siData, bodies, numBodies, manifoldPtr, numManifolds, constraints, + numConstraints, info, debugDrawer); - int maxIterations = siData.m_maxOverrideNumSolverIterations > info.m_numIterations ? siData.m_maxOverrideNumSolverIterations : info.m_numIterations; + int maxIterations = + siData.m_maxOverrideNumSolverIterations > info.m_numIterations + ? siData.m_maxOverrideNumSolverIterations + : info.m_numIterations; for (int iteration = 0; iteration < maxIterations; iteration++) { - leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, info); + leastSquaresResidual = + btSequentialImpulseConstraintSolver::solveSingleIterationInternal( + siData, iteration, constraints, numConstraints, info); - if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1))) + if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || + (iteration >= (maxIterations - 1))) { #ifdef VERBOSE_RESIDUAL_PRINTF - printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration); + printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, + iteration); #endif break; } } } - btScalar res = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info); + btScalar res = btSequentialImpulseConstraintSolver:: + solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info); return res; } - - -void btBlockSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher) +void btBlockSolver::solveMultiBodyGroup( + btCollisionObject** bodies, int numBodies, btPersistentManifold** manifold, + int numManifolds, btTypedConstraint** constraints, int numConstraints, + btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, + const btContactSolverInfo& info, btIDebugDraw* debugDrawer, + btDispatcher* dispatcher) { - btMultiBodyConstraintSolver::solveMultiBodyGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, dispatcher); + btMultiBodyConstraintSolver::solveMultiBodyGroup( + bodies, numBodies, manifold, numManifolds, constraints, numConstraints, + multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, + dispatcher); } void btBlockSolver::reset() { - //or just set m_data2->m_btSeed2=0? - delete m_data2; - m_data2 = new btBlockSolverInternalData; + // or just set m_data2->m_btSeed2=0? + delete m_data21; + delete m_data22; + m_data21 = new btBlockSolverInternalData; + m_data22 = new btBlockSolverInternalData; } diff --git a/examples/BlockSolver/btBlockSolver.h b/examples/BlockSolver/btBlockSolver.h index 7cb5dd3c2..271ad399d 100644 --- a/examples/BlockSolver/btBlockSolver.h +++ b/examples/BlockSolver/btBlockSolver.h @@ -1,24 +1,72 @@ #ifndef BT_BLOCK_SOLVER_H #define BT_BLOCK_SOLVER_H +#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h" +#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" +#include "Bullet3Common/b3Logging.h" + +enum BlockSolverOptions +{ + BLOCK_SOLVER_SI = 1 << 0, + BLOCK_SOLVER_MLCP_PGS = 1 << 1, + BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2, + BLOCK_SOLVER_BLOCK = 1 << 3, + + BLOCK_SOLVER_SCENE_MB_STACK = 1 << 5, + BLOCK_SOLVER_SCENE_CHAIN = 1 << 6, +}; class btBlockSolver : public btMultiBodyConstraintSolver { - struct btBlockSolverInternalData* m_data2; + struct btBlockSolverInternalData* m_data21; + struct btBlockSolverInternalData* m_data22; + public + : btBlockSolver(); -public: - btBlockSolver(); - virtual ~btBlockSolver(); + virtual ~btBlockSolver(); - //btRigidBody - virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + // btRigidBody + virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + class btIDebugDraw* debugDrawer, + btDispatcher* dispatcher); - //btMultibody - virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher); + btScalar solveGroupInternal(btCollisionObject** bodies, int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher); - ///clear internal cached data and reset random seed - virtual void reset(); + btScalar solveGroupInternalBlock(btCollisionObject** bodies, int numBodies, + btPersistentManifold** manifoldPtr, + int numManifolds, + btTypedConstraint** constraints, + int numConstraints, + const btContactSolverInfo& info, + btIDebugDraw* debugDrawer, + btDispatcher* dispatcher); + + // btMultibody + virtual void solveMultiBodyGroup( + btCollisionObject** bodies, int numBodies, + btPersistentManifold** manifold, int numManifolds, + btTypedConstraint** constraints, int numConstraints, + btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, + const btContactSolverInfo& info, btIDebugDraw* debugDrawer, + btDispatcher* dispatcher); + + /// clear internal cached data and reset random seed + virtual void reset(); virtual btConstraintSolverType getSolverType() const { @@ -26,4 +74,4 @@ public: } }; -#endif //BT_BLOCK_SOLVER_H +#endif //BT_BLOCK_SOLVER_H diff --git a/examples/BulletRobotics/FixJointBoxes.cpp b/examples/BulletRobotics/FixJointBoxes.cpp index 410b1e017..6e33cbfc9 100644 --- a/examples/BulletRobotics/FixJointBoxes.cpp +++ b/examples/BulletRobotics/FixJointBoxes.cpp @@ -1,4 +1,3 @@ - #include "FixJointBoxes.h" #include "../CommonInterfaces/CommonGraphicsAppInterface.h" @@ -61,7 +60,11 @@ public: b3RobotSimulatorLoadUrdfFileArgs args; b3RobotSimulatorChangeDynamicsArgs dynamicsArgs; - b3RobotJointInfo jointInfo; + for (int i = 0; i < numCubes; i++) + { + args.m_forceOverrideFixedBase = (i == 0); + args.m_startPosition.setValue(0, i * 0.05, 1); + cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args); b3RobotJointInfo jointInfo; @@ -71,7 +74,7 @@ public: if (i > 0) { m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo); - m_robotSim.setCollisionFilterGroupMask(cubeIds[i], -1, 0, 0); + m_robotSim.setCollisionFilterGroupMask(cubeIds[i], -1, 0, 0); } m_robotSim.loadURDF("plane.urdf"); @@ -107,8 +110,8 @@ public: { for (int i = 0; i < numCubes; i++) { - btVector3 pos (0, i * (btScalar)0.05, 1); - btQuaternion quar (0, 0, 0, 1); + btVector3 pos(0, i * (btScalar)0.05, 1); + btQuaternion quar(0, 0, 0, 1); m_robotSim.resetBasePositionAndOrientation(cubeIds[i], pos, quar); } } @@ -154,11 +157,11 @@ public: virtual void resetCamera() { - float dist = 1; - float pitch = -20; - float yaw = -30; - float targetPos[3] = {0, 0.2, 0.5}; - m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + float dist = 1; + float pitch = -20; + float yaw = -30; + float targetPos[3] = {0, 0.2, 0.5}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } }; diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index a0dcfe57e..ceb6d9590 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -211,6 +211,8 @@ SET(BulletExampleBrowser_SRCS ../BlockSolver/btBlockSolver.h ../BlockSolver/BlockSolverExample.cpp ../BlockSolver/BlockSolverExample.h + ../BlockSolver/RigidBodyBoxes.cpp + ../BlockSolver/RigidBodyBoxes.h ../Tutorial/Tutorial.cpp ../Tutorial/Tutorial.h ../Tutorial/Dof6ConstraintTutorial.cpp diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index cfc22e05d..6443df29d 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -132,14 +132,13 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans { btConvexShape* convex = (btConvexShape*)collisionShape; { - const btConvexPolyhedron* pol = 0; if (convex->isPolyhedral()) { btPolyhedralConvexShape* poly = (btPolyhedralConvexShape*)convex; pol = poly->getConvexPolyhedron(); } - + if (pol) { for (int v = 0; v < pol->m_vertices.size(); v++) @@ -151,19 +150,16 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans } for (int f = 0; f < pol->m_faces.size(); f++) { - for (int ii = 2; ii < pol->m_faces[f].m_indices.size(); ii++) { indicesOut.push_back(pol->m_faces[f].m_indices[0]); - indicesOut.push_back(pol->m_faces[f].m_indices[ii-1]); + indicesOut.push_back(pol->m_faces[f].m_indices[ii - 1]); indicesOut.push_back(pol->m_faces[f].m_indices[ii]); } } - - } + } else { - btShapeHull* hull = new btShapeHull(convex); hull->buildHull(0.0, 1); diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 15080e4dc..713e1d271 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -1,6 +1,8 @@ #include "ExampleEntries.h" +#include "../BlockSolver/btBlockSolver.h" #include "../BlockSolver/BlockSolverExample.h" +#include "../BlockSolver/RigidBodyBoxes.h" #include "LinearMath/btAlignedObjectArray.h" #include "EmptyExample.h" #include "../BulletRobotics/BoxStack.h" @@ -134,7 +136,6 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", RigidBodySoftContactCreateFunc), - ExampleEntry(0, "Bullet Robotics"), ExampleEntry(1, "Box Stack", "Create a stack of boxes of large mass ratio.", BoxStackExampleCreateFunc), ExampleEntry(1, "Joint Limit", "Create three objects joint together", JointLimitCreateFunc), @@ -149,8 +150,7 @@ static ExampleEntry gDefaultExamples[] = ExampleEntry(1, "Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), ExampleEntry(1, "Inverted Pendulum PD", "Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), ExampleEntry(1, "MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", MultiBodySoftContactCreateFunc, 0), - - + ExampleEntry(0, "Physics Client-Server"), ExampleEntry(1, "Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.", PhysicsServerCreateFuncBullet2), @@ -163,12 +163,13 @@ static ExampleEntry gDefaultExamples[] = // // ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT), - ExampleEntry(0, "BlockSolver"), - ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK+ BLOCK_SOLVER_SI), + ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_SI), ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS), ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG), ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK), + ExampleEntry(1, "Stack RigidBody SI", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_SI), + ExampleEntry(1, "Stack RigidBody Block", "Create a stack of blocks, with heavy block at the top", RigidBodyBoxesCreateFunc, BLOCK_SOLVER_BLOCK), ExampleEntry(0, "Inverse Dynamics"), ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF), diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index d5c9ccac5..c7a2e8a69 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -1127,7 +1127,6 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[]) gui2->registerFileOpenCallback(fileOpenCallback); gui2->registerQuitCallback(quitCallback); - } return true; From c44c7f2891bf23733cd10db5c913df807884d96c Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 25 Mar 2019 14:55:02 -0700 Subject: [PATCH 09/13] add files --- examples/BlockSolver/RigidBodyBoxes.cpp | 150 ++++++++++++++++++++++++ examples/BlockSolver/RigidBodyBoxes.h | 6 + 2 files changed, 156 insertions(+) create mode 100644 examples/BlockSolver/RigidBodyBoxes.cpp create mode 100644 examples/BlockSolver/RigidBodyBoxes.h diff --git a/examples/BlockSolver/RigidBodyBoxes.cpp b/examples/BlockSolver/RigidBodyBoxes.cpp new file mode 100644 index 000000000..2b3190df2 --- /dev/null +++ b/examples/BlockSolver/RigidBodyBoxes.cpp @@ -0,0 +1,150 @@ +#include "RigidBodyBoxes.h" +#include "../CommonInterfaces/CommonParameterInterface.h" +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "BlockSolverExample.h" +#include "btBlockSolver.h" + +class RigidBodyBoxes : public CommonRigidBodyBase +{ + int m_option; + int m_numIterations; + int m_numBoxes = 4; + btAlignedObjectArray boxes; + static btScalar numSolverIterations; + +public: + RigidBodyBoxes(GUIHelperInterface* helper, int option); + virtual ~RigidBodyBoxes(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + void resetCubePosition(); + virtual void resetCamera() + { + float dist = 3; + float pitch = -35; + float yaw = 50; + float targetPos[3] = {0, 0, .1}; + m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], + targetPos[2]); + } + + void createRigidBodyStack(); +}; + +btScalar RigidBodyBoxes::numSolverIterations = 50; + +RigidBodyBoxes::RigidBodyBoxes(GUIHelperInterface* helper, int option) + : CommonRigidBodyBase(helper), + m_option(option), + m_numIterations(numSolverIterations) +{ + m_guiHelper->setUpAxis(2); +} + +RigidBodyBoxes::~RigidBodyBoxes() +{ + // Do nothing +} + +void RigidBodyBoxes::createRigidBodyStack() +{ + // create ground + btBoxShape* groundShape = + createBoxShape(btVector3(btScalar(5.), btScalar(5.), btScalar(5.))); + m_collisionShapes.push_back(groundShape); + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0, 0, -5)); + btScalar mass(0.); + btRigidBody* body = createRigidBody(mass, groundTransform, groundShape, + btVector4(0, 0, 1, 1)); + + // create a few boxes + mass = 1; + for (int i = 0; i < m_numBoxes; i++) + { + btBoxShape* boxShape = + createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1))); + m_collisionShapes.push_back(boxShape); + mass *= 4; + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2)); + boxes.push_back(createRigidBody(mass, tr, boxShape)); + } +} + +void RigidBodyBoxes::initPhysics() +{ + /// collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + + /// use the default collision dispatcher. For parallel processing you can use + /// a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + m_broadphase = new btDbvtBroadphase(); + + { + SliderParams slider("numSolverIterations", &numSolverIterations); + slider.m_minVal = 5; + slider.m_maxVal = 500; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + if (m_option & BLOCK_SOLVER_SI) + { + m_solver = new btSequentialImpulseConstraintSolver; + b3Printf("Constraint Solver: Sequential Impulse"); + } + if (m_option & BLOCK_SOLVER_BLOCK) + { + m_solver = new btBlockSolver(); + b3Printf("Constraint Solver: Block solver"); + } + + btAssert(m_solver); + + m_dynamicsWorld = new btDiscreteDynamicsWorld( + m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); + + createRigidBodyStack(); + + m_dynamicsWorld->getSolverInfo().m_numIterations = numSolverIterations; + m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6); + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + +void RigidBodyBoxes::resetCubePosition() +{ + for (int i = 0; i < m_numBoxes; i++) + { + btTransform tr; + tr.setIdentity(); + tr.setOrigin(btVector3(0, 0, 0.1 + i * 0.2)); + boxes[i]->setWorldTransform(tr); + } +} + +void RigidBodyBoxes::stepSimulation(float deltaTime) +{ + if ((int)numSolverIterations != m_numIterations) + { + resetCubePosition(); + m_numIterations = (int)numSolverIterations; + m_dynamicsWorld->getSolverInfo().m_numIterations = m_numIterations; + b3Printf("New num iterations; %d", m_numIterations); + } + + m_dynamicsWorld->stepSimulation(deltaTime); +} + +CommonExampleInterface* RigidBodyBoxesCreateFunc( + CommonExampleOptions& options) +{ + return new RigidBodyBoxes(options.m_guiHelper, options.m_option); +} diff --git a/examples/BlockSolver/RigidBodyBoxes.h b/examples/BlockSolver/RigidBodyBoxes.h new file mode 100644 index 000000000..0b6e651d6 --- /dev/null +++ b/examples/BlockSolver/RigidBodyBoxes.h @@ -0,0 +1,6 @@ +#ifndef BLOCKSOLVER_RIGIDBODYBOXES_H_ +#define BLOCKSOLVER_RIGIDBODYBOXES_H_ + +class CommonExampleInterface* RigidBodyBoxesCreateFunc(struct CommonExampleOptions& options); + +#endif //BLOCKSOLVER_RIGIDBODYBOXES_H_ From 7223e51dc44c976d498d25644899ed94373e3932 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Mon, 25 Mar 2019 17:32:53 -0700 Subject: [PATCH 10/13] fix compile --- examples/BlockSolver/btBlockSolver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/BlockSolver/btBlockSolver.cpp b/examples/BlockSolver/btBlockSolver.cpp index fa7b0116b..8cf144c14 100644 --- a/examples/BlockSolver/btBlockSolver.cpp +++ b/examples/BlockSolver/btBlockSolver.cpp @@ -142,11 +142,11 @@ btScalar btBlockSolver::solveGroupInternalBlock( for (int iteration = 0; iteration < maxIterations; iteration++) { - auto res1 = + btScalar res1 = btSequentialImpulseConstraintSolver::solveSingleIterationInternal( siData1, iteration, constraints, halfNumConstraints1, info); - auto res2 = + btScalar res2 = btSequentialImpulseConstraintSolver::solveSingleIterationInternal( siData2, iteration, constraints + halfNumConstraints1, halfNumConstraints2, info); From 8a96dc67cc43fc17e54c75880a3a7a8191d7a1e8 Mon Sep 17 00:00:00 2001 From: mbennice Date: Tue, 26 Mar 2019 14:39:01 -0700 Subject: [PATCH 11/13] Initializes simulation timestamp to 0 explicitly. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0778d57df..f0a05d05e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1699,6 +1699,7 @@ struct PhysicsServerCommandProcessorInternalData m_logPlaybackUid(-1), m_physicsDeltaTime(1. / 240.), m_numSimulationSubSteps(0), + m_simulationTimestamp(0); m_userConstraintUIDGenerator(1), m_broadphaseCollisionFilterCallback(0), m_pairCache(0), From fed1a878d7e7b8a046d24d8a505e10694f73e913 Mon Sep 17 00:00:00 2001 From: mbennice Date: Tue, 26 Mar 2019 18:14:07 -0700 Subject: [PATCH 12/13] Replaces ; with , --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f0a05d05e..8cb562691 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1699,7 +1699,7 @@ struct PhysicsServerCommandProcessorInternalData m_logPlaybackUid(-1), m_physicsDeltaTime(1. / 240.), m_numSimulationSubSteps(0), - m_simulationTimestamp(0); + m_simulationTimestamp(0), m_userConstraintUIDGenerator(1), m_broadphaseCollisionFilterCallback(0), m_pairCache(0), From 231195843f25a728f8c9acc663453e909f995f23 Mon Sep 17 00:00:00 2001 From: Stephen Gold Date: Tue, 2 Apr 2019 21:51:58 -0700 Subject: [PATCH 13/13] in btSimpleBroadphase::destroyProxy(), remove pairs before freeing proxy --- .../BroadphaseCollision/btSimpleBroadphase.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp index 166cf771f..b7fe0a1f3 100644 --- a/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp +++ b/src/BulletCollision/BroadphaseCollision/btSimpleBroadphase.cpp @@ -123,11 +123,11 @@ protected: void btSimpleBroadphase::destroyProxy(btBroadphaseProxy* proxyOrg, btDispatcher* dispatcher) { + m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher); + btSimpleBroadphaseProxy* proxy0 = static_cast(proxyOrg); freeHandle(proxy0); - m_pairCache->removeOverlappingPairsContainingProxy(proxyOrg, dispatcher); - //validate(); }