diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.cpp b/examples/ExampleBrowser/OpenGLGuiHelper.cpp index e2824a6ea..5ab2e547f 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.cpp +++ b/examples/ExampleBrowser/OpenGLGuiHelper.cpp @@ -1214,7 +1214,7 @@ void OpenGLGuiHelper::autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWor btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i]; sortedObjects.push_back(colObj); } - //sortedObjects.quickSort(shapePointerCompareFunc); + sortedObjects.quickSort(shapePointerCompareFunc); for (int i=0;iaddConstraint(dof6,true); @@ -417,7 +417,7 @@ void ConvertURDF2BulletInternal( } else { - btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); + btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); if (enableConstraints) world1->addConstraint(dof6,true); @@ -449,9 +449,7 @@ void ConvertURDF2BulletInternal( } else { - //btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); - if (enableConstraints) world1->addConstraint(dof6,true); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d926e6b36..f1ff18d6e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6716,36 +6716,151 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (parentBody->m_rigidBody) { - if (clientCmd.m_userConstraintArguments.m_jointType == eGearType) + + btRigidBody* parentRb = 0; + if (clientCmd.m_userConstraintArguments.m_parentJointIndex==-1) { - btRigidBody* childRb = childBody->m_rigidBody; - if (childRb) + parentRb = parentBody->m_rigidBody; + } else + { + if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=0) && + (clientCmd.m_userConstraintArguments.m_parentJointIndexm_rigidBodyJoints.size())) { - 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; + parentRb = &parentBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_parentJointIndex]->getRigidBodyB(); } } + + + btRigidBody* childRb = 0; + if (childBody->m_rigidBody) + { + + if (clientCmd.m_userConstraintArguments.m_childJointIndex==-1) + { + childRb = childBody->m_rigidBody; + } + else + { + if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=0) + && (clientCmd.m_userConstraintArguments.m_childJointIndexm_rigidBodyJoints.size())) + { + childRb = &childBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_childJointIndex]->getRigidBodyB(); + } + + } + } + + switch (clientCmd.m_userConstraintArguments.m_jointType) + { + case eRevoluteType: + { + break; + } + case ePrismaticType: + { + break; + } + + case eFixedType: + { + if (childRb && parentRb && (childRb!=parentRb)) + { + btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); + btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); + + btTransform offsetTrA,offsetTrB; + offsetTrA.setIdentity(); + offsetTrA.setOrigin(pivotInParent); + offsetTrB.setIdentity(); + offsetTrB.setOrigin(pivotInChild); + + btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRb, *childRb, offsetTrA, offsetTrB); + + dof6->setLinearLowerLimit(btVector3(0,0,0)); + dof6->setLinearUpperLimit(btVector3(0,0,0)); + + dof6->setAngularLowerLimit(btVector3(0,0,0)); + dof6->setAngularUpperLimit(btVector3(0,0,0)); + m_data->m_dynamicsWorld->addConstraint(dof6); + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = dof6; + 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; + } + + break; + } + + case ePoint2PointType: + { + if (childRb && parentRb && (childRb!=parentRb)) + { + btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); + btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); + + btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*parentRb,*childRb,pivotInParent,pivotInChild); + p2p->m_setting.m_impulseClamp = defaultMaxForce; + m_data->m_dynamicsWorld->addConstraint(p2p); + InteralUserConstraintData userConstraintData; + userConstraintData.m_rbConstraint = p2p; + 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; + } + break; + } + + case eGearType: + { + + if (childRb && parentRb && (childRb!=parentRb)) + { + 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(*parentRb,*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; + } + break; + } + case eSphericalType: + { + b3Warning("constraint type not handled yet"); + break; + } + case ePlanarType: + { + b3Warning("constraint type not handled yet"); + break; + } + default: + { + b3Warning("unknown constraint type"); + } + }; } } } diff --git a/setup.py b/setup.py index f82142cdf..c57657c10 100644 --- a/setup.py +++ b/setup.py @@ -440,7 +440,7 @@ print("-----") setup( name = 'pybullet', - version='1.2.7', + version='1.3.0', 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',