diff --git a/examples/pybullet/examples/createMultiBodyBatch.py b/examples/pybullet/examples/createMultiBodyBatch.py index 3e8d33cab..dd6fdc38b 100644 --- a/examples/pybullet/examples/createMultiBodyBatch.py +++ b/examples/pybullet/examples/createMultiBodyBatch.py @@ -9,7 +9,7 @@ if (cid<0): p.setPhysicsEngineParameter(numSolverIterations=4, minimumSolverIslandSize=1024) p.setTimeStep(1./120.) -logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "visualShapeBench.json") +logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "createMultiBodyBatch.json") #useMaximalCoordinates is much faster then the default reduced coordinates (Featherstone) p.loadURDF("plane100.urdf", useMaximalCoordinates=True) #disable rendering during creation. @@ -112,9 +112,6 @@ indices=[ 0, 1, 2, 0, 2, 3, #//ground face #p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) #the visual shape and collision shape can be re-used by all createMultiBody instances (instancing) visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, vertices=vertices, indices=indices, uvs=uvs, normals=normals) -#visualShapeId = p.createVisualShape(shapeType=p.GEOM_BOX,rgbaColor=[1,1,1,1], halfExtents=[0.5,0.5,0.5],specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=[1,1,1], vertices=vertices, indices=indices) - -#visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,rgbaColor=[1,1,1,1], specularColor=[0.4,.4,0], visualFramePosition=shift, meshScale=meshScale, fileName="duck.obj") collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_BOX, halfExtents=meshScale)#MESH, vertices=vertices, collisionFramePosition=shift,meshScale=meshScale) texUid = p.loadTexture("tex256.png") @@ -124,16 +121,16 @@ batchPositions=[] -for x in range (33): - for y in range (34): - for z in range (4): +for x in range (32): + for y in range (32): + for z in range (10): batchPositions.append([x*meshScale[0]*5.5,y*meshScale[1]*5.5,(0.5+z)*meshScale[2]*2.5]) -bodyUid = p.createMultiBody(baseMass=1,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True) +bodyUid = p.createMultiBody(baseMass=0,baseInertialFramePosition=[0,0,0],baseCollisionShapeIndex=collisionShapeId, baseVisualShapeIndex = visualShapeId, basePosition =[0,0,2], batchPositions=batchPositions,useMaximalCoordinates=True) p.changeVisualShape(bodyUid,-1, textureUniqueId = texUid) -#p.syncBodyInfo() -#print("numBodies=",p.getNumBodies()) +p.syncBodyInfo() +print("numBodies=",p.getNumBodies()) p.stopStateLogging(logId) p.setGravity(0,0,-10) 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 c3e7ca54e..94e2bc040 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 @@ -186,7 +186,62 @@ class HumanoidStablePD(object): 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): + 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 + + + #self._jointDofCounts=[4,4,4,1,4,4,1,4,1,4,4,1] + maxForce = maxForces[startIndex] + startIndex+=4 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,chest,controlMode, targetPosition=self._poseInterpolator._chestRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=4 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,neck,controlMode,targetPosition=self._poseInterpolator._neckRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=4 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightHip,controlMode,targetPosition=self._poseInterpolator._rightHipRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=1 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightKnee,controlMode,targetPosition=self._poseInterpolator._rightKneeRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=4 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightAnkle,controlMode,targetPosition=self._poseInterpolator._rightAnkleRot,positionGain=kp, force=[maxForce]) + maxForce = [maxForces[startIndex],maxForces[startIndex+1],maxForces[startIndex+2],maxForces[startIndex+3]] + startIndex+=4 + maxForce = maxForces[startIndex] + maxForce = maxForces[startIndex] + startIndex+=1 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,rightElbow, controlMode,targetPosition=self._poseInterpolator._rightElbowRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=4 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftHip, controlMode,targetPosition=self._poseInterpolator._leftHipRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=1 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftKnee, controlMode,targetPosition=self._poseInterpolator._leftKneeRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=4 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftAnkle, controlMode,targetPosition=self._poseInterpolator._leftAnkleRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=4 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftShoulder, controlMode,targetPosition=self._poseInterpolator._leftShoulderRot,positionGain=kp, force=[maxForce]) + maxForce = maxForces[startIndex] + startIndex+=1 + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model,leftElbow, controlMode,targetPosition=self._poseInterpolator._leftElbowRot,positionGain=kp, force=[maxForce]) + #print("startIndex=",startIndex) def getPhase(self): keyFrameDuration = self._mocap_data.KeyFrameDuraction() 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 35f618cc8..03f52da20 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 @@ -17,6 +17,7 @@ class PyBulletDeepMimicEnv(Env): self._num_agents = 1 self._pybullet_client = pybullet_client self._isInitialized = False + self._useStablePD = True self.reset() def reset(self): @@ -260,8 +261,13 @@ class PyBulletDeepMimicEnv(Env): #self._pybullet_client.resetBasePositionAndOrientation(self._humanoid._kin_model, [pos[0]+3,pos[1],pos[2]],orn) #print("desiredPositions=",self.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 = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces) - self._humanoid.applyPDForces(taus) + + if self._useStablePD: + taus = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces) + self._humanoid.applyPDForces(taus) + else: + self._humanoid.setJointMotors(self.desiredPose, maxForces=maxForces) + self._pybullet_client.stepSimulation() diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py index 39c015f6b..ec5fd17ea 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/testrl.py @@ -71,7 +71,7 @@ def build_world(args, enable_draw): print("agent_type=",agent_type) agent = PPOAgent(world, id, json_data) - agent.set_enable_training(True) + agent.set_enable_training(False) world.reset() return world