def _setup_bullet_client(self, client: bc.BulletClient): client.resetSimulation() client.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0) # PyBullet defaults the timestep to 240Hz. Several parameters are tuned with # this value in mind. For example the number of solver iterations and the error # reduction parameters (erp) for contact, friction and non-contact joints. # Attempting to get around this we set the number of substeps so that # timestep * substeps = 240Hz. Bullet (C++) does something to this effect as # well (https://git.io/Jvf0M), but PyBullet does not expose it. client.setPhysicsEngineParameter( fixedTimeStep=self._timestep_sec, numSubSteps=math.ceil(self._timestep_sec / (1 / 240)), ) client.setGravity(0, 0, -9.8) plane_path = self._scenario.plane_filepath # 1e6 is the default value for plane length and width. plane_scale = (max(self._scenario.map_bounding_box[0], self._scenario.map_bounding_box[1]) / 1e6) if not os.path.exists(plane_path): with pkg_resources.path(models, "plane.urdf") as path: plane_path = str(path.absolute()) self._ground_bullet_id = client.loadURDF( plane_path, useFixedBase=True, basePosition=self._scenario.map_bounding_box[2], globalScaling=1.1 * plane_scale, )
def _bullet_connect(self, render: bool) -> BulletClient: if render: bullet_client = BulletClient(connection_mode=pybullet.GUI) bullet_client.configureDebugVisualizer( bullet_client.COV_ENABLE_Y_AXIS_UP, 1) else: bullet_client = BulletClient(connection_mode=pybullet.DIRECT) return bullet_client
def main(args): """Test GUI application.""" client = BulletClient(pb.GUI) client.setGravity(0, 0, -10) client.resetDebugVisualizerCamera(cameraDistance=0.5, cameraYaw=-40.0, cameraPitch=-40.0, cameraTargetPosition=[0.0, 0.0, 0.0]) if args.dofs == 20: hand_model = HandModel20(left_hand=args.left_hand) elif args.dofs == 45: hand_model = HandModel45(left_hand=args.left_hand) else: raise ValueError('Only 20 and 45 DoF models are supported.') flags = sum([ HandBody.FLAG_ENABLE_COLLISION_SHAPES, HandBody.FLAG_ENABLE_VISUAL_SHAPES * args.visual, HandBody.FLAG_JOINT_LIMITS, HandBody.FLAG_DYNAMICS, HandBody.FLAG_USE_SELF_COLLISION * args.self_collisions ]) client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) hand = HandBody(client, hand_model, flags=flags) client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) slider_ids = [] for coord in ('X', 'Y', 'Z'): uid = client.addUserDebugParameter(f'base_{coord}', -0.5, 0.5, 0) slider_ids.append(uid) for coord in ('R', 'P', 'Y'): uid = client.addUserDebugParameter(f'base_{coord}', -3.14, 3.14, 0) slider_ids.append(uid) for i, joint in enumerate(hand_model.joints): name = hand_model.link_names[i] for axis, (lower, upper) in zip(joint.axes, joint.limits): uid = client.addUserDebugParameter(f'{name}[{axis}]', lower, upper, 0) slider_ids.append(uid) client.setRealTimeSimulation(True) try: while client.isConnected(): values = [client.readUserDebugParameter(uid) for uid in slider_ids] position = values[0:3] rotation = client.getQuaternionFromEuler(values[3:6]) angles = values[6:] hand.set_target(position, rotation, angles) except pb.error as err: if str(err) not in [ 'Not connected to physics server.', 'Failed to read parameter.' ]: raise
def config_bullet(self): if self.configs['gui']: bullet_client = BulletClient(connection_mode=pybullet.GUI) else: bullet_client = BulletClient(connection_mode=pybullet.DIRECT) # enable planar reflection bullet_client.configureDebugVisualizer( bullet_client.COV_ENABLE_PLANAR_REFLECTION, 1) # enable shadows bullet_client.configureDebugVisualizer( bullet_client.COV_ENABLE_SHADOWS, 0) if self.configs['debug']: bullet_client.configureDebugVisualizer( bullet_client.COV_ENABLE_GUI, 1) else: bullet_client.configureDebugVisualizer( bullet_client.COV_ENABLE_GUI, 0) bullet_client.setGravity(0, 0, -self.configs['physics']['gravity']) bullet_client.setPhysicsEngineParameter( fixedTimeStep=self.configs['physics']['timestep'], contactERP=self.configs['physics']['contact_erp']) return bullet_client
class Humanoid(object): def __init__(self, pybullet_client, motion_data, baseShift): """Constructs a humanoid and reset it to the initial states. Args: pybullet_client: The instance of BulletClient to manage different simulations. """ self._baseShift = baseShift self._pybullet_client = pybullet_client self.kin_client = BulletClient( pybullet_client.DIRECT ) # use SHARED_MEMORY for visual debugging, start a GUI physics server first self.kin_client.resetSimulation() self.kin_client.setAdditionalSearchPath(pybullet_data.getDataPath()) self.kin_client.configureDebugVisualizer(self.kin_client.COV_ENABLE_Y_AXIS_UP, 1) self.kin_client.setGravity(0, -9.8, 0) self._motion_data = motion_data print("LOADING humanoid!") self._humanoid = self._pybullet_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0], globalScaling=0.25, useFixedBase=False) self._kinematicHumanoid = self.kin_client.loadURDF("humanoid/humanoid.urdf", [0, 0.9, 0], globalScaling=0.25, useFixedBase=False) #print("human #joints=", self._pybullet_client.getNumJoints(self._humanoid)) pose = HumanoidPose() for i in range(self._motion_data.NumFrames() - 1): frameData = self._motion_data._motion_data['Frames'][i] pose.PostProcessMotionData(frameData) self._pybullet_client.resetBasePositionAndOrientation(self._humanoid, self._baseShift, [0, 0, 0, 1]) self._pybullet_client.changeDynamics(self._humanoid, -1, linearDamping=0, angularDamping=0) for j in range(self._pybullet_client.getNumJoints(self._humanoid)): ji = self._pybullet_client.getJointInfo(self._humanoid, j) self._pybullet_client.changeDynamics(self._humanoid, j, linearDamping=0, angularDamping=0) self._pybullet_client.changeVisualShape(self._humanoid, j, rgbaColor=[1, 1, 1, 1]) #print("joint[",j,"].type=",jointTypes[ji[2]]) #print("joint[",j,"].name=",ji[1]) self._initial_state = self._pybullet_client.saveState() self._allowed_body_parts = [11, 14] self.Reset() def Reset(self): self._pybullet_client.restoreState(self._initial_state) self.SetSimTime(0) pose = self.InitializePoseFromMotionData() self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client) def RenderReference(self, t): self.SetSimTime(t) frameData = self._motion_data._motion_data['Frames'][self._frame] frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext] pose = HumanoidPose() pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client) self.ApplyPose(pose, True, True, self._humanoid, self._pybullet_client) 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 SetSimTime(self, t): self._simTime = t #print("SetTimeTime time =",t) keyFrameDuration = self._motion_data.KeyFrameDuraction() cycleTime = keyFrameDuration * (self._motion_data.NumFrames() - 1) #print("self._motion_data.NumFrames()=",self._motion_data.NumFrames()) #print("cycleTime=",cycleTime) cycles = self.CalcCycleCount(t, cycleTime) #print("cycles=",cycles) frameTime = t - cycles * cycleTime if (frameTime < 0): frameTime += cycleTime #print("keyFrameDuration=",keyFrameDuration) #print("frameTime=",frameTime) self._frame = int(frameTime / keyFrameDuration) #print("self._frame=",self._frame) self._frameNext = self._frame + 1 if (self._frameNext >= self._motion_data.NumFrames()): self._frameNext = self._frame self._frameFraction = (frameTime - self._frame * keyFrameDuration) / (keyFrameDuration) #print("self._frameFraction=",self._frameFraction) def Terminates(self): #check if any non-allowed body part hits the ground terminates = False pts = self._pybullet_client.getContactPoints() for p in pts: part = -1 if (p[1] == self._humanoid): part = p[3] if (p[2] == self._humanoid): part = p[4] if (part >= 0 and part not in self._allowed_body_parts): terminates = True return terminates def BuildHeadingTrans(self, rootOrn): #align root transform 'forward' with world-space x axis eul = self._pybullet_client.getEulerFromQuaternion(rootOrn) refDir = [1, 0, 0] rotVec = self._pybullet_client.rotateVector(rootOrn, refDir) heading = math.atan2(-rotVec[2], rotVec[0]) heading2 = eul[1] #print("heading=",heading) headingOrn = self._pybullet_client.getQuaternionFromAxisAngle([0, 1, 0], -heading) return headingOrn def GetPhase(self): keyFrameDuration = self._motion_data.KeyFrameDuraction() cycleTime = keyFrameDuration * (self._motion_data.NumFrames() - 1) phase = self._simTime / cycleTime phase = math.fmod(phase, 1.0) if (phase < 0): phase += 1 return phase def BuildOriginTrans(self): rootPos, rootOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) #print("rootPos=",rootPos, " rootOrn=",rootOrn) invRootPos = [-rootPos[0], 0, -rootPos[2]] #invOrigTransPos, invOrigTransOrn = self._pybullet_client.invertTransform(rootPos,rootOrn) headingOrn = self.BuildHeadingTrans(rootOrn) #print("headingOrn=",headingOrn) headingMat = self._pybullet_client.getMatrixFromQuaternion(headingOrn) #print("headingMat=",headingMat) #dummy, rootOrnWithoutHeading = self._pybullet_client.multiplyTransforms([0,0,0],headingOrn, [0,0,0], rootOrn) #dummy, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0,0,0],rootOrnWithoutHeading, invOrigTransPos, invOrigTransOrn) invOrigTransPos, invOrigTransOrn = self._pybullet_client.multiplyTransforms([0, 0, 0], headingOrn, invRootPos, [0, 0, 0, 1]) #print("invOrigTransPos=",invOrigTransPos) #print("invOrigTransOrn=",invOrigTransOrn) invOrigTransMat = self._pybullet_client.getMatrixFromQuaternion(invOrigTransOrn) #print("invOrigTransMat =",invOrigTransMat ) return invOrigTransPos, invOrigTransOrn def InitializePoseFromMotionData(self): frameData = self._motion_data._motion_data['Frames'][self._frame] frameDataNext = self._motion_data._motion_data['Frames'][self._frameNext] pose = HumanoidPose() pose.Slerp(self._frameFraction, frameData, frameDataNext, self._pybullet_client) return pose def ApplyAction(self, action): #turn action into pose pose = HumanoidPose() pose.Reset() index = 0 angle = action[index] axis = [action[index + 1], action[index + 2], action[index + 3]] index += 4 pose._chestRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) #print("pose._chestRot=",pose._chestRot) angle = action[index] axis = [action[index + 1], action[index + 2], action[index + 3]] index += 4 pose._neckRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) angle = action[index] axis = [action[index + 1], action[index + 2], action[index + 3]] index += 4 pose._rightHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) angle = action[index] index += 1 pose._rightKneeRot = [angle] angle = action[index] axis = [action[index + 1], action[index + 2], action[index + 3]] index += 4 pose._rightAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) angle = action[index] axis = [action[index + 1], action[index + 2], action[index + 3]] index += 4 pose._rightShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) angle = action[index] index += 1 pose._rightElbowRot = [angle] angle = action[index] axis = [action[index + 1], action[index + 2], action[index + 3]] index += 4 pose._leftHipRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) angle = action[index] index += 1 pose._leftKneeRot = [angle] angle = action[index] axis = [action[index + 1], action[index + 2], action[index + 3]] index += 4 pose._leftAnkleRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) angle = action[index] axis = [action[index + 1], action[index + 2], action[index + 3]] index += 4 pose._leftShoulderRot = self._pybullet_client.getQuaternionFromAxisAngle(axis, angle) angle = action[index] index += 1 pose._leftElbowRot = [angle] #print("index=",index) initializeBase = False initializeVelocities = False self.ApplyPose(pose, initializeBase, initializeVelocities, self._humanoid, self._pybullet_client) def ApplyPose(self, pose, initializeBase, initializeVelocities, humanoid, bc): #todo: get tunable parametes from a json file or from URDF (kd, maxForce) if (initializeBase): bc.changeVisualShape(humanoid, 2, rgbaColor=[1, 0, 0, 1]) basePos = [ pose._basePos[0] + self._baseShift[0], pose._basePos[1] + self._baseShift[1], pose._basePos[2] + self._baseShift[2] ] bc.resetBasePositionAndOrientation(humanoid, basePos, pose._baseOrn) if initializeVelocities: bc.resetBaseVelocity(humanoid, pose._baseLinVel, pose._baseAngVel) #print("resetBaseVelocity=",pose._baseLinVel) else: bc.changeVisualShape(humanoid, 2, rgbaColor=[1, 1, 1, 1]) kp = 0.03 chest = 1 neck = 2 rightShoulder = 3 rightElbow = 4 leftShoulder = 6 leftElbow = 7 rightHip = 9 rightKnee = 10 rightAnkle = 11 leftHip = 12 leftKnee = 13 leftAnkle = 14 controlMode = bc.POSITION_CONTROL if (initializeBase): if initializeVelocities: bc.resetJointStateMultiDof(humanoid, chest, pose._chestRot, pose._chestVel) bc.resetJointStateMultiDof(humanoid, neck, pose._neckRot, pose._neckVel) bc.resetJointStateMultiDof(humanoid, rightHip, pose._rightHipRot, pose._rightHipVel) bc.resetJointStateMultiDof(humanoid, rightKnee, pose._rightKneeRot, pose._rightKneeVel) bc.resetJointStateMultiDof(humanoid, rightAnkle, pose._rightAnkleRot, pose._rightAnkleVel) bc.resetJointStateMultiDof(humanoid, rightShoulder, pose._rightShoulderRot, pose._rightShoulderVel) bc.resetJointStateMultiDof(humanoid, rightElbow, pose._rightElbowRot, pose._rightElbowVel) bc.resetJointStateMultiDof(humanoid, leftHip, pose._leftHipRot, pose._leftHipVel) bc.resetJointStateMultiDof(humanoid, leftKnee, pose._leftKneeRot, pose._leftKneeVel) bc.resetJointStateMultiDof(humanoid, leftAnkle, pose._leftAnkleRot, pose._leftAnkleVel) bc.resetJointStateMultiDof(humanoid, leftShoulder, pose._leftShoulderRot, pose._leftShoulderVel) bc.resetJointStateMultiDof(humanoid, leftElbow, pose._leftElbowRot, pose._leftElbowVel) else: bc.resetJointStateMultiDof(humanoid, chest, pose._chestRot) bc.resetJointStateMultiDof(humanoid, neck, pose._neckRot) bc.resetJointStateMultiDof(humanoid, rightHip, pose._rightHipRot) bc.resetJointStateMultiDof(humanoid, rightKnee, pose._rightKneeRot) bc.resetJointStateMultiDof(humanoid, rightAnkle, pose._rightAnkleRot) bc.resetJointStateMultiDof(humanoid, rightShoulder, pose._rightShoulderRot) bc.resetJointStateMultiDof(humanoid, rightElbow, pose._rightElbowRot) bc.resetJointStateMultiDof(humanoid, leftHip, pose._leftHipRot) bc.resetJointStateMultiDof(humanoid, leftKnee, pose._leftKneeRot) bc.resetJointStateMultiDof(humanoid, leftAnkle, pose._leftAnkleRot) bc.resetJointStateMultiDof(humanoid, leftShoulder, pose._leftShoulderRot) bc.resetJointStateMultiDof(humanoid, leftElbow, pose._leftElbowRot) bc.setJointMotorControlMultiDof(humanoid, chest, controlMode, targetPosition=pose._chestRot, positionGain=kp, force=[200]) bc.setJointMotorControlMultiDof(humanoid, neck, controlMode, targetPosition=pose._neckRot, positionGain=kp, force=[50]) bc.setJointMotorControlMultiDof(humanoid, rightHip, controlMode, targetPosition=pose._rightHipRot, positionGain=kp, force=[200]) bc.setJointMotorControlMultiDof(humanoid, rightKnee, controlMode, targetPosition=pose._rightKneeRot, positionGain=kp, force=[150]) bc.setJointMotorControlMultiDof(humanoid, rightAnkle, controlMode, targetPosition=pose._rightAnkleRot, positionGain=kp, force=[90]) bc.setJointMotorControlMultiDof(humanoid, rightShoulder, controlMode, targetPosition=pose._rightShoulderRot, positionGain=kp, force=[100]) bc.setJointMotorControlMultiDof(humanoid, rightElbow, controlMode, targetPosition=pose._rightElbowRot, positionGain=kp, force=[60]) bc.setJointMotorControlMultiDof(humanoid, leftHip, controlMode, targetPosition=pose._leftHipRot, positionGain=kp, force=[200]) bc.setJointMotorControlMultiDof(humanoid, leftKnee, controlMode, targetPosition=pose._leftKneeRot, positionGain=kp, force=[150]) bc.setJointMotorControlMultiDof(humanoid, leftAnkle, controlMode, targetPosition=pose._leftAnkleRot, positionGain=kp, force=[90]) bc.setJointMotorControlMultiDof(humanoid, leftShoulder, controlMode, targetPosition=pose._leftShoulderRot, positionGain=kp, force=[100]) bc.setJointMotorControlMultiDof(humanoid, leftElbow, controlMode, targetPosition=pose._leftElbowRot, positionGain=kp, force=[60]) #debug space #if (False): # for j in range (bc.getNumJoints(self._humanoid)): # js = bc.getJointState(self._humanoid, j) # bc.resetJointState(self._humanoidDebug, j,js[0]) # jsm = bc.getJointStateMultiDof(self._humanoid, j) # if (len(jsm[0])>0): # bc.resetJointStateMultiDof(self._humanoidDebug,j,jsm[0]) def GetState(self): stateVector = [] phase = self.GetPhase() #print("phase=",phase) stateVector.append(phase) rootTransPos, rootTransOrn = self.BuildOriginTrans() basePos, baseOrn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) rootPosRel, dummy = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos, [0, 0, 0, 1]) #print("!!!rootPosRel =",rootPosRel ) #print("rootTransPos=",rootTransPos) #print("basePos=",basePos) localPos, localOrn = self._pybullet_client.multiplyTransforms(rootTransPos, rootTransOrn, basePos, baseOrn) localPos = [ localPos[0] - rootPosRel[0], localPos[1] - rootPosRel[1], localPos[2] - rootPosRel[2] ] #print("localPos=",localPos) stateVector.append(rootPosRel[1]) self.pb2dmJoints = [0, 1, 2, 9, 10, 11, 3, 4, 5, 12, 13, 14, 6, 7, 8] for pbJoint in range(self._pybullet_client.getNumJoints(self._humanoid)): j = self.pb2dmJoints[pbJoint] #print("joint order:",j) ls = self._pybullet_client.getLinkState(self._humanoid, j, computeForwardKinematics=True) linkPos = ls[0] linkOrn = ls[1] linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms( rootTransPos, rootTransOrn, linkPos, linkOrn) if (linkOrnLocal[3] < 0): linkOrnLocal = [-linkOrnLocal[0], -linkOrnLocal[1], -linkOrnLocal[2], -linkOrnLocal[3]] linkPosLocal = [ linkPosLocal[0] - rootPosRel[0], linkPosLocal[1] - rootPosRel[1], linkPosLocal[2] - rootPosRel[2] ] for l in linkPosLocal: stateVector.append(l) #re-order the quaternion, DeepMimic uses w,x,y,z stateVector.append(linkOrnLocal[3]) stateVector.append(linkOrnLocal[0]) stateVector.append(linkOrnLocal[1]) stateVector.append(linkOrnLocal[2]) for pbJoint in range(self._pybullet_client.getNumJoints(self._humanoid)): j = self.pb2dmJoints[pbJoint] ls = self._pybullet_client.getLinkState(self._humanoid, j, computeLinkVelocity=True) linkLinVel = ls[6] linkAngVel = ls[7] for l in linkLinVel: stateVector.append(l) for l in linkAngVel: stateVector.append(l) #print("stateVector len=",len(stateVector)) #for st in range (len(stateVector)): # print("state[",st,"]=",stateVector[st]) return stateVector def GetReward(self): #from DeepMimic double cSceneImitate::CalcRewardImitate pose_w = 0.5 vel_w = 0.05 end_eff_w = 0 #0.15 root_w = 0 #0.2 com_w = 0.1 total_w = pose_w + vel_w + end_eff_w + root_w + com_w pose_w /= total_w vel_w /= total_w end_eff_w /= total_w root_w /= total_w com_w /= total_w pose_scale = 2 vel_scale = 0.1 end_eff_scale = 40 root_scale = 5 com_scale = 10 err_scale = 1 reward = 0 pose_err = 0 vel_err = 0 end_eff_err = 0 root_err = 0 com_err = 0 heading_err = 0 #create a mimic reward, comparing the dynamics humanoid with a kinematic one pose = self.InitializePoseFromMotionData() #print("self._kinematicHumanoid=",self._kinematicHumanoid) #print("kinematicHumanoid #joints=",self.kin_client.getNumJoints(self._kinematicHumanoid)) self.ApplyPose(pose, True, True, self._kinematicHumanoid, self.kin_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(); #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, 0.10416, 0.0625, 0.0416, 0.0625, 0.0416, 0.0000 ] num_end_effs = 0 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) for j in range(num_joints): curr_pose_err = 0 curr_vel_err = 0 w = mJointWeights[j] simJointInfo = self._pybullet_client.getJointStateMultiDof(self._humanoid, j) #print("simJointInfo.pos=",simJointInfo[0]) #print("simJointInfo.vel=",simJointInfo[1]) kinJointInfo = self.kin_client.getJointStateMultiDof(self._kinematicHumanoid, j) #print("kinJointInfo.pos=",kinJointInfo[0]) #print("kinJointInfo.vel=",kinJointInfo[1]) if (len(simJointInfo[0]) == 1): angle = simJointInfo[0][0] - kinJointInfo[0][0] curr_pose_err = angle * angle velDiff = simJointInfo[1][0] - kinJointInfo[1][0] curr_vel_err = velDiff * velDiff if (len(simJointInfo[0]) == 4): #print("quaternion diff") diffQuat = self._pybullet_client.getDifferenceQuaternion(simJointInfo[0], kinJointInfo[0]) axis, angle = self._pybullet_client.getAxisAngleFromQuaternion(diffQuat) curr_pose_err = angle * angle diffVel = [ simJointInfo[1][0] - kinJointInfo[1][0], simJointInfo[1][1] - kinJointInfo[1][1], simJointInfo[1][2] - kinJointInfo[1][2] ] curr_vel_err = diffVel[0] * diffVel[0] + diffVel[1] * diffVel[1] + diffVel[2] * diffVel[2] 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 # #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_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 #com_err = 0.1 * (com_vel1_world - com_vel0_world).squaredNorm() #print("pose_err=",pose_err) #print("vel_err=",vel_err) pose_reward = math.exp(-err_scale * pose_scale * pose_err) vel_reward = math.exp(-err_scale * vel_scale * vel_err) end_eff_reward = math.exp(-err_scale * end_eff_scale * end_eff_err) root_reward = math.exp(-err_scale * root_scale * root_err) com_reward = math.exp(-err_scale * com_scale * com_err) 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); return reward def GetBasePosition(self): pos, orn = self._pybullet_client.getBasePositionAndOrientation(self._humanoid) return pos
currentdir = os.path.dirname( os.path.abspath(inspect.getfile(inspect.currentframe()))) parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0, parentdir) from pybullet_envs.deep_mimic.humanoid import Humanoid from pybullet_utils.bullet_client import BulletClient from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData import pybullet_data import pybullet import time import random bc = BulletClient(connection_mode=pybullet.GUI) bc.setAdditionalSearchPath(pybullet_data.getDataPath()) bc.configureDebugVisualizer(bc.COV_ENABLE_Y_AXIS_UP, 1) bc.setGravity(0, -9.8, 0) motion = MotionCaptureData() motionPath = pybullet_data.getDataPath( ) + "/motions/humanoid3d_walk.txt" #humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt" motion.Load(motionPath) #print("numFrames = ", motion.NumFrames()) simTimeId = bc.addUserDebugParameter("simTime", 0, motion.NumFrames() - 1.1, 0) y2zOrn = bc.getQuaternionFromEuler([-1.57, 0, 0]) bc.loadURDF("plane.urdf", [0, -0.04, 0], y2zOrn) humanoid = Humanoid(bc, motion, [0, 0, 0]) #4000,0,5000]) simTime = 0
class ModularEnv(gym.Env): """Abstract modular environment""" metadata = {'render.modes': ['human']} def __init__(self, terrain=None): # Create pybullet interfaces self.client = BulletClient(connection_mode=pyb.DIRECT) self._last_render = time.time() # Setup information needed for simulation self.dt = 1. / 240. self.client.setAdditionalSearchPath(ASSET_PATH) self.terrain_type = terrain # Stored for user interactions self.morphology = None self.multi_id = None self._joint_ids = [] self._joints = [] # Used for user interaction: self._real_time = False # Run setup self.setup() def load_terrain(self): """Helper method to load terrain for ground""" if not self.terrain_type: return None elif type(self.terrain_type) is PNGTerrain: shape = self.client.createCollisionShape( shapeType=pyb.GEOM_HEIGHTFIELD, meshScale=self.terrain_type.scale, fileName=self.terrain_type.height_field) assert shape >= 0, "Could not load PNGTerrain shape" terrain = self.client.createMultiBody(0, shape) if self.terrain_type.texture: texture = self.client.loadTexture(self.terrain_type.texture) assert texture >= 0, "Could not load PNGTerrain texture" self.client.changeVisualShape(terrain, -1, textureUniqueId=texture) if self.terrain_type.position: self.client.resetBasePositionAndOrientation( terrain, self.terrain_type.position, [0, 0, 0, 1]) elif type(self.terrain_type) is ArrayTerrain: shape = self.client.createCollisionShape( shapeType=pyb.GEOM_HEIGHTFIELD, meshScale=self.terrain_type.scale, heightfieldTextureScaling=(self.terrain_type.rows - 1) / 2., heightfieldData=self.terrain_type.field.flatten(), numHeightfieldRows=self.terrain_type.rows, numHeightfieldColumns=self.terrain_type.cols) assert shape >= 0, "Could not create ArrayTerrain shape" terrain = self.client.createMultiBody(0, shape) self.client.resetBasePositionAndOrientation( terrain, [0, 0, 0], [0, 0, 0, 1]) assert terrain >= 0, "Could not load terrain" self.client.changeVisualShape(terrain, -1, rgbaColor=[1, 1, 1, 1]) return terrain def setup(self): """Helper method to initialize default environment""" # This is abstracted out from '__init__' because we need to do this # first time 'render' is called self.client.resetSimulation() self.client.setGravity(0, 0, -9.81) # Load ground plane for robots to walk on self.plane_id = self.client.loadURDF('plane/plane.urdf', useMaximalCoordinates=1) assert self.plane_id >= 0, "Could not load 'plane.urdf'" # Change dynamics parameters from: # https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py#L45 self.client.changeDynamics(self.plane_id, -1, lateralFriction=0.9) # self.client.setPhysicsEngineParameter(numSolverIterations=10) # self.client.setPhysicsEngineParameter(minimumSolverIslandSize=100) self.client.setPhysicsEngineParameter(contactERP=0) # Extract time step for sleep during rendering self.dt = self.client.getPhysicsEngineParameters()['fixedTimeStep'] def close(self): self.client.disconnect() def reset(self, morphology=None, max_size=None): # Disable rendering during spawning self.client.configureDebugVisualizer(pyb.COV_ENABLE_RENDERING, 0) # Reset the environment by running all setup code again self.setup() # Reset internal state self.morphology = None self.multi_id = None self._joint_ids = [] self._joints = [] # Check method arguments for simple logic errors if morphology is None: raise TypeError("Morphology cannot be 'None'!") if max_size is not None and max_size < 1: raise ValueError("'max_size' must be larger than 0") # NOTE: Utilize deep copy here to avoid interfering with morphology, # this means that if we want to do structural changes we can do so on # our own private instance. Which is nice for maximum size etc. self.morphology = copy.deepcopy(morphology.root) # Mapping from module to PyBullet ID spawned_ids = {} # NOTE: We are using explicit queue handling here so that we can # ignore children of overlapping modules queue = deque([self.morphology.root]) while len(queue) > 0: module = queue.popleft() assert isinstance(module, Module3D), "{} does not inherit\ from 3D Module".format(type(module)) # Spawn module in world m_id = module.spawn(self.client) # Check if the module overlaps aabb_min, aabb_max = self.client.getAABB(m_id) # Check overlapping modules overlap = self.client.getOverlappingObjects(aabb_min, aabb_max) # NOTE: An object always collides with it self overlapping_modules = False if overlap: overlapping_modules = any( [u_id != m_id for u_id, _ in overlap]) # Check against plane aabb_min, aabb_max = self.client.getAABB(self.plane_id) plane_overlap = self.client.getOverlappingObjects( aabb_min, aabb_max) overlapping_plane = False if plane_overlap: overlapping_plane = any( [u_id != self.plane_id for u_id, _ in plane_overlap]) # If overlap is detected de-spawn module and continue if overlapping_modules or overlapping_plane: # Remove from simulation self.client.removeBody(m_id) # Remove from our private copy parent = module.parent if parent: del parent[module] else: raise RuntimeError( "Trying to remove root module due to collision!") continue # Add children to queue for processing queue.extend(module.children) # Add ID to spawned IDs so that we can remove them later spawned_ids[module] = m_id # Add joint if present if module.joint: self._joints.append(module.joint) # Check size constraints if max_size is not None and len(spawned_ids) >= max_size: # If we are above max desired spawn size drain queue and remove for module in queue: parent = module.parent if parent: del parent[module] else: raise RuntimeError("Trying to prune root link!") break # Create multi body builder and instantiate with morphological tree and # module IDs builder = MultiBodyBuilder(self.morphology, spawned_ids, self.client, flags=pyb.URDF_USE_SELF_COLLISION) # Before spawning multi body we remove all modules so far # NOTE: We must do that here after all modules have been spawned to not # interfere with collision detection for m_id in spawned_ids.values(): self.client.removeBody(m_id) # Instantiate the builder creating a multi body from the morphology multi_id = builder.build() assert multi_id >= 0, "Could not spawn multibody!" self.multi_id = multi_id for jid in range(self.client.getNumJoints(self.multi_id)): j_info = self.client.getJointInfo(self.multi_id, jid) if j_info[2] != pyb.JOINT_FIXED: self._joint_ids.append(jid) self.observation_space = gym.spaces.Box(-100., 100., shape=(3 * len(self._joints), ), dtype=np.float64) lows = np.repeat(-1.57, len(self._joints)) self.action_space = gym.spaces.Box(lows, -1. * lows, dtype=np.float64) # Load additional terrain if requested # NOTE: We load terrain after collision detection to avoid collisions # with terrain itself if self.load_terrain() is not None: # If a different terrain was loaded we remove the plane self.client.removeBody(self.plane_id) self.plane_id = None # Enable rendering here self.client.configureDebugVisualizer(pyb.COV_ENABLE_RENDERING, 1) return self.observation() def act(self, action): """Helper function to apply actions to robot""" assert action.shape[0] == len(self._joint_ids), 'Action not equal to\ number of joints' for j_id, cfg, act in zip(self._joint_ids, self._joints, action): # Create a local copy so we can delete from it l_cfg = cfg.copy() # If joint should be limited we check that here if 'limit' in l_cfg: act = max(l_cfg['limit'][0], min(l_cfg['limit'][1], act)) del l_cfg['limit'] # Extract type of 'target' for joint l_cfg[l_cfg['target']] = act del l_cfg['target'] self.client.setJointMotorControl2(self.multi_id, j_id, **l_cfg) def step(self, action): self.act(np.asarray(action)) self.client.stepSimulation() return self.observation(), self.reward(), False, {} def observation(self): """Get observation from environment The default observation is `numpy.concatenate([positions, velocities, torques])` for all movable joints.""" # If the body does not have any movable joints: if not self._joint_ids: return np.array([]) positions = [] velocities = [] torques = [] states = self.client.getJointStates(self.multi_id, self._joint_ids) for state in states: positions.append(state[0]) velocities.append(state[1]) torques.append(state[3]) return np.concatenate([positions, velocities, torques]) def reward(self): """Estimate current reward""" # Get state of root link pos, _ = self.client.getBasePositionAndOrientation(self.multi_id) # Extract position position = np.array(pos) # We are only interested in distance in (X, Y) position[-1] = 0.0 # Return the root's distance from World center return np.linalg.norm(position) def render(self, mode="human"): info = self.client.getConnectionInfo() if info['connectionMethod'] != pyb.GUI and mode == 'human': # Close current simulation and start new with GUI self.close() self.client = BulletClient(connection_mode=pyb.GUI) # Disable rendering during spawning self.client.configureDebugVisualizer(pyb.COV_ENABLE_RENDERING, 0) # Reset if morphology is set if self.morphology is not None: self.reset(self.morphology) else: self.setup() # Configure viewport self.client.configureDebugVisualizer(pyb.COV_ENABLE_GUI, 0) self.client.configureDebugVisualizer( pyb.COV_ENABLE_PLANAR_REFLECTION, 1) self.client.resetDebugVisualizerCamera(0.5, 50.0, -35.0, (0, 0, 0)) # Setup timing for correct sleeping when rendering self._last_render = time.time() elif info['connectionMethod'] == pyb.GUI: # Handle interaction with simulation self.handle_interaction() # Calculate time to sleep now = time.time() diff = now - self._last_render to_sleep = self.dt - diff if to_sleep > 0: time.sleep(to_sleep) self._last_render = now else: raise RuntimeError( "Unknown pybullet mode ({}) or render mode ({})".format( info['connectionMethod'], mode)) def handle_interaction(self): """Handle user interaction with simulation This function is called in the 'render' call and is only called if the GUI is visible""" keys = self.client.getKeyboardEvents() # If 'n' is pressed then we want to step the simulation next_key = ord('n') if next_key in keys and keys[next_key] & pyb.KEY_WAS_TRIGGERED: self.client.stepSimulation() # If space is pressed we start/stop real-time simulation space = ord(' ') if space in keys and keys[space] & pyb.KEY_WAS_TRIGGERED: self._real_time = not self._real_time self.client.setRealTimeSimulation(self._real_time) # if 'r' is pressed we restart simulation r = ord('r') if r in keys and keys[r] & pyb.KEY_WAS_TRIGGERED: real_time = self._real_time self.client.setRealTimeSimulation(False) self.reset(self.morphology) self.client.setRealTimeSimulation(real_time)
def run(): """Load URDF in PyBullet, move arms from initial to target state. """ # Start simulation and load URDF client = BulletClient(pb.GUI) client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) with prl_ur5_robot_urdf() as filename: body_id = client.loadURDF(filename, useFixedBase=1) client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) # Map joint names to indices joint_map = {} for i in range(client.getNumJoints(body_id)): info = client.getJointInfo(body_id, i) name = info[1].decode('utf-8', 'strict') joint_map[name] = i # Show joints map print('Robot joints:') for name, i in joint_map.items(): print('{}: {}'.format(i, name)) # Reset joints at initial state init_state = { 'left_elbow_joint': -1.57, 'left_shoulder_lift_joint': -1.57, 'left_shoulder_pan_joint': -1.57, 'left_wrist_1_joint': 0, 'left_wrist_2_joint': 1.57, 'left_wrist_3_joint': -0.785, 'right_elbow_joint': 1.57, 'right_shoulder_lift_joint': -1.57, 'right_shoulder_pan_joint': 1.57, 'right_wrist_1_joint': -3.14, 'right_wrist_2_joint': -1.57, 'right_wrist_3_joint': 0.785, } for name, value in init_state.items(): client.resetJointState(body_id, joint_map[name], value) # Apply target state target_state = { 'left_elbow_joint': -1.57, 'left_shoulder_lift_joint': -1.57, 'left_shoulder_pan_joint': -1.57, 'left_wrist_1_joint': -1.57, 'left_wrist_2_joint': 2.36, 'left_wrist_3_joint': -1.57, 'right_elbow_joint': 1.57, 'right_shoulder_lift_joint': -1.57, 'right_shoulder_pan_joint': 1.57, 'right_wrist_1_joint': -1.57, 'right_wrist_2_joint': -2.36, 'right_wrist_3_joint': 1.57, } client.setJointMotorControlArray( bodyUniqueId=body_id, jointIndices=[joint_map[name] for name in target_state], controlMode=pb.POSITION_CONTROL, targetPositions=[value for value in target_state.values()], forces=[10]*len(target_state)) # Simulate until the user to close the window while client.isConnected(): client.stepSimulation() time.sleep(0.1)
import os, inspect currentdir = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe()))) parentdir = os.path.dirname(os.path.dirname(currentdir)) os.sys.path.insert(0,parentdir) from pybullet_envs.deep_mimic.humanoid import Humanoid from pybullet_utils.bullet_client import BulletClient from pybullet_envs.deep_mimic.motion_capture_data import MotionCaptureData import pybullet_data import pybullet import time import random bc = BulletClient(connection_mode=pybullet.GUI) bc.setAdditionalSearchPath(pybullet_data.getDataPath()) bc.configureDebugVisualizer(bc.COV_ENABLE_Y_AXIS_UP,1) bc.setGravity(0,-9.8,0) motion=MotionCaptureData() motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_walk.txt"#humanoid3d_spinkick.txt"#/motions/humanoid3d_backflip.txt" motion.Load(motionPath) #print("numFrames = ", motion.NumFrames()) simTimeId= bc.addUserDebugParameter("simTime",0,motion.NumFrames()-1.1,0) y2zOrn = bc.getQuaternionFromEuler([-1.57,0,0]) bc.loadURDF("plane.urdf",[0,-0.04,0], y2zOrn) humanoid = Humanoid(bc, motion,[0,0,0])#4000,0,5000]) simTime = 0
class HandEnv(gym.Env): """The base Hand Environment class.""" # The environment can have both or a single hand HAND_RIGHT = 1 HAND_LEFT = 2 HAND_BOTH = HAND_LEFT + HAND_RIGHT # The environment can be controlled in joint or MANO-pose spaces MODE_JOINT = 1 MODE_MANO = 2 MODE_LIST = (MODE_JOINT, MODE_MANO) # Shape betas magnitude SHAPE_BETAS_MAGNITUDE = 0.1 metadata = { 'render.modes': ['rgb_array'], 'video.frames_per_second': 60.0, 'video.output_frames_per_second': 30.0, 'step_time': 1.0 / 60.0} def __init__(self, hand_side=HAND_RIGHT, control_mode=MODE_JOINT, hand_model_cls=HandModel20, randomize_hand_shape=False): """Constructor of a HandEnv. Keyword Arguments: hand_side {int} -- hand side: left, right or both (default: {HAND_BOTH}) control_mode {int} -- control modalities (default: {MODE_JOINT}) hand_model_cls {type} -- hand kinematic model (default: {HandModel20}) randomize_hand_shape {bool} -- randomize hand shape at reset (default: {True}) """ assert hand_side & self.HAND_BOTH, f'Wrong hand side flag: {hand_side}' assert control_mode in self.MODE_LIST, f'Wrong control mode flag: {control_mode}' self._hand_side = hand_side self._control_mode = control_mode self._randomize_hand_shape = randomize_hand_shape self._show_window = False self._random = None self._client = None self._hand_bodies = None self._camera_shape = (320, 240) self._camera_view = None self._camera_proj = None self._hand_models = [] if self.HAND_LEFT & hand_side: self._hand_models.append(hand_model_cls(left_hand=True)) if self.HAND_RIGHT & hand_side: self._hand_models.append(hand_model_cls(left_hand=False)) # if control in joint space if self.MODE_JOINT == control_mode: joint_low, joint_high = map(np.float32, self._hand_models[0].dofs_limits) self.action_space = gym.spaces.Tuple([ gym.spaces.Tuple(( gym.spaces.Box(-5.0, 5.0, shape=(3,)), # base position x,y,z gym.spaces.Box(-1.0, 1.0, shape=(4,)), # base quaternion x,y,z,w gym.spaces.Box(joint_low, joint_high), # joint angles )) for _ in self._hand_models]) self.observation_space = gym.spaces.Tuple([ gym.spaces.Tuple(( gym.spaces.Box(-5.0, 5.0, shape=(3,)), # base position x,y,z gym.spaces.Box(-1.0, 1.0, shape=(4,)), # base quaternion x,y,z,w gym.spaces.Box(-10.0, 10.0, shape=(6,)), # base constraint forces gym.spaces.Box(joint_low, joint_high), # joint angles gym.spaces.Box(-3.0, 3.0, shape=(len(joint_low),)), # joint velocities gym.spaces.Box(-1.0, 1.0, shape=(len(joint_low),)), # joint torques )) for _ in self._hand_models]) # if control in MANO space elif self.MODE_MANO == control_mode: self.action_space = gym.spaces.Tuple([ gym.spaces.Tuple(( gym.spaces.Box(-5.0, 5.0, shape=(3,)), # trans gym.spaces.Box(-np.pi, np.pi, shape=(16, 3)), # pose )) for _ in self._hand_models]) self.observation_space = gym.spaces.Tuple([ gym.spaces.Tuple(( gym.spaces.Box(-5.0, 5.0, shape=(3,)), # trans gym.spaces.Box(-np.pi, np.pi, shape=(16, 3)), # pose )) for _ in self._hand_models]) def show_window(self, show=True): """Show GUI window or not. Keyword Arguments: show {bool} -- show flag (default: {True}) """ self._show_window = show def reset(self, initial_hands_state=None, **_kwargs): """Resets the environment to an initial state and returns an initial observation. Keyword Arguments: initial_hands_state {int} -- override the initial hands state (default: {None}) Returns: observation (object): the initial observation. """ if self._client is None: self._client = BulletClient(pb.GUI if self._show_window else pb.DIRECT) self._client.configureDebugVisualizer(pb.COV_ENABLE_GUI, 0) self._setup_camera() self._client.resetSimulation() self._client.setAdditionalSearchPath(pd.getDataPath()) self._client.setPhysicsEngineParameter(deterministicOverlappingPairs=1) self._client.setGravity(0, 0, -10) self._client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 0) # randomize the handshape if needed betas = None if self._randomize_hand_shape: betas = self._random.rand(10) * self.SHAPE_BETAS_MAGNITUDE # spawn the hands self._hand_bodies = [] for i, model in enumerate(self._hand_models): hand_body = HandBody(self._client, model, shape_betas=betas) if initial_hands_state is not None: pos, orn, angles = initial_hands_state[i] else: pos_y, orn_z = (0.10, 0.0) if model.is_left_hand else (-0.10, -np.pi) pos, orn = (0.0, pos_y, 0.25), pb.getQuaternionFromEuler((np.pi/2, 0.0, orn_z)) angles = np.zeros(len(hand_body.joint_indices)) hand_body.reset(pos, orn, angles) hand_body.set_target(pos, orn, angles) self._hand_bodies.append(hand_body) self._client.configureDebugVisualizer(pb.COV_ENABLE_RENDERING, 1) return self._get_observation() def step(self, action): """Run one timestep of the environment's dynamics. Args: action (object): an action provided by the agent Returns: observation (object): agent's observation of the current environment reward (float) : amount of reward returned after previous action done (bool): whether the episode has ended info (dict): contains auxiliary diagnostic information """ self._take_action(action) num_steps = int(self.metadata.get('step_time') * 240.0) for _ in range(num_steps): self._client.stepSimulation() observation = self._get_observation() reward = self._get_reward(observation) done = self._is_done(observation) info = {} return observation, reward, done, info def render(self, mode='human'): """Renders the environment. Args: mode (str): the mode to render with """ if mode == 'rgb_array': data = pb.getCameraImage(*self._camera_shape, self._camera_view, self._camera_proj) rgba = data[2] return rgba[:, :, :3] return super().render(mode=mode) def close(self): """Perform necessary cleanup. Environments will automatically close() themselves when garbage collected or when the program exits. """ if self._client.isConnected(): self._client.disconnect() def seed(self, seed=None): """Sets the seed for this env's random number generator(s). Args: seed (int): provided number generators seed """ self._random = RandomState(seed) return [seed] def _take_action(self, action): """Compute observation of the current environment. Args: action (object): an action provided by the agent """ if self.MODE_JOINT == self._control_mode: for hand, (pos, orn, angles) in zip(self._hand_bodies, action): hand.set_target(pos, orn, angles) elif self.MODE_MANO == self._control_mode: for hand, (trans, pose) in zip(self._hand_bodies, action): hand.set_target_from_mano(trans, pose) def _get_observation(self): """Compute observation of the current environment. Returns: observation (object): agent's observation of the current environment """ if self.MODE_JOINT == self._control_mode: return [hand.get_state() for hand in self._hand_bodies] if self.MODE_MANO == self._control_mode: return [hand.get_mano_state() for hand in self._hand_bodies] return None def _get_reward(self, observation): """Compute reward at the current environment state. Returns: observation (object): agent's observation of the current environment """ # pylint: disable=no-self-use, unused-argument return 0.0 def _is_done(self, observation): """Check if the current environment state is final. Returns: observation (object): agent's observation of the current environment """ # pylint: disable=no-self-use, unused-argument return False def _setup_camera(self): """Setup virtual camera.""" self._client.resetDebugVisualizerCamera( cameraDistance=0.5, cameraYaw=-45.0, cameraPitch=-40.0, cameraTargetPosition=[0, 0, 0.1]) self._camera_view = self._client.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=[0.0, 0.0, 0.1], distance=0.5, yaw=-45.0, pitch=-40.0, roll=0.0, upAxisIndex=2) self._camera_proj = self._client.computeProjectionMatrixFOV( fov=60.0, aspect=self._camera_shape[0] / self._camera_shape[1], nearVal=0.1, farVal=10.0)
class LocomotionGymEnv(gym.Env): def __init__(self, gym_config: SimulationParameters, robot_class=None, robot_params: RobotSimParams = None, task: BaseTask = None): self.seed() self._gym_config = gym_config if robot_class is None: raise ValueError('robot_class cannot be None.') self._robot_class = robot_class # A dictionary containing the objects in the world other than the robot. self._world_dict = {} self._robot_params = robot_params self._task = task self._obs_sensor = None self._time_step = self._gym_config.time_step self._num_action_repeat = self._gym_config.num_action_repeat # self._num_bullet_solver_iterations = 12 self._is_render = self._gym_config.enable_rendering and ( not self._gym_config.egl_rendering) if self._is_render: self._pybullet_client = BulletClient(connection_mode=p.GUI) self._pybullet_client.configureDebugVisualizer( p.COV_ENABLE_GUI, gym_config.enable_rendering_gui) else: self._pybullet_client = BulletClient(connection_mode=p.DIRECT) self._pybullet_client.setAdditionalSearchPath( pybullet_data.getDataPath()) self._last_frame_time = 0.0 # set egl acceleration if self._gym_config.egl_rendering: egl = pkgutil.get_loader('eglRenderer') self._eglPlugin = self._pybullet_client.loadPlugin( egl.get_filename(), "_eglRendererPlugin") self._build_action_space() self._build_observation_space() # Set the default render options. self._camera_dist = self._gym_config.camera_distance self._camera_yaw = self._gym_config.camera_yaw self._camera_pitch = self._gym_config.camera_pitch self._render_width = self._gym_config.render_width self._render_height = self._gym_config.render_height self._hard_reset = True self._num_env_act = 0 self.set_gui_sliders() self._contact_fall = ContactDetection(self) self._reset_time = self._gym_config.reset_time self.reset(start_motor_angles=None, reset_duration=self._reset_time) if self._task is not None: self._task.reset(self) self._hard_reset = self._gym_config.enable_hard_reset # global_values.global_userDebugParams = UserDebugParams(self._pybullet_client) # global_values.global_userDebugParams.setPbClient(self._pybullet_client) # set_gui_sliders(self._pybullet_client) def _build_action_space(self): motor_mode = self._robot_params.motor_control_mode action_upper_bound = [] action_lower_bound = [] if (motor_mode == MotorControlMode.POSITION) or ( motor_mode == MotorControlMode.HYBRID_COMPUTED_POS): action_lower_bound.extend(self._robot_params.joint_angle_MinMax[0]) action_upper_bound.extend(self._robot_params.joint_angle_MinMax[1]) # action_config = self._robot_params.JOINT_ANGLE_LIMIT # for action in action_config: # action_upper_bound.append(action.upper_bound) # action_lower_bound.append(action.lower_bound) elif motor_mode == MotorControlMode.TORQUE: action_lower_bound.extend( self._robot_params.joint_torque_MinMax[0]) action_upper_bound.extend( self._robot_params.joint_torque_MinMax[1]) # action_config = self._robot_params.JOINT_TORQUE_LIMIT # for action in action_config: # action_upper_bound.append(action.upper_bound) # action_lower_bound.append(action.lower_bound) elif motor_mode == MotorControlMode.HYBRID_COMPUTED_POS_VEL: action_lower_bound.extend(self._robot_params.joint_angle_MinMax[0]) action_lower_bound.extend( self._robot_params.joint_velocity_MinMax[0]) action_upper_bound.extend(self._robot_params.joint_angle_MinMax[1]) action_upper_bound.extend( self._robot_params.joint_velocity_MinMax[1]) # for action_config in [self._robot_params.JOINT_ANGLE_LIMIT, self._robot_params.JOINT_VELOCITY_LIMIT]: # for action in action_config: # action_upper_bound.append(action.upper_bound) # action_lower_bound.append(action.lower_bound) elif motor_mode == MotorControlMode.HYBRID_COMPUTED_POS_SINGLE: action_lower_bound.extend( self._robot_params.joint_angle_MinMax[0][:3]) action_upper_bound.extend( self._robot_params.joint_angle_MinMax[1][:3]) elif motor_mode == MotorControlMode.HYBRID_COMPUTED_POS_TROT: action_lower_bound.extend( self._robot_params.joint_angle_MinMax[0][:6]) action_upper_bound.extend( self._robot_params.joint_angle_MinMax[1][:6]) self.action_space = spaces.Box(np.array(action_lower_bound), np.array(action_upper_bound), dtype=np.float32) def _build_observation_space(self): if self._task is None: return else: self._obs_sensor = SensorWrapper( self._task.get_observation_sensors()) self.observation_space = spaces.Box(self._obs_sensor.get_lower_bound(), self._obs_sensor.get_upper_bound(), dtype=np.float32) def seed(self, seed=None): self.np_random, self.np_random_seed = seeding.np_random(seed) return [self.np_random_seed] def reset(self, start_motor_angles=None, reset_duration=0.002, reset_visualization_camera=True, force_hard_reset=False): if self._is_render: self._pybullet_client.configureDebugVisualizer( p.COV_ENABLE_RENDERING, 0) # Clear the simulation world and rebuild the robot interface. if force_hard_reset or self._hard_reset: self._pybullet_client.resetSimulation() # self._pybullet_client.setPhysicsEngineParameter(numSolverIterations=self._num_bullet_solver_iterations) self._pybullet_client.setTimeStep(self._time_step) self._pybullet_client.setGravity(0, 0, -9.8) # Rebuild the world. self._world_dict = { "ground": self._pybullet_client.loadURDF("plane_implicit.urdf") } # Rebuild the robot self._robot = self._robot_class( self._pybullet_client, self._robot_params, time_step=self._time_step, num_action_repeat=self._num_action_repeat) if self._obs_sensor is not None: self._obs_sensor.set_robot(self._robot) # Reset the pose of the robot. # self._robot.Reset(reload_urdf=False, default_motor_angles=start_motor_angles, reset_time=reset_duration) if self._is_render and reset_visualization_camera: self._pybullet_client.resetDebugVisualizerCamera( self._camera_dist, self._camera_yaw, self._camera_pitch, [0, 0, 0]) if self._is_render: self._pybullet_client.configureDebugVisualizer( p.COV_ENABLE_RENDERING, 1) self._robot.Reset(reload_urdf=False, default_motor_angles=start_motor_angles, reset_time=reset_duration) if self._obs_sensor is not None: self._obs_sensor.on_reset() self._contact_fall.reset() self._num_env_act = 0 return self._get_observation() def step(self, action): time_spent = time.time() - self._last_frame_time self._last_frame_time = time.time() # if self._num_env_act % 1 == 0: # print(f"time_spent: {time_spent * 1000} ms") if self._is_render: # Sleep, otherwise the computation takes less time than real time, # which will make the visualization like a fast-forward video. time_to_sleep = self._time_step * self._num_action_repeat - time_spent if time_to_sleep > 0: time.sleep(time_to_sleep) # action = action * np.asarray(self.action_space.high) self._robot.Step(action, None) if self._obs_sensor is not None: self._obs_sensor.on_step() self._num_env_act += 1 return self._get_observation(), self._get_reward(), self.done, {} def render(self, mode='rgb_array'): if mode != 'rgb_array': raise ValueError('Unsupported render mode:{}'.format(mode)) base_pos = self._robot.GetBasePosition() view_matrix = self._pybullet_client.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=base_pos, distance=self._camera_dist, yaw=self._camera_yaw, pitch=self._camera_pitch, roll=0, upAxisIndex=2) proj_matrix = self._pybullet_client.computeProjectionMatrixFOV( fov=60, aspect=float(self._render_width) / self._render_height, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = self._pybullet_client.getCameraImage( width=self._render_width, height=self._render_height, renderer=self._pybullet_client.ER_BULLET_HARDWARE_OPENGL, viewMatrix=view_matrix, projectionMatrix=proj_matrix) rgb_array = np.array(px) rgb_array = rgb_array[:, :, :3] return rgb_array def close(self): if hasattr(self, '_robot') and self._robot: self._robot.Terminate() def getRobotID(self): return self._robot.getRobotID() @property def robot(self): return self._robot @property def ground(self): return self._world_dict["ground"] def getClient(self): return self._pybullet_client @property def env_step_counter(self): return self._num_env_act @property def baseHeight(self): return self._robot.ReadBaseHeight() def set_gui_sliders(self): set_gui_sliders(self._pybullet_client) def _get_observation(self): if self._obs_sensor is not None: return self._obs_sensor.get_observation() else: return None def _get_reward(self): if self._task is not None: return self._task.reward() else: return None @property def done(self) -> bool: # if self._obs_sensor is not None: # if self._obs_sensor.on_terminate(): # return True if self._contact_fall.is_contact_fall(): return True if self._task is not None: return self._task.done() else: return False
class PyBullet(Environment): """ Class to create a Mushroom environment using the PyBullet simulator. """ def __init__(self, files, actuation_spec, observation_spec, gamma, horizon, timestep=1 / 240, n_intermediate_steps=1, enforce_joint_velocity_limits=False, debug_gui=False, **viewer_params): """ Constructor. Args: files (dict): dictionary of the URDF/MJCF/SDF files to load (key) and parameters dictionary (value); actuation_spec (list): A list of tuples specifying the names of the joints which should be controllable by the agent and tehir control mode. Can be left empty when all actuators should be used in position control; observation_spec (list): A list containing the names of data that should be made available to the agent as an observation and their type (ObservationType). An entry in the list is given by: (name, type); gamma (float): The discounting factor of the environment; horizon (int): The maximum horizon for the environment; timestep (float, 0.00416666666): The timestep used by the PyBullet simulator; n_intermediate_steps (int): The number of steps between every action taken by the agent. Allows the user to modify, control and access intermediate states; enforce_joint_velocity_limits (bool, False): flag to enforce the velocity limits; debug_gui (bool, False): flag to activate the default pybullet visualizer, that can be used for debug purposes; **viewer_params: other parameters to be passed to the viewer. See PyBulletViewer documentation for the available options. """ assert len(observation_spec) > 0 assert len(actuation_spec) > 0 # Store simulation parameters self._timestep = timestep self._n_intermediate_steps = n_intermediate_steps # Create the simulation and viewer if debug_gui: self._client = BulletClient(connection_mode=pybullet.GUI) self._client.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0) else: self._client = BulletClient(connection_mode=pybullet.DIRECT) self._client.setTimeStep(self._timestep) self._client.setGravity(0, 0, -9.81) self._client.setAdditionalSearchPath(pybullet_data.getDataPath()) self._viewer = PyBulletViewer(self._client, self.dt, **viewer_params) self._state = None # Load model and create access maps self._model_map = dict() self._load_all_models(files, enforce_joint_velocity_limits) # Build utils self._indexer = IndexMap(self._client, self._model_map, actuation_spec, observation_spec) self.joints = JointsHelper(self._client, self._indexer, observation_spec) # Finally, we create the MDP information and call the constructor of the parent class action_space = Box(*self._indexer.action_limits) observation_space = Box(*self._indexer.observation_limits) mdp_info = MDPInfo(observation_space, action_space, gamma, horizon) # Let the child class modify the mdp_info data structure mdp_info = self._modify_mdp_info(mdp_info) # Provide the structure to the superclass super().__init__(mdp_info) # Save initial state of the MDP self._initial_state = self._client.saveState() def seed(self, seed): np.random.seed(seed) def reset(self, state=None): self._client.restoreState(self._initial_state) self.setup(state) self._state = self._indexer.create_sim_state() observation = self._create_observation(self._state) return observation def render(self): self._viewer.display() def stop(self): self._viewer.close() def step(self, action): curr_state = self._state.copy() action = self._preprocess_action(action) self._step_init(curr_state, action) for i in range(self._n_intermediate_steps): ctrl_action = self._compute_action(curr_state, action) self._indexer.apply_control(ctrl_action) self._simulation_pre_step() self._client.stepSimulation() self._simulation_post_step() curr_state = self._indexer.create_sim_state() self._step_finalize() absorbing = self.is_absorbing(curr_state) reward = self.reward(self._state, action, curr_state, absorbing) observation = self._create_observation(curr_state) self._state = curr_state return observation, reward, absorbing, {} def get_sim_state_index(self, name, obs_type): return self._indexer.get_index(name, obs_type) def get_sim_state(self, obs, name, obs_type): """ Returns a specific observation value Args: obs (np.ndarray): the observation vector; name (str): the name of the object to consider; obs_type (PyBulletObservationType): the type of observation to be used. Returns: The required elements of the input state vector. """ indices = self.get_sim_state_index(name, obs_type) return obs[indices] def _modify_mdp_info(self, mdp_info): """ This method can be overridden to modify the automatically generated MDPInfo data structure. By default, returns the given mdp_info structure unchanged. Args: mdp_info (MDPInfo): the MDPInfo structure automatically computed by the environment. Returns: The modified MDPInfo data structure. """ return mdp_info def _create_observation(self, state): """ This method can be overridden to ctreate an observation vector from the simulator state vector. By default, returns the simulator state vector unchanged. Args: state (np.ndarray): the simulator state vector. Returns: The environment observation. """ return state def _load_model(self, file_name, kwargs): if file_name.endswith('.urdf'): model_id = self._client.loadURDF(file_name, **kwargs) elif file_name.endswith('.sdf'): model_id = self._client.loadSDF(file_name, **kwargs)[0] else: model_id = self._client.loadMJCF(file_name, **kwargs)[0] for j in range(self._client.getNumJoints(model_id)): self._client.setJointMotorControl2(model_id, j, pybullet.POSITION_CONTROL, force=0) return model_id def _load_all_models(self, files, enforce_joint_velocity_limits): for file_name, kwargs in files.items(): model_id = self._load_model(file_name, kwargs) model_name = self._client.getBodyInfo(model_id)[1].decode('UTF-8') self._model_map[model_name] = model_id self._model_map.update(self._custom_load_models()) # Enforce velocity limits on every joint if enforce_joint_velocity_limits: for model_id in self._model_map.values(): for joint_id in range(self._client.getNumJoints(model_id)): joint_data = self._client.getJointInfo(model_id, joint_id) self._client.changeDynamics( model_id, joint_id, maxJointVelocity=joint_data[11]) def _preprocess_action(self, action): """ Compute a transformation of the action provided to the environment. Args: action (np.ndarray): numpy array with the actions provided to the environment. Returns: The action to be used for the current step """ return action def _step_init(self, state, action): """ Allows information to be initialized at the start of a step. """ pass def _compute_action(self, state, action): """ Compute a transformation of the action at every intermediate step. Useful to add control signals simulated directly in python. Args: state (np.ndarray): numpy array with the current state of teh simulation; action (np.ndarray): numpy array with the actions, provided at every step. Returns: The action to be set in the actual pybullet simulation. """ return action def _simulation_pre_step(self): """ Allows information to be accesed and changed at every intermediate step before taking a step in the pybullet simulation. Can be usefull to apply an external force/torque to the specified bodies. """ pass def _simulation_post_step(self): """ Allows information to be accesed at every intermediate step after taking a step in the pybullet simulation. Can be usefull to average forces over all intermediate steps. """ pass def _step_finalize(self): """ Allows information to be accesed at the end of a step. """ pass def _custom_load_models(self): """ Allows to custom load a set of objects in the simulation Returns: A dictionary with the names and the ids of the loaded objects """ return list() def reward(self, state, action, next_state, absorbing): """ Compute the reward based on the given transition. Args: state (np.array): the current state of the system; action (np.array): the action that is applied in the current state; next_state (np.array): the state reached after applying the given action; absorbing (bool): whether next_state is an absorbing state or not. Returns: The reward as a floating point scalar value. """ raise NotImplementedError def is_absorbing(self, state): """ Check whether the given state is an absorbing state or not. Args: state (np.array): the state of the system. Returns: A boolean flag indicating whether this state is absorbing or not. """ raise NotImplementedError def setup(self, state): """ A function that allows to execute setup code after an environment reset. Args: state (np.ndarray): the state to be restored. If the state should be chosen by the environment, state is None. Environments can ignore this value if the initial state cannot be set programmatically. """ pass @property def client(self): return self._client @property def dt(self): return self._timestep * self._n_intermediate_steps