def fun_prioritized_dis(x, y, goal, obs):
computefield = fieldDirection.FieldDirection()
d = np.ones(np.size(x))
d1 = np.ones(np.size(x))
d2 = np.ones(np.size(x))
deltax = np.ones(np.size(x)) * goal[0] - x
deltay = np.ones(np.size(y)) * goal[1] - y
odeltax = np.ones(np.size(x)) * obs[0] - x
odeltay = np.ones(np.size(y)) * obs[1] - y
for i in range(np.size(x)):
p_robot = np.array([x[i], y[i]])
di_g = np.sqrt(deltax[i]**2 + deltay[i]**2)
di_o = np.sqrt(odeltax[i]**2 + odeltay[i]**2)
d0, de, dm, p = penalty(di_g)
tau = de / d0
tau = tau #** 2
r = (1 - tau) * de + tau * dm - p
d[i] = -r
if d[i] < -10:
d[i] = -10
if d[i] > 10:
d[i] = 10
maxgoal = np.array([x[np.argmax(d)], y[np.argmax(d)], np.amax(d)])
minobs = np.array([x[np.argmin(d)], y[np.argmin(d)], np.amin(d)])
return d, maxgoal, minobs
def fun_normal_dis(x, y, goal, obs):
computefield = fieldDirection.FieldDirection()
d = np.ones(np.size(x))
d1 = np.ones(np.size(x))
d2 = np.ones(np.size(x))
deltax = np.ones(np.size(x)) * goal[0] - x
deltay = np.ones(np.size(y)) * goal[1] - y
odeltax = np.ones(np.size(x)) * obs[0] - x
odeltay = np.ones(np.size(y)) * obs[1] - y
for i in range(np.size(x)):
p_robot = np.array([x[i], y[i]])
di_g = np.sqrt(deltax[i]**2 + deltay[i]**2)
di_o = np.sqrt(odeltax[i]**2 + odeltay[i]**2)
if di_o < 0.05:
d1[i] = -1e1 - 10
elif di_o < 0.2:
d1[i] = -1 / di_o
else:
d1[i] = 0
if di_g < 0.05:
d2[i] = 1e1 + 10
elif di_g < 0.2:
d2[i] = 1 / (di_g**1) - di_g**2
else:
d2[i] = 1 / (di_g**1) - di_g**2
d[i] = d1[i] + d2[i]
maxgoal = np.array([x[np.argmax(d)], y[np.argmax(d)], np.amax(d)])
return d, maxgoal
def fun_field(x, y, goal, obs):
computefield = fieldDirection.FieldDirection()
d = np.ones(np.size(x))
d1 = np.ones(np.size(x))
d2 = np.ones(np.size(x))
deltax = np.ones(np.size(x)) * goal[0] - x
deltay = np.ones(np.size(y)) * goal[1] - y
odeltax = np.ones(np.size(x)) * obs[0] - x
odeltay = np.ones(np.size(y)) * obs[1] - y
for i in range(np.size(x)):
p_robot = np.array([x[i], y[i]])
di_g = np.sqrt(deltax[i]**2 + deltay[i]**2)
di_o = np.sqrt(odeltax[i]**2 + odeltay[i]**2)
force, d_force = computefield.compute_sum_force(p_robot, goal, obs)
rforce, d_rforce = computefield.compute_repulse(p_robot, obs)
aforce, d_aforce = computefield.compute_attract(goal, p_robot)
# print('force ', d_force)
if di_o < 0.0:
d1[i] = -1e1 - 7
elif di_o <= 0.2:
d1[i] = -force
else:
d1[i] = -force
if di_g < 0.00:
d2[i] = 1e2
elif di_g < 0.2:
d2[i] = 20 * aforce
else:
d2[i] = 20 * aforce
d[i] = -force #np.log(force)#np.exp(force)#d1[i] #+ d2[i]
if d[i] < -10:
d[i] = -10
#print('max ', x[np.argmax(d)], y[np.argmax(d)], np.amax(d))
maxgoal = np.array([x[np.argmax(d)], y[np.argmax(d)], np.amax(d)])
minobs = np.array([x[np.argmin(d)], y[np.argmin(d)], np.amin(d)])
return d, maxgoal, minobs
def __init__(self,
urdfRoot=parentdir,
actionRepeat=1,
isEnableSelfCollision=True,
isDiscrete=False,
renders=False,
maxSteps=1e3,
rewardtype='rdense',
action_dim=9,
randomInitial=True,
wsboundary=1,
colliObj=True):
self._urdfRoot = urdfRoot
self._actionRepeat = actionRepeat
self._isEnableSelfCollision = isEnableSelfCollision
self._isDiscrete = isDiscrete
self._renders = renders
self._maxSteps = maxSteps
self._rewardtype = rewardtype
self._action_dim = action_dim
self._isEnableRandInit = randomInitial
self.d_ws = wsboundary
self.objsOrNot = colliObj
self._observation = []
self._envStepCounter = 0
self._timeStep = 1. / 240.
self.r_penalty = 0
self._terminated = 0
self._cam_dist = 4
self._cam_yaw = 180
self._cam_pitch = -40
self._dis_vor = 100
self._count = 0
self._count_ep = 0
self.dis_init = 1e5
self.dis_collision = 1
self.ee_dis = 1e5
self.base_dis = 1e5
self.goal = []
self.obs_pose = []
self.flag_collide = 0
self._p = p
if self._renders:
cid = p.connect(p.SHARED_MEMORY)
if cid < 0:
cid = p.connect(p.GUI)
p.resetDebugVisualizerCamera(self._cam_dist, self._cam_yaw,
self._cam_pitch, [0.52, -0.2, -0.33])
else:
p.connect(p.DIRECT)
# p.setRealTimeSimulation(1)
self.seed()
p.resetSimulation()
p.setPhysicsEngineParameter(
numSolverIterations=150) # , enableFileCaching=0)
p.setTimeStep(self._timeStep)
p.setGravity(0, 0, -9.8)
p.loadURDF(
os.path.join(self._urdfRoot,
"neobotix_schunk_pybullet/data/plane.urdf"))
self.goal = np.zeros(3)
self.origoal = np.array([0, 0, 0, 1])
self.goalUid = p.loadURDF(os.path.join(
self._urdfRoot, "neobotix_schunk_pybullet/data/spheregoal.urdf"),
basePosition=self.goal)
self.obs_pose = np.ones(3)
if self.objsOrNot:
self.obsUid = p.loadURDF(os.path.join(
self._urdfRoot, "neobotix_schunk_pybullet/data/cube.urdf"),
basePosition=self.obs_pose)
self._neobotixschunk = neobotixschunk.NeobotixSchunk(
urdfRootPath=self._urdfRoot,
timeStep=self._timeStep,
randomInitial=self._isEnableRandInit,
wsboundary=self.d_ws)
self.reset()
self.roughDirection = np.zeros(3)
self.observation_dim = len(self.getExtendedObservation())
observation_high = np.array([largeValObservation] *
self.observation_dim)
# print('ob',observation_high)
daction = 1
if self._isDiscrete:
self.action_space = spaces.Discrete(9)
else:
self.action_bound = np.ones(self._action_dim) * daction
self.action_space = spaces.Box(low=-self.action_bound,
high=self.action_bound,
dtype=np.float32)
self.observation_space = spaces.Box(low=-observation_high,
high=observation_high,
dtype=np.float32)
self.viewer = None
self.calculateField = fieldDirection.FieldDirection()