diff options
Diffstat (limited to 'examples/pybullet')
-rw-r--r-- | examples/pybullet/examples/humanoidMotionCapture.py | 6 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_backflip.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_backflip.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_cartwheel.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_cartwheel.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_crawl.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_crawl.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_a.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_a.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_b.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_b.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_facedown.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_facedown.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_faceup.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_faceup.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_jump.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_jump.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_kick.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_kick.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_punch.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_punch.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_roll.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_roll.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_run.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_run.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spin.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_spin.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spinkick.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_spinkick.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_walk.txt (renamed from examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt) | 0 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py | 144 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py | 29 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py | 25 | ||||
-rw-r--r-- | examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py | 8 | ||||
-rw-r--r-- | examples/pybullet/pybullet.c | 2 |
21 files changed, 141 insertions, 73 deletions
diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py index 765a88498..3300e121c 100644 --- a/examples/pybullet/examples/humanoidMotionCapture.py +++ b/examples/pybullet/examples/humanoidMotionCapture.py @@ -38,9 +38,9 @@ timeStep = 1./600. p.setPhysicsEngineParameter(fixedTimeStep=timeStep) -path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" -#path = pybullet_data.getDataPath()+"/motions/humanoid3d_cartwheel.txt" -#path = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt" +path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt" +#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_cartwheel.txt" +#path = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt" #p.loadURDF("plane.urdf",[0,0,-1.03]) diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_backflip.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_backflip.txt index 42ea9a7c0..42ea9a7c0 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_backflip.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_backflip.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_cartwheel.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_cartwheel.txt index b59cfb09c..b59cfb09c 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_cartwheel.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_cartwheel.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_crawl.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_crawl.txt index 45a319163..45a319163 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_crawl.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_crawl.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_a.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_a.txt index ab5a4dac0..ab5a4dac0 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_a.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_a.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_b.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_b.txt index d380b5ef3..d380b5ef3 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_dance_b.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_dance_b.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_facedown.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_facedown.txt index b707e2ed9..b707e2ed9 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_facedown.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_facedown.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_faceup.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_faceup.txt index 43f83eff9..43f83eff9 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_getup_faceup.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_getup_faceup.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_jump.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_jump.txt index 540f080c2..540f080c2 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_jump.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_jump.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_kick.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_kick.txt index 44a8642eb..44a8642eb 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_kick.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_kick.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_punch.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_punch.txt index 8f3fdcb71..8f3fdcb71 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_punch.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_punch.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_roll.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_roll.txt index 1d0e78129..1d0e78129 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_roll.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_roll.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_run.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_run.txt index eb22f2f10..eb22f2f10 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_run.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_run.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_spin.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spin.txt index 0b6d61145..0b6d61145 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_spin.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spin.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_spinkick.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spinkick.txt index da920f0a3..da920f0a3 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_spinkick.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_spinkick.txt diff --git a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_walk.txt index 740688a16..740688a16 100644 --- a/examples/pybullet/gym/pybullet_data/motions/humanoid3d_walk.txt +++ b/examples/pybullet/gym/pybullet_data/data/motions/humanoid3d_walk.txt 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 index 94e2bc040..fb123a265 100644 --- 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 @@ -26,8 +26,15 @@ class HumanoidStablePD(object): self._sim_model = self._pybullet_client.loadURDF( "humanoid/humanoid.urdf", [0,0.889540259,0],globalScaling=0.25, useFixedBase=useFixedBase, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER) + #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_model = self._pybullet_client.loadURDF( - "humanoid/humanoid.urdf", [0,12.85,0],globalScaling=0.25, useFixedBase=True, flags=self._pybullet_client.URDF_MAINTAIN_LINK_ORDER) + "humanoid/humanoid.urdf", [0,0.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)): @@ -35,6 +42,18 @@ class HumanoidStablePD(object): self._pybullet_client.changeDynamics(self._sim_model, -1, linearDamping=0, angularDamping=0) self._pybullet_client.changeDynamics(self._kin_model, -1, linearDamping=0, angularDamping=0) + + #todo: add feature to disable simulation for a particular object. Until then, disable all collisions + self._pybullet_client.setCollisionFilterGroupMask(self._kin_model,-1,collisionFilterGroup=0,collisionFilterMask=0) + self._pybullet_client.changeDynamics(self._kin_model,-1,activationState=self._pybullet_client.ACTIVATION_STATE_SLEEP+self._pybullet_client.ACTIVATION_STATE_ENABLE_SLEEPING+self._pybullet_client.ACTIVATION_STATE_DISABLE_WAKEUP) + alpha = 0.4 + self._pybullet_client.changeVisualShape(self._kin_model,-1, rgbaColor=[1,1,1,alpha]) + for j in range (self._pybullet_client.getNumJoints(self._kin_model)): + self._pybullet_client.setCollisionFilterGroupMask(self._kin_model,j,collisionFilterGroup=0,collisionFilterMask=0) + self._pybullet_client.changeDynamics(self._kin_model,j,activationState=self._pybullet_client.ACTIVATION_STATE_SLEEP+self._pybullet_client.ACTIVATION_STATE_ENABLE_SLEEPING+self._pybullet_client.ACTIVATION_STATE_DISABLE_WAKEUP) + self._pybullet_client.changeVisualShape(self._kin_model,j, rgbaColor=[1,1,1,alpha]) + + self._poseInterpolator = humanoid_pose_interpolator.HumanoidPoseInterpolator() for i in range (self._mocap_data.NumFrames()-1): @@ -53,7 +72,7 @@ class HumanoidStablePD(object): 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 @@ -126,9 +145,9 @@ class HumanoidStablePD(object): keyFrameDuration = self._mocap_data.KeyFrameDuraction() cycleTime = self.getCycleTime() #print("self._motion_data.NumFrames()=",self._mocap_data.NumFrames()) - cycles = self.calcCycleCount(t, cycleTime) + self._cycleCount = self.calcCycleCount(t, cycleTime) #print("cycles=",cycles) - frameTime = t - cycles*cycleTime + frameTime = t - self._cycleCount*cycleTime if (frameTime<0): frameTime += cycleTime @@ -143,12 +162,29 @@ class HumanoidStablePD(object): self._frameFraction = (frameTime - self._frame*keyFrameDuration)/(keyFrameDuration) + + def computeCycleOffset(self): + firstFrame=0 + lastFrame = self._mocap_data.NumFrames()-1 + frameData = self._mocap_data._motion_data['Frames'][0] + frameDataNext = self._mocap_data._motion_data['Frames'][lastFrame] + + basePosStart = [frameData[1],frameData[2],frameData[3]] + basePosEnd = [frameDataNext[1],frameDataNext[2],frameDataNext[3]] + self._cycleOffset = [basePosEnd[0]-basePosStart[0],basePosEnd[1]-basePosStart[1],basePosEnd[2]-basePosStart[2]] + return self._cycleOffset + 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) + self._poseInterpolator.Slerp(frameFraction, frameData, frameDataNext, self._pybullet_client) #print("self._poseInterpolator.Slerp(", frameFraction,")=", pose) + self.computeCycleOffset() + oldPos = self._poseInterpolator._basePos + self._poseInterpolator._basePos = [oldPos[0]+self._cycleCount*self._cycleOffset[0],oldPos[1]+self._cycleCount*self._cycleOffset[1],oldPos[2]+self._cycleCount*self._cycleOffset[2]] + pose = self._poseInterpolator.GetPose() + return pose @@ -372,13 +408,32 @@ class HumanoidStablePD(object): return terminates + def quatMul(self, q1, q2): + return [ q1[3] * q2[0] + q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1], + q1[3] * q2[1] + q1[1] * q2[3] + q1[2] * q2[0] - q1[0] * q2[2], + q1[3] * q2[2] + q1[2] * q2[3] + q1[0] * q2[1] - q1[1] * q2[0], + q1[3] * q2[3] - q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2]] + + def calcRootAngVelErr(self, vel0, vel1): + diff = [vel0[0]-vel1[0],vel0[1]-vel1[1], vel0[2]-vel1[2]] + return diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2] + + + + def calcRootRotDiff(self,orn0, orn1): + orn0Conj = [-orn0[0],-orn0[1],-orn0[2],orn0[3]] + q_diff = self.quatMul(orn1, orn0Conj) + axis,angle = self._pybullet_client.getAxisAngleFromQuaternion(q_diff) + return angle*angle + def getReward(self, pose): #from DeepMimic double cSceneImitate::CalcRewardImitate + #todo: compensate for ground height in some parts, once we move to non-flat terrain pose_w = 0.5 vel_w = 0.05 - end_eff_w = 0 #0.15 - root_w = 0#0.2 - com_w = 0#0.1 + end_eff_w = 0.15 + root_w = 0.2 + com_w = 0 #0.1 total_w = pose_w + vel_w + end_eff_w + root_w + com_w pose_w /= total_w @@ -441,8 +496,21 @@ class HumanoidStablePD(object): 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) + rootPosSim,rootOrnSim = self._pybullet_client.getBasePositionAndOrientation(self._sim_model) + rootPosKin ,rootOrnKin = self._pybullet_client.getBasePositionAndOrientation(self._kin_model) + linVelSim, angVelSim = self._pybullet_client.getBaseVelocity(self._sim_model) + linVelKin, angVelKin = self._pybullet_client.getBaseVelocity(self._kin_model) + + + root_rot_err = self.calcRootRotDiff(rootOrnSim,rootOrnKin) + pose_err += root_rot_w * root_rot_err + + + root_vel_diff = [linVelSim[0]-linVelKin[0],linVelSim[1]-linVelKin[1],linVelSim[2]-linVelKin[2]] + root_vel_err = root_vel_diff[0]*root_vel_diff[0]+root_vel_diff[1]*root_vel_diff[1]+root_vel_diff[2]*root_vel_diff[2] + + root_ang_vel_err = self.calcRootAngVelErr( angVelSim, angVelKin) + vel_err += root_rot_w * root_ang_vel_err for j in range (num_joints): curr_pose_err = 0 @@ -473,35 +541,27 @@ class HumanoidStablePD(object): 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 - # + is_end_eff = j in self._end_effectors + if is_end_eff: + + linkStateSim = self._pybullet_client.getLinkState(self._sim_model, j) + linkStateKin = self._pybullet_client.getLinkState(self._kin_model, j) + linkPosSim = linkStateSim[0] + linkPosKin = linkStateKin[0] + linkPosDiff = [linkPosSim[0]-linkPosKin[0],linkPosSim[1]-linkPosKin[1],linkPosSim[2]-linkPosKin[2]] + curr_end_err = linkPosDiff[0]*linkPosDiff[0]+linkPosDiff[1]*linkPosDiff[1]+linkPosDiff[2]*linkPosDiff[2] + end_eff_err += curr_end_err + num_end_effs+=1 + + 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_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 @@ -509,10 +569,8 @@ class HumanoidStablePD(object): #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 + 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) @@ -525,7 +583,13 @@ class HumanoidStablePD(object): 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); + #print("reward=",reward) + #print("pose_reward=",pose_reward) + #print("vel_reward=",vel_reward) + #print("end_eff_reward=",end_eff_reward) + #print("root_reward=",root_reward) + #print("com_reward=",com_reward) return reward diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py index 03f52da20..68e4d613e 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py @@ -12,19 +12,18 @@ import random class PyBulletDeepMimicEnv(Env): - def __init__(self, args=None, enable_draw=False, pybullet_client=None): - super().__init__(args, enable_draw) + def __init__(self, arg_parser=None, enable_draw=False, pybullet_client=None): + super().__init__(arg_parser, enable_draw) self._num_agents = 1 self._pybullet_client = pybullet_client self._isInitialized = False self._useStablePD = True + self._arg_parser = arg_parser self.reset() def reset(self): - startTime = 0. #float(rn)/rnrange * self._humanoid.getCycleTime() - self.t = startTime if not self._isInitialized: if self.enable_draw: self._pybullet_client = bullet_client.BulletClient(connection_mode=p1.GUI) @@ -44,16 +43,20 @@ class PyBulletDeepMimicEnv(Env): self._pybullet_client.changeDynamics(self._planeId, linkIndex=-1, lateralFriction=0.9) self._mocapData = motion_capture_data.MotionCaptureData() - #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt" - motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" + + motion_file = self._arg_parser.parse_strings('motion_file') + 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) - timeStep = 1./600 + timeStep = 1./600. useFixedBase=False self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase) self._isInitialized = True self._pybullet_client.setTimeStep(timeStep) - self._pybullet_client.setPhysicsEngineParameter(numSubSteps=2) + self._pybullet_client.setPhysicsEngineParameter(numSubSteps=1) selfCheck = False @@ -74,6 +77,8 @@ class PyBulletDeepMimicEnv(Env): #startTime = random.randint(0,self._humanoid._mocap_data.NumFrames()-2) rnrange = 1000 rn = random.randint(0,rnrange) + startTime = float(rn)/rnrange * self._humanoid.getCycleTime() + self.t = startTime self._humanoid.setSimTime(startTime) @@ -248,15 +253,19 @@ class PyBulletDeepMimicEnv(Env): def log_val(self, agent_id, val): pass + def update(self, timeStep): #print("pybullet_deep_mimic_env:update timeStep=",timeStep," t=",self.t) + self._pybullet_client.setTimeStep(timeStep) + self._humanoid._timeStep = timeStep + for i in range(1): self.t += timeStep self._humanoid.setSimTime(self.t) if self.desiredPose: - #kinPose = self._humanoid.computePose(self._humanoid._frameFraction) - #self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, initBase=False) + kinPose = self._humanoid.computePose(self._humanoid._frameFraction) + self._humanoid.initializePose(self._humanoid._poseInterpolator, self._humanoid._kin_model, 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) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py index 5cf9a5bcc..2057dde5a 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py @@ -14,17 +14,18 @@ pybullet_client.setAdditionalSearchPath(pybullet_data.getDataPath()) z2y = pybullet_client.getQuaternionFromEuler([-math.pi*0.5,0,0]) #planeId = pybullet_client.loadURDF("plane.urdf",[0,0,0],z2y) planeId= pybullet_client.loadURDF("plane_implicit.urdf",[0,0,0],z2y, useMaximalCoordinates=True) - +pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9) #print("planeId=",planeId) + pybullet_client.configureDebugVisualizer(pybullet_client.COV_ENABLE_Y_AXIS_UP,1) pybullet_client.setGravity(0,-9.8,0) pybullet_client.setPhysicsEngineParameter(numSolverIterations=10) -pybullet_client.changeDynamics(planeId, linkIndex=-1, lateralFriction=0.9) + mocapData = motion_capture_data.MotionCaptureData() -#motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt" -motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" +#motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_walk.txt" +motionPath = pybullet_data.getDataPath()+"/data/motions/humanoid3d_backflip.txt" mocapData.Load(motionPath) timeStep = 1./600 useFixedBase=False @@ -94,23 +95,15 @@ while (1): #humanoid.initializePose(pose=pose, phys_model = humanoid._sim_model, initBase=True, initializeVelocity=True) #humanoid.resetPose() - action = [-6.541649997234344482e-02,1.138873845338821411e-01,-1.215099543333053589e-01,4.610761404037475586e-01,-4.278013408184051514e-01, - 4.064738750457763672e-02,7.801693677902221680e-02,4.934634566307067871e-01,1.321935355663299561e-01,-1.393979601562023163e-02,-6.699572503566741943e-02, - 4.778462052345275879e-01,3.740053176879882812e-01,-3.230125308036804199e-01,-3.549785539507865906e-02,-3.283375874161720276e-03,5.070925354957580566e-01, - 1.033667206764221191e+00,-3.644275963306427002e-01,-3.374500572681427002e-02,1.294951438903808594e-01,-5.880850553512573242e-01, - 1.185980588197708130e-01,6.445263326168060303e-02,1.625719368457794189e-01,4.615224599838256836e-01,3.817881345748901367e-01,-4.382217228412628174e-01, - 1.626710966229438782e-02,-4.743926972150802612e-02,3.833046853542327881e-01,1.067031383514404297e+00,3.039606213569641113e-01, - -1.891726106405258179e-01,3.595829010009765625e-02,-7.283059358596801758e-01] - pos_tar2 = humanoid.convertActionToPose(action) - desiredPose = np.array(pos_tar2) - #desiredPose = humanoid.computePose(humanoid._frameFraction) - #desiredPose = targetPose.GetPose() + desiredPose = humanoid.computePose(humanoid._frameFraction) + #desiredPose = desiredPose.GetPose() #curPose = HumanoidPoseInterpolator() #curPose.reset() s = humanoid.getState() #np.savetxt("pb_record_state_s.csv", s, delimiter=",") - taus = humanoid.computePDForces(desiredPose) + 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] + taus = humanoid.computePDForces(desiredPose, desiredVelocities=None, maxForces=maxForces) #print("taus=",taus) humanoid.applyPDForces(taus) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py index ec5fd17ea..38a9469e9 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py @@ -15,11 +15,11 @@ from pybullet_envs.deep_mimic.env.pybullet_deep_mimic_env import PyBulletDeepMim import sys import random -update_timestep = 1./600. +update_timestep = 1./240. animating = True def update_world(world, time_elapsed): - timeStep = 1./600. + timeStep = update_timestep world.update(timeStep) reward = world.env.calc_reward(agent_id=0) #print("reward=",reward) @@ -48,7 +48,7 @@ args = sys.argv[1:] def build_world(args, enable_draw): arg_parser = build_arg_parser(args) print("enable_draw=",enable_draw) - env = PyBulletDeepMimicEnv(args, enable_draw) + env = PyBulletDeepMimicEnv(arg_parser, enable_draw) world = RLWorld(env, arg_parser) #world.env.set_playback_speed(playback_speed) @@ -83,7 +83,7 @@ if __name__ == '__main__': world = build_world(args, True) while (world.env._pybullet_client.isConnected()): - timeStep = 1./600. + timeStep = update_timestep keys = world.env.getKeyboardEvents() diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index f3e01e5dc..b0d72691e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -10960,6 +10960,8 @@ initpybullet(void) PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping); PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp); PyModule_AddIntConstant(m, "ACTIVATION_STATE_SLEEP", eActivationStateSleep); + PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_WAKEUP", eActivationStateEnableWakeup); + PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_WAKEUP", eActivationStateDisableWakeup); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION); PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT); |