From 6be7e34dd6fecd283d90ff3d755913bf317ec80d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sun, 12 Nov 2017 10:36:30 -0800 Subject: [PATCH] pybullet vr_kuka_setup.py: add a gear joint, to keep the gripper centered, vr_kuka_control.py: control all joints, use analogue button to close gripper remove some debug warnings/prints pybullet, avoid crash in changeUserConstraint if not passing a [list] allow some gym environments (pybullet_pendulum and locomotors) to re-use an existing physics client connection. --- .../PhysicsClientSharedMemory.cpp | 2 +- examples/SharedMemory/PhysicsDirect.cpp | 2 +- .../PhysicsServerCommandProcessor.cpp | 2 +- .../plugins/vrSyncPlugin/vrSyncPlugin.cpp | 52 ++++++++++-------- examples/pybullet/examples/vr_kuka_control.py | 53 +++++++------------ examples/pybullet/examples/vr_kuka_setup.py | 4 ++ .../pybullet/gym/pybullet_envs/env_bases.py | 31 +++++++---- .../pybullet/gym/pybullet_envs/robot_bases.py | 13 +++-- .../gym/pybullet_envs/scene_abstract.py | 2 +- examples/pybullet/pybullet.c | 21 +++++--- 10 files changed, 101 insertions(+), 81 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 771d1ba40..463082980 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -688,7 +688,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { case CMD_CHANGE_USER_CONSTRAINT_FAILED: { B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_FAILED"); - b3Warning("changeConstraint failed"); + //b3Warning("changeConstraint failed"); break; } case CMD_ACTUAL_STATE_UPDATE_FAILED: diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 692024672..c747ff2fd 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -903,7 +903,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } case CMD_CHANGE_USER_CONSTRAINT_FAILED: { - b3Warning("changeConstraint failed"); + //b3Warning("changeConstraint failed"); break; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f0de991dc..0f7652666 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5542,7 +5542,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; } else { - b3Warning("Request state but no multibody or rigid body available"); + //b3Warning("Request state but no multibody or rigid body available"); SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED; hasStatus = true; diff --git a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp index 78dc439f1..d0fc25d9d 100644 --- a/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp +++ b/examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp @@ -62,33 +62,41 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con { if (obj->m_constraintId>=0) { - //this is basically equivalent to doing this in Python/pybullet: - //p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...) - b3SharedMemoryCommandHandle commandHandle; - int userConstraintUniqueId = obj->m_constraintId; - commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId); - double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1}; - b3InitChangeUserConstraintSetPivotInB(commandHandle, pos); - double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]}; - b3InitChangeUserConstraintSetFrameInB(commandHandle, orn); - b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + struct b3UserConstraint constraintInfo; + if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId, &constraintInfo)) + { + //this is basically equivalent to doing this in Python/pybullet: + //p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...) + b3SharedMemoryCommandHandle commandHandle; + int userConstraintUniqueId = obj->m_constraintId; + commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId); + double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1}; + b3InitChangeUserConstraintSetPivotInB(commandHandle, pos); + double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]}; + b3InitChangeUserConstraintSetFrameInB(commandHandle, orn); + b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + } } // apply the analogue button to close the constraint, using a gear constraint with position target if (obj->m_constraintId2>=0) { - //this block is similar to - //p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...) - //printf("obj->m_constraintId2=%d\n", obj->m_constraintId2); - b3SharedMemoryCommandHandle commandHandle; - commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2); + struct b3UserConstraint constraintInfo; + if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId2, &constraintInfo)) + { + //this block is similar to + //p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...) + //printf("obj->m_constraintId2=%d\n", obj->m_constraintId2); + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2); - //0 -> open, 1 = closed - double openPos = 1.; - double relPosTarget = openPos - (event.m_analogAxis*openPos); - b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget); - b3InitChangeUserConstraintSetERP(commandHandle,1); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + //0 -> open, 1 = closed + double openPos = 1.; + double relPosTarget = openPos - (event.m_analogAxis*openPos); + b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget); + b3InitChangeUserConstraintSetERP(commandHandle,1); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); + } } //printf("event.m_analogAxis=%f\n", event.m_analogAxis); diff --git a/examples/pybullet/examples/vr_kuka_control.py b/examples/pybullet/examples/vr_kuka_control.py index 08f71d336..04c3573dd 100644 --- a/examples/pybullet/examples/vr_kuka_control.py +++ b/examples/pybullet/examples/vr_kuka_control.py @@ -10,14 +10,16 @@ kuka = 3 kuka_gripper = 7 POSITION = 1 ORIENTATION = 2 +ANALOG=3 BUTTONS = 6 -THRESHOLD = 1.3 + +THRESHOLD = .5 LOWER_LIMITS = [-.967, -2.0, -2.96, 0.19, -2.96, -2.09, -3.05] UPPER_LIMITS = [.96, 2.0, 2.96, 2.29, 2.96, 2.09, 3.05] JOINT_RANGE = [5.8, 4, 5.8, 4, 5.8, 4, 6] REST_POSE = [0, 0, 0, math.pi / 2, 0, -math.pi * 0.66, 0] -JOINT_DAMP = [.1, .1, .1, .1, .1, .1, .1] +JOINT_DAMP = [0.1]*10 REST_JOINT_POS = [-0., -0., 0., 1.570793, 0., -1.036725, 0.000001] MAX_FORCE = 500 KUKA_GRIPPER_REST_POS = [0., -0.011130, -0.206421, 0.205143, -0.009999, 0., -0.010055, 0.] @@ -33,6 +35,8 @@ p.setRealTimeSimulation(1) controllers = [e[0] for e in p.getVREvents()] +for j in range(p.getNumJoints(kuka_gripper)): + print(p.getJointInfo(kuka_gripper,j)) while True: events = p.getVREvents() @@ -48,46 +52,29 @@ while True: # A simplistic version of gripper control #@TO-DO: Add slider for the gripper - if e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED: - # avg = 0. - for i in range(p.getNumJoints(kuka_gripper)): - p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_CLOZ_POS[i], force=50) - - if e[BUTTONS][33] & p.VR_BUTTON_WAS_RELEASED: - for i in range(p.getNumJoints(kuka_gripper)): - p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=KUKA_GRIPPER_REST_POS[i], force=50) + + + + #for i in range(p.getNumJoints(kuka_gripper)): + i=4 + p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=e[ANALOG]*0.05, force=10) + i=6 + p.setJointMotorControl2(kuka_gripper, i, p.POSITION_CONTROL, targetPosition=e[ANALOG]*0.05, force=10) + if sq_len < THRESHOLD * THRESHOLD: eef_pos = e[POSITION] - - joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos, + eef_orn = p.getQuaternionFromEuler([0,-math.pi,0]) + joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos, eef_orn, lowerLimits=LOWER_LIMITS, upperLimits=UPPER_LIMITS, jointRanges=JOINT_RANGE, restPoses=REST_POSE, jointDamping=JOINT_DAMP) - # Only need links 1- 4, no need for joint 5-6 with pure position IK - for i in range(len(joint_pos) - 2): + + for i in range(len(joint_pos)): p.setJointMotorControl2(kuka, i, p.POSITION_CONTROL, - targetPosition=joint_pos[i], targetVelocity=0, positionGain=0.05, + targetPosition=joint_pos[i], targetVelocity=0, positionGain=0.15, velocityGain=1.0, force=MAX_FORCE) - # Rotate the end effector - targetOrn = e[ORIENTATION] - - _, _, z = p.getEulerFromQuaternion(targetOrn) - # End effector needs protection, done by using triangular tricks - - if LOWER_LIMITS[6] < z < UPPER_LIMITS[6]: - p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, - targetPosition=z, targetVelocity=0, positionGain=0.03, velocityGain=1.0, force=MAX_FORCE) - - else: - p.setJointMotorControl2(kuka, 6, p.POSITION_CONTROL, - targetPosition=joint_pos[6], targetVelocity=0, positionGain=0.05, - velocityGain=1.0, force=MAX_FORCE) - - p.setJointMotorControl2(kuka, 5, p.POSITION_CONTROL, - targetPosition=-1.57, targetVelocity=0, - positionGain=0.03, velocityGain=1.0, force=MAX_FORCE) else: # Set back to original rest pose diff --git a/examples/pybullet/examples/vr_kuka_setup.py b/examples/pybullet/examples/vr_kuka_setup.py index 3b06c2573..cc278f8b4 100644 --- a/examples/pybullet/examples/vr_kuka_setup.py +++ b/examples/pybullet/examples/vr_kuka_setup.py @@ -48,6 +48,10 @@ for jointIndex in range (p.getNumJoints(kuka_gripper)): kuka_cid = p.createConstraint(kuka, 6, kuka_gripper,0,p.JOINT_FIXED, [0,0,0], [0,0,0.05],[0,0,0]) +pr2_cid2 = p.createConstraint(kuka_gripper,4,kuka_gripper,6,jointType=p.JOINT_GEAR,jointAxis =[1,1,1],parentFramePosition=[0,0,0],childFramePosition=[0,0,0]) +p.changeConstraint(pr2_cid2,gearRatio=-1, erp=0.5, relativePositionTarget=0, maxForce=100) + + objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)] diff --git a/examples/pybullet/gym/pybullet_envs/env_bases.py b/examples/pybullet/gym/pybullet_envs/env_bases.py index ef8ac0c5c..5d49c22c3 100644 --- a/examples/pybullet/gym/pybullet_envs/env_bases.py +++ b/examples/pybullet/gym/pybullet_envs/env_bases.py @@ -18,6 +18,7 @@ class MJCFBaseBulletEnv(gym.Env): def __init__(self, robot, render=False): self.scene = None self.physicsClientId=-1 + self.ownsPhysicsClient = 0 self.camera = Camera() self.isRender = render self.robot = robot @@ -39,17 +40,24 @@ class MJCFBaseBulletEnv(gym.Env): def _reset(self): if (self.physicsClientId<0): - self.physicsClientId = p.connect(p.SHARED_MEMORY) - if (self.physicsClientId<0): - if (self.isRender): - self.physicsClientId = p.connect(p.GUI) - else: - self.physicsClientId = p.connect(p.DIRECT) - p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) + conInfo = p.getConnectionInfo() + if (conInfo['isConnected']): + self.ownsPhysicsClient = False + + self.physicsClientId = 0 + else: + self.ownsPhysicsClient = True + self.physicsClientId = p.connect(p.SHARED_MEMORY) + if (self.physicsClientId<0): + if (self.isRender): + self.physicsClientId = p.connect(p.GUI) + else: + self.physicsClientId = p.connect(p.DIRECT) + p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) if self.scene is None: self.scene = self.create_single_player_scene() - if not self.scene.multiplayer: + if not self.scene.multiplayer and self.ownsPhysicsClient: self.scene.episode_restart() self.robot.scene = self.scene @@ -93,9 +101,10 @@ class MJCFBaseBulletEnv(gym.Env): return rgb_array def _close(self): - if (self.physicsClientId>=0): - p.disconnect(self.physicsClientId) - self.physicsClientId = -1 + if (self.ownsPhysicsClient): + if (self.physicsClientId>=0): + p.disconnect(self.physicsClientId) + self.physicsClientId = -1 def HUD(self, state, a, done): pass diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index e3b626adf..d5b83017c 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -17,6 +17,7 @@ class MJCFBasedRobot: def __init__(self, model_xml, robot_name, action_dim, obs_dim): self.parts = None + self.objects = [] self.jdict = None self.ordered_joints = None self.robot_body = None @@ -85,13 +86,15 @@ class MJCFBasedRobot: def reset(self): self.ordered_joints = [] - + for ob in self.objects: + p.removeBody(ob) + if self.self_collision: - self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( - p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)) + self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml), flags=p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects ) else: - self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene( - p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml))) + self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml)) + self.parts, self.jdict, self.ordered_joints, self.robot_body = self.addToScene(self.objects) self.robot_specific_reset() diff --git a/examples/pybullet/gym/pybullet_envs/scene_abstract.py b/examples/pybullet/gym/pybullet_envs/scene_abstract.py index 51fd81f77..2714bb920 100644 --- a/examples/pybullet/gym/pybullet_envs/scene_abstract.py +++ b/examples/pybullet/gym/pybullet_envs/scene_abstract.py @@ -62,7 +62,7 @@ class World: self.clean_everything() def clean_everything(self): - p.resetSimulation() + #p.resetSimulation() p.setGravity(0, 0, -self.gravity) p.setDefaultContactERP(0.9) p.setPhysicsEngineParameter(fixedTimeStep=self.timestep*self.frame_skip, numSolverIterations=5, numSubSteps=self.frame_skip) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d2a7cb2c7..ffee1389a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -200,17 +200,20 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) return 0; seq = PySequence_Fast(obVec, "expected a sequence"); - len = PySequence_Size(obVec); - if (len == 4) + if (seq) { - for (i = 0; i < len; i++) + len = PySequence_Size(obVec); + if (len == 4) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + for (i = 0; i < len; i++) + { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; } Py_DECREF(seq); - return 1; } - Py_DECREF(seq); return 0; } @@ -5155,10 +5158,16 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P if (pybullet_internalSetVectord(jointChildPivotObj, jointChildPivot)) { b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot); + } else + { + return NULL; } if (pybullet_internalSetVector4d(jointChildFrameOrnObj, jointChildFrameOrn)) { b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); + } else + { + return NULL; } if (relativePositionTarget<1e10)