diff --git a/data/gripper/wsg50_one_motor_gripper_left_finger.urdf b/data/gripper/wsg50_one_motor_gripper_left_finger.urdf new file mode 100644 index 000000000..a5e58f6c4 --- /dev/null +++ b/data/gripper/wsg50_one_motor_gripper_left_finger.urdf @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/gripper/wsg50_one_motor_gripper_no_finger.sdf b/data/gripper/wsg50_one_motor_gripper_no_finger.sdf new file mode 100644 index 000000000..878b1ee14 --- /dev/null +++ b/data/gripper/wsg50_one_motor_gripper_no_finger.sdf @@ -0,0 +1,307 @@ + + + + + 0 0 0.4 3.14 0 0 + + + + + + world + base_link + + 0 0 1 + + -10 + 10 + 1 + 1 + + + 0 + 0 + 0 + 0 + + + + + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1.2 + + 1 + 0 + 0 + 1 + 0 + 1 + + + + + 0 0 0 0 -0 0 + + + 1 1 1 + meshes/WSG50_110.stl + + + + + + + + + + + 0 0 0.03 0 0 0 + + 0 0 0 0 0 0 + 0.1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0 0 0.01 0 0 0 + + + 0.02 0.02 0.02 + + + + + + + motor + base_link + + 0 0 1 + + -0.055 + 0.001 + 10.0 + 10.0 + + + 0 + 0 + 0 + 0 + + + + + + 0 0 0.04 0 0 0 + + 0 0 0.035 0 0 0 + 0.1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + -0.03 0 0.01 0 -1.2 0 + + + 0.02 0.02 0.07 + + + + + + + left_hinge + motor + + 0 1 0 + + -20.0 + 20.0 + 10 + 10 + + + 0 + 0 + 0 + 0 + + 0 + + + + + 0 0 0.04 0 0 0 + + 0 0 0.035 0 0 0 + 0.1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + 0.03 0 0.01 0 1.2 0 + + + 0.02 0.02 0.07 + + + + + + + right_hinge + motor + + 0 1 0 + + -20.0 + 20.0 + 10 + 10 + + + 0 + 0 + 0 + 0 + + 0 + + + + + -0.055 0 0.06 0 -0 0 + + 0 0 0.0115 0 -0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 -0.06 0 0 0 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 -0.037 0 0 0 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + + + gripper_left + base_link + + 1 0 0 + + -0.01 + 0.04 + 1 + 1 + + + 0 + 0 + 0 + 0 + + + + + + 0.055 0 0.06 0 0 0 + + 0 0 0.0115 0 -0 0 + 0.2 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + 0 0 -0.06 0 0 3.14159 + + + 0.001 0.001 0.001 + meshes/GUIDE_WSG50_110.stl + + + + + 0 0 -0.037 0 0 3.14159 + + + 0.001 0.001 0.001 + meshes/WSG-FMF.stl + + + + + + + gripper_right + base_link + + 1 0 0 + + -0.04 + 0.01 + 1 + 1 + + + 0 + 0 + 0 + 0 + + + + + + + \ No newline at end of file diff --git a/data/gripper/wsg50_one_motor_gripper_right_finger.urdf b/data/gripper/wsg50_one_motor_gripper_right_finger.urdf new file mode 100644 index 000000000..c6257e399 --- /dev/null +++ b/data/gripper/wsg50_one_motor_gripper_right_finger.urdf @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/kuka_iiwa/model_free_base.urdf b/data/kuka_iiwa/model_free_base.urdf new file mode 100644 index 000000000..b87373346 --- /dev/null +++ b/data/kuka_iiwa/model_free_base.urdf @@ -0,0 +1,289 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index 258a70a23..f6af39031 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -270,6 +270,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 37e6c06a0..71e32924d 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -343,7 +343,6 @@ public: slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - if (1) { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; @@ -374,12 +373,10 @@ public: } } - - if (1) { b3RobotSimLoadFileArgs args(""); args.m_fileName = "plane.urdf"; - args.m_startPosition.setValue(0,0,0); + args.m_startPosition.setValue(0,0,-0.2); args.m_startOrientation.setEulerZYX(0,0,0); args.m_forceOverrideFixedBase = true; args.m_useMultiBody = true; @@ -388,7 +385,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.055; @@ -431,6 +428,46 @@ public: m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1); m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2); } + + 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 86d3e2ac5..8b1edb68d 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; @@ -1643,14 +1658,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm pm->m_kLST = 1.0; pm->m_flags -= btSoftBody::fMaterial::DebugDraw; psb->generateBendingConstraints(2,pm); - psb->m_cfg.piterations = 2; + psb->m_cfg.piterations = 50; 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); + psb->translate(btVector3(0,0,1.0)); + psb->scale(btVector3(scale,scale,scale)); + psb->setTotalMass(mass,true); + psb->getCollisionShape()->setMargin(collisionMargin); m_data->m_dynamicsWorld->addSoftBody(psb); #endif @@ -3772,4 +3787,4 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() } } -} \ No newline at end of file +} 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..d5de7c1b4 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" // @@ -3018,6 +3020,7 @@ void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) } } + // void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { @@ -3029,9 +3032,39 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) const sCti& cti = c.m_cti; if (cti.m_colObj->hasContactResponse()) { - btRigidBody* tmpRigid = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); - const btVector3 va = tmpRigid ? tmpRigid->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); - const btVector3 vb = c.m_node->m_x-c.m_node->m_q; + btVector3 va(0,0,0); + btRigidBody* rigidCol; + btMultiBodyLinkCollider* multibodyLinkCol; + btScalar* deltaV; + btMultiBodyJacobianData jacobianData; + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); + va = rigidCol ? rigidCol->getVelocityInLocalPoint(c.m_c1)*dt : btVector3(0,0,0); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(cti.m_colObj); + if (multibodyLinkCol) + { + const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; + jacobianData.m_jacobians.resize(ndof); + jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); + btScalar* jac=&jacobianData.m_jacobians[0]; + + multibodyLinkCol->m_multiBody->fillContactJacobianMultiDof(multibodyLinkCol->m_link, c.m_node->m_x, cti.m_normal, jac, jacobianData.scratch_r, jacobianData.scratch_v, jacobianData.scratch_m); + deltaV = &jacobianData.m_deltaVelocitiesUnitImpulse[0]; + multibodyLinkCol->m_multiBody->calcAccelerationDeltasMultiDof(&jacobianData.m_jacobians[0],deltaV,jacobianData.scratch_r, jacobianData.scratch_v); + + btScalar vel = 0.0; + for (int j = 0; j < ndof ; ++j) { + vel += multibodyLinkCol->m_multiBody->getVelocityVector()[j] * jac[j]; + } + va = cti.m_normal*vel*dt; + } + } + + const btVector3 vb = c.m_node->m_x-c.m_node->m_q; const btVector3 vr = vb-va; const btScalar dn = btDot(vr, cti.m_normal); if(dn<=SIMD_EPSILON) @@ -3041,8 +3074,20 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) // c0 is the impulse matrix, c3 is 1 - the friction coefficient or 0, c4 is the contact hardness coefficient const btVector3 impulse = c.m_c0 * ( (vr - (fv * c.m_c3) + (cti.m_normal * (dp * c.m_c4))) * kst ); c.m_node->m_x -= impulse * c.m_c2; - if (tmpRigid) - tmpRigid->applyImpulse(impulse,c.m_c1); + + if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) + { + if (rigidCol) + rigidCol->applyImpulse(impulse,c.m_c1); + } + else if (cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK) + { + if (multibodyLinkCol) + { + double multiplier = 0.5; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV,-impulse.length()*multiplier); + } + } } } }