def __init__(self, data):
self.numSteps = 4000
self.data = data
self.states = data["state"]
self.numStates = len(self.states)
self.actions = data["action"]
self.next_states = data["next_state"]
high = np.ones([len(self.states[0])])
self.observation_space = gym.spaces.Box(-high, high)
high = np.ones([len(self.actions[0])])
self.action_space = gym.spaces.Box(-high, high)
self.index = 0
self.restartDataOnRestart = True #False
#self.actionMults = np.array([2.0,1.0,1.0,2.0,1.0,1.0,2.0,1.0,1.0,2.0,1.0,1.0])
self.aMin = np.array([
-0.398132, -1.22173, -glm.pi(), -0.698132, -1.22173, -glm.pi(),
-0.398132, -1.22173, -glm.pi(), -0.698132, -1.22173, -glm.pi()
])
self.aMax = np.array([
0.698132, 1.22173, 0.0, 0.398132, 1.22173, 0.0, 0.698132, 1.22173,
0.0, 0.398132, 1.22173, 0.0
])
self.limitsMiddle = (self.aMin + self.aMax) * 0.5
self.limitsDiff = (self.aMax - self.aMin)
def _init_rotation(self):
self._rotation = glm.vec3(0)
if self.projection == "o":
self._rotation[0] = -glm.pi() / 4
elif self.projection in "e":
self._rotation[0] = -glm.pi() / 2.
elif self.projection in "p":
pass
elif self.projection == "i":
self._rotation[0] = -glm.pi() / 3.3
self._rotation[2] = glm.pi() / 4.
def transformation_matrix_4(self) -> glm.mat4:
m = glm.rotate(
glm.mat4(1), -self.rotation_deg / 180 * glm.pi(), glm.vec3(0, 0, 1)
)
m = m * glm.scale(glm.mat4(), glm.vec3(2. / self.scale))
m = m * glm.translate(glm.mat4(), glm.vec3(-self.location.x, -self.location.y, 0))
return m
def rotation(self, orig, dest):
identityQuat = glm.quat(1.0, 0.0, 0.0, 0.0)
epsilon = 0.00001
cosTheta = glm.dot(orig, dest)
if cosTheta >= 1.0 - epsilon:
#// orig and dest point in the same direction
return identityQuat
if cosTheta < -1.0 + epsilon:
'''
// special case when vectors in opposite directions :
// there is no "ideal" rotation axis
// So guess one; any will do as long as it's perpendicular to start
// This implementation favors a rotation around the Up axis (Y),
// since it's often what you want to do.
'''
rotationAxis = glm.cross(glm.vec3(0.0, 0.0, 1.0), orig)
if glm.length(
rotationAxis
) < epsilon: # // bad luck, they were parallel, try again!
rotationAxis = glm.cross(glm.vec3(1.0, 0.0, 0.0), orig)
rotationAxis = glm.normalize(rotationAxis)
return glm.angleAxis(glm.pi(), rotationAxis)
#// Implementation from Stan Melax's Game Programming Gems 1 article
rotationAxis = glm.cross(orig, dest)
s = math.sqrt((1.0 + cosTheta) * 2.0)
invs = 1.0 / s
return glm.quat(s * 0.5, rotationAxis.x * invs, rotationAxis.y * invs,
rotationAxis.z * invs)
def check_keys(self, dt):
dir_mapping = {
pyglet.window.key.LEFT: glm.vec3(-1, 0, 0),
pyglet.window.key.RIGHT: glm.vec3(1, 0, 0),
pyglet.window.key.UP: glm.vec3(0, 1, 0),
pyglet.window.key.DOWN: glm.vec3(0, -1, 0),
#pyglet.window.key.SPACE: glm.vec3(0,0,1),
}
sum_dir = glm.vec3(0)
num = 0
for symbol in dir_mapping:
if self.keys[symbol]:
dir = dir_mapping[symbol]
if self._projection.projection == self._projection.P_ISOMETRIC:
dir = glm.vec3(
glm.rotate(glm.mat4(1), -glm.pi() / 4.,
(0, 0, 1)) * glm.vec4(dir, 0))
#dir *= min(1, dt*10.)
sum_dir += dir
num += 1
if num:
self.agents["player"].move(sum_dir / num * 1.5)
if self.keys[pyglet.window.key.SPACE]:
self.agents["player"].jump()
def normAngle(self, angle):
#// 180-270
if angle >= glm.pi() and angle <= glm.three_over_two_pi():
return angle - glm.two_pi()
#// 270-360
if angle >= glm.three_over_two_pi() and angle <= glm.two_pi():
return angle - glm.two_pi()
return angle
def lookAt(self, worldDestinationDirection): # direction
newWorldDestinationRotation = glm.atan(
worldDestinationDirection[1],
worldDestinationDirection[0]) * 180.0 / glm.pi()
if newWorldDestinationRotation != self.worldDestinationRotation:
self.finishedRotate = False
self.worldDestinationRotation = newWorldDestinationRotation
def draw_cylinder(x, y, z, radius, height):
px = 0
pz = 0
c_angle = 0
angle_stepsize = 0.1
glBegin(GL_QUAD_STRIP)
c_angle = 0
while c_angle < 2 * glm.pi() + 1:
px = radius * glm.cos(c_angle)
pz = radius * glm.sin(c_angle)
glVertex3f(x + px, y + height, z + pz)
glVertex3f(x + px, y, z + pz)
c_angle += angle_stepsize
glEnd()
glBegin(GL_POLYGON)
c_angle = 0
while c_angle < 2 * glm.pi():
px = radius * glm.cos(c_angle)
pz = radius * glm.sin(c_angle)
glVertex3f(x + px, y + height, z + pz)
c_angle += angle_stepsize
glEnd()
glBegin(GL_POLYGON)
c_angle = 0
while c_angle < 2 * glm.pi():
px = radius * glm.cos(c_angle)
pz = radius * glm.sin(c_angle)
glVertex3f(x + px, y + height, z + pz)
c_angle += angle_stepsize
glEnd()
glBegin(GL_POLYGON)
c_angle = 0
while c_angle < 2 * glm.pi():
px = radius * glm.cos(c_angle)
pz = radius * glm.sin(c_angle)
glVertex3f(x + px, y, z + pz)
c_angle += angle_stepsize
glEnd()
def on_text(self, text):
if text == "o":
self._projection = "o"
self._init_rotation()
if text == "i":
self._projection = "i"
self._init_rotation()
self.rotate_z = glm.pi() / 4
if text == "p":
self._projection = "p"
self._init_rotation()
def update(self, dt: float):
"""Call this repeatedly with the time in seconds since last frame"""
rotations = self._rotations
self._rotations = []
for i, rot in enumerate(rotations):
# print(i, rot)
amount = rot["amount"] * dt * .1 * glm.sin(rot["t"] * glm.pi())
self._matrix = glm.rotate(self._matrix, amount, rot["axis"])
rot["t"] += dt / (1. + abs(rot["amount"])) * 5.
if rot["t"] < 0.99:
self._rotations.append(rot)
translations = self._translations
self._translations = []
for i, rot in enumerate(translations):
# print(i, rot)
amount = rot["amount"] * dt * .1 * glm.sin(rot["t"] * glm.pi())
self._matrix = glm.translate(self._matrix, amount * rot["axis"])
rot["t"] += dt / (1. + abs(rot["amount"])) * 5.
if rot["t"] < 0.99:
self._translations.append(rot)
def transformation_matrix(self) -> glm.mat4:
if not self.has_physics:
trans = glm.translate(glm.mat4(1), self._location)
trans *= glm.rotate(glm.mat4(1), self._rotation / 180 * glm.pi(), glm.vec3(0, 0, 1))
return trans
pos, quat = pybullet.getBasePositionAndOrientation(
bodyUniqueId=self._body_id,
physicsClientId=self._physics_client_id,
)
mat3 = pybullet.getMatrixFromQuaternion(quat)
return glm.rotate(glm.mat4(
mat3[0], mat3[3], mat3[6], 0,
mat3[1], mat3[4], mat3[7], 0,
mat3[2], mat3[5], mat3[8], 0,
pos[0], pos[1], pos[2], 1,
), math.pi/2., [1, 0, 0])
def calc_collide_rays(self, numViewDirections, length):
directions=[]
goldenRatio = (1 + glm.sqrt(5)) / 2;
angleIncrement = glm.pi() * 2 * goldenRatio;
i=0
while i < (numViewDirections):
t = i / numViewDirections;
inclination = glm.acos(1 - 2 * t);
azimuth = angleIncrement * i;
x = glm.sin(inclination) * glm.cos(azimuth);
y = glm.sin (inclination) * glm.sin(azimuth);
z = glm.cos (inclination);
directions.append(glm.vec3(x, y, z)*length)
i+=1
return directions
def render(self, projection, pos, dir, frame_num):
self._update()
trans = projection.matrix
mat = glm.translate(trans, pos + (0, 0, 0))
p1 = mat * glm.vec4(0, 0, 0, 1)
p2 = mat * glm.vec4(0, 1, 0, 1)
p = p2 - p1
a = glm.atan(p.x, p.y)
mat = glm.rotate(mat, a, (0, 0, 1))
mat = glm.rotate(mat, glm.pi() / 2., (1, 0, 0))
tile_idx = self.get_tileset_idx(dir, frame_num)
self.texture.bind()
self.drawable.shader.set_uniform("u_projection", mat)
self.drawable.shader.set_uniform("u_tex_offset",
self.tileset.get_uv_offset(tile_idx))
self.drawable.draw()
def on_text(self, text):
if text == "o":
self._projection = "o"
self._init_rotation()
if text == "i":
self._projection = "i"
self._init_rotation()
self.rotate_z = glm.pi() / 4
if text == "p":
self._projection = "p"
self._init_rotation()
if text == "e":
self._projection = "e"
self._init_rotation()
if text == "f":
self.set_fullscreen(not self.fullscreen)
if text == "+":
self.zoom += 1.
if text == "-":
self.zoom -= 1.
if text == "a":
self.fov += 0.01
print(self.fov)
def __init__(self, name):
super().__init__(name)
factory = MeshFactory()
self.mesh = TriangleMesh()
factory.add_cube(self.mesh, 1.)
self.drawable = self.mesh.create_drawable()
self.drawable.shader.set_vertex_source(DEFAULT_SHADER_VERSION + """
#line 19
uniform mat4 u_projection;
uniform mat4 u_transformation;
uniform mat4 u_transformation_inv;
in vec4 a_position;
in vec3 a_normal;
in vec4 a_color;
in vec2 a_texcoord;
in vec3 a_ambient;
in vec4 a_instance_color;
in mat4 a_instance_transform;
out vec4 v_pos;
out vec3 v_normal;
out vec4 v_color;
out vec2 v_texcoord;
out vec3 v_ambient;
out vec3 v_world_normal;
void main()
{
vec4 position =
a_instance_transform *
a_position;
v_pos = position;
v_normal = a_normal;
v_world_normal = normalize((vec4(a_normal, 0.) * u_transformation_inv).xyz);
v_color = a_instance_color;
v_texcoord = a_texcoord;
v_ambient = a_ambient;
gl_Position = u_projection * u_transformation * position;
}
""")
self.drawable.shader.set_fragment_source(DEFAULT_SHADER_VERSION + """
#line 18
in vec4 v_pos;
in vec4 v_color;
in vec3 v_normal;
in vec2 v_texcoord;
uniform float u_time;
out vec4 fragColor;
vec3 lighting(in vec3 pos, in vec3 norm, in vec3 light_pos, in vec3 color) {
vec3 light_norm = normalize(light_pos - pos);
float d = max(0., dot(norm, light_norm));
return color * pow(d, 5.);
}
void main() {
vec3 col = v_color.xyz + .02 * v_normal;
vec3 norm = v_normal; //normalize(v_normal + .1 * sin(length(v_pos.xyz) * 34. - 5. * u_time));
col += .3 * lighting(v_pos.xyz, norm, vec3(3, 0, 1), vec3(1, .6, .4));
col += .3 * lighting(v_pos.xyz, norm, vec3(-3, 1, 2), vec3(.5, 1, .4));
col += .3 * lighting(v_pos.xyz, norm, vec3(1, 5, 3), vec3(.3, .6, 1));
fragColor = vec4(col, 1);
}
""")
if 0:
self.instance_colors = [
1,
0,
0,
1,
0,
1,
0,
1,
0,
0,
1,
1,
1,
1,
0,
1,
]
self.instance_transforms = [
glm.mat4(1),
glm.translate(glm.mat4(1), glm.vec3(1, 0, 0)),
glm.translate(glm.mat4(1), glm.vec3(1, 1, 0)),
glm.rotate(glm.translate(glm.mat4(1), glm.vec3(-.5, 1, 0)),
glm.pi() / 4., glm.vec3(0, 1, 0)),
]
else:
self.instance_colors = []
self.instance_transforms = []
for i in range(500):
self.instance_colors.append(random.uniform(0.1, 1.))
self.instance_colors.append(random.uniform(0.1, 1.))
self.instance_colors.append(random.uniform(0.1, 1.))
self.instance_colors.append(1.)
self.instance_transforms.append(
glm.translate(
glm.mat4(1), 20. *
glm.vec3(random.uniform(-1, 1), random.uniform(-1, 1),
random.uniform(-1, 1))))
def calculateAnalyticalActions(self, state):
actions = self.robotLib.getActions(state.tolist(),
self.physics_time_step)
a = np.asarray(actions)
if self.ActionIsAngles == False:
if self.ActionsIsAdditive:
fl = [a[0] * glm.pi(), a[1] * glm.pi(), a[2] * glm.pi()]
fr = [a[3] * glm.pi(), a[4] * glm.pi(), a[5] * glm.pi()]
bl = [a[6] * glm.pi(), a[7] * glm.pi(), a[8] * glm.pi()]
br = [a[9] * glm.pi(), a[10] * glm.pi(), a[11] * glm.pi()]
self.syncLocalFromXYZ(fl, fr, bl, br,
self.inputsInChassisSpace)
a = np.concatenate([self.flPosN] + [self.frPosN] +
[self.blPosN] + [self.brPosN])
a = np.subtract(a, self.lastJointsStates)
else:
fl = [a[0] * glm.pi(), a[1] * glm.pi(), a[2] * glm.pi()]
fr = [a[3] * glm.pi(), a[4] * glm.pi(), a[5] * glm.pi()]
bl = [a[6] * glm.pi(), a[7] * glm.pi(), a[8] * glm.pi()]
br = [a[9] * glm.pi(), a[10] * glm.pi(), a[11] * glm.pi()]
self.syncLocalFromXYZ(fl, fr, bl, br,
self.inputsInChassisSpace)
a = np.concatenate([self.flPosN] + [self.frPosN] +
[self.blPosN] + [self.brPosN])
return a
def _init_rotation(self):
self.rotate_x = glm.pi() / (3.3 if self._projection == "i" else 4)
self.rotate_y = 0 #-glm.pi()/4.
self.rotate_z = 0
def angleDiff(self, a, b):
dif = math.fmod(b - a + glm.pi(), 2.0 * glm.pi())
if dif < 0:
dif += 2.0 * glm.pi()
return dif - glm.pi()