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, argparser=None): self._pybullet_client = pybullet_client self._mocap_data = mocap_data self._arg_parser = argparser 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.num_joints = self._pybullet_client.getNumJoints(self._sim_model) self._record_masses() #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.num_joints): 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.num_joints): 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 fall_contact_bodies = [] if self._arg_parser is not None: 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! 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): self._pybullet_client = pybullet_client self._mocap_data = mocap_data print("LOADING quadruped!") startPos = [0.007058990464444105, 0.03149299192130908, 0.4918981912395484] startOrn = [ 0.005934649695708604, 0.7065453990917289, 0.7076373820553712, -0.0027774940359030264 ] self._sim_model = self._pybullet_client.loadURDF("laikago/laikago.urdf", startPos, startOrn, flags=urdfFlags, useFixedBase=False) self._pybullet_client.resetBasePositionAndOrientation(_sim_model, startPos, startOrn) self._end_effectors = [] #ankle and wrist, both left and right self._kin_model = self._pybullet_client.loadURDF("laikago/laikago.urdf", startPos, startOrn, useFixedBase=True) 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 #todo: kp/pd 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()