Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1176 from erwincoumans/master

Browse Source 
mimic joint, enable btGearConstraint for multibody, expose contact stiffness/damping, erp to pybullet etc.
This commit is contained in:
erwincoumans
2017-06-08 10:20:22 -07:00
committed by GitHub
parent 877e2cf9f9 c1f6b7e9b8
commit e5ce067cb9
16 changed files with 582 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
View File

@@ -36,7 +36,7 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
//int textureIndex = -1;
//try to load some texture
for (int i=0;i<shapes.size();i++)
for (int i=0; meshData.m_textureImage==0 && i<shapes.size();i++)
{
const tinyobj::shape_t& shape = shapes[i];
if (shape.material.diffuse_texname.length()>0)
@@ -64,8 +64,10 @@ bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string&
meshData.m_textureHeight = height;
} else
{
b3Warning("Unsupported texture image format [%s]\n",relativeFileName);
meshData.m_textureWidth = 0;
meshData.m_textureHeight = 0;
break;
}
} else

8
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@@ -1243,6 +1243,14 @@ btCollisionShape* BulletURDFImporter::getAllocatedCollisionShape(int index)
const UrdfCollision& col = link->m_collisionArray[v];
btCollisionShape* childShape = convertURDFToCollisionShape(&col ,pathPrefix);
m_data->m_allocatedCollisionShapes.push_back(childShape);
if (childShape->getShapeType()==COMPOUND_SHAPE_PROXYTYPE)
{
btCompoundShape* compound = (btCompoundShape*) childShape;
for (int i=0;i<compound->getNumChildShapes();i++)
{
m_data->m_allocatedCollisionShapes.push_back(compound->getChildShape(i));
}
}
if (childShape)
{

56
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -452,10 +452,24 @@ int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle
command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP;
return 0;
}
int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP;
command->m_physSimParamArgs.m_defaultNonContactERP = defaultNonContactERP;
return 0;
}
int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP;
command->m_physSimParamArgs.m_frictionERP = frictionERP;
return 0;
}
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1496,6 +1510,8 @@ b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physC
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CHANGE_DYNAMICS_INFO;
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = -1;
command->m_changeDynamicsInfoArgs.m_linkIndex = -2;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
@@ -1580,6 +1596,31 @@ int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHan
return 0;
}
int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping)
{
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_contactStiffness =contactStiffness;
command->m_changeDynamicsInfoArgs.m_contactDamping = contactDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING;
return 0;
}
int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor)
{
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_frictionAnchor = frictionAnchor;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR;
return 0;
}
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -1663,7 +1704,18 @@ int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHan
return 0;
}
int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_RATIO;
command->m_userConstraintArguments.m_gearRatio = gearRatio;
return 0;
}
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{

10
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -89,7 +89,8 @@ int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHa
int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution);
int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping);
int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping);
int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping);
int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
@@ -101,7 +102,9 @@ b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHa
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
@@ -219,6 +222,9 @@ b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle phys
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP);
int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);

