revert to original humanoidMotionCapture.py example. Add a showJointMotorTorques variable (false by default)

1 files changed, 340 insertions, 292 deletions

diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py
index 841e2afa6..852fcda4f 100644
--- a/examples/pybullet/examples/humanoidMotionCapture.py
+++ b/examples/pybullet/examples/humanoidMotionCapture.py
@@ -1,86 +1,93 @@
-import pybullet as p
+import pybullet	as p
 import json
 import time
 
 useGUI = True
 if useGUI:
-  p.connect(p.GUI)
+	p.connect(p.GUI)
 else:
-  p.connect(p.DIRECT)
+	p.connect(p.DIRECT)
 
 useZUp = False
 useYUp = not useZUp
+showJointMotorTorques	=	False
 
 if useYUp:
-  p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)
-
-
-from pdControllerStable import PDControllerStableMultiDof
+	p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP	,	1)
 
+from pdControllerExplicit	import PDControllerExplicitMultiDof
+from pdControllerStable	import PDControllerStableMultiDof
 
+explicitPD = PDControllerExplicitMultiDof(p)
 stablePD = PDControllerStableMultiDof(p)
 
-p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94, cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])
+p.resetDebugVisualizerCamera(cameraDistance=7, cameraYaw=-94,	cameraPitch=-14, cameraTargetPosition=[0.31,0.03,-1.16])
 
 import pybullet_data
 p.setTimeOut(10000)
 useMotionCapture=False
-useMotionCaptureReset=False#not useMotionCapture
-
+useMotionCaptureReset=False#not	useMotionCapture
+useExplicitPD	=	True
 
 p.setAdditionalSearchPath(pybullet_data.getDataPath())
 p.setPhysicsEngineParameter(numSolverIterations=30)
 #p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)
 
-#explicit PD control requires small timestep
+#explicit	PD control requires	small	timestep
 timeStep = 1./600.
-#timeStep = 1./240.
+#timeStep	=	1./240.
 
 p.setPhysicsEngineParameter(fixedTimeStep=timeStep)
 
 path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt"
-#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
-#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
+#path	=	pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt"
+#path	=	pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt"
 
 
 #p.loadURDF("plane.urdf",[0,0,-1.03])
-print("path = ", path)
-with open(path, 'r') as f:
-    motion_dict = json.load(f)
-#print("motion_dict = ", motion_dict)
+print("path	=	", path)
+with open(path,	'r') as	f:
+		motion_dict	=	json.load(f)
+#print("motion_dict	=	", motion_dict)
 print("len motion=", len(motion_dict))
 print(motion_dict['Loop'])
-numFrames = len(motion_dict['Frames'])
-print("#frames = ", numFrames)
+numFrames	=	len(motion_dict['Frames'])
+print("#frames = ",	numFrames)
 
 
 frameId= p.addUserDebugParameter("frame",0,numFrames-1,0)
 
 
-erpId = p.addUserDebugParameter("erp",0,1,0.2)
+erpId	=	p.addUserDebugParameter("erp",0,1,0.2)
 
-kpMotorId = p.addUserDebugParameter("kpMotor",0,1,.2)
+kpMotorId	=	p.addUserDebugParameter("kpMotor",0,1,.2)
 forceMotorId = p.addUserDebugParameter("forceMotor",0,2000,1000)
 
 
 
 
 jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
-              "JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
-              
+							"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
+							
 startLocations=[[0,0,2],[0,0,0],[0,0,-2],[0,0,-4]]
 
-p.addUserDebugText("Stable PD", [startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]], [0,0,0])
-p.addUserDebugText("Spherical Drive", [startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]], [0,0,0])
-p.addUserDebugText("Explicit PD", [startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]], [0,0,0])
-p.addUserDebugText("Kinematic", [startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]], [0,0,0])
+p.addUserDebugText("Stable PD",	[startLocations[0][0],startLocations[0][1]+1,startLocations[0][2]],	[0,0,0])
+p.addUserDebugText("Spherical	Drive",	[startLocations[1][0],startLocations[1][1]+1,startLocations[1][2]],	[0,0,0])
+p.addUserDebugText("Explicit PD",	[startLocations[2][0],startLocations[2][1]+1,startLocations[2][2]],	[0,0,0])
+p.addUserDebugText("Kinematic",	[startLocations[3][0],startLocations[3][1]+1,startLocations[3][2]],	[0,0,0])
 
