Merge pull request #1135 from erwincoumans/master
disable Grasp Soft Body/SoftBody coupling demo unless USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD is defined, fixed related hang in experimental 'loadBunny' command
This commit is contained in:
@@ -277,9 +277,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),
|
||||
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 USE_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
|
||||
|
||||
#ifdef ENABLE_LUA
|
||||
ExampleEntry(1,"Lua Script", "Create the dynamics world, collision shapes and rigid bodies using Lua scripting",
|
||||
|
||||
@@ -290,6 +290,7 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
|
||||
b3Warning("Not connected");
|
||||
return;
|
||||
}
|
||||
b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
|
||||
|
||||
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
@@ -1142,5 +1143,5 @@ void b3RobotSimulatorClientAPI::loadBunny(double scale, double mass, double coll
|
||||
b3LoadBunnySetScale(command, scale);
|
||||
b3LoadBunnySetMass(command, mass);
|
||||
b3LoadBunnySetCollisionMargin(command, collisionMargin);
|
||||
b3SubmitClientCommand(m_data->m_physicsClientHandle, command);
|
||||
b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
|
||||
}
|
||||
@@ -3247,6 +3247,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
case CMD_LOAD_BUNNY:
|
||||
{
|
||||
serverStatusOut.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
|
||||
hasStatus = true;
|
||||
#ifdef USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
|
||||
double scale = 0.1;
|
||||
double mass = 0.1;
|
||||
@@ -3287,6 +3289,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
psb->getCollisionShape()->setMargin(collisionMargin);
|
||||
|
||||
m_data->m_dynamicsWorld->addSoftBody(psb);
|
||||
serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -238,8 +238,9 @@ struct MyMouseCommand
|
||||
struct MotionArgs
|
||||
{
|
||||
MotionArgs()
|
||||
:m_physicsServerPtr(0),
|
||||
m_debugDrawFlags(0)
|
||||
:
|
||||
m_debugDrawFlags(0),
|
||||
m_physicsServerPtr(0)
|
||||
{
|
||||
for (int i=0;i<MAX_VR_CONTROLLERS;i++)
|
||||
{
|
||||
|
||||
37
examples/pybullet/examples/humanoid_knee_position_control.py
Normal file
37
examples/pybullet/examples/humanoid_knee_position_control.py
Normal file
@@ -0,0 +1,37 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
|
||||
cid = p.connect(p.SHARED_MEMORY)
|
||||
if (cid<0):
|
||||
cid = p.connect(p.GUI)
|
||||
|
||||
p.resetSimulation()
|
||||
|
||||
useRealTime = 0
|
||||
|
||||
p.setRealTimeSimulation(useRealTime)
|
||||
|
||||
p.setGravity(0,0,0)
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
|
||||
obUids = p.loadMJCF("mjcf/humanoid_fixed.xml")
|
||||
human = obUids[0]
|
||||
|
||||
|
||||
|
||||
for i in range (p.getNumJoints(human)):
|
||||
p.setJointMotorControl2(human,i,p.POSITION_CONTROL,targetPosition=0,force=50)
|
||||
|
||||
kneeAngleTargetId = p.addUserDebugParameter("kneeAngle",-4,4,-1)
|
||||
maxForceId = p.addUserDebugParameter("maxForce",0,100,10)
|
||||
|
||||
kneeJointIndex=11
|
||||
|
||||
while (1):
|
||||
time.sleep(0.01)
|
||||
kneeAngleTarget = p.readUserDebugParameter(kneeAngleTargetId)
|
||||
maxForce = p.readUserDebugParameter(maxForceId)
|
||||
p.setJointMotorControl2(human,kneeJointIndex,p.POSITION_CONTROL,targetPosition=kneeAngleTarget,force=maxForce)
|
||||
if (useRealTime==0):
|
||||
p.stepSimulation()
|
||||
Reference in New Issue
Block a user