From a1f15ae01a6b885a0ad5ee40e42e462ead2ec373 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 7 Mar 2019 21:13:00 -0800 Subject: 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 + .../SharedMemory/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]; -- cgit v1.2.1