Load bunny through shared memory API and RobotSimAPI. Create grasp bunny example.
This commit is contained in:
@@ -267,6 +267,7 @@ static ExampleEntry gDefaultExamples[]=
|
|||||||
ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
|
ExampleEntry(1,"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,"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,"One Motor Gripper Grasp","Grasp experiment with a gripper with one motor to test slider constraint for closed loop structure", GripperGraspExampleCreateFunc, eONE_MOTOR_GRASP),
|
||||||
|
ExampleEntry(1,"Grasp Soft Body","Grasp soft body experiment", GripperGraspExampleCreateFunc, eGRASP_SOFT_BODY),
|
||||||
|
|
||||||
|
|
||||||
#ifdef ENABLE_LUA
|
#ifdef ENABLE_LUA
|
||||||
|
|||||||
@@ -217,7 +217,6 @@ public:
|
|||||||
|
|
||||||
if ((m_options & eONE_MOTOR_GRASP)!=0)
|
if ((m_options & eONE_MOTOR_GRASP)!=0)
|
||||||
{
|
{
|
||||||
|
|
||||||
{
|
{
|
||||||
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
|
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
|
||||||
slider.m_minVal=-2;
|
slider.m_minVal=-2;
|
||||||
@@ -232,7 +231,7 @@ public:
|
|||||||
slider.m_maxVal=1;
|
slider.m_maxVal=1;
|
||||||
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||||
}
|
}
|
||||||
if (0)
|
if (1)
|
||||||
{
|
{
|
||||||
b3RobotSimLoadFileArgs args("");
|
b3RobotSimLoadFileArgs args("");
|
||||||
b3RobotSimLoadFileResults results;
|
b3RobotSimLoadFileResults results;
|
||||||
@@ -288,49 +287,6 @@ public:
|
|||||||
}
|
}
|
||||||
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
|
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
|
||||||
|
|
||||||
/*
|
|
||||||
b3JointInfo sliderJoint1;
|
|
||||||
sliderJoint1.m_parentFrame[0] = 0;
|
|
||||||
sliderJoint1.m_parentFrame[1] = 0;
|
|
||||||
sliderJoint1.m_parentFrame[2] = 0.06;
|
|
||||||
sliderJoint1.m_parentFrame[3] = 0;
|
|
||||||
sliderJoint1.m_parentFrame[4] = 0;
|
|
||||||
sliderJoint1.m_parentFrame[5] = 0;
|
|
||||||
sliderJoint1.m_parentFrame[6] = 1.0;
|
|
||||||
sliderJoint1.m_childFrame[0] = 0;
|
|
||||||
sliderJoint1.m_childFrame[1] = 0;
|
|
||||||
sliderJoint1.m_childFrame[2] = 0;
|
|
||||||
sliderJoint1.m_childFrame[3] = 0;
|
|
||||||
sliderJoint1.m_childFrame[4] = 0;
|
|
||||||
sliderJoint1.m_childFrame[5] = 0;
|
|
||||||
sliderJoint1.m_childFrame[6] = 1.0;
|
|
||||||
sliderJoint1.m_jointAxis[0] = 1.0;
|
|
||||||
sliderJoint1.m_jointAxis[1] = 0.0;
|
|
||||||
sliderJoint1.m_jointAxis[2] = 0.0;
|
|
||||||
sliderJoint1.m_jointType = ePrismaticType;
|
|
||||||
b3JointInfo sliderJoint2;
|
|
||||||
sliderJoint2.m_parentFrame[0] = 0;
|
|
||||||
sliderJoint2.m_parentFrame[1] = 0;
|
|
||||||
sliderJoint2.m_parentFrame[2] = 0.06;
|
|
||||||
sliderJoint2.m_parentFrame[3] = 0;
|
|
||||||
sliderJoint2.m_parentFrame[4] = 0;
|
|
||||||
sliderJoint2.m_parentFrame[5] = 0;
|
|
||||||
sliderJoint2.m_parentFrame[6] = 1.0;
|
|
||||||
sliderJoint2.m_childFrame[0] = 0;
|
|
||||||
sliderJoint2.m_childFrame[1] = 0;
|
|
||||||
sliderJoint2.m_childFrame[2] = 0;
|
|
||||||
sliderJoint2.m_childFrame[3] = 0;
|
|
||||||
sliderJoint2.m_childFrame[4] = 0;
|
|
||||||
sliderJoint2.m_childFrame[5] = 1.0;
|
|
||||||
sliderJoint2.m_childFrame[6] = 0;
|
|
||||||
sliderJoint2.m_jointAxis[0] = 1.0;
|
|
||||||
sliderJoint2.m_jointAxis[1] = 0.0;
|
|
||||||
sliderJoint2.m_jointAxis[2] = 0.0;
|
|
||||||
sliderJoint2.m_jointType = ePrismaticType;
|
|
||||||
m_robotSim.createJoint(1, 0, 1, 3, &sliderJoint1);
|
|
||||||
m_robotSim.createJoint(1, 0, 1, 6, &sliderJoint2);
|
|
||||||
*/
|
|
||||||
/*
|
|
||||||
b3JointInfo revoluteJoint1;
|
b3JointInfo revoluteJoint1;
|
||||||
revoluteJoint1.m_parentFrame[0] = -0.055;
|
revoluteJoint1.m_parentFrame[0] = -0.055;
|
||||||
revoluteJoint1.m_parentFrame[1] = 0;
|
revoluteJoint1.m_parentFrame[1] = 0;
|
||||||
@@ -371,9 +327,112 @@ public:
|
|||||||
revoluteJoint2.m_jointType = ePoint2PointType;
|
revoluteJoint2.m_jointType = ePoint2PointType;
|
||||||
m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1);
|
m_robotSim.createJoint(1, 2, 1, 4, &revoluteJoint1);
|
||||||
m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2);
|
m_robotSim.createJoint(1, 3, 1, 6, &revoluteJoint2);
|
||||||
*/
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if ((m_options & eGRASP_SOFT_BODY)!=0)
|
||||||
|
{
|
||||||
|
{
|
||||||
|
SliderParams slider("Vertical velocity",&sGripperVerticalVelocity);
|
||||||
|
slider.m_minVal=-2;
|
||||||
|
slider.m_maxVal=2;
|
||||||
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||||
|
}
|
||||||
|
|
||||||
|
{
|
||||||
|
SliderParams slider("Closing velocity",&sGripperClosingTargetVelocity
|
||||||
|
);
|
||||||
|
slider.m_minVal=-1;
|
||||||
|
slider.m_maxVal=1;
|
||||||
|
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
|
||||||
|
}
|
||||||
|
if (1)
|
||||||
|
{
|
||||||
|
b3RobotSimLoadFileArgs args("");
|
||||||
|
args.m_fileName = "gripper/wsg50_one_motor_gripper_new.sdf";
|
||||||
|
args.m_fileType = B3_SDF_FILE;
|
||||||
|
args.m_useMultiBody = true;
|
||||||
|
b3RobotSimLoadFileResults results;
|
||||||
|
|
||||||
|
if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
|
||||||
|
{
|
||||||
|
|
||||||
|
m_gripperIndex = results.m_uniqueObjectIds[0];
|
||||||
|
int numJoints = m_robotSim.getNumJoints(m_gripperIndex);
|
||||||
|
b3Printf("numJoints = %d",numJoints);
|
||||||
|
|
||||||
|
for (int i=0;i<numJoints;i++)
|
||||||
|
{
|
||||||
|
b3JointInfo jointInfo;
|
||||||
|
m_robotSim.getJointInfo(m_gripperIndex,i,&jointInfo);
|
||||||
|
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int i=0;i<8;i++)
|
||||||
|
{
|
||||||
|
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
|
||||||
|
controlArgs.m_maxTorqueValue = 0.0;
|
||||||
|
m_robotSim.setJointMotorControl(m_gripperIndex,i,controlArgs);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (1)
|
||||||
|
{
|
||||||
|
b3RobotSimLoadFileArgs args("");
|
||||||
|
args.m_fileName = "plane.urdf";
|
||||||
|
args.m_startPosition.setValue(0,0,0);
|
||||||
|
args.m_startOrientation.setEulerZYX(0,0,0);
|
||||||
|
args.m_forceOverrideFixedBase = true;
|
||||||
|
args.m_useMultiBody = true;
|
||||||
|
b3RobotSimLoadFileResults results;
|
||||||
|
m_robotSim.loadFile(args,results);
|
||||||
|
|
||||||
|
}
|
||||||
|
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
|
||||||
|
m_robotSim.loadBunny();
|
||||||
|
|
||||||
|
b3JointInfo revoluteJoint1;
|
||||||
|
revoluteJoint1.m_parentFrame[0] = -0.055;
|
||||||
|
revoluteJoint1.m_parentFrame[1] = 0;
|
||||||
|
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;
|
||||||
|
revoluteJoint1.m_childFrame[3] = 0;
|
||||||
|
revoluteJoint1.m_childFrame[4] = 0;
|
||||||
|
revoluteJoint1.m_childFrame[5] = 0;
|
||||||
|
revoluteJoint1.m_childFrame[6] = 1.0;
|
||||||
|
revoluteJoint1.m_jointAxis[0] = 1.0;
|
||||||
|
revoluteJoint1.m_jointAxis[1] = 0.0;
|
||||||
|
revoluteJoint1.m_jointAxis[2] = 0.0;
|
||||||
|
revoluteJoint1.m_jointType = ePoint2PointType;
|
||||||
|
b3JointInfo revoluteJoint2;
|
||||||
|
revoluteJoint2.m_parentFrame[0] = 0.055;
|
||||||
|
revoluteJoint2.m_parentFrame[1] = 0;
|
||||||
|
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;
|
||||||
|
revoluteJoint2.m_childFrame[3] = 0;
|
||||||
|
revoluteJoint2.m_childFrame[4] = 0;
|
||||||
|
revoluteJoint2.m_childFrame[5] = 0;
|
||||||
|
revoluteJoint2.m_childFrame[6] = 1.0;
|
||||||
|
revoluteJoint2.m_jointAxis[0] = 1.0;
|
||||||
|
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);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
virtual void exitPhysics()
|
virtual void exitPhysics()
|
||||||
{
|
{
|
||||||
@@ -402,6 +461,22 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
if ((m_options & eONE_MOTOR_GRASP)!=0)
|
if ((m_options & eONE_MOTOR_GRASP)!=0)
|
||||||
|
{
|
||||||
|
int fingerJointIndices[2]={0,1};
|
||||||
|
double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
|
||||||
|
};
|
||||||
|
double maxTorqueValues[2]={50.0,50.0};
|
||||||
|
for (int i=0;i<2;i++)
|
||||||
|
{
|
||||||
|
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
|
||||||
|
controlArgs.m_targetVelocity = fingerTargetVelocities[i];
|
||||||
|
controlArgs.m_maxTorqueValue = maxTorqueValues[i];
|
||||||
|
controlArgs.m_kd = 1.;
|
||||||
|
m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((m_options & eGRASP_SOFT_BODY)!=0)
|
||||||
{
|
{
|
||||||
int fingerJointIndices[2]={0,1};
|
int fingerJointIndices[2]={0,1};
|
||||||
double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
|
double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity
|
||||||
|
|||||||
@@ -21,6 +21,7 @@ enum GripperGraspExampleOptions
|
|||||||
eGRIPPER_GRASP=1,
|
eGRIPPER_GRASP=1,
|
||||||
eTWO_POINT_GRASP=2,
|
eTWO_POINT_GRASP=2,
|
||||||
eONE_MOTOR_GRASP=4,
|
eONE_MOTOR_GRASP=4,
|
||||||
|
eGRASP_SOFT_BODY=8,
|
||||||
};
|
};
|
||||||
|
|
||||||
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
|
class CommonExampleInterface* GripperGraspExampleCreateFunc(struct CommonExampleOptions& options);
|
||||||
|
|||||||
@@ -256,7 +256,7 @@ public:
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
//m_robotSim.stepSimulation();
|
m_robotSim.stepSimulation();
|
||||||
}
|
}
|
||||||
virtual void renderScene()
|
virtual void renderScene()
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -999,3 +999,9 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP
|
|||||||
worldOrientation[3] = linkState.m_worldOrientation[3];
|
worldOrientation[3] = linkState.m_worldOrientation[3];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void b3RobotSimAPI::loadBunny()
|
||||||
|
{
|
||||||
|
b3SharedMemoryCommandHandle command = b3LoadBunnyCommandInit(m_data->m_physicsClient);
|
||||||
|
b3SubmitClientCommand(m_data->m_physicsClient, command);
|
||||||
|
}
|
||||||
@@ -164,6 +164,8 @@ public:
|
|||||||
void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
|
void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian);
|
||||||
|
|
||||||
void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
|
void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation);
|
||||||
|
|
||||||
|
void loadBunny();
|
||||||
};
|
};
|
||||||
|
|
||||||
#endif //B3_ROBOT_SIM_API_H
|
#endif //B3_ROBOT_SIM_API_H
|
||||||
|
|||||||
@@ -55,6 +55,20 @@ b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
|
|||||||
return (b3SharedMemoryCommandHandle) command;
|
return (b3SharedMemoryCommandHandle) command;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
|
||||||
|
{
|
||||||
|
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||||
|
b3Assert(cl);
|
||||||
|
b3Assert(cl->canSubmitCommand());
|
||||||
|
|
||||||
|
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||||
|
b3Assert(command);
|
||||||
|
command->m_type = CMD_LOAD_BUNNY;
|
||||||
|
command->m_updateFlags = 0;
|
||||||
|
|
||||||
|
return (b3SharedMemoryCommandHandle) command;
|
||||||
|
}
|
||||||
|
|
||||||
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
|
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
|
||||||
{
|
{
|
||||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
|||||||
@@ -215,6 +215,8 @@ b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandl
|
|||||||
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
|
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
|
||||||
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
|
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
|
||||||
|
|
||||||
|
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
|
||||||
|
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@@ -605,23 +605,14 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
|||||||
//m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
|
//m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration();
|
||||||
//m_collisionConfiguration->setConvexConvexMultipointIterations();
|
//m_collisionConfiguration->setConvexConvexMultipointIterations();
|
||||||
m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
|
m_data->m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
|
||||||
|
|
||||||
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
|
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
|
||||||
m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration);
|
m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration);
|
||||||
|
|
||||||
m_data->m_softBodyWorldInfo.m_dispatcher = m_data->m_dispatcher;
|
m_data->m_broadphase = new btDbvtBroadphase();
|
||||||
|
|
||||||
//m_data->m_broadphase = new btDbvtBroadphase();
|
|
||||||
btVector3 worldAabbMin(-1000,-1000,-1000);
|
|
||||||
btVector3 worldAabbMax(1000,1000,1000);
|
|
||||||
int maxProxies = 32766;
|
|
||||||
m_data->m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
|
|
||||||
|
|
||||||
m_data->m_solver = new btMultiBodyConstraintSolver;
|
m_data->m_solver = new btMultiBodyConstraintSolver;
|
||||||
|
|
||||||
m_data->m_softbodySolver = 0;
|
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
|
||||||
|
|
||||||
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration, m_data->m_softbodySolver);
|
|
||||||
|
|
||||||
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
|
//Workaround: in a VR application, where we avoid synchronizaing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
|
||||||
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
|
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(8192);
|
||||||
@@ -629,36 +620,9 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
|
|||||||
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
|
m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer();
|
||||||
|
|
||||||
|
|
||||||
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
|
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
|
||||||
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
|
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
|
||||||
|
|
||||||
m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2;
|
|
||||||
m_data->m_softBodyWorldInfo.water_density = 0;
|
|
||||||
m_data->m_softBodyWorldInfo.water_offset = 0;
|
|
||||||
m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0);
|
|
||||||
m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10);
|
|
||||||
|
|
||||||
m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase;
|
|
||||||
m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize();
|
|
||||||
|
|
||||||
btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny,
|
|
||||||
&gIndicesBunny[0][0],
|
|
||||||
BUNNY_NUM_TRIANGLES);
|
|
||||||
btSoftBody::Material* pm=psb->appendMaterial();
|
|
||||||
pm->m_kLST = 1.0
|
|
||||||
;
|
|
||||||
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
|
|
||||||
psb->generateBendingConstraints(2,pm);
|
|
||||||
psb->m_cfg.piterations = 2;
|
|
||||||
psb->m_cfg.kDF = 0.5;
|
|
||||||
psb->randomizeConstraints();
|
|
||||||
psb->rotate(btQuaternion(0.70711,0,0,0.70711));
|
|
||||||
psb->translate(btVector3(0,0,3.0));
|
|
||||||
psb->scale(btVector3(0.1,0.1,0.1));
|
|
||||||
psb->setTotalMass(1,true);
|
|
||||||
psb->getCollisionShape()->setMargin(0.01);
|
|
||||||
m_data->m_dynamicsWorld->addSoftBody(psb);
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
|
void PhysicsServerCommandProcessor::deleteCachedInverseDynamicsBodies()
|
||||||
@@ -1464,6 +1428,35 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
case CMD_LOAD_BUNNY:
|
||||||
|
{
|
||||||
|
m_data->m_softBodyWorldInfo.air_density = (btScalar)1.2;
|
||||||
|
m_data->m_softBodyWorldInfo.water_density = 0;
|
||||||
|
m_data->m_softBodyWorldInfo.water_offset = 0;
|
||||||
|
m_data->m_softBodyWorldInfo.water_normal = btVector3(0,0,0);
|
||||||
|
m_data->m_softBodyWorldInfo.m_gravity.setValue(0,0,-10);
|
||||||
|
m_data->m_softBodyWorldInfo.m_broadphase = m_data->m_broadphase;
|
||||||
|
m_data->m_softBodyWorldInfo.m_sparsesdf.Initialize();
|
||||||
|
|
||||||
|
btSoftBody* psb=btSoftBodyHelpers::CreateFromTriMesh(m_data->m_softBodyWorldInfo,gVerticesBunny, &gIndicesBunny[0][0], BUNNY_NUM_TRIANGLES);
|
||||||
|
|
||||||
|
btSoftBody::Material* pm=psb->appendMaterial();
|
||||||
|
pm->m_kLST = 1.0;
|
||||||
|
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
|
||||||
|
psb->generateBendingConstraints(2,pm);
|
||||||
|
psb->m_cfg.piterations = 2;
|
||||||
|
psb->m_cfg.kDF = 0.5;
|
||||||
|
psb->randomizeConstraints();
|
||||||
|
psb->rotate(btQuaternion(0.70711,0,0,0.70711));
|
||||||
|
psb->translate(btVector3(0,0,3.0));
|
||||||
|
psb->scale(btVector3(0.1,0.1,0.1));
|
||||||
|
psb->setTotalMass(1,true);
|
||||||
|
psb->getCollisionShape()->setMargin(0.01);
|
||||||
|
|
||||||
|
m_data->m_dynamicsWorld->addSoftBody(psb);
|
||||||
|
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
case CMD_CREATE_SENSOR:
|
case CMD_CREATE_SENSOR:
|
||||||
@@ -2813,7 +2806,7 @@ void PhysicsServerCommandProcessor::renderScene()
|
|||||||
btSoftBody* psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
|
btSoftBody* psb=(btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
|
||||||
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
|
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
|
||||||
{
|
{
|
||||||
btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
|
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
|
||||||
btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags());
|
btSoftBodyHelpers::Draw(psb,m_data->m_dynamicsWorld->getDebugDrawer(),m_data->m_dynamicsWorld->getDrawFlags());
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -7,6 +7,7 @@ enum EnumSharedMemoryClientCommand
|
|||||||
{
|
{
|
||||||
CMD_LOAD_SDF,
|
CMD_LOAD_SDF,
|
||||||
CMD_LOAD_URDF,
|
CMD_LOAD_URDF,
|
||||||
|
CMD_LOAD_BUNNY,
|
||||||
CMD_SEND_BULLET_DATA_STREAM,
|
CMD_SEND_BULLET_DATA_STREAM,
|
||||||
CMD_CREATE_BOX_COLLISION_SHAPE,
|
CMD_CREATE_BOX_COLLISION_SHAPE,
|
||||||
// CMD_DELETE_BOX_COLLISION_SHAPE,
|
// CMD_DELETE_BOX_COLLISION_SHAPE,
|
||||||
|
|||||||
Reference in New Issue
Block a user