Added laikago mocap data for a DeepMimic compatible walk cycle

Added testLaikago.py script to test this mocap data.

6 files changed, 395 insertions, 22 deletions

diff --git a/examples/pybullet/gym/pybullet_data/cube_no_rotation.urdf b/examples/pybullet/gym/pybullet_data/cube_no_rotation.urdf
new file mode 100644
index 000000000..b11411f76
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/cube_no_rotation.urdf
@@ -0,0 +1,93 @@
+<?xml version="1.0" ?>
+<robot name="cube">
+	<link name="world"/>
+	
+	<link name="x_prismatic">
+	 <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+	</link>
+	
+	<joint name="x_to_world" type="prismatic">
+    <parent link="world"/>
+    <child link="x_prismatic"/>
+    <axis xyz="1 0 0"/>
+    <limit effort="0.0" lower="1" upper="-1" velocity="1000.0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+    
+	<link name="y_prismatic">
+	 <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+	</link>
+	
+	<joint name="y_to_x" type="prismatic">
+    <parent link="x_prismatic"/>
+    <child link="y_prismatic"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.0" lower="1" upper="-1" velocity="1000.0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  
+  <link name="z_prismatic">
+	 <inertial>
+      <mass value="0.01"/>
+      <inertia ixx="0.2125" ixy="-0.005" ixz="0.0225" iyy="0.205" iyz="0.045" izz="0.0125"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </inertial>
+	</link>
+	
+	<joint name="z_to_y" type="prismatic">
+    <parent link="y_prismatic"/>
+    <child link="z_prismatic"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="0.0" lower="1" upper="-1" velocity="1000.0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  
+  
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <contact_cfm value="0.0"/>
+      <contact_erp value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value=".1"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="cube.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 	<box size="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+
+
+	<joint name="cube_to_z" type="continuous">
+    <parent link="z_prismatic"/>
+    <child link="baseLink"/>
+    <axis xyz="0 1 0"/>
+    <limit effort="0.0" lower="1" upper="-1" velocity="1000.0"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+    
+</robot>
+
diff --git a/examples/pybullet/gym/pybullet_data/data/motions/laikago_walk.json b/examples/pybullet/gym/pybullet_data/data/motions/laikago_walk.json
new file mode 100644
index 000000000..cf2c9a697
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/data/motions/laikago_walk.json
@@ -0,0 +1,26 @@
+{
+"Loop": "wrap",
+"Frames":
+[
+[0.0625, 0.004302391269371692, 0.032072827968764595, 0.4859280427279121, 0.004830772994558957, 0.707678371835374, 0.7065173686931822, 0.0010924301219892524, 0.08389, 0.8482, -1.547832, -0.068933, 0.625726, -1.272086, 0.074398, 0.61135, -1.255892, -0.068262, 0.836745, -1.534517]  ,
+[0.0625, -0.0005163758759639595, 0.035110484561327815, 0.4804070548325144, 0.0011057154495015514, 0.7066355270704724, 0.7075514171140866, 0.006000117917852832, 0.084177, 0.833013, -1.588003, -0.067016, 0.658305, -1.272626, 0.075932, 0.64075, -1.25652, -0.06855, 0.822922, -1.575236]  ,
+[0.0625, -0.002168291214916071, 0.04031742998326305, 0.4762997578849618, 0.0013844044765198003, 0.7058594743030352, 0.7082894410282018, 0.009303423501350345, 0.084944, 0.74196, -1.58658, -0.064427, 0.688353, -1.272596, 0.07459, 0.670441, -1.256788, -0.069029, 0.735924, -1.573623]  ,
+[0.0625, -0.0008731114859451673, 0.04854432762715426, 0.47227259707998265, 0.0031656949814854026, 0.7064659448475835, 0.7076633375546795, 0.010413828636341247, 0.085711, 0.64234, -1.490591, -0.061839, 0.722912, -1.272356, 0.075165, 0.700879, -1.256758, -0.067016, 0.639387, -1.481307]  ,
+[0.0625, 0.0035577298144520014, 0.05899309247174703, 0.47445007095590064, 0.008399131140596094, 0.7079489957667197, 0.7061549881168573, 0.009099821243154055, 0.081205, 0.588214, -1.298377, -0.060113, 0.7674, -1.305808, 0.065961, 0.7417, -1.286775, -0.060688, 0.572216, -1.262739]  ,
+[0.0625, 0.009680485356857524, 0.07231452798894085, 0.4880780016924288, 0.014014227060451283, 0.7069653681830796, 0.7070597854586259, 0.008368358074462682, 0.081301, 0.627316, -1.298437, -0.072481, 0.844306, -1.492046, 0.072864, 0.823344, -1.489674, -0.068166, 0.606872, -1.262679]  ,
+[0.0625, 0.014042852163416426, 0.08818761809627415, 0.48593650988960696, 0.0205518279874073, 0.7058764963106472, 0.707992701427402, 0.007894874800174631, 0.076795, 0.670441, -1.298437, -0.075165, 0.856637, -1.600356, 0.071426, 0.839049, -1.600161, -0.074877, 0.64698, -1.263158]  ,
+[0.0625, 0.01547861319846975, 0.10549937657595013, 0.4862504152725654, 0.020803973074311675, 0.7047057291720944, 0.7091767573910802, 0.005035548889942747, 0.074206, 0.707628, -1.298498, -0.076603, 0.755654, -1.594386, 0.076987, 0.747671, -1.593251, -0.074686, 0.686893, -1.263696]  ,
+[0.0625, 0.01377282138670678, 0.12463005416970553, 0.485586805835178, 0.01865628135321735, 0.7054008425412682, 0.7085506552967453, 0.004190869443355131, 0.065674, 0.749391, -1.298287, -0.079192, 0.643183, -1.538769, 0.074973, 0.645098, -1.540529, -0.075932, 0.726644, -1.263518]  ,
+[0.0625, 0.008156482480707666, 0.1458938741459106, 0.483608797602435, 0.012685929101712541, 0.7071771159252943, 0.7069015296797991, 0.005460883698568928, 0.067975, 0.795307, -1.308196, -0.071042, 0.560372, -1.337062, 0.07459, 0.535516, -1.304539, -0.068742, 0.767822, -1.267738]  ,
+[0.0625, 0.002473179092079117, 0.16826606809987307, 0.47803836979022124, 0.007137732359261557, 0.713473071635956, 0.7005519779330868, 0.011495870613463202, 0.076507, 0.861796, -1.458219, -0.071905, 0.611902, -1.329303, 0.072193, 0.583217, -1.287347, -0.071905, 0.835837, -1.416835]  ,
+[0.0625, 0.0007323162191444368, 0.1837119527640199, 0.48302479817861793, 0.003973011684439916, 0.7082452285957256, 0.7058151277353635, 0.014068290921662794, 0.082356, 0.903462, -1.630008, -0.067399, 0.662815, -1.328603, 0.076603, 0.630268, -1.287317, -0.07133, 0.881655, -1.604253]  ,
+[0.0625, -0.0006035392176749561, 0.2019348727325117, 0.4833528424799158, 0.003126705110581192, 0.7070742488737417, 0.7069334033292907, 0.016774789180554476, 0.085999, 0.799753, -1.627556, -0.065194, 0.707336, -1.328725, 0.075165, 0.674432, -1.287648, -0.071426, 0.783723, -1.598992]  ,
+[0.0625, -0.0006591829855084264, 0.22492522789855718, 0.48334616075271924, 0.002800420558512883, 0.7076517012704826, 0.7063038670852392, 0.018869941020677388, 0.091464, 0.659278, -1.599024, -0.061934, 0.755719, -1.327631, 0.076507, 0.71785, -1.287377, -0.070084, 0.647694, -1.57543]  ,
+[0.0625, 0.0012660331796939587, 0.24842439197503677, 0.48221319301348264, 0.004830723274295561, 0.7080551897222302, 0.7058296278960896, 0.02095349145444337, 0.082739, 0.535581, -1.359633, -0.057812, 0.799298, -1.318887, 0.074494, 0.765032, -1.287077, -0.068933, 0.52166, -1.328087]  ,
+[0.0625, 0.005987725745401395, 0.27193992375101134, 0.4754881724641968, 0.011446078914115974, 0.7140822979161163, 0.699764684759515, 0.016877352984019753, 0.08782, 0.596878, -1.350424, -0.069413, 0.86582, -1.419538, 0.065002, 0.833111, -1.385407, -0.063948, 0.569847, -1.293341]  ,
+[0.0625, 0.0115134196085071, 0.2874139572316929, 0.4775626658202976, 0.016321426738182154, 0.7107432188252844, 0.7031883398335369, 0.010190517418584365, 0.079384, 0.648862, -1.349049, -0.079384, 0.929032, -1.638191, 0.068742, 0.897004, -1.620079, -0.076507, 0.616348, -1.291954]  ,
+[0.0625, 0.01644179926469466, 0.30292567682496474, 0.4802768296607192, 0.021409783470007084, 0.706308023653194, 0.7075553883310891, 0.005997444333071436, 0.07574, 0.692701, -1.34911, -0.079479, 0.836778, -1.642749, 0.072097, 0.814907, -1.624093, -0.076699, 0.66116, -1.292859]  ,
+[0.0625, 0.02029705215402532, 0.3228442196975263, 0.4802955448684967, 0.023366791392598424, 0.7061682242572214, 0.7076561224217157, 0.0018012519199694643, 0.070563, 0.736151, -1.348591, -0.08178, 0.689035, -1.628145, 0.077178, 0.679916, -1.612321, -0.07737, 0.702209, -1.292255]  ,
+[0.0625, 0.02086242495305926, 0.34452499153312494, 0.48006979278650724, 0.022331225346257488, 0.7061923075710385, 0.7076666576111227, -0.0012817205345011084, 0.066536, 0.780315, -1.347125, -0.074973, 0.576013, -1.450448, 0.076795, 0.568517, -1.427251, -0.07737, 0.745886, -1.291713]  ,
+[0.0625, 0.018132712096620485, 0.3679732121039304, 0.4753887581385186, 0.017341478405081247, 0.7125583020112218, 0.701392838068211, 0.00283303163254124, 0.071618, 0.836064, -1.409669, -0.082835, 0.596586, -1.376695, 0.06529, 0.551675, -1.30064, -0.073631, 0.798812, -1.348561]]
+}
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data.py
index 7a9c86b76..ea9399899 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data.py
@@ -1,4 +1,5 @@
 import json
