From 91d164d88638d290e47c90519a34db4a06b75ca3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 9 Oct 2017 09:44:54 -0700 Subject: [PATCH 1/6] remove obsolete CartPoleBulletEnv --- data/multibody.bullet | Bin 14844 -> 14844 bytes .../pybullet/gym/pybullet_envs/__init__.py | 7 -- .../gym/pybullet_envs/bullet/__init__.py | 1 - .../pybullet_envs/bullet/cartpole_bullet.py | 101 ------------------ setup.py | 2 +- 5 files changed, 1 insertion(+), 110 deletions(-) delete mode 100644 examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py diff --git a/data/multibody.bullet b/data/multibody.bullet index e57061f7ae941a64931368b8fc823d88f3592bad..ebec1dad39795e8d109a00d09b277549fc337556 100644 GIT binary patch delta 392 zcmexU{HJ)r7FI3>1_tiQ6Im=LeyS4Vx~^;kRE8fT|f7KuZ4ufdoiB5P;Yqz`07{!0CJydyxZ< z`$6LA7C-mL#Z0%4mJr+v;(xf3{)2I{BC|FhGmrhmjD0& delta 404 zcmexU{HJ)rmdPTVHWNS8_zO6MZQ54P00RL~Mu0E_M4SgI-~dy>1Y>~dfNWXS?BDDR z7g!kzfG}tBZbs?J{~2Y3I2a-d`4;|1GI{bxMv2KXOe)$SjSgVlKmye`283}z4519J zKs~aP=P+_itl^r(B*n@M)W8fBlbpPdg=6w9CMk6s=Ab#Ncyc4FxsfVEgCoRK6Hq

