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.889540259,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,12.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.resetPose()
def __init__(self, pybullet_client, mocap_data, timeStep, useFixedBase=True, fall_contact_bodies=[]): self._pybullet_client = pybullet_client self._mocap_data = mocap_data print("LOADING humanoid!") flags = self._pybullet_client.URDF_MAINTAIN_LINK_ORDER + self._pybullet_client.URDF_USE_SELF_COLLISION + self._pybullet_client.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS self._sim_model = self._pybullet_client.loadURDF( "humanoid/humanoid.urdf", [0, 0.889540259, 0], globalScaling=0.25, 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_model = self._pybullet_client.loadURDF( "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)): 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) #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): 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._fall_contact_body_parts = fall_contact_bodies #[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.resetPose()