tweak ImportMJCFSetup.cpp example MJCF humanoid a bit, clamp target positions to be within joint limits to avoid solver problems (conflicting constraints)

add humanoid_manual_control.py PyBullet example which is similar to ImportMJCFSetup.cpp

examples/pybullet/examples/humanoid_manual_control.py

@@ -0,0 +1,32 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+obUids = p.loadMJCF("mjcf/humanoid.xml")
+humanoid = obUids[1]
+
+gravId = p.addUserDebugParameter("gravity",-10,10,-10)
+jointIds=[]
+paramIds=[]
+
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.changeDynamics(humanoid,-1,linearDamping=0, angularDamping=0)
+
+for j in range (p.getNumJoints(humanoid)):
+	p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
+	info = p.getJointInfo(humanoid,j)
+	#print(info)
+	jointName = info[1]
+	jointType = info[2]
+	if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
+		jointIds.append(j)
+		paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
+
+p.setRealTimeSimulation(1)
+while(1):
+	p.setGravity(0,0,p.readUserDebugParameter(gravId))
+	for i in range(len(paramIds)):
+		c = paramIds[i]
+		targetPos = p.readUserDebugParameter(c)
+		p.setJointMotorControl2(humanoid,jointIds[i],p.POSITION_CONTROL,targetPos, force=5*240.)
+	time.sleep(0.01)