def simuliateOneParameter(parameterDict):
wolfSize = 0.075
sheepSize = 0.05
blockSize = 0.2
wolfColor = np.array([0.85, 0.35, 0.35])
sheepColor = np.array([0.35, 0.85, 0.35])
blockColor = np.array([0.25, 0.25, 0.25])
wolvesID = [0,1]
sheepsID = [2]
blocksID = [3]
numWolves = len(wolvesID)
numSheeps = len(sheepsID)
numBlocks = len(blocksID)
sheepMaxSpeed = 1.3
wolfMaxSpeed =1.0
blockMaxSpeed = None
agentsMaxSpeedList = [wolfMaxSpeed]* numWolves + [sheepMaxSpeed] * numSheeps
numAgents = numWolves + numSheeps
numEntities = numAgents + numBlocks
entitiesSizeList = [wolfSize]* numWolves + [sheepSize] * numSheeps + [blockSize]* numBlocks
entityMaxSpeedList = [wolfMaxSpeed]* numWolves + [sheepMaxSpeed] * numSheeps + [blockMaxSpeed]* numBlocks
entitiesMovableList = [True]* numAgents + [False] * numBlocks
massList = [1.0] * numEntities
isCollision = IsCollision(getPosFromAgentState)
rewardWolf = RewardWolf(wolvesID, sheepsID, entitiesSizeList, isCollision)
punishForOutOfBound = PunishForOutOfBound()
rewardSheep = RewardSheep(wolvesID, sheepsID, entitiesSizeList, getPosFromAgentState, isCollision, punishForOutOfBound)
rewardFunc = lambda state, action, nextState: \
list(rewardWolf(state, action, nextState)) + list(rewardSheep(state, action, nextState))
makePropertyList=MakePropertyList(transferNumberListToStr)
#changeCollisionReleventParameter
dmin=parameterDict['dmin']
dmax=parameterDict['dmax']
width=parameterDict['width']
midpoint=parameterDict['midpoint']
power=parameterDict['power']
timeconst=parameterDict['timeconst']
dampratio=parameterDict['dampratio']
geomIds=[1,2]
keyNameList=['@solimp','@solref']
valueList=[[[dmin,dmax,width,midpoint,power],[timeconst,dampratio]]]*len(geomIds)
collisionParameter=makePropertyList(geomIds,keyNameList,valueList)
#changeSize
# geomIds=[1,2]
# keyNameList=['@size']
# valueList=[[[0.075,0.075]],[[0.1,0.1]]]
# sizeParameter=makePropertyList(geomIds,keyNameList,valueList)
#load env xml and change some geoms' property
# physicsDynamicsPath=os.path.join(dirName,'multiAgentcollision.xml')
physicsDynamicsPath=os.path.join(dirName,'..','..','environment','mujocoEnv','multiAgentcollision.xml')
with open(physicsDynamicsPath) as f:
xml_string = f.read()
envXmlDict = xmltodict.parse(xml_string.strip())
print(envXmlDict)
envXmlPropertyDictList=[collisionParameter]
changeEnvXmlPropertFuntionyList=[changeGeomProperty]
for propertyDict,changeXmlProperty in zip(envXmlPropertyDictList,changeEnvXmlPropertFuntionyList):
envXmlDict=changeXmlProperty(envXmlDict,propertyDict)
envXml=xmltodict.unparse(envXmlDict)
physicsModel = mujoco.load_model_from_xml(envXml)
physicsSimulation = mujoco.MjSim(physicsModel)
# MDP function
qPosInit = [0, 0]*numAgents
qVelInit = [0, 0]*numAgents
qVelInitNoise = 0
qPosInitNoise = 1
# reset=ResetUniformWithoutXPos(physicsSimulation, qPosInit, qVelInit, numAgents, numBlocks,qPosInitNoise, qVelInitNoise)
fixReset=ResetFixWithoutXPos(physicsSimulation, qPosInit, qVelInit, numAgents,numBlocks)
blocksState=[[0,0,0,0]]
reset=lambda :fixReset([-0.5,0.8,-0.5,0,0.5,0.8],[0,0,0,0,0,0],blocksState)
isTerminal = lambda state: False
numSimulationFrames = 1
visualize=False
# physicsViewer=None
dt=0.1
damping=2.5
transit = TransitionFunctionWithoutXPos(physicsSimulation,numAgents , numSimulationFrames,damping*dt/numSimulationFrames,agentsMaxSpeedList,visualize,isTerminal)
maxRunningSteps = 100
sampleTrajectory = SampleTrajectory(maxRunningSteps, transit, isTerminal, rewardFunc, reset)
observeOneAgent = lambda agentID: Observe(agentID, wolvesID, sheepsID, blocksID, getPosFromAgentState, getVelFromAgentState)
observe = lambda state: [observeOneAgent(agentID)(state) for agentID in range(numAgents)]
initObsForParams = observe(reset())
obsShape = [initObsForParams[obsID].shape for obsID in range(len(initObsForParams))]
worldDim = 2
evalNum=1
trajectorySaveExtension = '.pickle'
fixedParameters = {'isMujoco': 1,'isCylinder':1,'evalNum':evalNum}
trajectoriesSaveDirectory=trajSavePath = os.path.join(dirName,'..','trajectory')
generateTrajectorySavePath = GetSavePath(trajectoriesSaveDirectory, trajectorySaveExtension, fixedParameters)
trajectorySavePath = generateTrajectorySavePath(parameterDict)
if not os.path.isfile(trajectorySavePath):
trajList =list()
for i in range(evalNum):
# policy =lambda state: [[-3,0] for agent in range(numAgents)]
np.random.seed(i)
# policy =lambda state: [np.random.uniform(-5,5,2) for agent in range(numAgents)]sss
# policy =lambda state: [[0,1] for agent in range(numAgents)]
policy =lambda state: [[1,0] ]
# policy =lambda state: [[np.random.uniform(0,1,1),0] ,[np.random.uniform(-1,0,1),0] ]
traj = sampleTrajectory(policy)
# print(i,'traj',[state[1] for state in traj[:2]])
# print(traj)
trajList.append( traj)
# saveTraj
saveTraj = True
if saveTraj:
# trajSavePath = os.path.join(dirName,'traj', 'evaluateCollision', 'CollisionMoveTransitDamplingCylinder.pickle')
saveToPickle(trajList, trajectorySavePath)
# visualize
# physicsViewer.render()
visualize = True
if visualize:
entitiesColorList = [wolfColor] * numWolves + [sheepColor] * numSheeps + [blockColor] * numBlocks
render = Render(entitiesSizeList, entitiesColorList, numAgents, getPosFromAgentState)
render(trajList)
def simuliateOneParameter(parameterDict, evalNum, randomSeed, dt):
mujocoVisualize = False
demoVisualize = False
wolfSize = 0.075
sheepSize = 0.05
blockSize = 0.2
wolfColor = np.array([0.85, 0.35, 0.35])
sheepColor = np.array([0.35, 0.85, 0.35])
blockColor = np.array([0.25, 0.25, 0.25])
wolvesID = [0]
sheepsID = [1]
blocksID = [2]
numWolves = len(wolvesID)
numSheeps = len(sheepsID)
numBlocks = len(blocksID)
sheepMaxSpeed = 1.3
wolfMaxSpeed = 1.0
blockMaxSpeed = None
agentsMaxSpeedList = [wolfMaxSpeed] * numWolves + [sheepMaxSpeed
] * numSheeps
numAgents = numWolves + numSheeps
numEntities = numAgents + numBlocks
entitiesSizeList = [wolfSize] * numWolves + [sheepSize] * numSheeps + [
blockSize
] * numBlocks
entityMaxSpeedList = [wolfMaxSpeed] * numWolves + [
sheepMaxSpeed
] * numSheeps + [blockMaxSpeed] * numBlocks
entitiesMovableList = [True] * numAgents + [False] * numBlocks
massList = [1.0] * numEntities
isCollision = IsCollision(getPosFromAgentState)
rewardWolf = RewardWolf(wolvesID, sheepsID, entitiesSizeList, isCollision)
punishForOutOfBound = PunishForOutOfBound()
rewardSheep = RewardSheep(wolvesID, sheepsID, entitiesSizeList,
getPosFromAgentState, isCollision,
punishForOutOfBound)
rewardFunc = lambda state, action, nextState: \
list(rewardWolf(state, action, nextState)) + list(rewardSheep(state, action, nextState))
makePropertyList = MakePropertyList(transferNumberListToStr)
#changeCollisionReleventParameter
dmin = parameterDict['dmin']
dmax = parameterDict['dmax']
width = parameterDict['width']
midpoint = parameterDict['midpoint']
power = parameterDict['power']
timeconst = parameterDict['timeconst']
dampratio = parameterDict['dampratio']
geomIds = [1, 2, 3]
keyNameList = ['@solimp', '@solref']
valueList = [[[dmin, dmax, width, midpoint, power], [timeconst, dampratio]]
] * len(geomIds)
collisionParameter = makePropertyList(geomIds, keyNameList, valueList)
#changeSize
# geomIds=[1,2]
# keyNameList=['@size']
# valueList=[[[0.075,0.075]],[[0.1,0.1]]]
# sizeParameter=makePropertyList(geomIds,keyNameList,valueList)
#load env xml and change some geoms' property
physicsDynamicsPath = os.path.join(
dirName, '..', '..', 'environment', 'mujocoEnv',
'variousCollision_numBlocks=1_numSheeps=1_numWolves=1dt={}.xml'.format(
dt))
with open(physicsDynamicsPath) as f:
xml_string = f.read()
envXmlDict = xmltodict.parse(xml_string.strip())
envXmlPropertyDictList = [collisionParameter]
changeEnvXmlPropertFuntionyList = [changeGeomProperty]
for propertyDict, changeXmlProperty in zip(
envXmlPropertyDictList, changeEnvXmlPropertFuntionyList):
envXmlDict = changeXmlProperty(envXmlDict, propertyDict)
envXml = xmltodict.unparse(envXmlDict)
# print(envXmlDict['mujoco']['worldbody']['body'][0])
physicsModel = mujoco.load_model_from_xml(envXml)
physicsSimulation = mujoco.MjSim(physicsModel)
# MDP function
qPosInit = [0, 0] * numAgents
qVelInit = [0, 0] * numAgents
qVelInitNoise = 0
qPosInitNoise = 1
# reset=ResetUniformWithoutXPos(physicsSimulation, qPosInit, qVelInit, numAgents, numBlocks,qPosInitNoise, qVelInitNoise)
# fixReset=ResetFixWithoutXPos(physicsSimulation, qPosInit, qVelInit, numAgents,numBlocks)
# blocksState=[[0,-0.8,0,0]]
# reset=lambda :fixReset([-0.5,0.8,-0.5,0,0.5,0.8],[0,0,0,0,0,0],blocksState)
isTerminal = lambda state: False
numSimulationFrames = int(0.1 / dt)
print(numSimulationFrames)
# dt=0.01
damping = 2.5
reshapeAction = lambda action: action
# transit = TransitionFunctionWithoutXPos(physicsSimulation,numAgents , numSimulationFrames,damping*dt/numSimulationFrames,agentsMaxSpeedList,mujocoVisualize,reshapeAction)
maxRunningSteps = 10
# sampleTrajectory = SampleTrajectory(maxRunningSteps, transit, isTerminal, rewardFunc, reset)
# observeOneAgent = lambda agentID: Observe(agentID, wolvesID, sheepsID, blocksID, getPosFromAgentState, getVelFromAgentState)
# observe = lambda state: [observeOneAgent(agentID)(state) for agentID in range(numAgents)]
# initObsForParams = observe(reset())
# obsShape = [initObsForParams[obsID].shape for obsID in range(len(initObsForParams))]
class ImpulsePolicy(object):
"""docstring for c"""
def __init__(self, initAction):
self.initAction = initAction
def __call__(self, state, timeStep):
action = [[0, 0], [0, 0]]
if timeStep == 0:
action = self.initAction
return action
worldDim = 2
trajList = []
startTime = time.time()
for i in range(evalNum):
np.random.seed(randomSeed + i)
initSpeedDirection = np.random.uniform(-np.pi / 2, np.pi / 2, 1)[0]
initSpeed = np.random.uniform(0, 1, 1)[0]
initActionDirection = np.random.uniform(-np.pi / 2, np.pi / 2, 1)[0]
initForce = np.random.uniform(0, 5, 1)[0]
# print(initSpeedDirection,initSpeed,initActionDirection,initForce)
fixReset = ResetFixWithoutXPos(physicsSimulation, qPosInit, qVelInit,
numAgents, numBlocks)
blocksState = [[0, 0, 0, 0]] #posX posY velX velY
reset = lambda: fixReset([-0.28, 0, 8, 8], [
initSpeed * np.cos(initSpeedDirection), initSpeed * np.sin(
initSpeedDirection), 0, 0
], blocksState)
transit = TransitionFunctionWithoutXPos(
physicsSimulation, numAgents, numSimulationFrames,
damping * dt * numSimulationFrames, agentsMaxSpeedList,
mujocoVisualize, isTerminal, reshapeAction)
initAction = [[
initForce * np.cos(initActionDirection),
initForce * np.sin(initActionDirection)
], [0, 0]]
impulsePolicy = ImpulsePolicy(initAction)
sampleTrajectory = SampleTrajectory(maxRunningSteps, transit,
isTerminal, rewardFunc, reset)
traj = sampleTrajectory(impulsePolicy)
# print('traj',traj[0])
trajList.append(traj)
# saveTraj
# print(trajList)
saveTraj = True
if saveTraj:
# trajSavePath = os.path.join(dirName,'traj', 'evaluateCollision', 'CollisionMoveTransitDamplingCylinder.pickle')
trajectorySaveExtension = '.pickle'
fixedParameters = {
'isMujoco': 1,
'isCylinder': 1,
'randomSeed': randomSeed,
'evalNum': evalNum
}
trajectoriesSaveDirectory = trajSavePath = os.path.join(
dirName, '..', 'trajectory', 'variousCollsiondt={}'.format(dt))
generateTrajectorySavePath = GetSavePath(trajectoriesSaveDirectory,
trajectorySaveExtension,
fixedParameters)
trajectorySavePath = generateTrajectorySavePath(parameterDict)
saveToPickle(trajList, trajectorySavePath)
# visualize
if demoVisualize:
entitiesColorList = [wolfColor] * numWolves + [
sheepColor
] * numSheeps + [blockColor] * numBlocks
render = Render(entitiesSizeList, entitiesColorList, numAgents,
getPosFromAgentState)
trajToRender = np.concatenate(trajList)
render(trajToRender)
endTime = time.time()
print("Time taken {} seconds to generate {} trajectories".format(
(endTime - startTime), evalNum))
def simuliateOneParameter(parameterDict):
wolfSize = 0.075
sheepSize = 0.05
blockSize = 0.2
wolfColor = np.array([0.85, 0.35, 0.35])
sheepColor = np.array([0.35, 0.85, 0.35])
blockColor = np.array([0.25, 0.25, 0.25])
wolvesID = [0,1]
sheepsID = [2]
blocksID = [3]
numWolves = len(wolvesID)
numSheeps = len(sheepsID)
numBlocks = len(blocksID)
sheepMaxSpeed = 1.3
wolfMaxSpeed =1.0
blockMaxSpeed = None
agentsMaxSpeedList = [wolfMaxSpeed]* numWolves + [sheepMaxSpeed] * numSheeps
numAgents = numWolves + numSheeps
numEntities = numAgents + numBlocks
entitiesSizeList = [wolfSize]* numWolves + [sheepSize] * numSheeps + [blockSize]* numBlocks
entityMaxSpeedList = [wolfMaxSpeed]* numWolves + [sheepMaxSpeed] * numSheeps + [blockMaxSpeed]* numBlocks
entitiesMovableList = [True]* numAgents + [False] * numBlocks
massList = [1.0] * numEntities
isCollision = IsCollision(getPosFromAgentState)
rewardWolf = RewardWolf(wolvesID, sheepsID, entitiesSizeList, isCollision)
punishForOutOfBound = PunishForOutOfBound()
rewardSheep = RewardSheep(wolvesID, sheepsID, entitiesSizeList, getPosFromAgentState, isCollision, punishForOutOfBound)
rewardFunc = lambda state, action, nextState: \
list(rewardWolf(state, action, nextState)) + list(rewardSheep(state, action, nextState))
makePropertyList=MakePropertyList(transferNumberListToStr)
#changeCollisionReleventParameter
dmin=parameterDict['dmin']
dmax=parameterDict['dmax']
width=parameterDict['width']
midpoint=parameterDict['midpoint']
power=parameterDict['power']
timeconst=parameterDict['timeconst']
dampratio=parameterDict['dampratio']
geomIds=[1,2,3,4]
keyNameList=['@solimp','@solref']
valueList=[[[dmin,dmax,width,midpoint,power],[timeconst,dampratio]]]*len(geomIds)
collisionParameter=makePropertyList(geomIds,keyNameList,valueList)
#changeSize
# geomIds=[1,2]
# keyNameList=['@size']
# valueList=[[[0.075,0.075]],[[0.1,0.1]]]
# sizeParameter=makePropertyList(geomIds,keyNameList,valueList)
#load env xml and change some geoms' property
physicsDynamicsPath=os.path.join(dirName,'..','..','environment','mujocoEnv','rope','1leased.xml')
with open(physicsDynamicsPath) as f:
xml_string = f.read()
envXmlDict = xmltodict.parse(xml_string.strip())
# envXmlPropertyDictList=[collisionParameter]
# changeEnvXmlPropertFuntionyList=[changeGeomProperty]
# for propertyDict,changeXmlProperty in zip(envXmlPropertyDictList,changeEnvXmlPropertFuntionyList):
# envXmlDict=changeXmlProperty(envXmlDict,propertyDict)
envXml=xmltodict.unparse(envXmlDict)
# print(envXmlDict['mujoco']['worldbody']['body'][0])
physicsModel = mujoco.load_model_from_xml(envXml)
physicsSimulation = mujoco.MjSim(physicsModel)
# MDP function
qPosInit = [0, 0]*numAgents
qVelInit = [0, 0]*numAgents
qVelInitNoise = 0
qPosInitNoise = 1
# reset=ResetUniformWithoutXPos(physicsSimulation, qPosInit, qVelInit, numAgents, numBlocks,qPosInitNoise, qVelInitNoise)
fixReset=ResetFixWithoutXPos(physicsSimulation, qPosInit, qVelInit, numAgents,numBlocks)
blocksState=[[0,-0.8,0,0]]
reset=lambda :fixReset([-0.5,0.8,-0.5,0,0.5,0.8],[0,0,0,0,0,0],blocksState)
isTerminal = lambda state: False
numSimulationFrames = 10
visualize=True
# physicsViewer=None
dt=0.01
damping=2.5
reshapeAction=lambda action:action
physicsViewer = mujoco.MjViewer(physicsSimulation)
qPosInit = (0, ) * 24
qVelInit = (0, ) * 24
qPosInitNoise = 6
qVelInitNoise = 4
numAgent = 3
tiedAgentId = [1, 2]
ropePartIndex = list(range(3, 12))
maxRopePartLength = 0.6
reset = ResetUniformForLeashed(physicsSimulation, qPosInit, qVelInit, numAgent, tiedAgentId, \
ropePartIndex, maxRopePartLength, qPosInitNoise, qVelInitNoise)
reset()
for i in range(6000):
physicsViewer.render()