From f5d7aac5ccfe589061ccb92cf3901447ecb379b6 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 17 May 2017 21:21:33 -0700 Subject: [PATCH 1/5] bump up pybullet to 1.0.6, prepare to tag Bullet 2.87 release --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 23041e881..0365485f3 100644 --- a/setup.py +++ b/setup.py @@ -417,7 +417,7 @@ else: setup( name = 'pybullet', - version='1.0.4', + version='1.0.6', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From 16a8dceb73d91524290854635fbb678410230e2c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 18 May 2017 08:31:40 -0700 Subject: [PATCH 2/5] fix warning --- examples/SharedMemory/PhysicsServerExample.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index f68036439..cf5f0c275 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -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 Date: Sun, 21 May 2017 06:50:53 -0700 Subject: [PATCH 3/5] The softbody/bunny test is very experimental and shouldn't be enabled in the example browser, unless USE_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD is defined. I fixed a few bugs that cause the hang. --- examples/ExampleBrowser/ExampleEntries.cpp | 7 ++++--- examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp | 3 ++- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 3 +++ 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index fb4128fcd..04b2112c3 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -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", diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index fd744cea9..a0d2c376c 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -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); } \ No newline at end of file diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index ea720de67..6f3cd63dd 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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; } From 4dcf2b82b6ef81891cfcd5e4a906284cfb6816e3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 21 May 2017 06:53:06 -0700 Subject: [PATCH 4/5] bump up pybullet to version 1.0.7 --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 0365485f3..e3f893a48 100644 --- a/setup.py +++ b/setup.py @@ -417,7 +417,7 @@ else: setup( name = 'pybullet', - version='1.0.6', + version='1.0.7', description='Official Python Interface for the Bullet Physics SDK Robotics Simulator', long_description='pybullet is an easy to use Python module for physics simulation, robotics and machine learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From fd40ba424b27952e5f7c7787acc151308a5f1e3c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 21 May 2017 11:00:15 -0700 Subject: [PATCH 5/5] test for knee joint limits under position control --- data/mjcf/humanoid_fixed.xml | 115 ++++++++++++++++++ .../humanoid_knee_position_control.py | 37 ++++++ 2 files changed, 152 insertions(+) create mode 100644 data/mjcf/humanoid_fixed.xml create mode 100644 examples/pybullet/examples/humanoid_knee_position_control.py diff --git a/data/mjcf/humanoid_fixed.xml b/data/mjcf/humanoid_fixed.xml new file mode 100644 index 000000000..94e1dd2a0 --- /dev/null +++ b/data/mjcf/humanoid_fixed.xml @@ -0,0 +1,115 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/examples/humanoid_knee_position_control.py b/examples/pybullet/examples/humanoid_knee_position_control.py new file mode 100644 index 000000000..f4398b7fa --- /dev/null +++ b/examples/pybullet/examples/humanoid_knee_position_control.py @@ -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() \ No newline at end of file