remove comments and unused code

3 files changed, 85 insertions, 454 deletions

diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd_multiclip.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd_multiclip.py
index e243b227d..6855ff96f 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd_multiclip.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd_multiclip.py
@@ -34,10 +34,6 @@ class HumanoidStablePDMultiClip(object):
             useFixedBase=useFixedBase,
             flags=flags)
 
-        # self._pybullet_client.setCollisionFilterGroupMask(self._sim_model,-1,collisionFilterGroup=0,collisionFilterMask=0)
-        # for j in range (self._pybullet_client.getNumJoints(self._sim_model)):
-        #  self._pybullet_client.setCollisionFilterGroupMask(self._sim_model,j,collisionFilterGroup=0,collisionFilterMask=0)
-
         self._end_effectors = [5, 8, 11, 14]  # ankle and wrist, both left and right
 
         self._kin_models = {}
@@ -107,7 +103,6 @@ class HumanoidStablePDMultiClip(object):
             leftAnkle, leftShoulder, leftElbow
         ]
         for j in self._jointIndicesAll:
-            # self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, j, self._pybullet_client.POSITION_CONTROL, force=[1,1,1])
             self._pybullet_client.setJointMotorControl2(self._sim_model,
                                                         j,
                                                         self._pybullet_client.POSITION_CONTROL,
@@ -151,7 +146,7 @@ class HumanoidStablePDMultiClip(object):
             fall_contact_bodies = self._arg_parser.parse_ints("fall_contact_bodies")
         self._fall_contact_body_parts = fall_contact_bodies
 
-        # [x,y,z] base position and [x,y,z,w] base orientation!
+        # [x,y,z] base position and [x,y,z,w] base orientation
         self._totalDofs = 7
         for dof in self._jointDofCounts:
             self._totalDofs += dof
@@ -168,94 +163,44 @@ class HumanoidStablePDMultiClip(object):
         self.initializePose(self._poseInterpolators[0], self._kin_models[0], initBase=False)
         for i in range(1, self._n_clips):
             _ = self.computePose(self._frameFraction, i)
-            # self.initializePose(self._poseInterpolators[i], self._sim_model, initBase=True)
             self.initializePose(self._poseInterpolators[i], self._kin_models[i], initBase=False)
 
     def initializePose(self, pose, phys_model, initBase, initializeVelocity=True):
 
-        useArray = True
         if initializeVelocity:
             if initBase:
                 self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos,
                                                                       pose._baseOrn)
                 self._pybullet_client.resetBaseVelocity(phys_model, pose._baseLinVel, pose._baseAngVel)
-            if useArray:
-                indices = [chest, neck, rightHip, rightKnee,
-                           rightAnkle, rightShoulder, rightElbow, leftHip,
-                           leftKnee, leftAnkle, leftShoulder, leftElbow]
-                jointPositions = [pose._chestRot, pose._neckRot, pose._rightHipRot, pose._rightKneeRot,
-                                  pose._rightAnkleRot, pose._rightShoulderRot, pose._rightElbowRot, pose._leftHipRot,
-                                  pose._leftKneeRot, pose._leftAnkleRot, pose._leftShoulderRot, pose._leftElbowRot]
-
-                jointVelocities = [pose._chestVel, pose._neckVel, pose._rightHipVel, pose._rightKneeVel,
-                                   pose._rightAnkleVel, pose._rightShoulderVel, pose._rightElbowVel, pose._leftHipVel,
-                                   pose._leftKneeVel, pose._leftAnkleVel, pose._leftShoulderVel, pose._leftElbowVel]
-                self._pybullet_client.resetJointStatesMultiDof(phys_model, indices,
-                                                               jointPositions, jointVelocities)
-            else:
-                self._pybullet_client.resetJointStateMultiDof(phys_model, chest, pose._chestRot,
-                                                              pose._chestVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, neck, pose._neckRot, pose._neckVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightHip, pose._rightHipRot,
-                                                              pose._rightHipVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightKnee, pose._rightKneeRot,
-                                                              pose._rightKneeVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightAnkle, pose._rightAnkleRot,
-                                                              pose._rightAnkleVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightShoulder,
-                                                              pose._rightShoulderRot, pose._rightShoulderVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightElbow, pose._rightElbowRot,
-                                                              pose._rightElbowVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftHip, pose._leftHipRot,
-                                                              pose._leftHipVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftKnee, pose._leftKneeRot,
-                                                              pose._leftKneeVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftAnkle, pose._leftAnkleRot,
-                                                              pose._leftAnkleVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftShoulder,
-                                                              pose._leftShoulderRot, pose._leftShoulderVel)
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftElbow, pose._leftElbowRot,
-                                                              pose._leftElbowVel)
-        else:
 