+import math
 
 class MotionCaptureData(object):
   def __init__(self):
@@ -17,3 +18,25 @@ class MotionCaptureData(object):
   def KeyFrameDuraction(self):
   	return self._motion_data['Frames'][0][0]
  
+  def getCycleTime(self):
+    keyFrameDuration = self.KeyFrameDuraction()
+    cycleTime = keyFrameDuration*(self.NumFrames()-1)
+    return cycleTime
+    
+  def calcCycleCount(self, simTime, cycleTime):
+    phases = simTime / cycleTime;
+    count = math.floor(phases)
+    loop = True
+    #count = (loop) ? count : cMathUtil::Clamp(count, 0, 1);
+    return count
+    
+  def computeCycleOffset(self):
+    firstFrame=0
+    lastFrame = self.NumFrames()-1
+    frameData = self._motion_data['Frames'][0]
+    frameDataNext = self._motion_data['Frames'][lastFrame]
+    
+    basePosStart = [frameData[1],frameData[2],frameData[3]]
+    basePosEnd = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
+    self._cycleOffset = [basePosEnd[0]-basePosStart[0],basePosEnd[1]-basePosStart[1],basePosEnd[2]-basePosStart[2]]
+    return self._cycleOffset
\ No newline at end of file
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py
new file mode 100644
index 000000000..61a1f5824
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/quadrupedPoseInterpolator.py
@@ -0,0 +1,51 @@
+from pybullet_utils import bullet_client
+import math
+	
+class QuadrupedPoseInterpolator(object):
+  def __init__(self):
+    pass
+    
+    
+  def ComputeLinVel(self,posStart, posEnd, deltaTime):
+    vel = [(posEnd[0]-posStart[0])/deltaTime,(posEnd[1]-posStart[1])/deltaTime,(posEnd[2]-posStart[2])/deltaTime]
+    return vel
+  
+  def ComputeAngVel(self,ornStart, ornEnd, deltaTime, bullet_client):
+    dorn = bullet_client.getDifferenceQuaternion(ornStart,ornEnd)
+    axis,angle = bullet_client.getAxisAngleFromQuaternion(dorn)
+    angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
+    return angVel
+  
+  def ComputeAngVelRel(self,ornStart, ornEnd, deltaTime, bullet_client):
+    ornStartConjugate = [-ornStart[0],-ornStart[1],-ornStart[2],ornStart[3]]
+    pos_diff, q_diff =bullet_client.multiplyTransforms([0,0,0], ornStartConjugate, [0,0,0], ornEnd)
+    axis,angle = bullet_client.getAxisAngleFromQuaternion(q_diff)
+    angVel = [(axis[0]*angle)/deltaTime,(axis[1]*angle)/deltaTime,(axis[2]*angle)/deltaTime]
+    return angVel
+  
+  def Slerp(self, frameFraction, frameData, frameDataNext,bullet_client ):
+    keyFrameDuration = frameData[0]
+    basePos1Start = [frameData[1],frameData[2],frameData[3]]
+    basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
+    self._basePos = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]), 
+      basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]), 
+      basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
+    self._baseLinVel = self.ComputeLinVel(basePos1Start,basePos1End, keyFrameDuration)
+    baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
+    baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
+    self._baseOrn = bullet_client.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
+    self._baseAngVel = self.ComputeAngVel(baseOrn1Start,baseOrn1Next, keyFrameDuration, bullet_client)
+    
+    jointPositions=[]
+    jointVelocities=[]
+    for j in range (12):
+      index=j+8
+      jointPosStart=frameData[index]
+      jointPosEnd=frameDataNext[index]
+      jointPos=jointPosStart+frameFraction*(jointPosEnd-jointPosStart)
+      jointVel=(jointPosEnd-jointPosStart)/keyFrameDuration
+      jointPositions.append(jointPos)
+      jointVelocities.append(jointVel)
+    self._jointPositions = jointPositions
+    self._jointVelocities = jointVelocities
+    return jointPositions,jointVelocities
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
index 2057dde5a..d34ffe794 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py
@@ -36,27 +36,6 @@ pybullet_client.setTimeStep(timeStep)
 pybullet_client.setPhysicsEngineParameter(numSubSteps=2)
 timeId = pybullet_client.addUserDebugParameter("time",0,10,0)
 
