Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-09-02 00:18:29 +00:00
Code Issues Packages Projects Releases Wiki Activity

remove comments and unused code

Browse Source
This commit is contained in:
tfederico
2020-09-23 16:15:30 +01:00
parent ab8d927845
commit 61df6d2ff4
3 changed files with 80 additions and 449 deletions
Download Patch File Download Diff File Expand all files Collapse all files

403
examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd_multiclip.py vendored
View File

@@ -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)
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):

43
examples/pybullet/gym/pybullet_envs/deep_mimic/env/motion_capture_data_multiclip.py vendored
View File

@@ -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 = []

83
examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env_multiclip.py vendored
View File

@@ -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)
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.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
]
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._humanoid.computeAndApplyPDForces(self.desiredPose[0], maxForces=maxForces) # TODO check if this should be inside the for loop
self._pybullet_client.stepSimulation()
self._pybullet_client.stepSimulation()
def set_sample_count(self, count):
return