+            indices = [chest, neck, rightHip, rightKnee,
+                       rightAnkle, rightShoulder, rightElbow, leftHip,
+                       leftKnee, leftAnkle, leftShoulder, leftElbow]
+            jointPositions = [pose._chestRot, pose._neckRot, pose._rightHipRot, pose._rightKneeRot,
+                              pose._rightAnkleRot, pose._rightShoulderRot, pose._rightElbowRot, pose._leftHipRot,
+                              pose._leftKneeRot, pose._leftAnkleRot, pose._leftShoulderRot, pose._leftElbowRot]
+
+            jointVelocities = [pose._chestVel, pose._neckVel, pose._rightHipVel, pose._rightKneeVel,
+                               pose._rightAnkleVel, pose._rightShoulderVel, pose._rightElbowVel, pose._leftHipVel,
+                               pose._leftKneeVel, pose._leftAnkleVel, pose._leftShoulderVel, pose._leftElbowVel]
+            self._pybullet_client.resetJointStatesMultiDof(phys_model, indices,
+                                                           jointPositions, jointVelocities)
+        else:
             if initBase:
                 self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos,
                                                                       pose._baseOrn)
-            if useArray:
-                indices = [chest, neck, rightHip, rightKnee,
-                           rightAnkle, rightShoulder, rightElbow, leftHip,
-                           leftKnee, leftAnkle, leftShoulder, leftElbow]
-                jointPositions = [pose._chestRot, pose._neckRot, pose._rightHipRot, pose._rightKneeRot,
-                                  pose._rightAnkleRot, pose._rightShoulderRot, pose._rightElbowRot, pose._leftHipRot,
-                                  pose._leftKneeRot, pose._leftAnkleRot, pose._leftShoulderRot, pose._leftElbowRot]
-                self._pybullet_client.resetJointStatesMultiDof(phys_model, indices, jointPositions)
-
-            else:
-                self._pybullet_client.resetJointStateMultiDof(phys_model, chest, pose._chestRot, [0, 0, 0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, neck, pose._neckRot, [0, 0, 0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightHip, pose._rightHipRot,
-                                                              [0, 0, 0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightKnee, pose._rightKneeRot, [0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightAnkle, pose._rightAnkleRot,
-                                                              [0, 0, 0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightShoulder,
-                                                              pose._rightShoulderRot, [0, 0, 0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, rightElbow, pose._rightElbowRot,
-                                                              [0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftHip, pose._leftHipRot,
-                                                              [0, 0, 0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftKnee, pose._leftKneeRot, [0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftAnkle, pose._leftAnkleRot,
-                                                              [0, 0, 0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftShoulder,
-                                                              pose._leftShoulderRot, [0, 0, 0])
-                self._pybullet_client.resetJointStateMultiDof(phys_model, leftElbow, pose._leftElbowRot, [0])
+
+            indices = [chest, neck, rightHip, rightKnee,
+                       rightAnkle, rightShoulder, rightElbow, leftHip,
+                       leftKnee, leftAnkle, leftShoulder, leftElbow]
+            jointPositions = [pose._chestRot, pose._neckRot, pose._rightHipRot, pose._rightKneeRot,
+                              pose._rightAnkleRot, pose._rightShoulderRot, pose._rightElbowRot, pose._leftHipRot,
+                              pose._leftKneeRot, pose._leftAnkleRot, pose._leftShoulderRot, pose._leftElbowRot]
+            self._pybullet_client.resetJointStatesMultiDof(phys_model, indices, jointPositions)
 
     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 getCycleTime(self):
@@ -287,7 +232,6 @@ class HumanoidStablePDMultiClip(object):
         self._frameFraction = (frameTime - self._frame * keyFrameDuration) / (keyFrameDuration)
 
     def computeCycleOffset(self, i=0):
-        firstFrame = 0
         lastFrame = self._mocap_data.getNumFrames() - 1
         frameData = self._mocap_data._motion_data[i]['Frames'][0]
         frameDataNext = self._mocap_data._motion_data[i]['Frames'][lastFrame]
@@ -358,9 +302,7 @@ class HumanoidStablePDMultiClip(object):
             targetVelocities.append(targetVelocity)
             dofIndex += self._jointDofCounts[index]
 
-        # static char* kwlist[] = { "bodyUniqueId",
-        # "jointIndices",
-        # "controlMode", "targetPositions", "targetVelocities", "forces", "positionGains", "velocityGains", "maxVelocities", "physicsClientId", NULL };
+
         self._pybullet_client.setJointMotorControlMultiDofArray(self._sim_model,
                                                                 indices,
                                                                 self._pybullet_client.STABLE_PD_CONTROL,
@@ -371,215 +313,6 @@ class HumanoidStablePDMultiClip(object):
                                                                 velocityGains=kds,
                                                                 )
 
-    def computePDForces(self, desiredPositions, desiredVelocities, maxForces):
-        """Compute torques from the PD controller."""
-        if desiredVelocities == None:
-            desiredVelocities = [0] * self._totalDofs
-
-        taus = self._stablePD.computePD(bodyUniqueId=self._sim_model,
-                                        jointIndices=self._jointIndicesAll,
-                                        desiredPositions=desiredPositions,
-                                        desiredVelocities=desiredVelocities,
-                                        kps=self._kpOrg,
-                                        kds=self._kdOrg,
-                                        maxForces=maxForces,
-                                        timeStep=self._timeStep)
-        return taus
-
-    def applyPDForces(self, taus):
-        """Apply pre-computed torques."""
-        dofIndex = 7
-        scaling = 1
-        useArray = True
-        indices = []
-        forces = []
-
-        if (useArray):
-            for index in range(len(self._jointIndicesAll)):
-                jointIndex = self._jointIndicesAll[index]
-                indices.append(jointIndex)
-                if self._jointDofCounts[index] == 4:
-                    force = [
-                        scaling * taus[dofIndex + 0], scaling * taus[dofIndex + 1],
-                        scaling * taus[dofIndex + 2]
-                    ]
-                if self._jointDofCounts[index] == 1:
-                    force = [scaling * taus[dofIndex]]
-                    # print("force[", jointIndex,"]=",force)
-                forces.append(force)
-                dofIndex += self._jointDofCounts[index]
-            self._pybullet_client.setJointMotorControlMultiDofArray(self._sim_model,
-                                                                    indices,
-                                                                    self._pybullet_client.TORQUE_CONTROL,
-                                                                    forces=forces)
-        else:
-            for index in range(len(self._jointIndicesAll)):
-                jointIndex = self._jointIndicesAll[index]
-                if self._jointDofCounts[index] == 4:
-                    force = [
-                        scaling * taus[dofIndex + 0], scaling * taus[dofIndex + 1],
-                        scaling * taus[dofIndex + 2]
-                    ]
-                    # print("force[", jointIndex,"]=",force)
-                    self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,
-                                                                       jointIndex,
-                                                                       self._pybullet_client.TORQUE_CONTROL,
-                                                                       force=force)
-                if self._jointDofCounts[index] == 1:
-                    force = [scaling * taus[dofIndex]]
-                    # print("force[", jointIndex,"]=",force)
-                    self._pybullet_client.setJointMotorControlMultiDof(
-                        self._sim_model,
-                        jointIndex,
-                        controlMode=self._pybullet_client.TORQUE_CONTROL,
-                        force=force)
-                dofIndex += self._jointDofCounts[index]
-
-    def setJointMotors(self, desiredPositions, maxForces, i=0):
-        controlMode = self._pybullet_client.POSITION_CONTROL
-        startIndex = 7
-        chest = 1
-        neck = 2
-        rightHip = 3
-        rightKnee = 4
-        rightAnkle = 5
-        rightShoulder = 6
-        rightElbow = 7
-        leftHip = 9
-        leftKnee = 10
-        leftAnkle = 11
-        leftShoulder = 12
-        leftElbow = 13
-        kp = 0.2
-
-        forceScale = 1
-        # self._jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1]
-        maxForce = [
-            forceScale * maxForces[startIndex], forceScale * maxForces[startIndex + 1],
-            forceScale * maxForces[startIndex + 2], forceScale * maxForces[startIndex + 3]
-        ]
-        startIndex += 4
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            chest,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._chestRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [
-            maxForces[startIndex], maxForces[startIndex + 1], maxForces[startIndex + 2],
-            maxForces[startIndex + 3]
-        ]
-        startIndex += 4
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            neck,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._neckRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [
-            maxForces[startIndex], maxForces[startIndex + 1], maxForces[startIndex + 2],
-            maxForces[startIndex + 3]
-        ]
-        startIndex += 4
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            rightHip,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._rightHipRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [forceScale * maxForces[startIndex]]
-        startIndex += 1
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            rightKnee,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._rightKneeRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [
-            maxForces[startIndex], maxForces[startIndex + 1], maxForces[startIndex + 2],
-            maxForces[startIndex + 3]
-        ]
-        startIndex += 4
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            rightAnkle,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._rightAnkleRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [
-            forceScale * maxForces[startIndex], forceScale * maxForces[startIndex + 1],
-            forceScale * maxForces[startIndex + 2], forceScale * maxForces[startIndex + 3]
-        ]
-        startIndex += 4
-        maxForce = [forceScale * maxForces[startIndex]]
-        startIndex += 1
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            rightElbow,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._rightElbowRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [
-            maxForces[startIndex], maxForces[startIndex + 1], maxForces[startIndex + 2],
-            maxForces[startIndex + 3]
-        ]
-        startIndex += 4
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            leftHip,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._leftHipRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [forceScale * maxForces[startIndex]]
-        startIndex += 1
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            leftKnee,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._leftKneeRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [
-            maxForces[startIndex], maxForces[startIndex + 1], maxForces[startIndex + 2],
-            maxForces[startIndex + 3]
-        ]
-        startIndex += 4
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            leftAnkle,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._leftAnkleRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [
-            maxForces[startIndex], maxForces[startIndex + 1], maxForces[startIndex + 2],
-            maxForces[startIndex + 3]
-        ]
-        startIndex += 4
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            leftShoulder,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._leftShoulderRot,
-            positionGain=kp,
-            force=maxForce)
-        maxForce = [forceScale * maxForces[startIndex]]
-        startIndex += 1
-        self._pybullet_client.setJointMotorControlMultiDof(
-            self._sim_model,
-            leftElbow,
-            controlMode,
-            targetPosition=self._poseInterpolators[i]._leftElbowRot,
-            positionGain=kp,
-            force=maxForce)
-        # print("startIndex=",startIndex)
 
     def getPhase(self):
         keyFrameDuration = self._mocap_data.getKeyFrameDuration()
@@ -649,8 +382,7 @@ class HumanoidStablePDMultiClip(object):
 
         stateVector.append(rootPosRel[1])
 
-        # self.pb2dmJoints=[0,1,2,9,10,11,3,4,5,12,13,14,6,7,8]
-        self.pb2dmJoints = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14]
+        self.pb2dmJoints = range(15)
 
         linkIndicesSim = []
         for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)):
@@ -662,7 +394,6 @@ class HumanoidStablePDMultiClip(object):
         for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)):
             j = self.pb2dmJoints[pbJoint]
             # print("joint order:",j)
-            # ls = self._pybullet_client.getLinkState(self._sim_model, j, computeForwardKinematics=True)
             ls = linkStatesSim[pbJoint]
             linkPos = ls[0]
             linkOrn = ls[1]
@@ -691,14 +422,13 @@ class HumanoidStablePDMultiClip(object):
 
         for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)):
             j = self.pb2dmJoints[pbJoint]
-            # ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True)
             ls = linkStatesSim[pbJoint]
 
             linkLinVel = ls[6]
             linkAngVel = ls[7]
             linkLinVelLocal, unused = self._pybullet_client.multiplyTransforms([0, 0, 0], rootTransOrn,
                                                                                linkLinVel, [0, 0, 0, 1])
-            # linkLinVelLocal=[linkLinVelLocal[0]-rootPosRel[0],linkLinVelLocal[1]-rootPosRel[1],linkLinVelLocal[2]-rootPosRel[2]]
+
             linkAngVelLocal, unused = self._pybullet_client.multiplyTransforms([0, 0, 0], rootTransOrn,
                                                                                linkAngVel, [0, 0, 0, 1])
 
@@ -789,36 +519,11 @@ class HumanoidStablePDMultiClip(object):
 
         # create a mimic reward, comparing the dynamics humanoid with a kinematic one
 
-        # pose = self.InitializePoseFromMotionData()
-        # print("self._kin_model=",self._kin_model)
-        # print("kinematicHumanoid #joints=",self._pybullet_client.getNumJoints(self._kin_model))
-        # self.ApplyPose(pose, True, True, self._kin_model, self._pybullet_client)
-
-        # const Eigen::VectorXd& pose0 = sim_char.GetPose();
-        # const Eigen::VectorXd& vel0 = sim_char.GetVel();
-        # const Eigen::VectorXd& pose1 = kin_char.GetPose();
-        # const Eigen::VectorXd& vel1 = kin_char.GetVel();
-        # tMatrix origin_trans = sim_char.BuildOriginTrans();
-        # tMatrix kin_origin_trans = kin_char.BuildOriginTrans();
-        #
-        # tVector com0_world = sim_char.CalcCOM();
         if self._useComReward:
             comSim, comSimVel = self.computeCOMposVel(self._sim_model)
             comKin, comKinVel = self.computeCOMposVel(self._kin_models[i])
-        # tVector com_vel0_world = sim_char.CalcCOMVel();
-        # tVector com1_world;
-        # tVector com_vel1_world;
-        # cRBDUtil::CalcCoM(joint_mat, body_defs, pose1, vel1, com1_world, com_vel1_world);
-        #
+
         root_id = 0
-        # tVector root_pos0 = cKinTree::GetRootPos(joint_mat, pose0);
-        # tVector root_pos1 = cKinTree::GetRootPos(joint_mat, pose1);
-        # tQuaternion root_rot0 = cKinTree::GetRootRot(joint_mat, pose0);
-        # tQuaternion root_rot1 = cKinTree::GetRootRot(joint_mat, pose1);
-        # tVector root_vel0 = cKinTree::GetRootVel(joint_mat, vel0);
-        # tVector root_vel1 = cKinTree::GetRootVel(joint_mat, vel1);
-        # tVector root_ang_vel0 = cKinTree::GetRootAngVel(joint_mat, vel0);
-        # tVector root_ang_vel1 = cKinTree::GetRootAngVel(joint_mat, vel1);
 
         mJointWeights = [
             0.20833, 0.10416, 0.0625, 0.10416, 0.0625, 0.041666666666666671, 0.0625, 0.0416, 0.00,
@@ -849,29 +554,24 @@ class HumanoidStablePDMultiClip(object):
         root_ang_vel_err = self.calcRootAngVelErr(angVelSim, angVelKin)
         vel_err += root_rot_w * root_ang_vel_err
 
-        useArray = True
+        jointIndices = range(num_joints)
+        simJointStates = self._pybullet_client.getJointStatesMultiDof(self._sim_model, jointIndices)
+        kinJointStates = self._pybullet_client.getJointStatesMultiDof(self._kin_models[i], jointIndices)
+        linkStatesSim = self._pybullet_client.getLinkStates(self._sim_model, jointIndices)
+        linkStatesKin = self._pybullet_client.getLinkStates(self._kin_models[i], jointIndices)
 
-        if useArray:
-            jointIndices = range(num_joints)
-            simJointStates = self._pybullet_client.getJointStatesMultiDof(self._sim_model, jointIndices)
-            kinJointStates = self._pybullet_client.getJointStatesMultiDof(self._kin_models[i], jointIndices)
-            linkStatesSim = self._pybullet_client.getLinkStates(self._sim_model, jointIndices)
-            linkStatesKin = self._pybullet_client.getLinkStates(self._kin_models[i], jointIndices)
         for j in range(num_joints):
             curr_pose_err = 0
             curr_vel_err = 0
             w = mJointWeights[j]
-            if useArray:
-                simJointInfo = simJointStates[j]
-            else:
-                simJointInfo = self._pybullet_client.getJointStateMultiDof(self._sim_model, j)
+
+            simJointInfo = simJointStates[j]
 
             # print("simJointInfo.pos=",simJointInfo[0])
             # print("simJointInfo.vel=",simJointInfo[1])
-            if useArray:
-                kinJointInfo = kinJointStates[j]
-            else:
-                kinJointInfo = self._pybullet_client.getJointStateMultiDof(self._kin_models[i], j)
+
+            kinJointInfo = kinJointStates[j]
+
             # print("kinJointInfo.pos=",kinJointInfo[0])
             # print("kinJointInfo.vel=",kinJointInfo[1])
             if (len(simJointInfo[0]) == 1):
@@ -897,12 +597,9 @@ class HumanoidStablePDMultiClip(object):
 
             if is_end_eff:
 
-                if useArray:
-                    linkStateSim = linkStatesSim[j]
-                    linkStateKin = linkStatesKin[j]
-                else:
-                    linkStateSim = self._pybullet_client.getLinkState(self._sim_model, j)
-                    linkStateKin = self._pybullet_client.getLinkState(self._kin_models[i], j)
+                linkStateSim = linkStatesSim[j]
+                linkStateKin = linkStatesKin[j]
+
                 linkPosSim = linkStateSim[0]
                 linkPosKin = linkStateKin[0]
                 linkPosDiff = [
@@ -917,21 +614,11 @@ class HumanoidStablePDMultiClip(object):
         if (num_end_effs > 0):
             end_eff_err /= num_end_effs
 
-        # double root_ground_h0 = mGround->SampleHeight(sim_char.GetRootPos())
-        # double root_ground_h1 = kin_char.GetOriginPos()[1]
-        # root_pos0[1] -= root_ground_h0
-        # root_pos1[1] -= root_ground_h1
         root_pos_diff = [
             rootPosSim[0] - rootPosKin[0], rootPosSim[1] - rootPosKin[1], rootPosSim[2] - rootPosKin[2]
         ]
         root_pos_err = root_pos_diff[0] * root_pos_diff[0] + root_pos_diff[1] * root_pos_diff[
             1] + root_pos_diff[2] * root_pos_diff[2]
-        #
-        # root_rot_err = cMathUtil::QuatDiffTheta(root_rot0, root_rot1)
-        # root_rot_err *= root_rot_err
-
-        # root_vel_err = (root_vel1 - root_vel0).squaredNorm()
-        # root_ang_vel_err = (root_ang_vel1 - root_ang_vel0).squaredNorm()
 
         root_err = root_pos_err + 0.1 * root_rot_err + 0.01 * root_vel_err + 0.001 * root_ang_vel_err
 
@@ -959,22 +646,6 @@ class HumanoidStablePDMultiClip(object):
         # print("root_reward=",root_reward)
         # print("com_reward=",com_reward)
 
-        info_rew = dict(
-            pose_reward=pose_reward,
-            vel_reward=vel_reward,
-            end_eff_reward=end_eff_reward,
-            root_reward=root_reward,
-            com_reward=com_reward
-        )
-
-        info_errs = dict(
-            pose_err=pose_err,
-            vel_err=vel_err,
-            end_eff_err=end_eff_err,
-            root_err=root_err,
-            com_err=com_err
-        )
-
         return reward
 
     def computeCOMposVel(self, uid: int):
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data_multiclip.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data_multiclip.py
index 569204ddb..863da4999 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data_multiclip.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data_multiclip.py
@@ -26,6 +26,7 @@ class MotionCaptureDataMultiClip(object):
             files = [path]
         else:
             files = [join(path, f) for f in listdir(path) if isfile(join(path, f))]
+            files.sort()
         for i in range(len(files)):
             with open(files[i], 'r') as f:
                 self._names.append(files[i].split('/')[-1])
@@ -34,14 +35,19 @@ class MotionCaptureDataMultiClip(object):
                 self._num_frames_min = min(t, self._num_frames_min)
                 self._num_frames_max = max(t, self._num_frames_max)
         self._num_clips = len(self._motion_data)
-        downsample = False
-        if downsample:
+        self._downsample = True
+        if self._downsample:
             self.downsampleClips()
+            self._num_frames_max = self._num_frames_min
         else:
             self.upsampleClips()
+            self._num_frames_min = self._num_frames_max
 
     def getNumFrames(self):
-        return self._num_frames_min
+        if self._downsample:
+            return self._num_frames_min
+        else:
+            return self._num_frames_max
 
     def getKeyFrameDuration(self, id=0):
         return self._motion_data[id]['Frames'][0][0]
@@ -87,7 +93,7 @@ class MotionCaptureDataMultiClip(object):
     def upsampleClips(self):
         print("Max number of frames: ", self._num_frames_max)
         for i in range(self._num_clips):
-            print("Uspsampling clip number: ", i)
+            #print("Uspsampling clip number: ", i)
             keyframe_duration = self.getKeyFrameDuration(i)
             old_times = np.arange(0, len(self._motion_data[i]['Frames']) * keyframe_duration, keyframe_duration)
             while len(old_times) < self._num_frames_max:
@@ -98,24 +104,23 @@ class MotionCaptureDataMultiClip(object):
             #s = json.dumps(self._motion_data[i])
             #with open("output/{}".format(self._names[i]), 'w') as f:
             #    f.writelines(s)
-        self._num_frames_min = self._num_frames_max
 
 
     def slerpSingleClip(self, clip, key_times):
-        print("Number of initial frames: ", len(key_times))
-        dict_clip = self.extractQuaternions(clip)
-        dict_clip = np.asarray(dict_clip)
-        t = dict_clip[:, 0]
-        root_pos = dict_clip[:, 1]
-        key_rots = dict_clip[:, 2]
+        #print("Number of initial frames: ", len(key_times))
+        org_clip = self.quatlist_to_quatlists(clip)
+        org_clip = np.asarray(org_clip)
+        t = org_clip[:, 0]
+        root_pos = org_clip[:, 1]
+        key_rots = org_clip[:, 2]
         n_frames = len(key_rots)
         assert len(key_times) == n_frames
         needed_frames = self._num_frames_max - n_frames
-        print("Needed frames: ", needed_frames)
+        #print("Needed frames: ", needed_frames)
         inter_times = self.calc_inter_times(key_times)
         inter_times = sorted(random.sample(inter_times, min(len(inter_times), needed_frames)))
-        print("Number of frames to interpolate: ", len(inter_times))
-        print("Number of rots: ", len(key_rots[0]))
+        #print("Number of frames to interpolate: ", len(inter_times))
+        #print("Number of rots: ", len(key_rots[0]))
         inter_joint = []
         for i in range(len(key_rots[0])):
             quats = [rot[i] for rot in key_rots]
@@ -145,11 +150,11 @@ class MotionCaptureDataMultiClip(object):
         ord_keys = sorted(new_dict.keys())
         ord_rots = [new_dict[k] for k in ord_keys]
         ord_root_pos = [new_rp_dict[k] for k in ord_keys]
-        new_clip = self.insertClip(t, ord_root_pos, ord_rots)
+        new_clip = self.quatlists_to_quatlist(t, ord_root_pos, ord_rots)
 
         return np.array(ord_keys), new_clip
 
-    def extractQuaternions(self, clip):
+    def quatlist_to_quatlists(self, clip):
         new_clips = []
         for c in clip:
             t = c[0]
@@ -213,9 +218,9 @@ class MotionCaptureDataMultiClip(object):
             inter_root_pos.append(((np.array(root_pos[up_index]) + np.array(root_pos[low_index]))/2).tolist())
         return inter_root_pos
 
-    def insertClip(self, t, ord_root_pos, ord_rots):
+    def quatlists_to_quatlist(self, t, ord_root_pos, ord_rots):
         delta_t = t[0]
-        new_quats = self.insertQuaternions(ord_rots)
+        new_quats = self.merge_quaternions(ord_rots)
         a = []
         for i in range(len(ord_root_pos)):
             rot = new_quats[i]
@@ -227,7 +232,7 @@ class MotionCaptureDataMultiClip(object):
             ])
         return a
 
-    def insertQuaternions(self, rotations):
+    def merge_quaternions(self, rotations):
         quats = []
         for rot in rotations:
             rots = []
diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env_multiclip.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env_multiclip.py
index 7cd846ca7..7557c394a 100644
--- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env_multiclip.py
+++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env_multiclip.py
@@ -28,7 +28,7 @@ class PyBulletDeepMimicEnvMultiClip(Env):
         self._num_agents = 1
         self._pybullet_client = pybullet_client
         self._isInitialized = False
-        self._useStablePD = True
+        #self._useStablePD = True
         self._arg_parser = arg_parser
         self.timeStep = time_step
         self._init_strategy = init_strategy
@@ -64,7 +64,7 @@ class PyBulletDeepMimicEnvMultiClip(Env):
             print("motion_file=", motion_file[0])
 
             motionPath = pybullet_data.getDataPath() + "/" + motion_file[0]
-            # motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
+
             self._mocapData.Load(motionPath)
             self._n_clips = self._mocapData.getNumClips()
             timeStep = self.timeStep
@@ -76,22 +76,7 @@ class PyBulletDeepMimicEnvMultiClip(Env):
             self._pybullet_client.setTimeStep(timeStep)
             self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1)
 
