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()