diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 87382fbc8..1b58431d2 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -269,6 +269,7 @@ static ExampleEntry gDefaultExamples[]= 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), + ExampleEntry(1,"Softbody Multibody Coupling","Two way coupling between soft body and multibody experiment", GripperGraspExampleCreateFunc, eSOFTBODY_MULTIBODY_COUPLING), #ifdef ENABLE_LUA diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 831255ef2..08cc463ca 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -408,7 +408,7 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); - m_robotSim.loadBunny(); + m_robotSim.loadBunny(0.1,0.1,0.02); b3JointInfo revoluteJoint1; revoluteJoint1.m_parentFrame[0] = -0.007; @@ -571,6 +571,46 @@ public: m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint7); m_robotSim.createJoint(0, 3, 0, 5, &revoluteJoint8); } + + if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0) + { + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "kuka_iiwa/model_free_base.urdf"; + args.m_startPosition.setValue(0,1.0,2.0); + args.m_startOrientation.setEulerZYX(0,0,1.57); + args.m_forceOverrideFixedBase = false; + args.m_useMultiBody = true; + b3RobotSimLoadFileResults results; + m_robotSim.loadFile(args,results); + + int kukaId = results.m_uniqueObjectIds[0]; + int numJoints = m_robotSim.getNumJoints(kukaId); + b3Printf("numJoints = %d",numJoints); + + for (int i=0;im_physicsClient); + b3LoadBunnySetScale(command, scale); + b3LoadBunnySetMass(command, mass); + b3LoadBunnySetCollisionMargin(command, collisionMargin); 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 882817384..886aafcad 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -165,7 +165,7 @@ public: void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation); - void loadBunny(); + void loadBunny(double scale, double mass, double collisionMargin); }; #endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 71ac91832..478b2eaa1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -89,6 +89,33 @@ b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physCli return (b3SharedMemoryCommandHandle) command; } +int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_LOAD_BUNNY); + command->m_loadBunnyArguments.m_scale = scale; + command->m_updateFlags |= LOAD_BUNNY_UPDATE_SCALE; + return 0; +} + +int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_LOAD_BUNNY); + command->m_loadBunnyArguments.m_mass = mass; + command->m_updateFlags |= LOAD_BUNNY_UPDATE_MASS; + return 0; +} + +int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_LOAD_BUNNY); + command->m_loadBunnyArguments.m_collisionMargin = collisionMargin; + command->m_updateFlags |= LOAD_BUNNY_UPDATE_COLLISION_MARGIN; + return 0; +} + int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -1580,4 +1607,4 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status } return true; -} \ No newline at end of file +} diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 9fa0cbbfb..49876e983 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -229,6 +229,9 @@ void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUni void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags); b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient); +int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale); +int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass); +int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin); #ifdef __cplusplus } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b8962bb62..bd497b494 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1629,6 +1629,21 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm case CMD_LOAD_BUNNY: { #ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD + double scale = 0.1; + double mass = 0.1; + double collisionMargin = 0.02; + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_SCALE) + { + scale = clientCmd.m_loadBunnyArguments.m_scale; + } + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS) + { + mass = clientCmd.m_loadBunnyArguments.m_mass; + } + if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN) + { + collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin; + } m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2; m_data->m_softBodyWorldInfo.water_density = 0; m_data->m_softBodyWorldInfo.water_offset = 0; @@ -1648,9 +1663,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm 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(0.1,true); - psb->getCollisionShape()->setMargin(0.02); + psb->scale(btVector3(scale,scale,scale)); + psb->setTotalMass(mass,true); + psb->getCollisionShape()->setMargin(collisionMargin); m_data->m_dynamicsWorld->addSoftBody(psb); #endif diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 8a34189be..e37dff0e0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -255,6 +255,13 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64 }; +enum EnumLoadBunnyUpdateFlags +{ + LOAD_BUNNY_UPDATE_SCALE=1, + LOAD_BUNNY_UPDATE_MASS=2, + LOAD_BUNNY_UPDATE_COLLISION_MARGIN=4 +}; + enum EnumSimParamInternalSimFlags { SIM_PARAM_INTERNAL_CREATE_ROBOT_ASSETS=1, @@ -274,6 +281,13 @@ struct SendPhysicsSimulationParameters double m_defaultContactERP; }; +struct LoadBunnyArgs +{ + double m_scale; + double m_mass; + double m_collisionMargin; +}; + struct RequestActualStateArgs { int m_bodyUniqueId; @@ -501,6 +515,7 @@ struct SharedMemoryCommand struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments; struct LoadTextureArgs m_loadTextureArguments; struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments; + struct LoadBunnyArgs m_loadBunnyArguments; }; }; diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 51f4b33d0..dea35c34e 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -18,6 +18,8 @@ subject to the following restrictions: #include "BulletSoftBody/btSoftBodySolvers.h" #include "btSoftBodyData.h" #include "LinearMath/btSerializer.h" +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyConstraint.h" //