-humanoid =  p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid =	p.loadURDF("humanoid/humanoid.urdf", startLocations[0],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid2	=	p.loadURDF("humanoid/humanoid.urdf", startLocations[1],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid3	=	p.loadURDF("humanoid/humanoid.urdf", startLocations[2],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
+humanoid4	=	p.loadURDF("humanoid/humanoid.urdf", startLocations[3],globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER)
 
 humanoid_fix = p.createConstraint(humanoid,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[0],[0,0,0,1])
+humanoid2_fix	=	p.createConstraint(humanoid2,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[1],[0,0,0,1])
+humanoid3_fix	=	p.createConstraint(humanoid3,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[2],[0,0,0,1])
+humanoid3_fix	=	p.createConstraint(humanoid4,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],startLocations[3],[0,0,0,1])
 
 
-startPose = [2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447,
+startPose	=	[2,0.847532,0,0.9986781045,0.01410400148,-0.0006980000731,-0.04942300517,0.9988133229,0.009485003066,-0.04756001538,-0.004475001447,
 1,0,0,0,0.9649395871,0.02436898957,-0.05755497537,0.2549218909,-0.249116,0.9993661511,0.009952001505,0.03265400494,0.01009800153,
 0.9854981188,-0.06440700776,0.09324301124,-0.1262970152,0.170571,0.9927545808,-0.02090099117,0.08882396249,-0.07817796699,-0.391532,0.9828788495,
 0.1013909845,-0.05515999155,0.143618978,0.9659421276,0.1884590249,-0.1422460188,0.105854014,0.581348]
@@ -89,62 +96,74 @@ startVel = [1.235314324,-0.008525509087,0.1515293946,-1.161516553,0.1866449799,-
 -0.4505575061,0,-1.725374735,-0.5052852598,-0.8555179722,-0.2221173515,0,-0.1837617357,0.00171895706,0.03912837591,0,0.147945294,1.837653345,0.1534535548,1.491385941,0,
 -4.632454387,-0.9111172777,-1.300648184,-1.345694622,0,-1.084238535,0.1313680236,-0.7236998534,0,-0.5278312973]
 
-p.resetBasePositionAndOrientation(humanoid, startLocations[0],[0,0,0,1])
+p.resetBasePositionAndOrientation(humanoid,	startLocations[0],[0,0,0,1])
+p.resetBasePositionAndOrientation(humanoid2, startLocations[1],[0,0,0,1])
+p.resetBasePositionAndOrientation(humanoid3, startLocations[2],[0,0,0,1])
+p.resetBasePositionAndOrientation(humanoid4, startLocations[3],[0,0,0,1])
+
 
 
 index0=7
-for j in range (p.getNumJoints(humanoid)):
-  ji = p.getJointInfo(humanoid,j)
-  targetPosition=[0]
-  jointType = ji[2]
-  if (jointType == p.JOINT_SPHERICAL):
-    targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]]
-    targetVel = [startVel[index0+0],startVel[index0+1],startVel[index0+2]]
-    index0+=4
-    print("spherical position: ",targetPosition)
-    print("spherical velocity: ",targetVel)
-    p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
-  if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
-    targetPosition=[startPose[index0]]
-    targetVel = [startVel[index0]]
-    index0+=1
-    print("revolute:", targetPosition)
-    print("revolute velocity:", targetVel)
-    p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
-
-
-for j in range (p.getNumJoints(humanoid)):
-  ji = p.getJointInfo(humanoid,j)
-  targetPosition=[0]
-  jointType = ji[2]
-  if (jointType == p.JOINT_SPHERICAL):
-    targetPosition=[0,0,0,1]
-    p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[0,0,0])
-
-  if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
-    p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0, force=0)
-  
-  #print(ji)
-  print("joint[",j,"].type=",jointTypes[ji[2]])
-  print("joint[",j,"].name=",ji[1])
-
-
-  
+for	j	in range (p.getNumJoints(humanoid)):
+	ji = p.getJointInfo(humanoid,j)
+	targetPosition=[0]
+	jointType	=	ji[2]
+	if (jointType	== p.JOINT_SPHERICAL):
+		targetPosition=[startPose[index0+1],startPose[index0+2],startPose[index0+3],startPose[index0+0]]
+		targetVel	=	[startVel[index0+0],startVel[index0+1],startVel[index0+2]]
+		index0+=4
+		print("spherical position: ",targetPosition)
+		print("spherical velocity: ",targetVel)
+		p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
+		p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
+	if (jointType==p.JOINT_PRISMATIC or	jointType==p.JOINT_REVOLUTE):
+		targetPosition=[startPose[index0]]
+		targetVel	=	[startVel[index0]]
+		index0+=1
+		print("revolute:", targetPosition)
+		print("revolute	velocity:",	targetVel)
+		p.resetJointStateMultiDof(humanoid,j,targetValue=targetPosition,targetVelocity=targetVel)
+		p.resetJointStateMultiDof(humanoid2,j,targetValue=targetPosition,targetVelocity=targetVel)
+	
+
+
+
+for	j	in range (p.getNumJoints(humanoid)):
+	ji = p.getJointInfo(humanoid,j)
+	targetPosition=[0]
+	jointType	=	ji[2]
+	if (jointType	== p.JOINT_SPHERICAL):
+		targetPosition=[0,0,0,1]
+		p.setJointMotorControlMultiDof(humanoid,j,p.POSITION_CONTROL,targetPosition, targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[0,0,0])
+		p.setJointMotorControlMultiDof(humanoid3,j,p.POSITION_CONTROL,targetPosition,	targetVelocity=[0,0,0],	positionGain=0,velocityGain=1,force=[31,31,31])
+		p.setJointMotorControlMultiDof(humanoid4,j,p.POSITION_CONTROL,targetPosition,	targetVelocity=[0,0,0],	positionGain=0,velocityGain=1,force=[1,1,1])
+
+	if (jointType==p.JOINT_PRISMATIC or	jointType==p.JOINT_REVOLUTE):
+		p.setJointMotorControl2(humanoid,j,p.VELOCITY_CONTROL,targetVelocity=0,	force=0)
+		p.setJointMotorControl2(humanoid3,j,p.VELOCITY_CONTROL,targetVelocity=0, force=31)
+		p.setJointMotorControl2(humanoid4,j,p.VELOCITY_CONTROL,targetVelocity=0, force=10)
+	
+	#print(ji)
+	print("joint[",j,"].type=",jointTypes[ji[2]])
+	print("joint[",j,"].name=",ji[1])
+
+
+	
 jointIds=[]
 paramIds=[]
