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.

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:

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;
 	}
 

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;

examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp

@@ -61,6 +61,9 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
 					if (event.m_controllerId ==obj->m_controllerId)
 					{
 						if (obj->m_constraintId>=0)
+						{
+							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=...)
@@ -74,8 +77,12 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
 								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)
+						{
+							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=...)
@@ -90,6 +97,7 @@ B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* con
 								b3InitChangeUserConstraintSetERP(commandHandle,1);
 								b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
 							}
+						}
 						//printf("event.m_analogAxis=%f\n", event.m_analogAxis);
 
 						// use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis

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]
-
+			eef_orn = p.getQuaternionFromEuler([0,-math.pi,0])
-			joint_pos = p.calculateInverseKinematics(kuka, 6, eef_pos, 
+			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

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)]

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,6 +40,13 @@ class MJCFBaseBulletEnv(gym.Env):
 
 	def _reset(self):
 		if (self.physicsClientId<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):
@@ -49,7 +57,7 @@ class MJCFBaseBulletEnv(gym.Env):
 
 		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,6 +101,7 @@ class MJCFBaseBulletEnv(gym.Env):
 		return rgb_array
 
 	def _close(self):
+		if (self.ownsPhysicsClient):
 			if (self.physicsClientId>=0):
 				p.disconnect(self.physicsClientId)
 		self.physicsClientId = -1

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(
+			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)
-				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(
+			self.objects = p.loadMJCF(os.path.join(pybullet_data.getDataPath(),"mjcf", self.model_xml))
-				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()
 

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)

examples/pybullet/pybullet.c

@@ -200,6 +200,8 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4])
 		return 0;
 
 	seq = PySequence_Fast(obVec, "expected a sequence");
+	if (seq)
+	{
 		len = PySequence_Size(obVec);
 		if (len == 4)
 		{
@@ -211,6 +213,7 @@ static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4])
 			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)