From 9d54f0cf8d9bf18c1b6d3c917695317b041c25e2 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 1 Nov 2016 15:46:09 -0700 Subject: [PATCH 1/7] Grasp soft body with rigid fingers. --- .../wsg50_one_motor_gripper_left_finger.urdf | 27 ++ .../wsg50_one_motor_gripper_no_finger.sdf | 307 ++++++++++++++++++ .../wsg50_one_motor_gripper_right_finger.urdf | 27 ++ .../RoboticsLearning/GripperGraspExample.cpp | 164 +++++++++- .../PhysicsServerCommandProcessor.cpp | 10 +- 5 files changed, 518 insertions(+), 17 deletions(-) create mode 100644 data/gripper/wsg50_one_motor_gripper_left_finger.urdf create mode 100644 data/gripper/wsg50_one_motor_gripper_no_finger.sdf create mode 100644 data/gripper/wsg50_one_motor_gripper_right_finger.urdf 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/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 37e6c06a0..831255ef2 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -346,7 +346,7 @@ public: if (1) { b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; + args.m_fileName = "gripper/wsg50_one_motor_gripper_no_finger.sdf"; args.m_fileType = B3_SDF_FILE; args.m_useMultiBody = true; b3RobotSimLoadFileResults results; @@ -365,7 +365,7 @@ public: b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } - for (int i=0;i<8;i++) + for (int i=0;i<6;i++) { b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; @@ -374,7 +374,27 @@ public: } } - + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "gripper/wsg50_one_motor_gripper_left_finger.urdf"; + args.m_startPosition.setValue(-0.05,0,0.27); + args.m_startOrientation.setEulerZYX(0,0,3.14); + args.m_forceOverrideFixedBase = false; + args.m_useMultiBody = false; + b3RobotSimLoadFileResults results; + m_robotSim.loadFile(args,results); + + } + { + b3RobotSimLoadFileArgs args(""); + args.m_fileName = "gripper/wsg50_one_motor_gripper_right_finger.urdf"; + args.m_startPosition.setValue(0.05,0,0.27); + args.m_startOrientation.setEulerZYX(0,0,3.14); + args.m_forceOverrideFixedBase = false; + args.m_useMultiBody = false; + b3RobotSimLoadFileResults results; + m_robotSim.loadFile(args,results); + } if (1) { b3RobotSimLoadFileArgs args(""); @@ -391,16 +411,16 @@ public: m_robotSim.loadBunny(); b3JointInfo revoluteJoint1; - revoluteJoint1.m_parentFrame[0] = -0.055; + revoluteJoint1.m_parentFrame[0] = -0.007; revoluteJoint1.m_parentFrame[1] = 0; - revoluteJoint1.m_parentFrame[2] = 0.02; + revoluteJoint1.m_parentFrame[2] = 0.0735; revoluteJoint1.m_parentFrame[3] = 0; revoluteJoint1.m_parentFrame[4] = 0; revoluteJoint1.m_parentFrame[5] = 0; revoluteJoint1.m_parentFrame[6] = 1.0; revoluteJoint1.m_childFrame[0] = 0; revoluteJoint1.m_childFrame[1] = 0; - revoluteJoint1.m_childFrame[2] = 0; + revoluteJoint1.m_childFrame[2] = -0.05; revoluteJoint1.m_childFrame[3] = 0; revoluteJoint1.m_childFrame[4] = 0; revoluteJoint1.m_childFrame[5] = 0; @@ -410,16 +430,16 @@ public: revoluteJoint1.m_jointAxis[2] = 0.0; revoluteJoint1.m_jointType = ePoint2PointType; b3JointInfo revoluteJoint2; - revoluteJoint2.m_parentFrame[0] = 0.055; + revoluteJoint2.m_parentFrame[0] = 0.007; revoluteJoint2.m_parentFrame[1] = 0; - revoluteJoint2.m_parentFrame[2] = 0.02; + revoluteJoint2.m_parentFrame[2] = 0.0735; revoluteJoint2.m_parentFrame[3] = 0; revoluteJoint2.m_parentFrame[4] = 0; revoluteJoint2.m_parentFrame[5] = 0; revoluteJoint2.m_parentFrame[6] = 1.0; revoluteJoint2.m_childFrame[0] = 0; revoluteJoint2.m_childFrame[1] = 0; - revoluteJoint2.m_childFrame[2] = 0; + revoluteJoint2.m_childFrame[2] = -0.05; revoluteJoint2.m_childFrame[3] = 0; revoluteJoint2.m_childFrame[4] = 0; revoluteJoint2.m_childFrame[5] = 0; @@ -428,8 +448,128 @@ public: revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointType = ePoint2PointType; - m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1); - m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2); + b3JointInfo revoluteJoint3; + revoluteJoint3.m_parentFrame[0] = -0.207; + revoluteJoint3.m_parentFrame[1] = 0; + revoluteJoint3.m_parentFrame[2] = 0.0735; + revoluteJoint3.m_parentFrame[3] = 0; + revoluteJoint3.m_parentFrame[4] = 0; + revoluteJoint3.m_parentFrame[5] = 0; + revoluteJoint3.m_parentFrame[6] = 1.0; + revoluteJoint3.m_childFrame[0] = -0.2; + revoluteJoint3.m_childFrame[1] = 0; + revoluteJoint3.m_childFrame[2] = -0.05; + revoluteJoint3.m_childFrame[3] = 0; + revoluteJoint3.m_childFrame[4] = 0; + revoluteJoint3.m_childFrame[5] = 0; + revoluteJoint3.m_childFrame[6] = 1.0; + revoluteJoint3.m_jointAxis[0] = 1.0; + revoluteJoint3.m_jointAxis[1] = 0.0; + revoluteJoint3.m_jointAxis[2] = 0.0; + revoluteJoint3.m_jointType = ePoint2PointType; + b3JointInfo revoluteJoint4; + revoluteJoint4.m_parentFrame[0] = 0.207; + revoluteJoint4.m_parentFrame[1] = 0; + revoluteJoint4.m_parentFrame[2] = 0.0735; + revoluteJoint4.m_parentFrame[3] = 0; + revoluteJoint4.m_parentFrame[4] = 0; + revoluteJoint4.m_parentFrame[5] = 0; + revoluteJoint4.m_parentFrame[6] = 1.0; + revoluteJoint4.m_childFrame[0] = 0.2; + revoluteJoint4.m_childFrame[1] = 0; + revoluteJoint4.m_childFrame[2] = -0.05; + revoluteJoint4.m_childFrame[3] = 0; + revoluteJoint4.m_childFrame[4] = 0; + revoluteJoint4.m_childFrame[5] = 0; + revoluteJoint4.m_childFrame[6] = 1.0; + revoluteJoint4.m_jointAxis[0] = 1.0; + revoluteJoint4.m_jointAxis[1] = 0.0; + revoluteJoint4.m_jointAxis[2] = 0.0; + revoluteJoint4.m_jointType = ePoint2PointType; + b3JointInfo revoluteJoint5; + revoluteJoint5.m_parentFrame[0] = -0.007; + revoluteJoint5.m_parentFrame[1] = 0; + revoluteJoint5.m_parentFrame[2] = 0.2735; + revoluteJoint5.m_parentFrame[3] = 0; + revoluteJoint5.m_parentFrame[4] = 0; + revoluteJoint5.m_parentFrame[5] = 0; + revoluteJoint5.m_parentFrame[6] = 1.0; + revoluteJoint5.m_childFrame[0] = 0; + revoluteJoint5.m_childFrame[1] = 0; + revoluteJoint5.m_childFrame[2] = 0.15; + revoluteJoint5.m_childFrame[3] = 0; + revoluteJoint5.m_childFrame[4] = 0; + revoluteJoint5.m_childFrame[5] = 0; + revoluteJoint5.m_childFrame[6] = 1.0; + revoluteJoint5.m_jointAxis[0] = 1.0; + revoluteJoint5.m_jointAxis[1] = 0.0; + revoluteJoint5.m_jointAxis[2] = 0.0; + revoluteJoint5.m_jointType = ePoint2PointType; + b3JointInfo revoluteJoint6; + revoluteJoint6.m_parentFrame[0] = 0.007; + revoluteJoint6.m_parentFrame[1] = 0; + revoluteJoint6.m_parentFrame[2] = 0.2735; + revoluteJoint6.m_parentFrame[3] = 0; + revoluteJoint6.m_parentFrame[4] = 0; + revoluteJoint6.m_parentFrame[5] = 0; + revoluteJoint6.m_parentFrame[6] = 1.0; + revoluteJoint6.m_childFrame[0] = 0; + revoluteJoint6.m_childFrame[1] = 0; + revoluteJoint6.m_childFrame[2] = 0.15; + revoluteJoint6.m_childFrame[3] = 0; + revoluteJoint6.m_childFrame[4] = 0; + revoluteJoint6.m_childFrame[5] = 0; + revoluteJoint6.m_childFrame[6] = 1.0; + revoluteJoint6.m_jointAxis[0] = 1.0; + revoluteJoint6.m_jointAxis[1] = 0.0; + revoluteJoint6.m_jointAxis[2] = 0.0; + revoluteJoint6.m_jointType = ePoint2PointType; + b3JointInfo revoluteJoint7; + revoluteJoint7.m_parentFrame[0] = -0.055; + revoluteJoint7.m_parentFrame[1] = 0; + revoluteJoint7.m_parentFrame[2] = 0.02; + revoluteJoint7.m_parentFrame[3] = 0; + revoluteJoint7.m_parentFrame[4] = 0; + revoluteJoint7.m_parentFrame[5] = 0; + revoluteJoint7.m_parentFrame[6] = 1.0; + revoluteJoint7.m_childFrame[0] = 0; + revoluteJoint7.m_childFrame[1] = 0; + revoluteJoint7.m_childFrame[2] = 0; + revoluteJoint7.m_childFrame[3] = 0; + revoluteJoint7.m_childFrame[4] = 0; + revoluteJoint7.m_childFrame[5] = 0; + revoluteJoint7.m_childFrame[6] = 1.0; + revoluteJoint7.m_jointAxis[0] = 1.0; + revoluteJoint7.m_jointAxis[1] = 0.0; + revoluteJoint7.m_jointAxis[2] = 0.0; + revoluteJoint7.m_jointType = ePoint2PointType; + b3JointInfo revoluteJoint8; + revoluteJoint8.m_parentFrame[0] = 0.055; + revoluteJoint8.m_parentFrame[1] = 0; + revoluteJoint8.m_parentFrame[2] = 0.02; + revoluteJoint8.m_parentFrame[3] = 0; + revoluteJoint8.m_parentFrame[4] = 0; + revoluteJoint8.m_parentFrame[5] = 0; + revoluteJoint8.m_parentFrame[6] = 1.0; + revoluteJoint8.m_childFrame[0] = 0; + revoluteJoint8.m_childFrame[1] = 0; + revoluteJoint8.m_childFrame[2] = 0; + revoluteJoint8.m_childFrame[3] = 0; + revoluteJoint8.m_childFrame[4] = 0; + revoluteJoint8.m_childFrame[5] = 0; + revoluteJoint8.m_childFrame[6] = 1.0; + revoluteJoint8.m_jointAxis[0] = 1.0; + revoluteJoint8.m_jointAxis[1] = 0.0; + revoluteJoint8.m_jointAxis[2] = 0.0; + revoluteJoint8.m_jointType = ePoint2PointType; + m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint1); + m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint2); + m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint3); + m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint4); + m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint5); + m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint6); + m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint7); + m_robotSim.createJoint(0, 3, 0, 5, &revoluteJoint8); } } virtual void exitPhysics() @@ -479,7 +619,7 @@ public: int fingerJointIndices[2]={0,1}; double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity }; - double maxTorqueValues[2]={50.0,50.0}; + double maxTorqueValues[2]={50.0,10.0}; for (int i=0;i<2;i++) { b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 86d3e2ac5..b8962bb62 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1643,14 +1643,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 = 15; 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->translate(btVector3(0,0,1.0)); psb->scale(btVector3(0.1,0.1,0.1)); - psb->setTotalMass(1,true); - psb->getCollisionShape()->setMargin(0.01); + psb->setTotalMass(0.1,true); + psb->getCollisionShape()->setMargin(0.02); m_data->m_dynamicsWorld->addSoftBody(psb); #endif @@ -3772,4 +3772,4 @@ void PhysicsServerCommandProcessor::createDefaultRobotAssets() } } -} \ No newline at end of file +} From 3c37db0804d95d560ffcf335249241fa61ebda60 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 1 Nov 2016 16:45:10 -0700 Subject: [PATCH 2/7] Add API to set bunny properties. Add example to show coupling between softbody and multibody. --- examples/ExampleBrowser/ExampleEntries.cpp | 1 + .../RoboticsLearning/GripperGraspExample.cpp | 42 ++++++++++++++++++- .../RoboticsLearning/GripperGraspExample.h | 1 + examples/RoboticsLearning/b3RobotSimAPI.cpp | 7 +++- examples/RoboticsLearning/b3RobotSimAPI.h | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 29 ++++++++++++- examples/SharedMemory/PhysicsClientC_API.h | 3 ++ .../PhysicsServerCommandProcessor.cpp | 21 ++++++++-- examples/SharedMemory/SharedMemoryCommands.h | 15 +++++++ src/BulletSoftBody/btSoftBody.cpp | 2 + 10 files changed, 115 insertions(+), 8 deletions(-) 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" // From 295c1b7c883159e85e339af3586744da4c73da61 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 2 Nov 2016 12:49:51 -0700 Subject: [PATCH 3/7] Modify the setup for grasping bunny example. --- examples/RoboticsLearning/GripperGraspExample.cpp | 6 +++--- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 08cc463ca..33b24eda7 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -377,7 +377,7 @@ public: { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_one_motor_gripper_left_finger.urdf"; - args.m_startPosition.setValue(-0.05,0,0.27); + args.m_startPosition.setValue(-0.1,0,0.5); args.m_startOrientation.setEulerZYX(0,0,3.14); args.m_forceOverrideFixedBase = false; args.m_useMultiBody = false; @@ -388,7 +388,7 @@ public: { b3RobotSimLoadFileArgs args(""); args.m_fileName = "gripper/wsg50_one_motor_gripper_right_finger.urdf"; - args.m_startPosition.setValue(0.05,0,0.27); + args.m_startPosition.setValue(0.1,0,0.5); args.m_startOrientation.setEulerZYX(0,0,3.14); args.m_forceOverrideFixedBase = false; args.m_useMultiBody = false; @@ -399,7 +399,7 @@ public: { 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; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index bd497b494..0191b5f69 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1658,7 +1658,7 @@ 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 = 15; + psb->m_cfg.piterations = 20; psb->m_cfg.kDF = 0.5; psb->randomizeConstraints(); psb->rotate(btQuaternion(0.70711,0,0,0.70711)); From ab6ce65abebd8934a7ebdaed77c155386159dbad Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 2 Nov 2016 13:21:01 -0700 Subject: [PATCH 4/7] Add contact handling between multibody and softbody. --- .../RoboticsLearning/GripperGraspExample.cpp | 2 +- src/BulletSoftBody/btSoftBody.cpp | 53 +++++++++++++++++-- 2 files changed, 49 insertions(+), 6 deletions(-) diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 33b24eda7..d0b363465 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -604,7 +604,7 @@ public: args.m_startPosition.setValue(0,0,0); args.m_startOrientation.setEulerZYX(0,0,0); args.m_forceOverrideFixedBase = true; - args.m_useMultiBody = true; + args.m_useMultiBody = false; b3RobotSimLoadFileResults results; m_robotSim.loadFile(args,results); } diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index dea35c34e..54808f1ef 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3020,6 +3020,7 @@ void btSoftBody::PSolve_Anchors(btSoftBody* psb,btScalar kst,btScalar ti) } } + // void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { @@ -3031,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; + 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; + btMultiBodyJacobianData jacobianData; + 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) @@ -3043,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.2; + multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV,-impulse.length()*multiplier); + } + } } } } From 479497997cfe57404d2dfee00cf6c8a90e175e46 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Wed, 2 Nov 2016 14:53:28 -0700 Subject: [PATCH 5/7] Add grasp bunny example with multibody gripper. --- .../RoboticsLearning/GripperGraspExample.cpp | 163 ++---------------- src/BulletSoftBody/btSoftBody.cpp | 2 +- 2 files changed, 11 insertions(+), 154 deletions(-) diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index d0b363465..71e32924d 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -343,10 +343,9 @@ public: slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } - if (1) { b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_one_motor_gripper_no_finger.sdf"; + args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf"; args.m_fileType = B3_SDF_FILE; args.m_useMultiBody = true; b3RobotSimLoadFileResults results; @@ -365,7 +364,7 @@ public: b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } - for (int i=0;i<6;i++) + for (int i=0;i<8;i++) { b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_maxTorqueValue = 0.0; @@ -374,28 +373,6 @@ public: } } - { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_one_motor_gripper_left_finger.urdf"; - args.m_startPosition.setValue(-0.1,0,0.5); - args.m_startOrientation.setEulerZYX(0,0,3.14); - args.m_forceOverrideFixedBase = false; - args.m_useMultiBody = false; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); - - } - { - b3RobotSimLoadFileArgs args(""); - args.m_fileName = "gripper/wsg50_one_motor_gripper_right_finger.urdf"; - args.m_startPosition.setValue(0.1,0,0.5); - args.m_startOrientation.setEulerZYX(0,0,3.14); - args.m_forceOverrideFixedBase = false; - args.m_useMultiBody = false; - b3RobotSimLoadFileResults results; - m_robotSim.loadFile(args,results); - } - if (1) { b3RobotSimLoadFileArgs args(""); args.m_fileName = "plane.urdf"; @@ -411,16 +388,16 @@ public: m_robotSim.loadBunny(0.1,0.1,0.02); b3JointInfo revoluteJoint1; - revoluteJoint1.m_parentFrame[0] = -0.007; + revoluteJoint1.m_parentFrame[0] = -0.055; revoluteJoint1.m_parentFrame[1] = 0; - revoluteJoint1.m_parentFrame[2] = 0.0735; + revoluteJoint1.m_parentFrame[2] = 0.02; revoluteJoint1.m_parentFrame[3] = 0; revoluteJoint1.m_parentFrame[4] = 0; revoluteJoint1.m_parentFrame[5] = 0; revoluteJoint1.m_parentFrame[6] = 1.0; revoluteJoint1.m_childFrame[0] = 0; revoluteJoint1.m_childFrame[1] = 0; - revoluteJoint1.m_childFrame[2] = -0.05; + revoluteJoint1.m_childFrame[2] = 0; revoluteJoint1.m_childFrame[3] = 0; revoluteJoint1.m_childFrame[4] = 0; revoluteJoint1.m_childFrame[5] = 0; @@ -430,16 +407,16 @@ public: revoluteJoint1.m_jointAxis[2] = 0.0; revoluteJoint1.m_jointType = ePoint2PointType; b3JointInfo revoluteJoint2; - revoluteJoint2.m_parentFrame[0] = 0.007; + revoluteJoint2.m_parentFrame[0] = 0.055; revoluteJoint2.m_parentFrame[1] = 0; - revoluteJoint2.m_parentFrame[2] = 0.0735; + revoluteJoint2.m_parentFrame[2] = 0.02; revoluteJoint2.m_parentFrame[3] = 0; revoluteJoint2.m_parentFrame[4] = 0; revoluteJoint2.m_parentFrame[5] = 0; revoluteJoint2.m_parentFrame[6] = 1.0; revoluteJoint2.m_childFrame[0] = 0; revoluteJoint2.m_childFrame[1] = 0; - revoluteJoint2.m_childFrame[2] = -0.05; + revoluteJoint2.m_childFrame[2] = 0; revoluteJoint2.m_childFrame[3] = 0; revoluteJoint2.m_childFrame[4] = 0; revoluteJoint2.m_childFrame[5] = 0; @@ -448,128 +425,8 @@ public: revoluteJoint2.m_jointAxis[1] = 0.0; revoluteJoint2.m_jointAxis[2] = 0.0; revoluteJoint2.m_jointType = ePoint2PointType; - b3JointInfo revoluteJoint3; - revoluteJoint3.m_parentFrame[0] = -0.207; - revoluteJoint3.m_parentFrame[1] = 0; - revoluteJoint3.m_parentFrame[2] = 0.0735; - revoluteJoint3.m_parentFrame[3] = 0; - revoluteJoint3.m_parentFrame[4] = 0; - revoluteJoint3.m_parentFrame[5] = 0; - revoluteJoint3.m_parentFrame[6] = 1.0; - revoluteJoint3.m_childFrame[0] = -0.2; - revoluteJoint3.m_childFrame[1] = 0; - revoluteJoint3.m_childFrame[2] = -0.05; - revoluteJoint3.m_childFrame[3] = 0; - revoluteJoint3.m_childFrame[4] = 0; - revoluteJoint3.m_childFrame[5] = 0; - revoluteJoint3.m_childFrame[6] = 1.0; - revoluteJoint3.m_jointAxis[0] = 1.0; - revoluteJoint3.m_jointAxis[1] = 0.0; - revoluteJoint3.m_jointAxis[2] = 0.0; - revoluteJoint3.m_jointType = ePoint2PointType; - b3JointInfo revoluteJoint4; - revoluteJoint4.m_parentFrame[0] = 0.207; - revoluteJoint4.m_parentFrame[1] = 0; - revoluteJoint4.m_parentFrame[2] = 0.0735; - revoluteJoint4.m_parentFrame[3] = 0; - revoluteJoint4.m_parentFrame[4] = 0; - revoluteJoint4.m_parentFrame[5] = 0; - revoluteJoint4.m_parentFrame[6] = 1.0; - revoluteJoint4.m_childFrame[0] = 0.2; - revoluteJoint4.m_childFrame[1] = 0; - revoluteJoint4.m_childFrame[2] = -0.05; - revoluteJoint4.m_childFrame[3] = 0; - revoluteJoint4.m_childFrame[4] = 0; - revoluteJoint4.m_childFrame[5] = 0; - revoluteJoint4.m_childFrame[6] = 1.0; - revoluteJoint4.m_jointAxis[0] = 1.0; - revoluteJoint4.m_jointAxis[1] = 0.0; - revoluteJoint4.m_jointAxis[2] = 0.0; - revoluteJoint4.m_jointType = ePoint2PointType; - b3JointInfo revoluteJoint5; - revoluteJoint5.m_parentFrame[0] = -0.007; - revoluteJoint5.m_parentFrame[1] = 0; - revoluteJoint5.m_parentFrame[2] = 0.2735; - revoluteJoint5.m_parentFrame[3] = 0; - revoluteJoint5.m_parentFrame[4] = 0; - revoluteJoint5.m_parentFrame[5] = 0; - revoluteJoint5.m_parentFrame[6] = 1.0; - revoluteJoint5.m_childFrame[0] = 0; - revoluteJoint5.m_childFrame[1] = 0; - revoluteJoint5.m_childFrame[2] = 0.15; - revoluteJoint5.m_childFrame[3] = 0; - revoluteJoint5.m_childFrame[4] = 0; - revoluteJoint5.m_childFrame[5] = 0; - revoluteJoint5.m_childFrame[6] = 1.0; - revoluteJoint5.m_jointAxis[0] = 1.0; - revoluteJoint5.m_jointAxis[1] = 0.0; - revoluteJoint5.m_jointAxis[2] = 0.0; - revoluteJoint5.m_jointType = ePoint2PointType; - b3JointInfo revoluteJoint6; - revoluteJoint6.m_parentFrame[0] = 0.007; - revoluteJoint6.m_parentFrame[1] = 0; - revoluteJoint6.m_parentFrame[2] = 0.2735; - revoluteJoint6.m_parentFrame[3] = 0; - revoluteJoint6.m_parentFrame[4] = 0; - revoluteJoint6.m_parentFrame[5] = 0; - revoluteJoint6.m_parentFrame[6] = 1.0; - revoluteJoint6.m_childFrame[0] = 0; - revoluteJoint6.m_childFrame[1] = 0; - revoluteJoint6.m_childFrame[2] = 0.15; - revoluteJoint6.m_childFrame[3] = 0; - revoluteJoint6.m_childFrame[4] = 0; - revoluteJoint6.m_childFrame[5] = 0; - revoluteJoint6.m_childFrame[6] = 1.0; - revoluteJoint6.m_jointAxis[0] = 1.0; - revoluteJoint6.m_jointAxis[1] = 0.0; - revoluteJoint6.m_jointAxis[2] = 0.0; - revoluteJoint6.m_jointType = ePoint2PointType; - b3JointInfo revoluteJoint7; - revoluteJoint7.m_parentFrame[0] = -0.055; - revoluteJoint7.m_parentFrame[1] = 0; - revoluteJoint7.m_parentFrame[2] = 0.02; - revoluteJoint7.m_parentFrame[3] = 0; - revoluteJoint7.m_parentFrame[4] = 0; - revoluteJoint7.m_parentFrame[5] = 0; - revoluteJoint7.m_parentFrame[6] = 1.0; - revoluteJoint7.m_childFrame[0] = 0; - revoluteJoint7.m_childFrame[1] = 0; - revoluteJoint7.m_childFrame[2] = 0; - revoluteJoint7.m_childFrame[3] = 0; - revoluteJoint7.m_childFrame[4] = 0; - revoluteJoint7.m_childFrame[5] = 0; - revoluteJoint7.m_childFrame[6] = 1.0; - revoluteJoint7.m_jointAxis[0] = 1.0; - revoluteJoint7.m_jointAxis[1] = 0.0; - revoluteJoint7.m_jointAxis[2] = 0.0; - revoluteJoint7.m_jointType = ePoint2PointType; - b3JointInfo revoluteJoint8; - revoluteJoint8.m_parentFrame[0] = 0.055; - revoluteJoint8.m_parentFrame[1] = 0; - revoluteJoint8.m_parentFrame[2] = 0.02; - revoluteJoint8.m_parentFrame[3] = 0; - revoluteJoint8.m_parentFrame[4] = 0; - revoluteJoint8.m_parentFrame[5] = 0; - revoluteJoint8.m_parentFrame[6] = 1.0; - revoluteJoint8.m_childFrame[0] = 0; - revoluteJoint8.m_childFrame[1] = 0; - revoluteJoint8.m_childFrame[2] = 0; - revoluteJoint8.m_childFrame[3] = 0; - revoluteJoint8.m_childFrame[4] = 0; - revoluteJoint8.m_childFrame[5] = 0; - revoluteJoint8.m_childFrame[6] = 1.0; - revoluteJoint8.m_jointAxis[0] = 1.0; - revoluteJoint8.m_jointAxis[1] = 0.0; - revoluteJoint8.m_jointAxis[2] = 0.0; - revoluteJoint8.m_jointType = ePoint2PointType; - m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint1); - m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint2); - m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint3); - m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint4); - m_robotSim.createJoint(0, 4, 1, 0, &revoluteJoint5); - m_robotSim.createJoint(0, 5, 2, 0, &revoluteJoint6); - m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint7); - m_robotSim.createJoint(0, 3, 0, 5, &revoluteJoint8); + m_robotSim.createJoint(0, 2, 0, 4, &revoluteJoint1); + m_robotSim.createJoint(0, 3, 0, 6, &revoluteJoint2); } if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0) diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index 54808f1ef..b45540b53 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3084,7 +3084,7 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) { if (multibodyLinkCol) { - double multiplier = 0.2; + double multiplier = 0.5; multibodyLinkCol->m_multiBody->applyDeltaVeeMultiDof(deltaV,-impulse.length()*multiplier); } } From b7482c114200347b81e5736036ac59713888d754 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 3 Nov 2016 09:54:56 -0700 Subject: [PATCH 6/7] Add model file for kuka with free base. --- data/kuka_iiwa/model_free_base.urdf | 289 ++++++++++++++++++++++++++++ 1 file changed, 289 insertions(+) create mode 100644 data/kuka_iiwa/model_free_base.urdf 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From ff2c79e6bc3cea33bc104de3642900ad58b2a6e8 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Thu, 3 Nov 2016 12:14:39 -0700 Subject: [PATCH 7/7] Fix a bug in softbody contact handling. --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- src/BulletSoftBody/btSoftBody.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0191b5f69..8b1edb68d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1658,7 +1658,7 @@ 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 = 20; + psb->m_cfg.piterations = 50; psb->m_cfg.kDF = 0.5; psb->randomizeConstraints(); psb->rotate(btQuaternion(0.70711,0,0,0.70711)); diff --git a/src/BulletSoftBody/btSoftBody.cpp b/src/BulletSoftBody/btSoftBody.cpp index b45540b53..d5de7c1b4 100644 --- a/src/BulletSoftBody/btSoftBody.cpp +++ b/src/BulletSoftBody/btSoftBody.cpp @@ -3036,6 +3036,7 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) btRigidBody* rigidCol; btMultiBodyLinkCollider* multibodyLinkCol; btScalar* deltaV; + btMultiBodyJacobianData jacobianData; if (cti.m_colObj->getInternalType() == btCollisionObject::CO_RIGID_BODY) { rigidCol = (btRigidBody*)btRigidBody::upcast(cti.m_colObj); @@ -3047,7 +3048,6 @@ void btSoftBody::PSolve_RContacts(btSoftBody* psb, btScalar kst, btScalar ti) if (multibodyLinkCol) { const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6; - btMultiBodyJacobianData jacobianData; jacobianData.m_jacobians.resize(ndof); jacobianData.m_deltaVelocitiesUnitImpulse.resize(ndof); btScalar* jac=&jacobianData.m_jacobians[0];