-            selfCheck = False
-            if (selfCheck):
-                curTime = 0
-                while self._pybullet_client.isConnected():
-                    self._humanoid.setSimTime(curTime)
-                    state = self._humanoid.getState()
-                    # print("state=",state)
-                    # pose = self._humanoid.computePose(self._humanoid._frameFraction)  this would need an id
-                    for i in range(10):
-                        curTime += timeStep
-                        # taus = self._humanoid.computePDForces(pose)
-                        # self._humanoid.applyPDForces(taus)
-                        # self._pybullet_client.stepSimulation()
-                    time.sleep(timeStep)
         # print("numframes = ", self._humanoid._mocap_data.NumFrames())
-        # startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2)
 
         if self._init_strategy == InitializationStrategy.RANDOM:
             rnrange = 1000
@@ -106,7 +91,6 @@ class PyBulletDeepMimicEnvMultiClip(Env):
 
         self._humanoid.resetPose()
         # this clears the contact points. Todo: add API to explicitly clear all contact points?
-        # self._pybullet_client.stepSimulation()
         self._humanoid.resetPose()
         self.needs_update_time = self.t - 1  # force update
 
@@ -139,12 +123,6 @@ class PyBulletDeepMimicEnvMultiClip(Env):
         return 197
 
     def build_state_norm_groups(self, agent_id):
