Merge pull request #1500 from YunfeiBai/master
Load softbody from obj file.
This commit is contained in:
erwincoumans
GitHub
@@ -26,7 +26,7 @@ SET(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_DEBUG")
|
||||
OPTION(USE_DOUBLE_PRECISION "Use double precision" OFF)
|
||||
OPTION(USE_GRAPHICAL_BENCHMARK "Use Graphical Benchmark" ON)
|
||||
OPTION(BUILD_SHARED_LIBS "Use shared libraries" OFF)
|
||||
OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" OFF)
|
||||
OPTION(USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD "Use btSoftMultiBodyDynamicsWorld" ON)
|
||||
|
||||
OPTION(BULLET2_USE_THREAD_LOCKS "Build Bullet 2 libraries with mutex locking around certain operations (required for multi-threading)" OFF)
|
||||
IF (BULLET2_USE_THREAD_LOCKS)
|
||||
@@ -217,8 +217,8 @@ ADD_DEFINITIONS( -DBT_USE_DOUBLE_PRECISION)
|
||||
SET( BULLET_DOUBLE_DEF "-DBT_USE_DOUBLE_PRECISION")
|
||||
ENDIF (USE_DOUBLE_PRECISION)
|
||||
|
||||
IF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
|
||||
ADD_DEFINITIONS(-DUSE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
|
||||
IF (NOT USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
|
||||
ADD_DEFINITIONS(-DSKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
|
||||
ENDIF (USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
|
||||
|
||||
IF(USE_GRAPHICAL_BENCHMARK)
|
||||
|
||||
1360
data/bunny.obj
Normal file
1360
data/bunny.obj
Normal file
File diff suppressed because it is too large
Load Diff
@@ -279,10 +279,10 @@ 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),
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
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),
|
||||
#endif //USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
|
||||
#ifdef ENABLE_LUA
|
||||
ExampleEntry(1,"Lua Script", "Create the dynamics world, collision shapes and rigid bodies using Lua scripting",
|
||||
|
||||
@@ -1143,7 +1143,7 @@ void b3RobotSimulatorClientAPI::submitProfileTiming(const std::string& profileN
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
|
||||
}
|
||||
|
||||
void b3RobotSimulatorClientAPI::loadBunny(double scale, double mass, double collisionMargin)
|
||||
void b3RobotSimulatorClientAPI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
|
||||
{
|
||||
if (!isConnected())
|
||||
{
|
||||
@@ -1151,9 +1151,9 @@ void b3RobotSimulatorClientAPI::loadBunny(double scale, double mass, double coll
|
||||
return;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClientHandle);
|
||||
b3LoadBunnySetScale(command, scale);
|
||||
b3LoadBunnySetMass(command, mass);
|
||||
b3LoadBunnySetCollisionMargin(command, collisionMargin);
|
||||
b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
|
||||
b3LoadSoftBodySetScale(command, scale);
|
||||
b3LoadSoftBodySetMass(command, mass);
|
||||
b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
}
|
||||
@@ -229,7 +229,7 @@ public:
|
||||
|
||||
//////////////// INTERNAL
|
||||
|
||||
void loadBunny(double scale, double mass, double collisionMargin);
|
||||
void loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin);
|
||||
|
||||
//setGuiHelper is only used when embedded in existing example browser
|
||||
void setGuiHelper(struct GUIHelperInterface* guiHelper);
|
||||
|
||||
@@ -26,6 +26,11 @@ class GripperGraspExample : public CommonExampleInterface
|
||||
b3RobotSimulatorClientAPI m_robotSim;
|
||||
int m_options;
|
||||
int m_gripperIndex;
|
||||
double m_time;
|
||||
b3Vector3 m_targetPos;
|
||||
b3Vector3 m_worldPos;
|
||||
b3Vector4 m_targetOri;
|
||||
b3Vector4 m_worldOri;
|
||||
|
||||
b3AlignedObjectArray<int> m_movingInstances;
|
||||
enum
|
||||
@@ -331,7 +336,7 @@ public:
|
||||
|
||||
}
|
||||
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
|
||||
m_robotSim.loadBunny(0.1,0.1,0.02);
|
||||
m_robotSim.loadSoftBody("bunny.obj",0.1,0.1,0.02);
|
||||
|
||||
b3JointInfo revoluteJoint1;
|
||||
revoluteJoint1.m_parentFrame[0] = -0.055;
|
||||
@@ -379,11 +384,11 @@ public:
|
||||
{
|
||||
{
|
||||
b3RobotSimulatorLoadUrdfFileArgs args;
|
||||
args.m_startPosition.setValue(0,1.0,2.0);
|
||||
args.m_startOrientation.setEulerZYX(0,0,1.57);
|
||||
args.m_forceOverrideFixedBase = false;
|
||||
args.m_startPosition.setValue(-0.5,0,0.1);
|
||||
args.m_startOrientation.setEulerZYX(0,0,0);
|
||||
args.m_forceOverrideFixedBase = true;
|
||||
args.m_useMultiBody = true;
|
||||
int kukaId = m_robotSim.loadURDF("kuka_iiwa/model_free_base.urdf", args);
|
||||
int kukaId = m_robotSim.loadURDF("kuka_iiwa/model.urdf", args);
|
||||
|
||||
int numJoints = m_robotSim.getNumJoints(kukaId);
|
||||
b3Printf("numJoints = %d",numJoints);
|
||||
@@ -394,7 +399,7 @@ public:
|
||||
m_robotSim.getJointInfo(kukaId,i,&jointInfo);
|
||||
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
|
||||
b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
|
||||
controlArgs.m_maxTorqueValue = 0.0;
|
||||
controlArgs.m_maxTorqueValue = 500.0;
|
||||
m_robotSim.setJointMotorControl(kukaId,i,controlArgs);
|
||||
}
|
||||
}
|
||||
@@ -407,7 +412,7 @@ public:
|
||||
m_robotSim.loadURDF("plane.urdf", args);
|
||||
}
|
||||
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
|
||||
m_robotSim.loadBunny(0.5,10.0,0.1);
|
||||
m_robotSim.loadSoftBody("bunny.obj",0.3,10.0,0.1);
|
||||
}
|
||||
}
|
||||
virtual void exitPhysics()
|
||||
@@ -468,6 +473,120 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
if ((m_options & eSOFTBODY_MULTIBODY_COUPLING)!=0)
|
||||
{
|
||||
float dt = deltaTime;
|
||||
btClamp(dt,0.0001f,0.01f);
|
||||
|
||||
m_time+=dt;
|
||||
m_targetPos.setValue(0, 0, 0.5+0.2*b3Cos( m_time));
|
||||
m_targetOri.setValue(0, 1.0, 0, 0);
|
||||
|
||||
|
||||
int numJoints = m_robotSim.getNumJoints(0);
|
||||
|
||||
if (numJoints==7)
|
||||
{
|
||||
double q_current[7]={0,0,0,0,0,0,0};
|
||||
|
||||
b3JointStates2 jointStates;
|
||||
|
||||
if (m_robotSim.getJointStates(0,jointStates))
|
||||
{
|
||||
//skip the base positions (7 values)
|
||||
b3Assert(7+numJoints == jointStates.m_numDegreeOfFreedomQ);
|
||||
for (int i=0;i<numJoints;i++)
|
||||
{
|
||||
q_current[i] = jointStates.m_actualStateQ[i+7];
|
||||
}
|
||||
}
|
||||
// compute body position and orientation
|
||||
b3LinkState linkState;
|
||||
m_robotSim.getLinkState(0, 6, &linkState);
|
||||
|
||||
m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
|
||||
m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);
|
||||
|
||||
b3Vector3DoubleData targetPosDataOut;
|
||||
m_targetPos.serializeDouble(targetPosDataOut);
|
||||
b3Vector3DoubleData worldPosDataOut;
|
||||
m_worldPos.serializeDouble(worldPosDataOut);
|
||||
b3Vector3DoubleData targetOriDataOut;
|
||||
m_targetOri.serializeDouble(targetOriDataOut);
|
||||
b3Vector3DoubleData worldOriDataOut;
|
||||
m_worldOri.serializeDouble(worldOriDataOut);
|
||||
|
||||
|
||||
b3RobotSimulatorInverseKinematicArgs ikargs;
|
||||
b3RobotSimulatorInverseKinematicsResults ikresults;
|
||||
|
||||
ikargs.m_bodyUniqueId = 0;
|
||||
// ikargs.m_currentJointPositions = q_current;
|
||||
// ikargs.m_numPositions = 7;
|
||||
ikargs.m_endEffectorTargetPosition[0] = targetPosDataOut.m_floats[0];
|
||||
ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1];
|
||||
ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2];
|
||||
|
||||
ikargs.m_flags |= B3_HAS_IK_TARGET_ORIENTATION/* + B3_HAS_NULL_SPACE_VELOCITY*/;
|
||||
|
||||
ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0];
|
||||
ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1];
|
||||
ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2];
|
||||
ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3];
|
||||
ikargs.m_endEffectorLinkIndex = 6;
|
||||
|
||||
// Settings based on default KUKA arm setting
|
||||
ikargs.m_lowerLimits.resize(numJoints);
|
||||
ikargs.m_upperLimits.resize(numJoints);
|
||||
ikargs.m_jointRanges.resize(numJoints);
|
||||
ikargs.m_restPoses.resize(numJoints);
|
||||
ikargs.m_lowerLimits[0] = -2.32;
|
||||
ikargs.m_lowerLimits[1] = -1.6;
|
||||
ikargs.m_lowerLimits[2] = -2.32;
|
||||
ikargs.m_lowerLimits[3] = -1.6;
|
||||
ikargs.m_lowerLimits[4] = -2.32;
|
||||
ikargs.m_lowerLimits[5] = -1.6;
|
||||
ikargs.m_lowerLimits[6] = -2.4;
|
||||
ikargs.m_upperLimits[0] = 2.32;
|
||||
ikargs.m_upperLimits[1] = 1.6;
|
||||
ikargs.m_upperLimits[2] = 2.32;
|
||||
ikargs.m_upperLimits[3] = 1.6;
|
||||
ikargs.m_upperLimits[4] = 2.32;
|
||||
ikargs.m_upperLimits[5] = 1.6;
|
||||
ikargs.m_upperLimits[6] = 2.4;
|
||||
ikargs.m_jointRanges[0] = 5.8;
|
||||
ikargs.m_jointRanges[1] = 4;
|
||||
ikargs.m_jointRanges[2] = 5.8;
|
||||
ikargs.m_jointRanges[3] = 4;
|
||||
ikargs.m_jointRanges[4] = 5.8;
|
||||
ikargs.m_jointRanges[5] = 4;
|
||||
ikargs.m_jointRanges[6] = 6;
|
||||
ikargs.m_restPoses[0] = 0;
|
||||
ikargs.m_restPoses[1] = 0;
|
||||
ikargs.m_restPoses[2] = 0;
|
||||
ikargs.m_restPoses[3] = SIMD_HALF_PI;
|
||||
ikargs.m_restPoses[4] = 0;
|
||||
ikargs.m_restPoses[5] = -SIMD_HALF_PI*0.66;
|
||||
ikargs.m_restPoses[6] = 0;
|
||||
ikargs.m_numDegreeOfFreedom = numJoints;
|
||||
|
||||
if (m_robotSim.calculateInverseKinematics(ikargs,ikresults))
|
||||
{
|
||||
//copy the IK result to the desired state of the motor/actuator
|
||||
for (int i=0;i<numJoints;i++)
|
||||
{
|
||||
b3RobotSimulatorJointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
|
||||
t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
|
||||
t.m_maxTorqueValue = 100.0;
|
||||
t.m_kp= 1.0;
|
||||
t.m_targetVelocity = 0;
|
||||
t.m_kd = 1.0;
|
||||
m_robotSim.setJointMotorControl(0,i,t);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
m_robotSim.stepSimulation();
|
||||
}
|
||||
virtual void renderScene()
|
||||
|
||||
@@ -256,44 +256,57 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command
|
||||
}
|
||||
}
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
|
||||
if (cl->canSubmitCommand())
|
||||
{
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
command->m_type = CMD_LOAD_BUNNY;
|
||||
command->m_updateFlags = 0;
|
||||
command->m_type = CMD_LOAD_SOFT_BODY;
|
||||
int len = strlen(fileName);
|
||||
if (len < MAX_FILENAME_LENGTH)
|
||||
{
|
||||
strcpy(command->m_loadSoftBodyArguments.m_fileName, fileName);
|
||||
}
|
||||
else
|
||||
{
|
||||
command->m_loadSoftBodyArguments.m_fileName[0] = 0;
|
||||
}
|
||||
command->m_updateFlags = LOAD_SOFT_BODY_FILE_NAME;
|
||||
|
||||
return (b3SharedMemoryCommandHandle) command;
|
||||
}
|
||||
|
||||
B3_SHARED_API 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;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
|
||||
B3_SHARED_API int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
|
||||
{
|
||||
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;
|
||||
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
|
||||
command->m_loadSoftBodyArguments.m_scale = scale;
|
||||
command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_SCALE;
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin)
|
||||
B3_SHARED_API int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
|
||||
{
|
||||
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;
|
||||
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
|
||||
command->m_loadSoftBodyArguments.m_mass = mass;
|
||||
command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_MASS;
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
|
||||
command->m_loadSoftBodyArguments.m_collisionMargin = collisionMargin;
|
||||
command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN;
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@@ -517,10 +517,10 @@ B3_SHARED_API void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandl
|
||||
B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[/*3*/], int flag);
|
||||
|
||||
///experiments of robots interacting with non-rigid objects (such as btSoftBody)
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
|
||||
B3_SHARED_API int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
|
||||
B3_SHARED_API int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
|
||||
B3_SHARED_API int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
|
||||
B3_SHARED_API int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
|
||||
B3_SHARED_API int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
|
||||
B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
|
||||
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
|
||||
|
||||
@@ -51,7 +51,7 @@
|
||||
#include "../TinyAudio/b3SoundEngine.h"
|
||||
#endif
|
||||
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
|
||||
#include "BulletSoftBody/btSoftBodySolvers.h"
|
||||
#include "BulletSoftBody/btSoftBodyHelpers.h"
|
||||
@@ -170,6 +170,9 @@ struct InternalBodyData
|
||||
{
|
||||
btMultiBody* m_multiBody;
|
||||
btRigidBody* m_rigidBody;
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
btSoftBody* m_softBody;
|
||||
#endif
|
||||
int m_testData;
|
||||
std::string m_bodyName;
|
||||
|
||||
@@ -192,6 +195,9 @@ struct InternalBodyData
|
||||
{
|
||||
m_multiBody=0;
|
||||
m_rigidBody=0;
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
m_softBody = 0;
|
||||
#endif
|
||||
m_testData=0;
|
||||
m_bodyName="";
|
||||
m_rootLocalInertialFrame.setIdentity();
|
||||
@@ -1521,7 +1527,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
btMultiBodyConstraintSolver* m_solver;
|
||||
btDefaultCollisionConfiguration* m_collisionConfiguration;
|
||||
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
|
||||
btSoftBodySolver* m_softbodySolver;
|
||||
btSoftBodyWorldInfo m_softBodyWorldInfo;
|
||||
@@ -2182,7 +2188,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
||||
{
|
||||
///collision configuration contains default setup for memory, collision setup
|
||||
//m_collisionConfiguration->setConvexConvexMultipointIterations();
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
|
||||
#else
|
||||
m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
|
||||
@@ -2205,7 +2211,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
||||
|
||||
m_data->m_solver = new btMultiBodyConstraintSolver;
|
||||
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
|
||||
#else
|
||||
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
|
||||
@@ -2370,6 +2376,14 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
|
||||
m_data->m_dynamicsWorld->removeMultiBody(mb);
|
||||
delete mb;
|
||||
}
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
for (i=m_data->m_dynamicsWorld->getSoftBodyArray().size()-1;i>=0;i--)
|
||||
{
|
||||
btSoftBody* sb = m_data->m_dynamicsWorld->getSoftBodyArray()[i];
|
||||
m_data->m_dynamicsWorld->removeSoftBody(sb);
|
||||
delete sb;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
for (int i=0;i<constraints.size();i++)
|
||||
@@ -5741,26 +5755,35 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe
|
||||
return hasStatus;
|
||||
}
|
||||
|
||||
bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
{
|
||||
serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
|
||||
bool hasStatus = true;
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#ifndef SKIP_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)
|
||||
const LoadSoftBodyArgs& loadSoftBodyArgs = clientCmd.m_loadSoftBodyArguments;
|
||||
if (m_data->m_verboseOutput)
|
||||
{
|
||||
scale = clientCmd.m_loadBunnyArguments.m_scale;
|
||||
b3Printf("Processed CMD_LOAD_SOFT_BODY:%s", loadSoftBodyArgs.m_fileName);
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_MASS)
|
||||
btAssert((clientCmd.m_updateFlags & LOAD_SOFT_BODY_FILE_NAME) !=0);
|
||||
btAssert(loadSoftBodyArgs.m_fileName);
|
||||
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_SCALE)
|
||||
{
|
||||
mass = clientCmd.m_loadBunnyArguments.m_mass;
|
||||
scale = clientCmd.m_loadSoftBodyArguments.m_scale;
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_BUNNY_UPDATE_COLLISION_MARGIN)
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_MASS)
|
||||
{
|
||||
collisionMargin = clientCmd.m_loadBunnyArguments.m_collisionMargin;
|
||||
mass = clientCmd.m_loadSoftBodyArguments.m_mass;
|
||||
}
|
||||
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN)
|
||||
{
|
||||
collisionMargin = clientCmd.m_loadSoftBodyArguments.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;
|
||||
@@ -5769,22 +5792,58 @@ bool PhysicsServerCommandProcessor::processLoadBunnyCommand(const struct SharedM
|
||||
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);
|
||||
|
||||
{
|
||||
char relativeFileName[1024];
|
||||
char pathPrefix[1024];
|
||||
pathPrefix[0] = 0;
|
||||
if (b3ResourcePath::findResourcePath(loadSoftBodyArgs.m_fileName, relativeFileName, 1024))
|
||||
{
|
||||
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
|
||||
}
|
||||
const std::string& error_message_prefix="";
|
||||
std::string out_found_filename;
|
||||
int out_type;
|
||||
bool foundFile = findExistingMeshFile(pathPrefix, relativeFileName,error_message_prefix,&out_found_filename, &out_type);
|
||||
std::vector<tinyobj::shape_t> shapes;
|
||||
std::string err = tinyobj::LoadObj(shapes,out_found_filename.c_str());
|
||||
if (shapes.size()>0)
|
||||
{
|
||||
const tinyobj::shape_t& shape = shapes[0];
|
||||
btAlignedObjectArray<btScalar> vertices;
|
||||
btAlignedObjectArray<int> indices;
|
||||
for (int i=0;i<shape.mesh.positions.size();i++)
|
||||
{
|
||||
vertices.push_back(shape.mesh.positions[i]);
|
||||
}
|
||||
for (int i=0;i<shape.mesh.indices.size();i++)
|
||||
{
|
||||
indices.push_back(shape.mesh.indices[i]);
|
||||
}
|
||||
int numTris = indices.size()/3;
|
||||
if (numTris>0)
|
||||
{
|
||||
btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,&vertices[0],&indices[0],numTris);
|
||||
btSoftBody::Material* pm=psb->appendMaterial();
|
||||
pm->m_kLST = 1.0;
|
||||
pm->m_kLST = 0.5;
|
||||
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
|
||||
psb->generateBendingConstraints(2,pm);
|
||||
psb->m_cfg.piterations = 50;
|
||||
psb->m_cfg.piterations = 20;
|
||||
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->translate(btVector3(-0.05,0,1.0));
|
||||
psb->scale(btVector3(scale,scale,scale));
|
||||
psb->setTotalMass(mass,true);
|
||||
psb->getCollisionShape()->setMargin(collisionMargin);
|
||||
|
||||
m_data->m_dynamicsWorld->addSoftBody(psb);
|
||||
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
|
||||
InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);
|
||||
bodyHandle->m_softBody = psb;
|
||||
serverStatusOut.m_loadSoftBodyResultArguments.m_objectUniqueId = bodyUniqueId;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
||||
#endif
|
||||
return hasStatus;
|
||||
@@ -8831,9 +8890,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
hasStatus = processLoadURDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
|
||||
break;
|
||||
}
|
||||
case CMD_LOAD_BUNNY:
|
||||
case CMD_LOAD_SOFT_BODY:
|
||||
{
|
||||
hasStatus = processLoadBunnyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
|
||||
hasStatus = processLoadSoftBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
|
||||
break;
|
||||
}
|
||||
case CMD_CREATE_SENSOR:
|
||||
@@ -9079,7 +9138,7 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
|
||||
|
||||
m_data->m_guiHelper->render(m_data->m_dynamicsWorld);
|
||||
}
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
for ( int i=0;i<m_data->m_dynamicsWorld->getSoftBodyArray().size();i++)
|
||||
{
|
||||
btSoftBody* psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
|
||||
|
||||
@@ -46,7 +46,7 @@ protected:
|
||||
bool processLoadSDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processCreateMultiBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processLoadBunnyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processLoadSoftBodyCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processCreateSensorCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processProfileTimingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
bool processRequestCollisionInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
@@ -439,11 +439,12 @@ enum EnumSimParamUpdateFlags
|
||||
SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP = 32768,
|
||||
};
|
||||
|
||||
enum EnumLoadBunnyUpdateFlags
|
||||
enum EnumLoadSoftBodyUpdateFlags
|
||||
{
|
||||
LOAD_BUNNY_UPDATE_SCALE=1,
|
||||
LOAD_BUNNY_UPDATE_MASS=2,
|
||||
LOAD_BUNNY_UPDATE_COLLISION_MARGIN=4
|
||||
LOAD_SOFT_BODY_FILE_NAME=1,
|
||||
LOAD_SOFT_BODY_UPDATE_SCALE=2,
|
||||
LOAD_SOFT_BODY_UPDATE_MASS=4,
|
||||
LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN=8
|
||||
};
|
||||
|
||||
enum EnumSimParamInternalSimFlags
|
||||
@@ -455,13 +456,19 @@ enum EnumSimParamInternalSimFlags
|
||||
///Controlling a robot involves sending the desired state to its joint motor controllers.
|
||||
///The control mode determines the state variables used for motor control.
|
||||
|
||||
struct LoadBunnyArgs
|
||||
struct LoadSoftBodyArgs
|
||||
{
|
||||
char m_fileName[MAX_FILENAME_LENGTH];
|
||||
double m_scale;
|
||||
double m_mass;
|
||||
double m_collisionMargin;
|
||||
};
|
||||
|
||||
struct b3LoadSoftBodyResultArgs
|
||||
{
|
||||
int m_objectUniqueId;
|
||||
};
|
||||
|
||||
struct RequestActualStateArgs
|
||||
{
|
||||
int m_bodyUniqueId;
|
||||
@@ -991,7 +998,7 @@ struct SharedMemoryCommand
|
||||
struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
|
||||
struct UserDebugDrawArgs m_userDebugDrawArgs;
|
||||
struct RequestRaycastIntersections m_requestRaycastIntersections;
|
||||
struct LoadBunnyArgs m_loadBunnyArguments;
|
||||
struct LoadSoftBodyArgs m_loadSoftBodyArguments;
|
||||
struct VRCameraState m_vrCameraStateArguments;
|
||||
struct StateLoggingRequest m_stateLoggingArguments;
|
||||
struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments;
|
||||
@@ -1080,6 +1087,7 @@ struct SharedMemoryStatus
|
||||
struct b3CustomCommandResultArgs m_customCommandResultArgs;
|
||||
struct b3PhysicsSimulationParameters m_simulationParameterResultArgs;
|
||||
struct b3StateSerializationArguments m_saveStateResultArgs;
|
||||
struct b3LoadSoftBodyResultArgs m_loadSoftBodyResultArguments;
|
||||
struct SendCollisionShapeDataArgs m_sendCollisionShapeArgs;
|
||||
};
|
||||
};
|
||||
|
||||
@@ -23,7 +23,7 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_LOAD_BULLET,
|
||||
CMD_SAVE_BULLET,
|
||||
CMD_LOAD_MJCF,
|
||||
CMD_LOAD_BUNNY,
|
||||
CMD_LOAD_SOFT_BODY,
|
||||
CMD_SEND_BULLET_DATA_STREAM,
|
||||
CMD_CREATE_BOX_COLLISION_SHAPE,
|
||||
CMD_CREATE_RIGID_BODY,
|
||||
|
||||
20
examples/pybullet/examples/load_soft_body.py
Normal file
20
examples/pybullet/examples/load_soft_body.py
Normal file
@@ -0,0 +1,20 @@
|
||||
import pybullet as p
|
||||
from time import sleep
|
||||
|
||||
physicsClient = p.connect(p.GUI)
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
planeId = p.loadURDF("plane.urdf")
|
||||
bunnyId = p.loadSoftBody("bunny.obj")
|
||||
|
||||
useRealTimeSimulation = 1
|
||||
|
||||
if (useRealTimeSimulation):
|
||||
p.setRealTimeSimulation(1)
|
||||
|
||||
while 1:
|
||||
if (useRealTimeSimulation):
|
||||
p.setGravity(0,0,-10)
|
||||
sleep(0.01) # Time in seconds.
|
||||
else:
|
||||
p.stepSimulation()
|
||||
@@ -1367,6 +1367,67 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
|
||||
return pylist;
|
||||
}
|
||||
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
// Load a softbody from an obj file
|
||||
static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int physicsClientId = 0;
|
||||
int flags = 0;
|
||||
|
||||
static char* kwlist[] = {"fileName", "scale", "mass", "collisionMargin", "physicsClientId", NULL};
|
||||
|
||||
int bodyUniqueId= -1;
|
||||
const char* fileName = "";
|
||||
double scale = -1;
|
||||
double mass = -1;
|
||||
double collisionMargin = -1;
|
||||
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &fileName, &scale, &mass, &collisionMargin, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (strlen(fileName))
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command =
|
||||
b3LoadSoftBodyCommandInit(sm, fileName);
|
||||
|
||||
if (scale>0)
|
||||
{
|
||||
b3LoadSoftBodySetScale(command,scale);
|
||||
}
|
||||
if (mass>0)
|
||||
{
|
||||
b3LoadSoftBodySetMass(command,mass);
|
||||
}
|
||||
if (collisionMargin>0)
|
||||
{
|
||||
b3LoadSoftBodySetCollisionMargin(command,collisionMargin);
|
||||
}
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType != CMD_CLIENT_COMMAND_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Cannot load soft body.");
|
||||
return NULL;
|
||||
}
|
||||
bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
|
||||
}
|
||||
return PyLong_FromLong(bodyUniqueId);
|
||||
}
|
||||
#endif
|
||||
|
||||
// Reset the simulation to remove all loaded objects
|
||||
static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
@@ -8227,7 +8288,10 @@ static PyMethodDef SpamMethods[] = {
|
||||
|
||||
{"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS,
|
||||
"Load multibodies from an SDF file."},
|
||||
|
||||
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
{"loadSoftBody", (PyCFunction)pybullet_loadSoftBody, METH_VARARGS | METH_KEYWORDS,
|
||||
"Load a softbody from an obj file."},
|
||||
#endif
|
||||
{"loadBullet", (PyCFunction)pybullet_loadBullet, METH_VARARGS | METH_KEYWORDS,
|
||||
"Load a world from a .bullet file."},
|
||||
|
||||
|
||||
Reference in New Issue
Block a user