From 0b41fe1a491097cfd9ce102679108f64a7e76b38 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 7 Mar 2019 17:55:45 -0800 Subject: [PATCH 1/7] 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 2/7] 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 3/7] 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 4/7] 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 5/7] 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 6/7] 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 7/7] 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 @@ - +