158
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -10,6 +10,10 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
@@ -4345,6 +4349,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
{
mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
mb->getBaseCollider()->setFriction(lateralFriction);
@@ -4356,7 +4364,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
{
mb->getBaseCollider()->setRollingFriction(rollingFriction);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
{
if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
{
mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
} else
{
mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
}
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
@@ -4388,10 +4407,27 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
{
if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
{
mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
} else
{
mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
{
mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
{
mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
}
}
@@ -4422,6 +4458,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
body->m_rigidBody->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);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
{
body->m_rigidBody->setRestitution(restitution);
@@ -4439,6 +4479,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
body->m_rigidBody->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);
} else
{
body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
{
btVector3 localInertia;
@@ -4555,6 +4606,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP)
{
m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP)
{
m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP;
}
if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
{
m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold;
@@ -5700,7 +5763,31 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
if (clientCmd.m_userConstraintArguments.m_jointType == eGearType)
{
if (childBody && childBody->m_multiBody)
{
if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=-1) && (clientCmd.m_userConstraintArguments.m_childJointIndex <childBody->m_multiBody->getNumLinks()))
{
btMultiBodyGearConstraint* multibodyGear = new btMultiBodyGearConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
multibodyGear->setMaxAppliedImpulse(defaultMaxForce);
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyGear);
InteralUserConstraintData userConstraintData;
userConstraintData.m_mbConstraint = multibodyGear;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
}
else if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
{
if (childBody && childBody->m_multiBody)
{
@@ -5818,7 +5905,48 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
else
{
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
if (parentBody && childBody)
{
if (parentBody->m_rigidBody)
{
if (clientCmd.m_userConstraintArguments.m_jointType == eGearType)
{
btRigidBody* childRb = childBody->m_rigidBody;
if (childRb)
{
btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
//for now we use the same local axis for both objects
btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
btScalar ratio=1;
btGearConstraint* gear = new btGearConstraint(*parentBody->m_rigidBody,*childRb, axisA,axisB,ratio);
m_data->m_dynamicsWorld->addConstraint(gear,true);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = gear;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
}
}
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
@@ -5859,10 +5987,30 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce;
userConstraintPtr->m_mbConstraint->setMaxAppliedImpulse(maxImp);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
}
}
if (userConstraintPtr->m_rbConstraint)
{
//todo
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
{
btScalar maxImp = clientCmd.m_userConstraintArguments.m_maxAppliedForce*m_data->m_physicsDeltaTime;
userConstraintPtr->m_userConstraintData.m_maxAppliedForce = clientCmd.m_userConstraintArguments.m_maxAppliedForce;
//userConstraintPtr->m_rbConstraint->setMaxAppliedImpulse(maxImp);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE)
{
btGearConstraint* gear = (btGearConstraint*) userConstraintPtr->m_rbConstraint;
gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
}
}
}
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange;
@@ -5885,7 +6033,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (userConstraintPtr->m_rbConstraint)
{
m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint);
delete userConstraintPtr->m_rbConstraint;
m_data->m_userConstraints.remove(userConstraintUidRemove);
}
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove;
serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;

5
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -723,6 +723,11 @@ public:
virtual ~MultiThreadedOpenGLGuiHelper()
{
//delete m_childGuiHelper;
if (m_debugDraw)
{
delete m_debugDraw;
m_debugDraw = 0;
}
}
void setCriticalSection(b3CriticalSection* cs)

14
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -119,6 +119,9 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_RESTITUTION=32,
CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING=64,
CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING=128,
CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING=256,
CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR = 512,
};
struct ChangeDynamicsInfoArgs
@@ -133,6 +136,9 @@ struct ChangeDynamicsInfoArgs
double m_restitution;
double m_linearDamping;
double m_angularDamping;
double m_contactStiffness;
double m_contactDamping;
int m_frictionAnchor;
};
struct GetDynamicsInfoArgs
@@ -367,8 +373,8 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_MAX_CMD_PER_1MS = 2048,
SIM_PARAM_ENABLE_FILE_CACHING = 4096,
SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192,
SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384,
SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
};
enum EnumLoadBunnyUpdateFlags
@@ -402,6 +408,8 @@ struct SendPhysicsSimulationParameters
int m_collisionFilterMode;
int m_enableFileCaching;
double m_restitutionVelocityThreshold;
double m_defaultNonContactERP;
double m_frictionERP;
};
struct LoadBunnyArgs
@@ -619,7 +627,7 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16,
USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
USER_CONSTRAINT_REQUEST_INFO=64,
USER_CONSTRAINT_CHANGE_GEAR_RATIO=128,
};
enum EnumBodyChangeFlags

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -184,6 +184,7 @@ enum JointType {
ePlanarType = 3,
eFixedType = 4,
ePoint2PointType = 5,
eGearType=6
};
@@ -227,6 +228,8 @@ struct b3UserConstraint
int m_jointType;
double m_maxAppliedForce;
int m_userConstraintUniqueId;
double m_gearRatio;
};
struct b3BodyInfo

19
examples/pybullet/examples/mimicJointConstraint.py Normal file
View File