-for j in range (p.getNumJoints(humanoid)):
-  #p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
-  p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1])
-  info = p.getJointInfo(humanoid,j)
-  #print(info)
-  if (not useMotionCapture):
-    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))
-      #print("jointName=",jointName, "at ", j)
-    
+for	j	in range (p.getNumJoints(humanoid)):
+	#p.changeDynamics(humanoid,j,linearDamping=0,	angularDamping=0)
+	p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1])
+	info = p.getJointInfo(humanoid,j)
+	#print(info)
+	if (not	useMotionCapture):
+		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))
+			#print("jointName=",jointName, "at ",	j)
+		
 p.changeVisualShape(humanoid,2,rgbaColor=[1,0,0,1])
 chest=1
 neck=2
@@ -163,7 +182,7 @@ leftElbow=13
 #rightElbow=4
 #leftShoulder=6
 #leftElbow = 7
-#rightHip = 9
+#rightHip	=	9
 #rightKnee=10
 #rightAnkle=11
 #leftHip = 12
@@ -173,219 +192,248 @@ leftElbow=13
 import time
 
 
-kpOrg = [0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300]
-kdOrg = [0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30]
+kpOrg	=	[0,0,0,0,0,0,0,1000,1000,1000,1000,100,100,100,100,500,500,500,500,500,400,400,400,400,400,400,400,400,300,500,500,500,500,500,400,400,400,400,400,400,400,400,300]
+kdOrg	=	[0,0,0,0,0,0,0,100,100,100,100,10,10,10,10,50,50,50,50,50,40,40,40,40,40,40,40,40,30,50,50,50,50,50,40,40,40,40,40,40,40,40,30]
 
 once=True
 p.getCameraImage(320,200)
 
 
 