-
-#startPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
-#startPose.Reset(basePos=[-0.000000,0.889540,0.000000],baseOrn=[0.029215,-0.000525,-0.017963,0.999412],chestRot=[0.000432,0.000572,0.005500,0.999985],
-#  neckRot=[0.001660,-0.011168,-0.140597,0.990003],rightHipRot=[-0.024450,-0.000839,-0.038869,0.998945],rightKneeRot=[-0.014186],rightAnkleRot=[0.010757,-0.105223,0.035405,0.993760],
-#  rightShoulderRot=[-0.003003,-0.124234,0.073280,0.989539],rightElbowRot=[0.240463],leftHipRot=[-0.020920,-0.012925,-0.006300,0.999678],leftKneeRot=[-0.027859],
-#  leftAnkleRot=[-0.010764,0.105284,-0.009276,0.994341],leftShoulderRot=[0.055661,-0.019608,0.098917,0.993344],leftElbowRot=[0.148934],
-#  baseLinVel=[-0.340837,0.377742,0.009662],baseAngVel=[0.047057,0.285253,-0.248554],chestVel=[-0.016455,-0.070035,-0.231662],neckVel=[0.072168,0.097898,-0.059063],
-#  rightHipVel=[-0.315908,-0.131685,1.114815],rightKneeVel=[0.103419],rightAnkleVel=[-0.409780,-0.099954,-4.241572],rightShoulderVel=[-3.324227,-2.510209,1.834637],
-#  rightElbowVel=[-0.212299],leftHipVel=[0.173056,-0.191063,1.226781,0.000000],leftKneeVel=[-0.665322],leftAnkleVel=[0.282716,0.086217,-3.007098,0.000000],
-#  leftShoulderVel=[4.253144,2.038637,1.170750],leftElbowVel=[0.387993])
-#  
-#targetPose = humanoid_pose_interpolator.HumanoidPoseInterpolator()
-#targetPose.Reset(basePos=[0.000000,0.000000,0.000000],baseOrn=[0.000000,0.000000,0.000000,1.000000],chestRot=[-0.006711,0.007196,-0.027119,0.999584],neckRot=[-0.017613,-0.033879,-0.209250,0.977116],
-#  rightHipRot=[-0.001697,-0.006510,0.046117,0.998913],rightKneeRot=[0.366954],rightAnkleRot=[0.012605,0.001208,-0.187007,0.982277],rightShoulderRot=[-0.468057,-0.044589,0.161134,0.867739],
-#  rightElbowRot=[-0.593650],leftHipRot=[0.006993,0.017242,0.049703,0.998591],leftKneeRot=[0.395147],leftAnkleRot=[-0.008922,0.026517,-0.217852,0.975581],
-#  leftShoulderRot=[0.426160,-0.266177,0.044672,0.863447],leftElbowRot=[-0.726281])
-  
-#out_tau= [0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,-33.338211,-1.381748,-118.708975,0.000000,-2.813919,-2.773850,-0.772481,0.000000,31.885372,2.658243,64.988216,0.000000,94.773133,1.784944,6.240010,5.407563,0.000000,-180.441290,-6.821173,-19.502417,0.000000,-44.518261,9.992627,-2.380950,53.057697,0.000000,111.728594,-1.218496,-4.630812,4.268995,0.000000,89.741829,-8.460265,-117.727884,0.000000,-79.481906]
-#,mSimWorld->stepSimulation(timestep:0.001667, mParams.mNumSubsteps:2, subtimestep:0.000833)
-#cImpPDController::CalcControlForces timestep=0.001667
-  
   
 def isKeyTriggered(keys, key):
   o = ord(key)