@@ -0,0 +1,19 @@
#a mimic joint can act as a gear between two joints
#you can control the gear ratio in magnitude and sign (>0 reverses direction)
import pybullet as p
import time
p.connect(p.GUI)
wheelA = p.loadURDF("wheel.urdf",[0,0,0])
wheelB = p.loadURDF("wheel.urdf",[0,0,1])
p.setJointMotorControl2(wheelA,0,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
p.setJointMotorControl2(wheelB,0,p.VELOCITY_CONTROL,targetVelocity=1,force=1)
c = p.createConstraint(wheelA,0,wheelB,0,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-0.1, maxForce=10000)
p.setRealTimeSimulation(1)
while(1):
time.sleep(0.01)
#p.removeConstraint(c)

49
examples/pybullet/pybullet.c
View File

@@ -625,12 +625,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
double restitution = -1;
double linearDamping = -1;
double angularDamping = -1;
double contactStiffness = -1;
double contactDamping = -1;
int frictionAnchor = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddii", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &physicsClientId))
{
return NULL;
}
@@ -671,12 +674,19 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
{
b3ChangeDynamicsInfoSetAngularDamping(command,bodyUniqueId,angularDamping);
}
if (restitution>=0)
{
b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, restitution);
}
if (contactStiffness>=0 && contactDamping >=0)
{
b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command,bodyUniqueId,linkIndex,contactStiffness, contactDamping);
}
if (frictionAnchor>=0)
{
b3ChangeDynamicsInfoSetFrictionAnchor(command,bodyUniqueId,linkIndex, frictionAnchor);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
@@ -753,13 +763,16 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
int maxNumCmdPer1ms = -2;
int enableFileCaching = -1;
double restitutionVelocityThreshold=-1;
double erp;
double contactERP = -1;
double frictionERP = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "physicsClientId", NULL};
static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiidi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps,
&collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &physicsClientId))
{
return NULL;
}
@@ -819,6 +832,18 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
b3PhysicsParamSetEnableFileCaching(command, enableFileCaching);
}
if (erp>=0)
{
b3PhysicsParamSetDefaultNonContactERP(command,erp);
}
if (contactERP>=0)
{
b3PhysicsParamSetDefaultContactERP(command,contactERP);
}
if (frictionERP >=0)
{
b3PhysicsParamSetDefaultFrictionERP(command,frictionERP);
}
//ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
@@ -4583,7 +4608,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds)
{
static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "physicsClientId", NULL};
static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL};
int userConstraintUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -4595,8 +4620,8 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
double jointChildPivot[3];
double jointChildFrameOrn[4];
double maxForce = -1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &physicsClientId))
double gearRatio = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId))
{
return NULL;
}
@@ -4622,7 +4647,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
{
b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce);
}
if (gearRatio!=0)
{
b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
Py_INCREF(Py_None);
@@ -6744,6 +6772,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "JOINT_PLANAR", ePlanarType); // user read
PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read
PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read
PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType); // user read
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read

2
examples/pybullet/tensorflow/humanoid_running.py
View File

@@ -188,7 +188,7 @@ def demo_run():
distance=5
yaw = 0
humanPos, humanOrn = p.getBasePositionAndOrientation(human)
p.resetDebugVisualizerCamera(distance,yaw,20,humanPos);
p.resetDebugVisualizerCamera(distance,yaw,-20,humanPos);
frame += 1
if frame==1000: break

3
setup.py
View File

@@ -253,6 +253,7 @@ sources = ["examples/pybullet/pybullet.c"]\
+["src/BulletDynamics/Featherstone/btMultiBody.cpp"]\
+["src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp"]\
+["src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp"]\
+["src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"]\
+["src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"]\
+["src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"]\
+["src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"]\
@@ -418,7 +419,7 @@ else:
setup(
name = 'pybullet',
version='1.1.2',
version='1.1.3',
description='Official Python Interface for the Bullet Physics SDK Robotics Simulator',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine 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',

2
src/BulletDynamics/CMakeLists.txt
View File

@@ -37,6 +37,7 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyFixedConstraint.cpp
Featherstone/btMultiBodySliderConstraint.cpp
Featherstone/btMultiBodyJointMotor.cpp
Featherstone/btMultiBodyGearConstraint.cpp
MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp
MLCPSolvers/btLemkeAlgorithm.cpp
@@ -98,6 +99,7 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodyFixedConstraint.h
Featherstone/btMultiBodySliderConstraint.h
Featherstone/btMultiBodyJointMotor.h
Featherstone/btMultiBodyGearConstraint.h
)
SET(MLCPSolvers_HDRS

1
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -183,6 +183,7 @@ public:
virtual void debugDraw(class btIDebugDraw* drawer)=0;
virtual void setGearRatio(btScalar ratio) {}
};
#endif //BT_MULTIBODY_CONSTRAINT_H

