diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py new file mode 100644 index 000000000..0a6581c62 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_pose_interpolator.py @@ -0,0 +1,249 @@ +from pybullet_envs.minitaur.envs import bullet_client +import math + +class HumanoidPoseInterpolator(object): + def __init__(self): + pass + + def Reset(self): + + self._basePos = [0,0,0] + self._baseLinVel = [0,0,0] + self._baseOrn = [0,0,0,1] + self._baseAngVel = [0,0,0] + + self._chestRot = [0,0,0,1] + self._chestVel = [0,0,0] + self._neckRot = [0,0,0,1] + self._neckVel = [0,0,0] + + self._rightHipRot = [0,0,0,1] + self._rightHipVel = [0,0,0] + self._rightKneeRot = [0] + self._rightKneeVel = [0] + self._rightAnkleRot = [0,0,0,1] + self._rightAnkleVel = [0,0,0] + + self._rightShoulderRot = [0,0,0,1] + self._rightShoulderVel = [0,0,0] + self._rightElbowRot = [0] + self._rightElbowVel = [0] + + self._leftHipRot = [0,0,0,1] + self._leftHipVel = [0,0,0] + self._leftKneeRot = [0] + self._leftKneeVel = [0] + self._leftAnkleRot = [0,0,0,1] + self._leftAnkleVel = [0,0,0] + + self._leftShoulderRot = [0,0,0,1] + self._leftShoulderVel = [0,0,0] + self._leftElbowRot = [0] + self._leftElbowVel = [0] + + 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 NormalizeVector(self, vec): + length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2] + if (length2>0): + length = math.sqrt(length2) + + def NormalizeQuaternion(self, orn): + length2 = orn[0]*orn[0]+orn[1]*orn[1]+orn[2]*orn[2]+orn[3]*orn[3] + if (length2>0): + length = math.sqrt(length2) + orn[0]/=length + orn[1]/=length + orn[2]/=length + orn[3]/=length + return orn + + #print("Normalize? length=",length) + + + def PostProcessMotionData(self, frameData): + baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]] + + chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] + + neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] + rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]] + rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] + rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] + leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] + leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] + leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] + + + def GetPose(self): + pose = [ self._basePos[0],self._basePos[1],self._basePos[2], + self._baseOrn[0],self._baseOrn[1],self._baseOrn[2],self._baseOrn[3], + self._chestRot[0],self._chestRot[1],self._chestRot[2],self._chestRot[3], + self._neckRot[0],self._neckRot[1],self._neckRot[2],self._neckRot[3], + self._rightHipRot[0],self._rightHipRot[1],self._rightHipRot[2],self._rightHipRot[3], + self._rightKneeRot[0], + self._rightAnkleRot[0],self._rightAnkleRot[1],self._rightAnkleRot[2],self._rightAnkleRot[3], + self._rightShoulderRot[0],self._rightShoulderRot[1],self._rightShoulderRot[2],self._rightShoulderRot[3], + self._rightElbowRot[0], + self._leftHipRot[0],self._leftHipRot[1],self._leftHipRot[2],self._leftHipRot[3], + self._leftKneeRot[0], + self._leftAnkleRot[0],self._leftAnkleRot[1],self._leftAnkleRot[2],self._leftAnkleRot[3], + self._leftShoulderRot[0],self._leftShoulderRot[1],self._leftShoulderRot[2],self._leftShoulderRot[3], + self._leftElbowRot[0] ] + return pose + + 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) + + ##pre-rotate to make z-up + #y2zPos=[0,0,0.0] + #y2zOrn = p.getQuaternionFromEuler([1.57,0,0]) + #basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1) + + chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]] + chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]] + self._chestRot = bullet_client.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction) + self._chestVel = self.ComputeAngVel(chestRotStart,chestRotEnd,keyFrameDuration,bullet_client) + + neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]] + neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]] + self._neckRot = bullet_client.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction) + self._neckVel = self.ComputeAngVel(neckRotStart,neckRotEnd,keyFrameDuration,bullet_client) + + rightHipRotStart = [frameData[17],frameData[18],frameData[19],frameData[16]] + rightHipRotEnd = [frameDataNext[17],frameDataNext[18],frameDataNext[19],frameDataNext[16]] + self._rightHipRot = bullet_client.getQuaternionSlerp(rightHipRotStart,rightHipRotEnd,frameFraction) + self._rightHipVel = self.ComputeAngVel(rightHipRotStart,rightHipRotEnd,keyFrameDuration,bullet_client) + + rightKneeRotStart = [frameData[20]] + rightKneeRotEnd = [frameDataNext[20]] + self._rightKneeRot = [rightKneeRotStart[0]+frameFraction*(rightKneeRotEnd[0]-rightKneeRotStart[0])] + self._rightKneeVel = [(rightKneeRotEnd[0]-rightKneeRotStart[0])/keyFrameDuration] + + rightAnkleRotStart = [frameData[22],frameData[23],frameData[24],frameData[21]] + rightAnkleRotEnd = [frameDataNext[22],frameDataNext[23],frameDataNext[24],frameDataNext[21]] + self._rightAnkleRot = bullet_client.getQuaternionSlerp(rightAnkleRotStart,rightAnkleRotEnd,frameFraction) + self._rightAnkleVel = self.ComputeAngVel(rightAnkleRotStart,rightAnkleRotEnd,keyFrameDuration,bullet_client) + + rightShoulderRotStart = [frameData[26],frameData[27],frameData[28],frameData[25]] + rightShoulderRotEnd = [frameDataNext[26],frameDataNext[27],frameDataNext[28],frameDataNext[25]] + self._rightShoulderRot = bullet_client.getQuaternionSlerp(rightShoulderRotStart,rightShoulderRotEnd,frameFraction) + self._rightShoulderVel = self.ComputeAngVel(rightShoulderRotStart,rightShoulderRotEnd, keyFrameDuration,bullet_client) + + rightElbowRotStart = [frameData[29]] + rightElbowRotEnd = [frameDataNext[29]] + self._rightElbowRot = [rightElbowRotStart[0]+frameFraction*(rightElbowRotEnd[0]-rightElbowRotStart[0])] + self._rightElbowVel = [(rightElbowRotEnd[0]-rightElbowRotStart[0])/keyFrameDuration] + + leftHipRotStart = [frameData[31],frameData[32],frameData[33],frameData[30]] + leftHipRotEnd = [frameDataNext[31],frameDataNext[32],frameDataNext[33],frameDataNext[30]] + self._leftHipRot = bullet_client.getQuaternionSlerp(leftHipRotStart,leftHipRotEnd,frameFraction) + self._leftHipVel = self.ComputeAngVel(leftHipRotStart, leftHipRotEnd,keyFrameDuration,bullet_client) + + leftKneeRotStart = [frameData[34]] + leftKneeRotEnd = [frameDataNext[34]] + self._leftKneeRot = [leftKneeRotStart[0] +frameFraction*(leftKneeRotEnd[0]-leftKneeRotStart[0]) ] + self._leftKneeVel = [(leftKneeRotEnd[0]-leftKneeRotStart[0])/keyFrameDuration] + + leftAnkleRotStart = [frameData[36],frameData[37],frameData[38],frameData[35]] + leftAnkleRotEnd = [frameDataNext[36],frameDataNext[37],frameDataNext[38],frameDataNext[35]] + self._leftAnkleRot = bullet_client.getQuaternionSlerp(leftAnkleRotStart,leftAnkleRotEnd,frameFraction) + self._leftAnkleVel = self.ComputeAngVel(leftAnkleRotStart,leftAnkleRotEnd,keyFrameDuration,bullet_client) + + leftShoulderRotStart = [frameData[40],frameData[41],frameData[42],frameData[39]] + leftShoulderRotEnd = [frameDataNext[40],frameDataNext[41],frameDataNext[42],frameDataNext[39]] + self._leftShoulderRot = bullet_client.getQuaternionSlerp(leftShoulderRotStart,leftShoulderRotEnd,frameFraction) + self._leftShoulderVel = self.ComputeAngVel(leftShoulderRotStart,leftShoulderRotEnd,keyFrameDuration,bullet_client) + + leftElbowRotStart = [frameData[43]] + leftElbowRotEnd = [frameDataNext[43]] + self._leftElbowRot = [leftElbowRotStart[0]+frameFraction*(leftElbowRotEnd[0]-leftElbowRotStart[0])] + self._leftElbowVel = [(leftElbowRotEnd[0]-leftElbowRotStart[0])/keyFrameDuration] + + pose = self.GetPose() + return pose + + def ConvertFromAction(self, pybullet_client, action): + #turn action into pose + + self.Reset()#?? needed? + index=0 + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + self._chestRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle) + #print("pose._chestRot=",pose._chestRot) + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + self._neckRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + self._rightHipRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + index+=1 + self._rightKneeRot = [angle] + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + self._rightAnkleRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + self._rightShoulderRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + index+=1 + self._rightElbowRot = [angle] + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + self._leftHipRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + index+=1 + self._leftKneeRot = [angle] + + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + self._leftAnkleRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + axis = [action[index+1],action[index+2],action[index+3]] + index+=4 + self._leftShoulderRot = pybullet_client.getQuaternionFromAxisAngle(axis,angle) + + angle = action[index] + index+=1 + self._leftElbowRot = [angle] + + pose = self.GetPose() + return pose + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py new file mode 100644 index 000000000..464593650 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py @@ -0,0 +1,454 @@ + +from pybullet_envs.deep_mimic.env import pd_controller_stable +from pybullet_envs.deep_mimic.env import humanoid_pose_interpolator +import math + +chest=1 +neck=2 +rightHip=3 +rightKnee=4 +rightAnkle=5 +rightShoulder=6 +rightElbow=7 +leftHip=9 +leftKnee=10 +leftAnkle=11 +leftShoulder=12 +leftElbow=13 +jointFrictionForce = 0 + +class HumanoidStablePD(object): + def __init__(self, pybullet_client, mocap_data, timeStep, useFixedBase=True): + self._pybullet_client = pybullet_client + self._mocap_data = mocap_data + print("LOADING humanoid!") + + self._sim_model = self._pybullet_client.loadURDF( + "humanoid/humanoid.urdf", [0,0.85,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER) + + self._kin_model = self._pybullet_client.loadURDF( + "humanoid/humanoid.urdf", [0,2.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER) + + self._pybullet_client.changeDynamics(self._sim_model, -1, lateralFriction=0.9) + for j in range (self._pybullet_client.getNumJoints(self._sim_model)): + self._pybullet_client.changeDynamics(self._sim_model, j, lateralFriction=0.9) + + self._pybullet_client.changeDynamics(self._sim_model, -1, linearDamping=0, angularDamping=0) + self._pybullet_client.changeDynamics(self._kin_model, -1, linearDamping=0, angularDamping=0) + self._poseInterpolator = humanoid_pose_interpolator.HumanoidPoseInterpolator() + + for i in range (self._mocap_data.NumFrames()-1): + frameData = self._mocap_data._motion_data['Frames'][i] + self._poseInterpolator.PostProcessMotionData(frameData) + + self._stablePD = pd_controller_stable.PDControllerStableMultiDof(self._pybullet_client) + self._timeStep = timeStep + self._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] + self._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] + + self._jointIndicesAll = [chest,neck, rightHip,rightKnee,rightAnkle,rightShoulder,rightElbow,leftHip,leftKnee,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, targetPosition=0, positionGain=0, targetVelocity=0,force=jointFrictionForce) + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, j, self._pybullet_client.POSITION_CONTROL,targetPosition=[0,0,0,1], targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[jointFrictionForce,jointFrictionForce,jointFrictionForce]) + self._pybullet_client.setJointMotorControl2(self._kin_model, j, self._pybullet_client.POSITION_CONTROL, targetPosition=0, positionGain=0, targetVelocity=0,force=0) + self._pybullet_client.setJointMotorControlMultiDof(self._kin_model, j, self._pybullet_client.POSITION_CONTROL,targetPosition=[0,0,0,1], targetVelocity=[0,0,0], positionGain=0,velocityGain=1,force=[jointFrictionForce,jointFrictionForce,0]) + + self._jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1] + + #only those body parts/links are allowed to touch the ground, otherwise the episode terminates + self._allowed_body_parts=[5,11] + + #[x,y,z] base position and [x,y,z,w] base orientation! + self._totalDofs = 7 + for dof in self._jointDofCounts: + self._totalDofs += dof + self.setSimTime(0) + self._maxForce = 6000 + self.resetPose() + + def resetPose(self): + print("resetPose with self._frameFraction=",self._frameFraction) + pose = self.computePose(self._frameFraction) + self.initializePose(self._poseInterpolator, self._sim_model, initBase=True) + self.initializePose(self._poseInterpolator, self._kin_model, initBase=False) + + def initializePose(self, pose, phys_model,initBase, initializeVelocity = 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) + 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: + if initBase: + self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos, pose._baseOrn) + 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]) + + 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): + keyFrameDuration = self._mocap_data.KeyFrameDuraction() + cycleTime = keyFrameDuration*(self._mocap_data.NumFrames()-1) + return cycleTime + + def setSimTime(self, t): + self._simTime = t + #print("SetTimeTime time =",t) + keyFrameDuration = self._mocap_data.KeyFrameDuraction() + cycleTime = self.getCycleTime() + #print("self._motion_data.NumFrames()=",self._mocap_data.NumFrames()) + #print("cycleTime=",cycleTime) + cycles = self.calcCycleCount(t, cycleTime) + #print("cycles=",cycles) + frameTime = t - cycles*cycleTime + if (frameTime<0): + frameTime += cycleTime + + #print("keyFrameDuration=",keyFrameDuration) + #print("frameTime=",frameTime) + self._frame = int(frameTime/keyFrameDuration) + #print("self._frame=",self._frame) + + self._frameNext = self._frame+1 + if (self._frameNext >= self._mocap_data.NumFrames()): + self._frameNext = self._frame + + self._frameFraction = (frameTime - self._frame*keyFrameDuration)/(keyFrameDuration) + + def computePose(self, frameFraction): + frameData = self._mocap_data._motion_data['Frames'][self._frame] + frameDataNext = self._mocap_data._motion_data['Frames'][self._frameNext] + pose = self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client) + return pose + + + def convertActionToPose(self, action): + pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action) + return pose + + def computePDForces(self, desiredPositions, desiredVelocities = None): + 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 = [self._maxForce]*self._totalDofs, + timeStep=self._timeStep) + return taus + + def applyPDForces(self, taus): + dofIndex=7 + for index in range (len(self._jointIndicesAll)): + jointIndex = self._jointIndicesAll[index] + if self._jointDofCounts[index]==4: + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,jointIndex,self._pybullet_client.TORQUE_CONTROL,force=[taus[dofIndex+0],taus[dofIndex+1],taus[dofIndex+2]]) + if self._jointDofCounts[index]==1: + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, jointIndex, controlMode=self._pybullet_client.TORQUE_CONTROL, force=[taus[dofIndex]]) + dofIndex+=self._jointDofCounts[index] + + + + def getPhase(self): + keyFrameDuration = self._mocap_data.KeyFrameDuraction() + cycleTime = keyFrameDuration*(self._mocap_data.NumFrames()-1) + phase = self._simTime / cycleTime + phase = math.fmod(phase,1.0) + if (phase<0): + phase += 1 + return phase + + def buildHeadingTrans(self, rootOrn): + #align root transform 'forward' with world-space x axis + eul = self._pybullet_client.getEulerFromQuaternion(rootOrn) + refDir = [1,0,0] + rotVec = self._pybullet_client.rotateVector(rootOrn, refDir) + heading = math.atan2(-rotVec[2], rotVec[0]) + heading2=eul[1] + #print("heading=",heading) + headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0,1,0],-heading) + return headingOrn + + + def buildOriginTrans(self): + rootPos,rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._sim_model) + + #print("rootPos=",rootPos, " rootOrn=",rootOrn) + invRootPos=[-rootPos[0], 0, -rootPos[2]] + #invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn) + headingOrn = self.buildHeadingTrans(rootOrn) + #print("headingOrn=",headingOrn) + headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn) + #print("headingMat=",headingMat) + #dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn) + #dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn) + + invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms( [0,0,0],headingOrn, invRootPos,[0,0,0,1]) + #print("invOrigTransPos=",invOrigTransPos) + #print("invOrigTransOrn=",invOrigTransOrn) + invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn) + #print("invOrigTransMat =",invOrigTransMat ) + return invOrigTransPos, invOrigTransOrn + + def getState(self): + + stateVector = [] + phase = self.getPhase() + #print("phase=",phase) + stateVector.append(phase) + + rootTransPos, rootTransOrn=self.buildOriginTrans() + basePos,baseOrn = self._pybullet_client.getBasePositionAndOrientation(self._sim_model) + + rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos,[0,0,0,1]) + #print("!!!rootPosRel =",rootPosRel ) + #print("rootTransPos=",rootTransPos) + #print("basePos=",basePos) + localPos,localOrn = self._pybullet_client.multiplyTransforms( rootTransPos, rootTransOrn , basePos,baseOrn ) + + localPos=[localPos[0]-rootPosRel[0],localPos[1]-rootPosRel[1],localPos[2]-rootPosRel[2]] + #print("localPos=",localPos) + + 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] + + 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) + linkPos = ls[0] + linkOrn = ls[1] + linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, linkPos,linkOrn) + if (linkOrnLocal[3]<0): + linkOrnLocal=[-linkOrnLocal[0],-linkOrnLocal[1],-linkOrnLocal[2],-linkOrnLocal[3]] + linkPosLocal=[linkPosLocal[0]-rootPosRel[0],linkPosLocal[1]-rootPosRel[1],linkPosLocal[2]-rootPosRel[2]] + for l in linkPosLocal: + stateVector.append(l) + #re-order the quaternion, DeepMimic uses w,x,y,z + stateVector.append(linkOrnLocal[3]) + stateVector.append(linkOrnLocal[0]) + stateVector.append(linkOrnLocal[1]) + stateVector.append(linkOrnLocal[2]) + + + 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) + linkLinVel = ls[6] + linkAngVel = ls[7] + for l in linkLinVel: + stateVector.append(l) + for l in linkAngVel: + stateVector.append(l) + + #print("stateVector len=",len(stateVector)) + #for st in range (len(stateVector)): + # print("state[",st,"]=",stateVector[st]) + return stateVector + + def terminates(self): + #check if any non-allowed body part hits the ground + terminates=False + pts = self._pybullet_client.getContactPoints() + for p in pts: + part = -1 + if (p[1]==self._sim_model): + part=p[3] + if (p[2]==self._sim_model): + part=p[4] + if (part >=0 and part not in self._allowed_body_parts): + #print("terminating part:", part) + terminates=True + + return terminates + + def getReward(self, pose): + #from DeepMimic double cSceneImitate::CalcRewardImitate + pose_w = 0.5 + vel_w = 0.05 + end_eff_w = 0 #0.15 + root_w = 0#0.2 + com_w = 0#0.1 + + total_w = pose_w + vel_w + end_eff_w + root_w + com_w + pose_w /= total_w + vel_w /= total_w + end_eff_w /= total_w + root_w /= total_w + com_w /= total_w + + pose_scale = 2 + vel_scale = 0.1 + end_eff_scale = 40 + root_scale = 5 + com_scale = 10 + err_scale = 1 + + reward = 0 + + pose_err = 0 + vel_err = 0 + end_eff_err = 0 + root_err = 0 + com_err = 0 + heading_err = 0 + + #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(); + #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, 0.10416, 0.0625, 0.0416, 0.0625, 0.0416, 0.0000] + + num_end_effs = 0 + num_joints = 15 + + root_rot_w = mJointWeights[root_id] + #pose_err += root_rot_w * cKinTree::CalcRootRotErr(joint_mat, pose0, pose1) + #vel_err += root_rot_w * cKinTree::CalcRootAngVelErr(joint_mat, vel0, vel1) + + for j in range (num_joints): + curr_pose_err = 0 + curr_vel_err = 0 + w = mJointWeights[j]; + + simJointInfo = self._pybullet_client.getJointStateMultiDof(self._sim_model, j) + + #print("simJointInfo.pos=",simJointInfo[0]) + #print("simJointInfo.vel=",simJointInfo[1]) + kinJointInfo = self._pybullet_client.getJointStateMultiDof(self._kin_model,j) + #print("kinJointInfo.pos=",kinJointInfo[0]) + #print("kinJointInfo.vel=",kinJointInfo[1]) + if (len(simJointInfo[0])==1): + angle = simJointInfo[0][0]-kinJointInfo[0][0] + curr_pose_err = angle*angle + velDiff = simJointInfo[1][0]-kinJointInfo[1][0] + curr_vel_err = velDiff*velDiff + if (len(simJointInfo[0])==4): + #print("quaternion diff") + diffQuat = self._pybullet_client.getDifferenceQuaternion(simJointInfo[0],kinJointInfo[0]) + axis,angle = self._pybullet_client.getAxisAngleFromQuaternion(diffQuat) + curr_pose_err = angle*angle + diffVel = [simJointInfo[1][0]-kinJointInfo[1][0],simJointInfo[1][1]-kinJointInfo[1][1],simJointInfo[1][2]-kinJointInfo[1][2]] + curr_vel_err = diffVel[0]*diffVel[0]+diffVel[1]*diffVel[1]+diffVel[2]*diffVel[2] + + + pose_err += w * curr_pose_err + vel_err += w * curr_vel_err + + # bool is_end_eff = sim_char.IsEndEffector(j) + # if (is_end_eff) + # { + # tVector pos0 = sim_char.CalcJointPos(j) + # tVector pos1 = cKinTree::CalcJointWorldPos(joint_mat, pose1, j) + # double ground_h0 = mGround->SampleHeight(pos0) + # double ground_h1 = kin_char.GetOriginPos()[1] + # + # tVector pos_rel0 = pos0 - root_pos0 + # tVector pos_rel1 = pos1 - root_pos1 + # pos_rel0[1] = pos0[1] - ground_h0 + # pos_rel1[1] = pos1[1] - ground_h1 + # + # pos_rel0 = origin_trans * pos_rel0 + # pos_rel1 = kin_origin_trans * pos_rel1 + # + # curr_end_err = (pos_rel1 - pos_rel0).squaredNorm() + # end_eff_err += curr_end_err; + # ++num_end_effs; + # } + #} + #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_err = (root_pos0 - root_pos1).squaredNorm() + # + #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 + #com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm() + + #print("pose_err=",pose_err) + #print("vel_err=",vel_err) + pose_reward = math.exp(-err_scale * pose_scale * pose_err) + vel_reward = math.exp(-err_scale * vel_scale * vel_err) + end_eff_reward = math.exp(-err_scale * end_eff_scale * end_eff_err) + root_reward = math.exp(-err_scale * root_scale * root_err) + com_reward = math.exp(-err_scale * com_scale * com_err) + + reward = pose_w * pose_reward + vel_w * vel_reward + end_eff_w * end_eff_reward + root_w * root_reward + com_w * com_reward + + #print("reward = %f (pose_reward=%f, vel_reward=%f, end_eff_reward=%f, root_reward=%f, com_reward=%f)\n", reward, + # pose_reward,vel_reward,end_eff_reward, root_reward, com_reward); + + return reward 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 new file mode 100644 index 000000000..7a9c86b76 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data.py @@ -0,0 +1,19 @@ +import json + +class MotionCaptureData(object): + def __init__(self): + self.Reset() + + def Reset(self): + self._motion_data = [] + + def Load(self, path): + with open(path, 'r') as f: + self._motion_data = json.load(f) + + def NumFrames(self): + return len(self._motion_data['Frames']) + + def KeyFrameDuraction(self): + return self._motion_data['Frames'][0][0] + diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py new file mode 100644 index 000000000..76f13bbf5 --- /dev/null +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pd_controller_stable.py @@ -0,0 +1,144 @@ +import numpy as np + + + +class PDControllerStableMultiDof(object): + def __init__(self, pb): + self._pb = pb + + def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): + + numJoints = len(jointIndices)#self._pb.getNumJoints(bodyUniqueId) + curPos,curOrn = self._pb.getBasePositionAndOrientation(bodyUniqueId) + #q1 = [desiredPositions[0],desiredPositions[1],desiredPositions[2],desiredPositions[3],desiredPositions[4],desiredPositions[5],desiredPositions[6]] + q1 = [curPos[0],curPos[1],curPos[2],curOrn[0],curOrn[1],curOrn[2],curOrn[3]] + + #qdot1 = [0,0,0, 0,0,0,0] + baseLinVel, baseAngVel = self._pb.getBaseVelocity(bodyUniqueId) + + qdot1 = [baseLinVel[0],baseLinVel[1],baseLinVel[2],baseAngVel[0],baseAngVel[1],baseAngVel[2],0] + qError = [0,0,0, 0,0,0,0] + + qIndex=7 + qdotIndex=7 + zeroAccelerations=[0,0,0, 0,0,0,0] + for i in range (numJoints): + js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i]) + + jointPos=js[0] + jointVel = js[1] + q1+=jointPos + + if len(js[0])==1: + desiredPos=desiredPositions[qIndex] + + qdiff=desiredPos - jointPos[0] + qError.append(qdiff) + zeroAccelerations.append(0.) + qdot1+=jointVel + qIndex+=1 + qdotIndex+=1 + if len(js[0])==4: + desiredPos=[desiredPositions[qIndex],desiredPositions[qIndex+1],desiredPositions[qIndex+2],desiredPositions[qIndex+3]] + axis = self._pb.getAxisDifferenceQuaternion(desiredPos,jointPos) + jointVelNew = [jointVel[0],jointVel[1],jointVel[2],0] + qdot1+=jointVelNew + qError.append(axis[0]) + qError.append(axis[1]) + qError.append(axis[2]) + qError.append(0) + desiredVel=[desiredVelocities[qdotIndex],desiredVelocities[qdotIndex+1],desiredVelocities[qdotIndex+2]] + zeroAccelerations+=[0.,0.,0.,0.] + qIndex+=4 + qdotIndex+=4 + + q = np.array(q1) + qdot=np.array(qdot1) + + qdotdesired = np.array(desiredVelocities) + qdoterr = qdotdesired-qdot + + + Kp = np.diagflat(kps) + Kd = np.diagflat(kds) + + p = Kp.dot(qError) + + #np.savetxt("pb_qError.csv", qError, delimiter=",") + #np.savetxt("pb_p.csv", p, delimiter=",") + + d = Kd.dot(qdoterr) + + #np.savetxt("pb_d.csv", d, delimiter=",") + #np.savetxt("pbqdoterr.csv", qdoterr, delimiter=",") + + + M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1, flags=1) + + + M2 = np.array(M1) + #np.savetxt("M2.csv", M2, delimiter=",") + + M = (M2 + Kd * timeStep) + + #np.savetxt("pbM_tKd.csv",M, delimiter=",") + + + + c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations, flags=1) + + + c = np.array(c1) + #np.savetxt("pbC.csv",c, delimiter=",") + A = M + #p = [0]*43 + b = p + d -c + #np.savetxt("pb_acc.csv",b, delimiter=",") + qddot = np.linalg.solve(A, b) + tau = p + d - Kd.dot(qddot) * timeStep + #print("len(tau)=",len(tau)) + maxF = np.array(maxForces) + forces = np.clip(tau, -maxF , maxF ) + return forces + + + +class PDControllerStable(object): + def __init__(self, pb): + self._pb = pb + + def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep): + numJoints = self._pb.getNumJoints(bodyUniqueId) + jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices) + q1 = [] + qdot1 = [] + zeroAccelerations = [] + for i in range (numJoints): + q1.append(jointStates[i][0]) + qdot1.append(jointStates[i][1]) + zeroAccelerations.append(0) + q = np.array(q1) + qdot=np.array(qdot1) + qdes = np.array(desiredPositions) + qdotdes = np.array(desiredVelocities) + qError = qdes - q + qdotError = qdotdes - qdot + Kp = np.diagflat(kps) + Kd = np.diagflat(kds) + p = Kp.dot(qError) + d = Kd.dot(qdotError) + forces = p + d + + M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1) + M2 = np.array(M1) + M = (M2 + Kd * timeStep) + c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations) + c = np.array(c1) + A = M + b = -c + p + d + qddot = np.linalg.solve(A, b) + tau = p + d - Kd.dot(qddot) * timeStep + maxF = np.array(maxForces) + forces = np.clip(tau, -maxF , maxF ) + #print("c=",c) + return tau \ No newline at end of file