@@ -67,7 +46,7 @@ def isKeyTriggered(keys, key):
 animating = False
 singleStep = False
 
-#humanoid.initializePose(pose=startPose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True)
+
 t=0
 while (1):
 
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py
new file mode 100644
index 000000000..a429bad5b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testLaikago.py
@@ -0,0 +1,201 @@
+import pybullet as p
+import pybullet_data
+
+import time
+import motion_capture_data
+import quadrupedPoseInterpolator
+
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pybullet_data.getDataPath())
+
+plane = p.loadURDF("plane.urdf")
+p.setGravity(0,0,-9.8)
+timeStep=1./500
+p.setTimeStep(timeStep)
+#p.setDefaultContactERP(0)
+#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS 
+urdfFlags = p.URDF_USE_SELF_COLLISION
+
+
+startPos=[0.007058990464444105, 0.03149299192130908, 0.4918981912395484]
+startOrn=[0.005934649695708604, 0.7065453990917289, 0.7076373820553712, -0.0027774940359030264]
+quadruped = p.loadURDF("laikago/laikago.urdf",startPos,startOrn, flags = urdfFlags,useFixedBase=False)
+p.resetBasePositionAndOrientation(quadruped,startPos,startOrn)
+
+#This cube is added as a soft constraint to keep the laikago from falling 
+#since we didn't train it yet, it doesn't balance
+cube = p.loadURDF("cube_no_rotation.urdf",[0,0,-0.5],[0,0.5,0.5,0])
+p.setCollisionFilterGroupMask(cube,-1,0,0)
+for j in range(p.getNumJoints(cube)):
+  p.setJointMotorControl2(cube,j,p.POSITION_CONTROL,force=0)
+  p.setCollisionFilterGroupMask(cube,j,0,0)
+  p.changeVisualShape(cube,j,rgbaColor=[1,0,0,0])
+cid = p.createConstraint(cube,p.getNumJoints(cube)-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,1,0],[0,0,0])
+p.changeConstraint(cid, maxForce=10)
+
+
+jointIds=[]
+paramIds=[]
+jointOffsets=[]
+jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
+jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0]
+
+for i in range (4):
+  jointOffsets.append(0)
+  jointOffsets.append(-0.7)
+  jointOffsets.append(0.7)
+
+maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
+
+for j in range (p.getNumJoints(quadruped)):
+        p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
+        info = p.getJointInfo(quadruped,j)
+        #print(info)
+        jointName = info[1]
+        jointType = info[2]
+        if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
+                jointIds.append(j)
+
+    
+startQ=[0.08389, 0.8482, -1.547832, -0.068933, 0.625726, -1.272086, 0.074398, 0.61135, -1.255892, -0.068262, 0.836745, -1.534517]
+for j in range(p.getNumJoints(quadruped)):
+  p.resetJointState(quadruped,jointIds[j],jointDirections[j]*startQ[j]+jointOffsets[j])
+
+
+qpi = quadrupedPoseInterpolator.QuadrupedPoseInterpolator()
+
+#enable collision between lower legs
+
+for j in range (p.getNumJoints(quadruped)):
+		print(p.getJointInfo(quadruped,j))
+
+#2,5,8 and 11 are the lower legs
+lower_legs = [2,5,8,11]
+for l0 in lower_legs:
+	for l1 in lower_legs:
+		if (l1>l0):
+			enableCollision = 1
+			print("collision for pair",l0,l1, p.getJointInfo(quadruped,l0)[12],p.getJointInfo(quadruped,l1)[12], "enabled=",enableCollision)
+			p.setCollisionFilterPair(quadruped, quadruped, 2,5,enableCollision)
+
+jointIds=[]
+paramIds=[]
+jointOffsets=[]
+jointDirections=[-1,1,1,1,1,1,-1,1,1,1,1,1]
+jointAngles=[0,0,0,0,0,0,0,0,0,0,0,0]
+
+for i in range (4):
+	jointOffsets.append(0)
+	jointOffsets.append(-0.7)
+	jointOffsets.append(0.7)
+
+maxForceId = p.addUserDebugParameter("maxForce",0,100,20)
+
+for j in range (p.getNumJoints(quadruped)):
+        p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
+        info = p.getJointInfo(quadruped,j)
+        #print(info)
+        jointName = info[1]
+        jointType = info[2]
+        if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
+                jointIds.append(j)
+
+		
+p.getCameraImage(480,320)
+p.setRealTimeSimulation(0)
+
+joints=[]
+
+
+mocapData = motion_capture_data.MotionCaptureData()
+
+motionPath = pybullet_data.getDataPath()+"/data/motions/laikago_walk.json"
+
+mocapData.Load(motionPath)
+print("mocapData.NumFrames=",mocapData.NumFrames())
+print("mocapData.KeyFrameDuraction=",mocapData.KeyFrameDuraction())
+print("mocapData.getCycleTime=",mocapData.getCycleTime())
+print("mocapData.computeCycleOffset=",mocapData.computeCycleOffset())
+
+
+cycleTime = mocapData.getCycleTime()
+t=0
+
+while t<10.*cycleTime:
+  #get interpolated joint
+  keyFrameDuration = mocapData.KeyFrameDuraction()
+  cycleTime = mocapData.getCycleTime()
+  cycleCount = mocapData.calcCycleCount(t, cycleTime)
+  
+  #print("cycleTime=",cycleTime)
+  #print("cycleCount=",cycleCount)
+  
+  #print("cycles=",cycles)
+  frameTime = t - cycleCount*cycleTime
+  #print("frameTime=",frameTime)
+  if (frameTime<0):
+    frameTime += cycleTime
+
+  frame = int(frameTime/keyFrameDuration)
+  frameNext = frame+1
+  if (frameNext >=  mocapData.NumFrames()):
+    frameNext = frame
+  frameFraction = (frameTime - frame*keyFrameDuration)/(keyFrameDuration)
+  #print("frame=",frame)
+  #print("frameFraction=",frameFraction)
+  frameData = mocapData._motion_data['Frames'][frame]
+  frameDataNext = mocapData._motion_data['Frames'][frameNext]
+    
+  joints,qdot=qpi.Slerp(frameFraction, frameData, frameDataNext, p)
+  
+  maxForce = p.readUserDebugParameter(maxForceId)
+  for j in range (12):
+    targetPos = float(joints[j])
+    p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
+  p.stepSimulation()
+  t+=timeStep
+  time.sleep(timeStep)
+
+useOrgData=False
+if useOrgData:
+  with open("data1.txt","r") as filestream:
+  	for line in filestream:
+  		maxForce = p.readUserDebugParameter(maxForceId)
+  		currentline = line.split(",")
+  		frame = currentline[0]
+  		t = currentline[1]
+  		joints=currentline[2:14]
+  		for j in range (12):
+  			targetPos = float(joints[j])
+  			p.setJointMotorControl2(quadruped,jointIds[j],p.POSITION_CONTROL,jointDirections[j]*targetPos+jointOffsets[j], force=maxForce)
+  		p.stepSimulation()
+  		for lower_leg in lower_legs:
+  			#print("points for ", quadruped, " link: ", lower_leg)
+  			pts = p.getContactPoints(quadruped,-1, lower_leg)
+  			#print("num points=",len(pts))
+  			#for pt in pts:
+  			#	print(pt[9])
+  		time.sleep(1./500.)
+
+
+for j in range (p.getNumJoints(quadruped)):
+        p.changeDynamics(quadruped,j,linearDamping=0, angularDamping=0)
+        info = p.getJointInfo(quadruped,j)
+        js = p.getJointState(quadruped,j)
+        #print(info)
+        jointName = info[1]
+        jointType = info[2]
+        if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
+                paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,(js[0]-jointOffsets[j])/jointDirections[j]))
+
+
+p.setRealTimeSimulation(1)
+
+while (1):
+	
+	for i in range(len(paramIds)):
+		c = paramIds[i]
+		targetPos = p.readUserDebugParameter(c)
+		maxForce = p.readUserDebugParameter(maxForceId)
+		p.setJointMotorControl2(quadruped,jointIds[i],p.POSITION_CONTROL,jointDirections[i]*targetPos+jointOffsets[i], force=maxForce)
+