169
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp Normal file
View File

@@ -0,0 +1,169 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#include "btMultiBodyGearConstraint.h"
#include "btMultiBody.h"
#include "btMultiBodyLinkCollider.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyGearConstraint::btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB)
:btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,1,false),
m_gearRatio(1)
{
}
void btMultiBodyGearConstraint::finalizeMultiDof()
{
allocateJacobiansMultiDof();
m_numDofsFinalized = m_jacSizeBoth;
}
btMultiBodyGearConstraint::~btMultiBodyGearConstraint()
{
}
int btMultiBodyGearConstraint::getIslandIdA() const
{
if (m_bodyA)
{
btMultiBodyLinkCollider* col = m_bodyA->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyA->getNumLinks();i++)
{
if (m_bodyA->getLink(i).m_collider)
return m_bodyA->getLink(i).m_collider->getIslandTag();
}
}
return -1;
}
int btMultiBodyGearConstraint::getIslandIdB() const
{
if (m_bodyB)
{
btMultiBodyLinkCollider* col = m_bodyB->getBaseCollider();
if (col)
return col->getIslandTag();
for (int i=0;i<m_bodyB->getNumLinks();i++)
{
col = m_bodyB->getLink(i).m_collider;
if (col)
return col->getIslandTag();
}
}
return -1;
}
void btMultiBodyGearConstraint::createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
}
//don't crash
if (m_numDofsFinalized != m_jacSizeBoth)
return;
if (m_maxAppliedImpulse==0.f)
return;
// note: we rely on the fact that data.m_jacobians are
// always initialized to zero by the Constraint ctor
int linkDoF = 0;
unsigned int offsetA = 6 + (m_bodyA->getLink(m_linkA).m_dofOffset + linkDoF);
unsigned int offsetB = 6 + (m_bodyB->getLink(m_linkB).m_dofOffset + linkDoF);
// row 0: the lower bound
jacobianA(0)[offsetA] = 1;
jacobianB(0)[offsetB] = m_gearRatio;
const btScalar posError = 0;
const btVector3 dummy(0, 0, 0);
btScalar erp = infoGlobal.m_erp;
btScalar kp = 1;
btScalar kd = 1;
int numRows = getNumRows();
for (int row=0;row<numRows;row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
//btScalar positionStabiliationTerm = erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
//btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar desiredRelativeVelocity = 0;
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,false,1,false,desiredRelativeVelocity);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}

103
src/BulletDynamics/Featherstone/btMultiBodyGearConstraint.h Normal file
View File

@@ -0,0 +1,103 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
///This file was written by Erwin Coumans
#ifndef BT_MULTIBODY_GEAR_CONSTRAINT_H
#define BT_MULTIBODY_GEAR_CONSTRAINT_H
#include "btMultiBodyConstraint.h"
class btMultiBodyGearConstraint : public btMultiBodyConstraint
{
protected:
btRigidBody* m_rigidBodyA;
btRigidBody* m_rigidBodyB;
btVector3 m_pivotInA;
btVector3 m_pivotInB;
btMatrix3x3 m_frameInA;
btMatrix3x3 m_frameInB;
btScalar m_gearRatio;
public:
btMultiBodyGearConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
btMultiBodyGearConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB);
virtual ~btMultiBodyGearConstraint();
virtual void finalizeMultiDof();
virtual int getIslandIdA() const;
virtual int getIslandIdB() const;
virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
const btVector3& getPivotInA() const
{
return m_pivotInA;
}
void setPivotInA(const btVector3& pivotInA)
{
m_pivotInA = pivotInA;
}
const btVector3& getPivotInB() const
{
return m_pivotInB;
}
virtual void setPivotInB(const btVector3& pivotInB)
{
m_pivotInB = pivotInB;
}
const btMatrix3x3& getFrameInA() const
{
return m_frameInA;
}
void setFrameInA(const btMatrix3x3& frameInA)
{
m_frameInA = frameInA;
}
const btMatrix3x3& getFrameInB() const
{
return m_frameInB;
}
virtual void setFrameInB(const btMatrix3x3& frameInB)
{
m_frameInB = frameInB;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)
}
virtual void setGearRatio(btScalar gearRatio)
{
m_gearRatio = gearRatio;
}
};
#endif //BT_MULTIBODY_GEAR_CONSTRAINT_H