-        # if (mEnablePhaseInput)
-        # {
-        # int phase_group = gNormGroupNone;
-        # int phase_offset = GetStatePhaseOffset();
-        # int phase_size = GetStatePhaseSize();
-        # out_groups.segment(phase_offset, phase_size) = phase_group * Eigen::VectorXi::Ones(phase_size);
         groups = [0] * self.get_state_size(agent_id)
         groups[0] = -1
         return groups
@@ -179,7 +157,6 @@ class PyBulletDeepMimicEnvMultiClip(Env):
         return np.array([])
 
     def build_action_offset(self, agent_id):
-        out_offset = [0] * self.get_action_size(agent_id)
         out_offset = [
             0.0000000000, 0.0000000000, 0.0000000000, -0.200000000, 0.0000000000, 0.0000000000,
             0.0000000000, -0.200000000, 0.0000000000, 0.0000000000, 0.00000000, -0.2000000, 1.57000000,
@@ -193,7 +170,6 @@ class PyBulletDeepMimicEnvMultiClip(Env):
         return np.array(out_offset)
 
     def build_action_scale(self, agent_id):
-        out_scale = [1] * self.get_action_size(agent_id)
         # see cCtCtrlUtil::BuildOffsetScalePDPrismatic and
         # see cCtCtrlUtil::BuildOffsetScalePDSpherical
         out_scale = [
@@ -210,7 +186,6 @@ class PyBulletDeepMimicEnvMultiClip(Env):
 
     def build_action_bound_min(self, agent_id):
         # see cCtCtrlUtil::BuildBoundsPDSpherical
-        out_scale = [-1] * self.get_action_size(agent_id)
         out_scale = [
             -4.79999999999, -1.00000000000, -1.00000000000, -1.00000000000, -4.00000000000,
             -1.00000000000, -1.00000000000, -1.00000000000, -7.77999999999, -1.00000000000,
@@ -224,7 +199,6 @@ class PyBulletDeepMimicEnvMultiClip(Env):
         return out_scale
 
     def build_action_bound_max(self, agent_id):
-        out_scale = [1] * self.get_action_size(agent_id)
         out_scale = [
             4.799999999, 1.000000000, 1.000000000, 1.000000000, 4.000000000, 1.000000000, 1.000000000,
             1.000000000, 8.779999999, 1.000000000, 1.0000000, 1.0000000, 4.7100000, 6.2800000,
@@ -245,7 +219,6 @@ class PyBulletDeepMimicEnvMultiClip(Env):
 
     def record_state(self, agent_id):
         state = self._humanoid.getState()
-
         return np.array(state)
 
     def record_goal(self, agent_id):
@@ -263,8 +236,7 @@ class PyBulletDeepMimicEnvMultiClip(Env):
         # print("action=",)
         # for a in action:
         #  print(a)
-        # np.savetxt("pb_action.csv", action, delimiter=",")
-        self.desiredPose = {} # TODO: is this necessary???
+        self.desiredPose = {}
         for i in range(self._n_clips):
             self.desiredPose[i] = self._humanoid.convertActionToPose(action, i)
             # we need the target root positon and orientation to be zero, to be compatible with deep mimic
@@ -277,8 +249,6 @@ class PyBulletDeepMimicEnvMultiClip(Env):
             self.desiredPose[i][6] = 0
             target_pose = np.array(self.desiredPose[i])
 
-        # np.savetxt("pb_target_pose.csv", target_pose, delimiter=",")
-
         # print("set_action: desiredPose=", self.desiredPose)
 
     def log_val(self, agent_id, val):
@@ -290,40 +260,25 @@ class PyBulletDeepMimicEnvMultiClip(Env):
         self._humanoid._timeStep = timeStep
         self.timeStep = timeStep
 
-        for j in range(1):
-            self.t += timeStep
-            self._humanoid.setSimTime(self.t)
-
-            if self.desiredPose:
-                for i in range(self._n_clips):
-                    kinPose = self._humanoid.computePose(self._humanoid._frameFraction, i)
-                    self._humanoid.initializePose(self._humanoid._poseInterpolators[i],
-                                                  self._humanoid._kin_models[i],
-                                                  initBase=True)
-                # pos,orn=self._pybullet_client.getBasePositionAndOrientation(self._humanoid._sim_model)
-                # self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0]+3,pos[1],pos[2]],orn)
-                # print("desiredPositions=",self.desiredPose[i])
-                maxForces = [
-                    0, 0, 0, 0, 0, 0, 0, 200, 200, 200, 200, 50, 50, 50, 50, 200, 200, 200, 200, 150, 90,
-                    90, 90, 90, 100, 100, 100, 100, 60, 200, 200, 200, 200, 150, 90, 90, 90, 90, 100, 100,
-                    100, 100, 60
-                ]
-
-                if self._useStablePD:
-                    usePythonStablePD = False
-                    if usePythonStablePD:
-                        taus = self._humanoid.computePDForces(self.desiredPose[0], # TODO: check if correct
-                                                              desiredVelocities=None,
-                                                              maxForces=maxForces)
-                        # taus = [0]*43
-                        self._humanoid.applyPDForces(taus)
-                    else:
-                        self._humanoid.computeAndApplyPDForces(self.desiredPose[0],
-                                                               maxForces=maxForces)
-                else:
-                    self._humanoid.setJointMotors(self.desiredPose[0], maxForces=maxForces, i=0)
-
-                self._pybullet_client.stepSimulation()
+        self.t += timeStep
+        self._humanoid.setSimTime(self.t)
+
+        if self.desiredPose:
+            for i in range(self._n_clips):
+                kinPose = self._humanoid.computePose(self._humanoid._frameFraction, i)
+                self._humanoid.initializePose(self._humanoid._poseInterpolators[i],
+                                              self._humanoid._kin_models[i],
+                                              initBase=True)
+            # print("desiredPositions=",self.desiredPose[i])
+            maxForces = [
+                0, 0, 0, 0, 0, 0, 0, 200, 200, 200, 200, 50, 50, 50, 50, 200, 200, 200, 200, 150, 90,
+                90, 90, 90, 100, 100, 100, 100, 60, 200, 200, 200, 200, 150, 90, 90, 90, 90, 100, 100,
+                100, 100, 60
+            ]
+
+            self._humanoid.computeAndApplyPDForces(self.desiredPose[0], maxForces=maxForces) # TODO check if this should be inside the for loop
+
+            self._pybullet_client.stepSimulation()
 
     def set_sample_count(self, count):
         return