L7kyAIX^cyF)wB6Q3nOqNenzdtHebEQlr6MoWKrN1tBJHU{*0LaDo`e0+n!p pDS&$FTnbntgF!NgHh_f4iS?8Cyr%)!9AIh- diff --git a/examples/pybullet/gym/pybullet_envs/__init__.py b/examples/pybullet/gym/pybullet_envs/__init__.py index 93b55108d..74b9b7c9b 100644 --- a/examples/pybullet/gym/pybullet_envs/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/__init__.py @@ -2,13 +2,6 @@ from gym.envs.registration import registry, register, make, spec # ------------bullet------------- -register( - id='CartPoleBulletEnv-v0', - entry_point='pybullet_envs.bullet:CartPoleBulletEnv', - timestep_limit=1000, - reward_threshold=950.0, -) - register( id='MinitaurBulletEnv-v0', entry_point='pybullet_envs.bullet:MinitaurBulletEnv', diff --git a/examples/pybullet/gym/pybullet_envs/bullet/__init__.py b/examples/pybullet/gym/pybullet_envs/bullet/__init__.py index 8b1735fb1..682266d85 100644 --- a/examples/pybullet/gym/pybullet_envs/bullet/__init__.py +++ b/examples/pybullet/gym/pybullet_envs/bullet/__init__.py @@ -1,4 +1,3 @@ -from pybullet_envs.bullet.cartpole_bullet import CartPoleBulletEnv from pybullet_envs.bullet.minitaur_gym_env import MinitaurBulletEnv from pybullet_envs.bullet.racecarGymEnv import RacecarGymEnv from pybullet_envs.bullet.racecarZEDGymEnv import RacecarZEDGymEnv diff --git a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py b/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py deleted file mode 100644 index e28a9e962..000000000 --- a/examples/pybullet/gym/pybullet_envs/bullet/cartpole_bullet.py +++ /dev/null @@ -1,101 +0,0 @@ -""" -Classic cart-pole system implemented by Rich Sutton et al. -Copied from https://webdocs.cs.ualberta.ca/~sutton/book/code/pole.c -""" -import os, inspect -currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) -parentdir = os.path.dirname(os.path.dirname(currentdir)) -os.sys.path.insert(0,parentdir) - -import logging -import math -import gym -from gym import spaces -from gym.utils import seeding -import numpy as np -import time -import subprocess -import pybullet as p -import pybullet_data - - -logger = logging.getLogger(__name__) - -class CartPoleBulletEnv(gym.Env): - metadata = { - 'render.modes': ['human', 'rgb_array'], - 'video.frames_per_second' : 50 - } - - def __init__(self, renders=True): - # start the bullet physics server - self._renders = renders - if (renders): - p.connect(p.GUI) - else: - p.connect(p.DIRECT) - - observation_high = np.array([ - np.finfo(np.float32).max, - np.finfo(np.float32).max, - np.finfo(np.float32).max, - np.finfo(np.float32).max]) - action_high = np.array([0.1]) - - self.action_space = spaces.Discrete(9) - self.observation_space = spaces.Box(-observation_high, observation_high) - - self.theta_threshold_radians = 1 - self.x_threshold = 2.4 - self._seed() -# self.reset() - self.viewer = None - self._configure() - - def _configure(self, display=None): - self.display = display - - def _seed(self, seed=None): - self.np_random, seed = seeding.np_random(seed) - return [seed] - - def _step(self, action): - p.stepSimulation() -# time.sleep(self.timeStep) - self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] - theta, theta_dot, x, x_dot = self.state - - dv = 0.1 - deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action] - - p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3])) - - done = x < -self.x_threshold \ - or x > self.x_threshold \ - or theta < -self.theta_threshold_radians \ - or theta > self.theta_threshold_radians - reward = 1.0 - - return np.array(self.state), reward, done, {} - - def _reset(self): -# print("-----------reset simulation---------------") - p.resetSimulation() - self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0]) - self.timeStep = 0.01 - p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0) - p.setGravity(0,0, -10) - p.setTimeStep(self.timeStep) - p.setRealTimeSimulation(0) - - initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) - initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,)) - p.resetJointState(self.cartpole, 1, initialAngle) - p.resetJointState(self.cartpole, 0, initialCartPos) - - self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2] - - return np.array(self.state) - - def _render(self, mode='human', close=False): - return diff --git a/setup.py b/setup.py index 0321e9658..612404491 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.5.1', + version='1.5.5', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement 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 d48e2a20af96746b6bee54a3d232c0244b1f4dc9 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 9 Oct 2017 12:28:39 -0700 Subject: [PATCH 2/6] fix a string check issue --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 204d5e479..568901ace 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4493,7 +4493,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm //store needed info for tinyrenderer visualHandle->m_localInertiaFrame = localInertiaFrame; visualHandle->m_visualShape = visualShape; - visualHandle->m_pathPrefix = pathPrefix ? pathPrefix : ""; + visualHandle->m_pathPrefix = pathPrefix[0] ? pathPrefix : ""; serverStatusOut.m_createUserShapeResultArgs.m_userShapeUniqueId = visualShapeUniqueId; serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_COMPLETED; From e5d4c73c4040e5df78a170e52a6c13a7f24fffea Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 9 Oct 2017 22:40:53 -0700 Subject: [PATCH 3/6] fix very rare threading issue, let main thread compute the UID for user debug items add collision model to cartpole.urdf --- data/cartpole.urdf | 6 ++++++ examples/SharedMemory/PhysicsServerExample.cpp | 15 ++++++++++----- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/data/cartpole.urdf b/data/cartpole.urdf index c22ca920c..e78b066c1 100644 --- a/data/cartpole.urdf +++ b/data/cartpole.urdf @@ -62,6 +62,12 @@ + + + + + + diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 3ab943e3c..3c9b4a70c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1131,7 +1131,7 @@ public: btAlignedObjectArray m_userDebugText; UserDebugText m_tmpText; - + int m_resultUserDebugTextUid; virtual int addUserDebugText3D( const char* txt, const double positionXYZ[3], const double orientation[4], const double textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags) { @@ -1165,7 +1165,7 @@ public: m_cs->setSharedParam(1, eGUIUserDebugAddText); workerThreadWait(); - return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId; + return m_resultUserDebugTextUid; } btAlignedObjectArray m_userDebugParams; @@ -1183,6 +1183,7 @@ public: } return 0; } + int m_userDebugParamUid; virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue) { @@ -1196,13 +1197,13 @@ public: m_cs->setSharedParam(1, eGUIUserDebugAddParameter); workerThreadWait(); - return (*m_userDebugParams[m_userDebugParams.size()-1]).m_itemUniqueId; + return m_userDebugParamUid; } btAlignedObjectArray m_userDebugLines; UserDebugDrawLine m_tmpLine; - + int m_resultDebugLineUid; virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex) { m_tmpLine.m_lifeTime = lifeTime; @@ -1223,7 +1224,7 @@ public: m_cs->lock(); m_cs->setSharedParam(1, eGUIUserDebugAddLine); workerThreadWait(); - return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId; + return m_resultDebugLineUid; } int m_removeDebugItemUid; @@ -2171,6 +2172,7 @@ void PhysicsServerExample::updateGraphics() B3_PROFILE("eGUIUserDebugAddText"); m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText); + m_multiThreadedHelper->m_resultUserDebugTextUid = m_multiThreadedHelper->m_userDebugText[m_multiThreadedHelper->m_userDebugText.size()-1].m_itemUniqueId; m_multiThreadedHelper->mainThreadRelease(); break; } @@ -2190,6 +2192,8 @@ void PhysicsServerExample::updateGraphics() m_multiThreadedHelper->m_childGuiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } + m_multiThreadedHelper->m_userDebugParamUid = (*m_multiThreadedHelper->m_userDebugParams[m_multiThreadedHelper->m_userDebugParams.size()-1]).m_itemUniqueId; + //also add actual menu m_multiThreadedHelper->mainThreadRelease(); break; @@ -2199,6 +2203,7 @@ void PhysicsServerExample::updateGraphics() B3_PROFILE("eGUIUserDebugAddLine"); m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); + m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId; m_multiThreadedHelper->mainThreadRelease(); break; } From 7bddc7706d7ba330b59f14bbcc12d1728024cb0b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 10 Oct 2017 09:08:31 -0700 Subject: [PATCH 4/6] bump up pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 612404491..aacca7f02 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.5.5', + version='1.5.6', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement 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 c155e105a52f1768df8251e1da132b61991382b7 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 10 Oct 2017 11:10:42 -0700 Subject: [PATCH 5/6] unlock thread at exitPhysics pybullet: don't crash in inverse kinematic if #dofs don't match due to free base C-API: don't crash if status/statusHandle = 0 --- .../InProcessExampleBrowser.cpp | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 63 +++++-- .../PhysicsServerCommandProcessor.cpp | 155 +++++++++--------- .../SharedMemory/PhysicsServerExample.cpp | 3 +- 4 files changed, 128 insertions(+), 95 deletions(-) diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 7b42da5be..caf32a51a 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -391,7 +391,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data) } else { // printf("polling.."); - b3Clock::usleep(1000); + b3Clock::usleep(0); } }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d8de6b489..58a251949 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1654,6 +1654,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3Ph B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + btAssert(status); + if (status==0) + return 0; const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs; btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED); if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED) @@ -1695,6 +1698,9 @@ B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle const double* actualStateQdot[], const double* jointReactionForces[]) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + btAssert(status); + if (status==0) + return 0; const SendActualStateArgs &args = status->m_sendActualStateArgs; btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED); if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) @@ -1910,6 +1916,8 @@ B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle stat const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; b3Assert(status); + if (status==0) + return statusUniqueId; b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) { @@ -1924,10 +1932,13 @@ B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHan const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; b3Assert(status); - b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); - if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) + if (status) { - statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId; + b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); + if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) + { + statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId; + } } return statusUniqueId; } @@ -2317,12 +2328,14 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); - b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED); - if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED) + if (status) { - return status->m_userConstraintResultArgs.m_userConstraintUniqueId; + b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED); + if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED) + { + return status->m_userConstraintResultArgs.m_userConstraintUniqueId; + } } - return -1; } @@ -2635,11 +2648,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; - btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED); - if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED)) + if (status) { - *paramValue = status->m_userDebugDrawArgs.m_parameterValue; - return 1; + btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED); + if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED)) + { + *paramValue = status->m_userDebugDrawArgs.m_parameterValue; + return 1; + } } return 0; } @@ -3280,10 +3296,13 @@ B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHa { int uid = -1; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; - btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED); - if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED) + if (status) { - uid = status->m_loadTextureResultArguments.m_textureUniqueId; + btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED); + if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED) + { + uid = status->m_loadTextureResultArguments.m_textureUniqueId; + } } return uid; } @@ -3420,6 +3439,9 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand double* jointForces) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status==0) + return false; + btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED); if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) return false; @@ -3475,6 +3497,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status==0) + return false; + btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED); if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED) return false; @@ -3528,6 +3553,9 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar b3Assert(cl); const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status==0) + return false; + btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED); if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED) return false; @@ -3661,9 +3689,12 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu double* jointPositions) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status); + if (status==0) + return 0; btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED); if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED) - return false; + return 0; if (dofCount) @@ -3682,7 +3713,7 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu } } - return true; + return 1; } B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 4053158e3..6fbce838b 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -7659,7 +7659,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray q_current; q_current.resize(numDofs); - if (tree) + if (tree && (numDofs == tree->numDoFs())) { jacSize = jacobian_linear.size(); // Set jacobian value @@ -7697,92 +7697,93 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - } - btAlignedObjectArray q_new; - q_new.resize(numDofs); - int ikMethod = 0; - if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) - { - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; - } - else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) - { - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; - } - else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; - } - else - { - ikMethod = IK2_VEL_DLS; - } - if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray rest_pose; - lower_limit.resize(numDofs); - upper_limit.resize(numDofs); - joint_range.resize(numDofs); - rest_pose.resize(numDofs); - for (int i = 0; i < numDofs; ++i) - { - lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; - upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; - joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; - rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; - } - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); - } + btAlignedObjectArray q_new; + q_new.resize(numDofs); + int ikMethod = 0; + if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; + } + else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + } + else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; + } + else + { + ikMethod = IK2_VEL_DLS; + } - btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); + if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + rest_pose.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; + upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; + joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; + rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + } + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); + } + + btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); - btVector3DoubleData endEffectorWorldPosition; - btVector3DoubleData endEffectorWorldOrientation; + btVector3DoubleData endEffectorWorldPosition; + btVector3DoubleData endEffectorWorldOrientation; - btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin(); - btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation(); - btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); + btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin(); + btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation(); + btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); - endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); - endEffectorOri.serializeDouble(endEffectorWorldOrientation); + endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); + endEffectorOri.serializeDouble(endEffectorWorldOrientation); - // Set joint damping coefficents. A small default - // damping constant is added to prevent singularity - // with pseudo inverse. The user can set joint damping - // coefficients differently for each joint. The larger - // the damping coefficient is, the less we rely on - // this joint to achieve the IK target. - btAlignedObjectArray joint_damping; - joint_damping.resize(numDofs,0.5); - if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) - { - for (int i = 0; i < numDofs; ++i) - { - joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; - } - } - ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); + // Set joint damping coefficents. A small default + // damping constant is added to prevent singularity + // with pseudo inverse. The user can set joint damping + // coefficients differently for each joint. The larger + // the damping coefficient is, the less we rely on + // this joint to achieve the IK target. + btAlignedObjectArray joint_damping; + joint_damping.resize(numDofs,0.5); + if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) + { + for (int i = 0; i < numDofs; ++i) + { + joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + } + } + ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); - double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; - ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, - endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - &q_current[0], - numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); + double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; + ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, + endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, + &q_current[0], + numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - for (int i=0;iunlock(); m_args[i].m_cs->lock(); m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion); m_args[i].m_cs->unlock(); @@ -1805,7 +1806,7 @@ void PhysicsServerExample::exitPhysics() } else { - b3Clock::usleep(1000); + b3Clock::usleep(0); } //we need to call 'stepSimulation' to make sure that //other threads get out of blocking state (workerThreadWait) From 291afebad800696ab56a62964ac0ac8d5004adbb Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 10 Oct 2017 11:21:12 -0700 Subject: [PATCH 6/6] update pybullet version --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index aacca7f02..4362b92a3 100644 --- a/setup.py +++ b/setup.py @@ -441,7 +441,7 @@ print("-----") setup( name = 'pybullet', - version='1.5.6', + version='1.5.7', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement 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',