-while (p.isConnected()):
-
-  if useGUI:  
-    erp = p.readUserDebugParameter(erpId)
-    kpMotor = p.readUserDebugParameter(kpMotorId)
-    maxForce=p.readUserDebugParameter(forceMotorId)
-    frameReal = p.readUserDebugParameter(frameId)
-  else:
-    erp = 0.2
-    kpMotor = 0.2
-    maxForce=1000
-    frameReal = 0
-
-  kp=kpMotor
-  
-  frame = int(frameReal)
-  frameNext = frame+1
-  if (frameNext >=  numFrames):
-    frameNext = frame
-  
-  frameFraction = frameReal - frame
-  
-  frameData = motion_dict['Frames'][frame]
-  frameDataNext = motion_dict['Frames'][frameNext]
-  
-  #print("duration=",frameData[0])
-  #print(pos=[frameData])
-  
-  basePos1Start = [frameData[1],frameData[2],frameData[3]]
-  basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
-  basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), 
-    basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), 
-    basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
-  baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
-  baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
-  baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
-  #pre-rotate to make z-up
-  
-  
-  if (useZUp):
-    y2zPos=[0,0,0.0]
-    y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
-    basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
-    p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
-  
-  
-  once=False
-  #print("chestRot=",chestRot)
-  p.setGravity(0,0,0)
-  
-  kp=kpMotor
-    
-    
-    
-    
-    
-    
-  
-  basePos1Start = [frameData[1],frameData[2],frameData[3]]
-  basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
-  basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), 
-    basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), 
-    basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
-  baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
-  baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
-  baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
-  #pre-rotate to make z-up
-  if (useZUp):
-    y2zPos=[0,0,0.0]
-    y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
-    basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
-    p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
-  
-  chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
-  chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
-  chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
-  
-  neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
-  neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
-  neckRot =  p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
-  
-  rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
-  rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
-  rightHipRot = p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
-  
-  rightKneeRotStart = [frameData[20]]
-  rightKneeRotEnd = [frameDataNext[20]]
-  rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])]
-  
-  rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
-  rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
-  rightAnkleRot =  p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
-    
-  rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]]
-  rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
-  rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
-  
-  rightElbowRotStart = [frameData[29]]
-  rightElbowRotEnd = [frameDataNext[29]]
-  rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
-  
-  leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]]
-  leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
-  leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
-  
-  leftKneeRotStart = [frameData[34]]
-  leftKneeRotEnd = [frameDataNext[34]]
-  leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ]
-  
-  leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]]
-  leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
-  leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
-  
-  leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
-  leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
-  leftShoulderRot = p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
-  leftElbowRotStart = [frameData[43]]
-  leftElbowRotEnd = [frameDataNext[43]]
-  leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
-  
-  once=False
-  
-  
-  
-  kp=kpMotor
-  
-  basePos,baseOrn = p.getBasePositionAndOrientation(humanoid)
-  pose = [ basePos[0],basePos[1],basePos[2],
-                          baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],
-                          chestRot[0],chestRot[1],chestRot[2],chestRot[3],
-                          neckRot[0],neckRot[1],neckRot[2],neckRot[3],
-                          rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3],
-                          rightKneeRot[0],
-                          rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3],
-                          rightShoulderRot[0],rightShoulderRot[1],rightShoulderRot[2],rightShoulderRot[3],
-                          rightElbowRot[0],
-                          leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3],
-                          leftKneeRot[0],
-                          leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3],
-                          leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3],
-                          leftElbowRot[0] ]
-                          
-  jointIndicesAll = [
-                chest,
-                neck, 
-                rightHip,
-                        rightKnee,
-                rightAnkle,
-                rightShoulder,
-                        rightElbow,
-                leftHip,
-                        leftKnee,
-                leftAnkle,
-                leftShoulder,
-                        leftElbow
-    ]
-  dofIndex=7
-  jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
-  totalDofs = sum(jointDofCounts)+7
-  print("total=",totalDofs)
-  
-  useStablePD=False
-  if (useStablePD):
-    taus = stablePD.computePD(bodyUniqueId=humanoid, 
-      jointIndices = jointIndicesAll,
-      desiredPositions = pose,
-      desiredVelocities = [0]*totalDofs,
-      kps = kpOrg,
-      kds = kdOrg,
-      maxForces = [maxForce]*totalDofs, 
-      timeStep=timeStep)
-      
-  
-    #taus=[0]*43
-
-    for index in range (len(jointIndicesAll)):
-      jointIndex = jointIndicesAll[index]
-      if jointDofCounts[index]==4:
-        p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]])
-      if jointDofCounts[index]==1:
-        p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]])
-      dofIndex+=jointDofCounts[index]
-  else:
-    p.setJointMotorControlMultiDof(humanoid,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,leftHip, p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,leftAnkle, p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp, force=[maxForce])
-    p.setJointMotorControlMultiDof(humanoid,leftElbow, p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp, force=[maxForce])
-  p.stepSimulation()
-  for j in range(p.getNumJoints(humanoid)):
-    jointState = p.getJointStateMultiDof(humanoid,j)
-    print("jointStateMultiDof[",j,"].pos=",jointState[0])
-    print("jointStateMultiDof[",j,"].vel=",jointState[1])
-    print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
-    
-  #j=4
-  #jointState = p.getJointStateMultiDof(humanoid,j)
-  #print("jointStateMultiDof[",j,"].pos=",jointState[0])
-  #print("jointStateMultiDof[",j,"].vel=",jointState[1])
-  #print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
-  time.sleep(timeStep)
+
+while	(p.isConnected()):
+
+	if useGUI:	
+		erp	=	p.readUserDebugParameter(erpId)
+		kpMotor	=	p.readUserDebugParameter(kpMotorId)
+		maxForce=p.readUserDebugParameter(forceMotorId)
+		frameReal	=	p.readUserDebugParameter(frameId)
+	else:
+		erp	=	0.2
+		kpMotor	=	0.2
+		maxForce=1000
+		frameReal	=	0
+
+	kp=kpMotor
+	
+	frame	=	int(frameReal)
+	frameNext	=	frame+1
+	if (frameNext	>=	numFrames):
+		frameNext	=	frame
+	
+	frameFraction	=	frameReal	-	frame
+	#print("frameFraction=",frameFraction)
+	#print("frame=",frame)
+	#print("frameNext=", frameNext)
+	
+	#getQuaternionSlerp
+	
+	frameData	=	motion_dict['Frames'][frame]
+	frameDataNext	=	motion_dict['Frames'][frameNext]
+	
+	#print("duration=",frameData[0])
+	#print(pos=[frameData])
+	
+	basePos1Start	=	[frameData[1],frameData[2],frameData[3]]
+	basePos1End	=	[frameDataNext[1],frameDataNext[2],frameDataNext[3]]
+	basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),	
+		basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),	
+		basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
+	baseOrn1Start	=	[frameData[5],frameData[6],	frameData[7],frameData[4]]
+	baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
+	baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
+	#pre-rotate	to make	z-up
+	if (useZUp):
+		y2zPos=[0,0,0.0]
+		y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
+		basePos,baseOrn	=	p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
+		p.resetBasePositionAndOrientation(humanoid,	basePos,baseOrn)
+		
+		y2zPos=[0,2,0.0]
+		y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
+		basePos,baseOrn	=	p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
+		p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
+	
+	
+	chestRotStart	=	[frameData[9],frameData[10],frameData[11],frameData[8]]
+	chestRotEnd	=	[frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
+	chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
+	
+	neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
+	neckRotEnd=	[frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
+	neckRot	=	 p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
+	
+	rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]]
+	rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]]
+	rightHipRot	=	p.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction)
+	
+	rightKneeRotStart	=	[frameData[20]]
+	rightKneeRotEnd	=	[frameDataNext[20]]
+	rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])]
+	
+	rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]]
+	rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]]
+	rightAnkleRot	=	 p.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction)
+		
+	rightShoulderRotStart	=	[frameData[26],frameData[27],frameData[28],frameData[25]]
+	rightShoulderRotEnd	=	[frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]]
+	rightShoulderRot = p.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction)
+	
+	rightElbowRotStart = [frameData[29]]
+	rightElbowRotEnd = [frameDataNext[29]]
+	rightElbowRot	=	[rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])]
+	
+	leftHipRotStart	=	[frameData[31],frameData[32],frameData[33],frameData[30]]
+	leftHipRotEnd	=	[frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]]
+	leftHipRot = p.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction)
+	
+	leftKneeRotStart = [frameData[34]]
+	leftKneeRotEnd = [frameDataNext[34]]
+	leftKneeRot	=	[leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0])	]
+	
+	leftAnkleRotStart	=	[frameData[36],frameData[37],frameData[38],frameData[35]]
+	leftAnkleRotEnd	=	[frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]]
+	leftAnkleRot = p.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction)
+	
+	leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]]
+	leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]]
+	leftShoulderRot	=	p.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction)
+	leftElbowRotStart	=	[frameData[43]]
+	leftElbowRotEnd	=	[frameDataNext[43]]
+	leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])]
+	
+	if (0):#if (once):
+		p.resetJointStateMultiDof(humanoid,chest,chestRot)
+		p.resetJointStateMultiDof(humanoid,neck,neckRot)
+		p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
+		p.resetJointStateMultiDof(humanoid,rightKnee,rightKneeRot)
+		p.resetJointStateMultiDof(humanoid,rightAnkle,rightAnkleRot)
+		p.resetJointStateMultiDof(humanoid,rightShoulder,rightShoulderRot)
+		p.resetJointStateMultiDof(humanoid,rightElbow, rightElbowRot)
+		p.resetJointStateMultiDof(humanoid,leftHip,	leftHipRot)
+		p.resetJointStateMultiDof(humanoid,leftKnee, leftKneeRot)
+		p.resetJointStateMultiDof(humanoid,leftAnkle,	leftAnkleRot)
+		p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
+		p.resetJointStateMultiDof(humanoid,leftElbow,	leftElbowRot)
+	once=False
+	#print("chestRot=",chestRot)
+	p.setGravity(0,0,-10)
+	
+	kp=kpMotor
+	if (useExplicitPD):
+		jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
+		#[x,y,z] base	position and [x,y,z,w] base	orientation!
+		totalDofs	=7 
+		for	dof	in jointDofCounts:
+			totalDofs	+= dof
+		
+		jointIndicesAll	=	[
+								chest,
+								neck,	
+								rightHip,
+												rightKnee,
+								rightAnkle,
+								rightShoulder,
+												rightElbow,
+								leftHip,
+												leftKnee,
+								leftAnkle,
+								leftShoulder,
+												leftElbow
+		]
+		basePos,baseOrn	=	p.getBasePositionAndOrientation(humanoid)
+		pose = [ basePos[0],basePos[1],basePos[2],
+													baseOrn[0],baseOrn[1],baseOrn[2],baseOrn[3],
+													chestRot[0],chestRot[1],chestRot[2],chestRot[3],
+													neckRot[0],neckRot[1],neckRot[2],neckRot[3],
+													rightHipRot[0],rightHipRot[1],rightHipRot[2],rightHipRot[3],
+													rightKneeRot[0],
+													rightAnkleRot[0],rightAnkleRot[1],rightAnkleRot[2],rightAnkleRot[3],
+													rightShoulderRot[0],rightShoulderRot[1],rightShoulderRot[2],rightShoulderRot[3],
+													rightElbowRot[0],
+													leftHipRot[0],leftHipRot[1],leftHipRot[2],leftHipRot[3],
+													leftKneeRot[0],
+													leftAnkleRot[0],leftAnkleRot[1],leftAnkleRot[2],leftAnkleRot[3],
+													leftShoulderRot[0],leftShoulderRot[1],leftShoulderRot[2],leftShoulderRot[3],
+													leftElbowRot[0]	]
+		
+				
+		#print("pose=")
+		#for po	in pose:
+		#	print(po)
+		
+		
+		taus = stablePD.computePD(bodyUniqueId=humanoid, 
+			jointIndices = jointIndicesAll,
+			desiredPositions = pose,
+			desiredVelocities	=	[0]*totalDofs,
+			kps	=	kpOrg,
+			kds	=	kdOrg,
+			maxForces	=	[maxForce]*totalDofs,	
+			timeStep=timeStep)
+			
+		taus3	=	explicitPD.computePD(bodyUniqueId=humanoid3, 
+			jointIndices = jointIndicesAll,
+			desiredPositions = pose,
+			desiredVelocities	=	[0]*totalDofs,
+			kps	=	kpOrg,
+			kds	=	kdOrg,
+			maxForces	=	[maxForce*0.05]*totalDofs, 
+			timeStep=timeStep)
+
+		#taus=[0]*43
+		dofIndex=7
+		for	index	in range (len(jointIndicesAll)):
+			jointIndex = jointIndicesAll[index]
+			if jointDofCounts[index]==4:
+				
+				p.setJointMotorControlMultiDof(humanoid,jointIndex,p.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]])
+				p.setJointMotorControlMultiDof(humanoid3,jointIndex,p.TORQUE_CONTROL,force=[taus3[dofIndex+0],taus3[dofIndex+1],taus3[dofIndex+2]])
+	
+			if jointDofCounts[index]==1:
+				
+				p.setJointMotorControlMultiDof(humanoid, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]])
+				p.setJointMotorControlMultiDof(humanoid3,	jointIndex,	controlMode=p.TORQUE_CONTROL,	force=[taus3[dofIndex]])
+
+			dofIndex+=jointDofCounts[index]
+		
+		#print("len(taus)=",len(taus))
+		#print("taus=",taus)
+		
+		
+	p.setJointMotorControlMultiDof(humanoid2,chest,p.POSITION_CONTROL, targetPosition=chestRot,positionGain=kp,	force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,neck,p.POSITION_CONTROL,targetPosition=neckRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightHip,p.POSITION_CONTROL,targetPosition=rightHipRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightKnee,p.POSITION_CONTROL,targetPosition=rightKneeRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightAnkle,p.POSITION_CONTROL,targetPosition=rightAnkleRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightShoulder,p.POSITION_CONTROL,targetPosition=rightShoulderRot,positionGain=kp, force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,rightElbow, p.POSITION_CONTROL,targetPosition=rightElbowRot,positionGain=kp,	force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftHip,	p.POSITION_CONTROL,targetPosition=leftHipRot,positionGain=kp,	force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftKnee, p.POSITION_CONTROL,targetPosition=leftKneeRot,positionGain=kp,	force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftAnkle,	p.POSITION_CONTROL,targetPosition=leftAnkleRot,positionGain=kp,	force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftShoulder, p.POSITION_CONTROL,targetPosition=leftShoulderRot,positionGain=kp,	force=[maxForce])
+	p.setJointMotorControlMultiDof(humanoid2,leftElbow,	p.POSITION_CONTROL,targetPosition=leftElbowRot,positionGain=kp,	force=[maxForce])
+	
+	kinematicHumanoid4 = True
+	if (kinematicHumanoid4):
+		p.resetJointStateMultiDof(humanoid4,chest,chestRot)
+		p.resetJointStateMultiDof(humanoid4,neck,neckRot)
+		p.resetJointStateMultiDof(humanoid4,rightHip,rightHipRot)
+		p.resetJointStateMultiDof(humanoid4,rightKnee,rightKneeRot)
+		p.resetJointStateMultiDof(humanoid4,rightAnkle,rightAnkleRot)
+		p.resetJointStateMultiDof(humanoid4,rightShoulder,rightShoulderRot)
+		p.resetJointStateMultiDof(humanoid4,rightElbow,	rightElbowRot)
+		p.resetJointStateMultiDof(humanoid4,leftHip, leftHipRot)
+		p.resetJointStateMultiDof(humanoid4,leftKnee,	leftKneeRot)
+		p.resetJointStateMultiDof(humanoid4,leftAnkle, leftAnkleRot)
+		p.resetJointStateMultiDof(humanoid4,leftShoulder,	leftShoulderRot)
+		p.resetJointStateMultiDof(humanoid4,leftElbow, leftElbowRot)
+	p.stepSimulation()
+	
+	if showJointMotorTorques:
+		for	j	in range(p.getNumJoints(humanoid2)):
+			jointState = p.getJointStateMultiDof(humanoid2,j)
+			print("jointStateMultiDof[",j,"].pos=",jointState[0])
+			print("jointStateMultiDof[",j,"].vel=",jointState[1])
+			print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
+	time.sleep(timeStep)