From b07df4d504299a6bf45eae277d1a9deb3af60e7b Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Mon, 17 Oct 2016 13:01:04 -0700 Subject: [PATCH] Load bunny through shared memory API and RobotSimAPI. Create grasp bunny example. --- examples/ExampleBrowser/ExampleEntries.cpp | 1 + .../RoboticsLearning/GripperGraspExample.cpp | 167 +++++++++++++----- .../RoboticsLearning/GripperGraspExample.h | 1 + .../RoboticsLearning/KukaGraspExample.cpp | 2 +- examples/RoboticsLearning/b3RobotSimAPI.cpp | 6 + examples/RoboticsLearning/b3RobotSimAPI.h | 2 + examples/SharedMemory/PhysicsClientC_API.cpp | 14 ++ examples/SharedMemory/PhysicsClientC_API.h | 2 + .../PhysicsServerCommandProcessor.cpp | 101 +++++------ examples/SharedMemory/SharedMemoryPublic.h | 1 + 10 files changed, 196 insertions(+), 101 deletions(-) diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 0608f0bd1..b371ca660 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -267,6 +267,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP), ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP), ExampleEntry(1,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP), + ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY), #ifdef ENABLE_LUA diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index ea349fa11..f96dd7c33 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -217,7 +217,6 @@ public: if ((m_options & eONE_MOTOR_GRASP)!=0) { - { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; @@ -232,7 +231,7 @@ public: slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - if (0) + if (1) { b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; @@ -288,49 +287,6 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); - /* - b3JointInfo sliderJoint1; - sliderJoint1.m_parentFrame[0] = 0; - sliderJoint1.m_parentFrame[1] = 0; - sliderJoint1.m_parentFrame[2] = 0.06; - sliderJoint1.m_parentFrame[3] = 0; - sliderJoint1.m_parentFrame[4] = 0; - sliderJoint1.m_parentFrame[5] = 0; - sliderJoint1.m_parentFrame[6] = 1.0; - sliderJoint1.m_childFrame[0] = 0; - sliderJoint1.m_childFrame[1] = 0; - sliderJoint1.m_childFrame[2] = 0; - sliderJoint1.m_childFrame[3] = 0; - sliderJoint1.m_childFrame[4] = 0; - sliderJoint1.m_childFrame[5] = 0; - sliderJoint1.m_childFrame[6] = 1.0; - sliderJoint1.m_jointAxis[0] = 1.0; - sliderJoint1.m_jointAxis[1] = 0.0; - sliderJoint1.m_jointAxis[2] = 0.0; - sliderJoint1.m_jointType = ePrismaticType; - b3JointInfo sliderJoint2; - sliderJoint2.m_parentFrame[0] = 0; - sliderJoint2.m_parentFrame[1] = 0; - sliderJoint2.m_parentFrame[2] = 0.06; - sliderJoint2.m_parentFrame[3] = 0; - sliderJoint2.m_parentFrame[4] = 0; - sliderJoint2.m_parentFrame[5] = 0; - sliderJoint2.m_parentFrame[6] = 1.0; - sliderJoint2.m_childFrame[0] = 0; - sliderJoint2.m_childFrame[1] = 0; - sliderJoint2.m_childFrame[2] = 0; - sliderJoint2.m_childFrame[3] = 0; - sliderJoint2.m_childFrame[4] = 0; - sliderJoint2.m_childFrame[5] = 1.0; - sliderJoint2.m_childFrame[6] = 0; - sliderJoint2.m_jointAxis[0] = 1.0; - sliderJoint2.m_jointAxis[1] = 0.0; - sliderJoint2.m_jointAxis[2] = 0.0; - sliderJoint2.m_jointType = ePrismaticType; - 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; @@ -371,9 +327,112 @@ public: revoluteJoint2.m_jointType = ePoint2PointType; m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1); m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2); - */ } + if ((m_options & eGRASP_SOFT_BODY)!=0) + { + { + SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); + slider.m_minVal=-2; + slider.m_maxVal=2; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + { + SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity + ); + slider.m_minVal=-1; + slider.m_maxVal=1; + m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + if (1) + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; + args.m_fileType = B3_SDF_FILE; + args.m_useMultiBody = true; + b3RobotSimLoadFileResults results; + + if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1) + { + + m_gripperIndex = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(m_gripperIndex); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;im_physicsClient); + b3SubmitClientCommand(m_data->m_physicsClient, command); } \ No newline at end of file diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 798b892e0..882817384 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -164,6 +164,8 @@ public: void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation); + + void loadBunny(); }; #endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 0e75054cc..30be95fc2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -55,6 +55,20 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie return (b3SharedMemoryCommandHandle) command; } +b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_LOAD_BUNNY; + command->m_updateFlags = 0; + + return (b3SharedMemoryCommandHandle) command; +} + int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b44b9a452..fb4a3c29a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -214,6 +214,8 @@ b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle phys b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient); void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags); void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags); + +b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient); #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 29dcf5337..da9f95785 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -601,64 +601,28 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor() void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { - ///collision configuration contains default setup for memory, collision setup - //m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); - //m_collisionConfiguration->setConvexConvexMultipointIterations(); + ///collision configuration contains default setup for memory, collision setup + //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); + ///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); - m_data->m_softBodyWorldInfo.m_dispatcher = m_data->m_dispatcher; - - //m_data->m_broadphase = new btDbvtBroadphase(); - btVector3 worldAabbMin(-1000,-1000,-1000); - btVector3 worldAabbMax(1000,1000,1000); - int maxProxies = 32766; - m_data->m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies); - - m_data->m_solver = new btMultiBodyConstraintSolver; + m_data->m_broadphase = new btDbvtBroadphase(); - m_data->m_softbodySolver = 0; - - 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); - - m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); - - - m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); - m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; + m_data->m_solver = new btMultiBodyConstraintSolver; - m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2; - m_data->m_softBodyWorldInfo.water_density = 0; - m_data->m_softBodyWorldInfo.water_offset = 0; - m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0); - m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10); - - m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase; - m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize(); + m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); + + //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); + + m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); + + + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); + m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; - btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, - &gIndicesBunny[0][0], - BUNNY_NUM_TRIANGLES); - btSoftBody::Material* pm=psb->appendMaterial(); - pm->m_kLST = 1.0 - ; - 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,3.0)); - psb->scale(btVector3(0.1,0.1,0.1)); - psb->setTotalMass(1,true); - psb->getCollisionShape()->setMargin(0.01); - m_data->m_dynamicsWorld->addSoftBody(psb); - } void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies() @@ -1464,6 +1428,35 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm + break; + } + case CMD_LOAD_BUNNY: + { + m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2; + m_data->m_softBodyWorldInfo.water_density = 0; + m_data->m_softBodyWorldInfo.water_offset = 0; + m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0); + m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10); + m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase; + m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize(); + + btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES); + + btSoftBody::Material* pm=psb->appendMaterial(); + pm->m_kLST = 1.0; + 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,3.0)); + psb->scale(btVector3(0.1,0.1,0.1)); + psb->setTotalMass(1,true); + psb->getCollisionShape()->setMargin(0.01); + + m_data->m_dynamicsWorld->addSoftBody(psb); + break; } case CMD_CREATE_SENSOR: @@ -2813,7 +2806,7 @@ void PhysicsServerCommandProcessor::renderScene() 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::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer()); btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags()); } } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 1490fae68..fdae85d29 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,6 +7,7 @@ enum EnumSharedMemoryClientCommand { CMD_LOAD_SDF, CMD_LOAD_URDF, + CMD_LOAD_BUNNY, CMD_SEND_BULLET_DATA_STREAM, CMD_CREATE_BOX_COLLISION_SHAPE, // CMD_DELETE_BOX_COLLISION_SHAPE,