diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_new.sdf index a3a11f3b2..1555a5c09 100644 --- a/data/gripper/wsg50_one_motor_gripper_new.sdf +++ b/data/gripper/wsg50_one_motor_gripper_new.sdf @@ -2,7 +2,7 @@ - 0 0 0.26 3.14 0 0 + 0 0 0.6 3.14 0 0 diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index b46706e65..4d53f6a8c 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -232,6 +232,7 @@ public: slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } + if (0) { b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; @@ -241,7 +242,7 @@ public: args.m_useMultiBody = true; m_robotSim.loadFile(args, results); } - + if (1) { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; @@ -329,6 +330,7 @@ public: m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1); m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2); */ + /* b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.055; revoluteJoint1.m_parentFrame[1] = 0; @@ -369,6 +371,7 @@ public: revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1); m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2); + */ } } @@ -398,6 +401,7 @@ public: } } + /* if ((m_options & eONE_MOTOR_GRASP)!=0) { int fingerJointIndices[2]={0,1}; @@ -413,9 +417,9 @@ public: m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs); } } + */ - - m_robotSim.stepSimulation(); + //m_robotSim.stepSimulation(); } virtual void renderScene() { diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index a07d6f643..48e3b7476 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -256,7 +256,7 @@ public: } - m_robotSim.stepSimulation(); + //m_robotSim.stepSimulation(); } virtual void renderScene() { diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp index e0c9cadd1..2b1c1a8f8 100644 --- a/examples/RoboticsLearning/R2D2GraspExample.cpp +++ b/examples/RoboticsLearning/R2D2GraspExample.cpp @@ -179,7 +179,7 @@ public: } virtual void stepSimulation(float deltaTime) { - m_robotSim.stepSimulation(); + //m_robotSim.stepSimulation(); } virtual void renderScene() { diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index a0636381b..590bc0fa7 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -967,7 +967,7 @@ void b3RobotSimAPI::renderScene() m_data->m_clientServerDirect->renderScene(); } - m_data->m_physicsServer.renderScene(); + //m_data->m_physicsServer.renderScene(); } void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 5ba9e9490..282567bf2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5,7 +5,7 @@ #include "../Importers/ImportURDFDemo/URDF2Bullet.h" #include "../Extras/InverseDynamics/btMultiBodyTreeCreator.hpp" #include "TinyRendererVisualShapeConverter.h" -#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" +#include "BulletDynamics/Featherstone/btSoftMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" @@ -26,6 +26,10 @@ #include "Bullet3Common/b3Logging.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "SharedMemoryCommands.h" +#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h" +#include "BulletSoftBody/btSoftBodySolvers.h" +#include "BulletSoftBody/btSoftBodyHelpers.h" +#include "../SoftDemo/BunnyMesh.h" //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet! btVector3 gLastPickPos(0, 0, 0); @@ -421,8 +425,9 @@ struct PhysicsServerCommandProcessorInternalData btBroadphaseInterface* m_broadphase; btCollisionDispatcher* m_dispatcher; btMultiBodyConstraintSolver* m_solver; + btSoftBodySolver* m_softbodySolver; btDefaultCollisionConfiguration* m_collisionConfiguration; - btMultiBodyDynamicsWorld* m_dynamicsWorld; + btSoftMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_remoteDebugDrawer; btAlignedObjectArray m_cachedContactPoints; @@ -596,8 +601,9 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { ///collision configuration contains default setup for memory, collision setup - m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); + //m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); //m_collisionConfiguration->setConvexConvexMultipointIterations(); + m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration(); ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration); @@ -605,8 +611,10 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_broadphase = new btDbvtBroadphase(); m_data->m_solver = new btMultiBodyConstraintSolver; + + m_data->m_softbodySolver = 0; - m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration, m_data->m_softbodySolver); //Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192); @@ -616,6 +624,29 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; + + btSoftBodyWorldInfo softBodyWorldInfo; + softBodyWorldInfo.air_density = (btScalar)1.2; + softBodyWorldInfo.water_density = 0; + softBodyWorldInfo.water_offset = 0; + softBodyWorldInfo.water_normal = btVector3(0,0,0); + softBodyWorldInfo.m_gravity.setValue(0,0,0); + + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(softBodyWorldInfo,gVerticesBunny, + &gIndicesBunny[0][0], + BUNNY_NUM_TRIANGLES); + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 0.5; + pm->m_flags -= btSoftBody::fMaterial::DebugDraw; + psb->generateBendingConstraints(2,pm); + psb->m_cfg.piterations = 2; + psb->m_cfg.kDF = 0.5; + psb->randomizeConstraints(); + psb->rotate(btQuaternion(0.70711,0,0,0.70711)); + psb->translate(btVector3(0,0,1.0)); + psb->scale(btVector3(0.1,0.1,0.1)); + psb->setTotalMass(1,true); + m_data->m_dynamicsWorld->addSoftBody(psb); } @@ -2766,6 +2797,15 @@ void PhysicsServerCommandProcessor::renderScene() m_data->m_guiHelper->render(m_data->m_dynamicsWorld); } + for ( int i=0;im_dynamicsWorld->getSoftBodyArray().size();i++) + { + btSoftBody* psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i]; + if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe))) + { + btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); + btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags()); + } + } } void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) diff --git a/examples/SoftDemo/SoftDemo.cpp b/examples/SoftDemo/SoftDemo.cpp index 6b1611825..286633464 100644 --- a/examples/SoftDemo/SoftDemo.cpp +++ b/examples/SoftDemo/SoftDemo.cpp @@ -1019,7 +1019,7 @@ static void Init_Bunny(SoftDemo* pdemo) psb->scale(btVector3(6,6,6)); psb->setTotalMass(1,true); pdemo->getSoftDynamicsWorld()->addSoftBody(psb); - pdemo->m_cutting=true; + //pdemo->m_cutting=true; } @@ -2214,7 +2214,7 @@ void SoftDemo::initPhysics() if( g_softBodyOutput ) delete g_softBodyOutput; - if (1) + if (0) { g_openCLSIMDSolver = new btOpenCLSoftBodySolverSIMDAware( g_cqCommandQue, g_cxMainContext); // g_openCLSIMDSolver = new btOpenCLSoftBodySolver( g_cqCommandQue, g_cxMainContext); @@ -2223,7 +2223,7 @@ void SoftDemo::initPhysics() - softBodySolver = g_openCLSIMDSolver; + //softBodySolver = g_openCLSIMDSolver; g_softBodyOutput = new btSoftBodySolverOutputCLtoCPU; #endif //USE_AMD_OPENCL @@ -2289,7 +2289,7 @@ void SoftDemo::initPhysics() m_autocam = false; m_raycast = false; - m_cutting = false; + //m_cutting = false; m_results.fraction = 1.f; demofncs[current_demo](this);