def rand_target(self):
pos = (np.random.rand(3,1)-0.5)*4
self.invalpos = (np.random.rand(3,1)-0.5)
self.invalpos += np.sign(self.invalpos)* 0.2
self.invalpos += pos
self.targetGeom = ode.GeomSphere(self.space, radius=0.1)
self.targetGeom.setPosition((pos[0],pos[1],pos[2]))
spikes = [3,4,5,7,9]
temp= np.random.randint(3)
self.targetParams = [0.12,spikes[temp]]
self.invalidParams= [0.08,1,0.10*np.random.rand()+0.4]
self.target = rendering.make_valid_target(self.targetParams[0],self.targetParams[1])
self.targetTrans = rendering.Transform()
self.target.add_attr(self.targetTrans)
self.invalid_target = rendering.make_invalid_target(self.invalidParams[0],self.invalidParams[1],self.invalidParams[2])
self.invalidTargetTrans = rendering.Transform()
self.invalid_target.add_attr(self.invalidTargetTrans)
self.intargetGeom = ode.GeomSphere(self.space, radius=0.1)
self.intargetGeom.setPosition((self.invalpos[0],self.invalpos[1],self.invalpos[2]))
return pos
def _render(self, mode='human', close=False):
if close:
if self.viewer is not None:
self.viewer.close()
self.viewer = None
return
x1, y1, z1 = self.body1.getPosition()
x2, y2, z2 = self.body2.getPosition()
x3, y3, z3 = self.body3.getPosition()
x4, y4, z4 = self.body4.getPosition()
x5, y5, z5 = self.body5.getPosition()
x6, y6, z6 = self.body6.getPosition()
x7, y7, z7 = self.body7.getPosition()
if self.viewer is None:
self.viewer = rendering.Viewer(self.viewerSize, self.viewerSize)
self.viewer.perspective_transform_on = self.perspective_transform_on
self.viewer.set_bounds(-self.spaceSize / 2.0, self.spaceSize / 2.0,
-self.spaceSize / 2.0, self.spaceSize / 2.0)
rod1 = rendering.make_cuboid(.6, .05)
rod1.set_color(0.4, 0.4, 0.4)
self.colours.append([0.4, 0.4, 0.4])
self.pole_transform1 = rendering.Transform()
self.pole_transform11 = rendering.Transform()
rod1.add_attr(self.pole_transform1)
rod1.add_attr(self.pole_transform11)
rod2 = rendering.make_cuboid(.6, .05)
rod2.set_color(0.2, 0.2, 0.2)
self.colours.append([0.2, 0.2, 0.2])
self.pole_transform2 = rendering.Transform()
self.pole_transform21 = rendering.Transform()
rod2.add_attr(self.pole_transform2)
rod2.add_attr(self.pole_transform21)
rod3 = rendering.make_cuboid(.6, .05)
rod3.set_color(0, 0, 0)
self.colours.append([0.0, 0.0, 0.0])
self.pole_transform3 = rendering.Transform()
self.pole_transform31 = rendering.Transform()
rod3.add_attr(self.pole_transform3)
rod3.add_attr(self.pole_transform31)
rod4 = rendering.make_cuboid(.6, .05)
rod4.set_color(0.5, 0.5, 0.5)
self.colours.append([0.5, 0.5, 0.5])
self.pole_transform4 = rendering.Transform()
self.pole_transform41 = rendering.Transform()
rod4.add_attr(self.pole_transform4)
rod4.add_attr(self.pole_transform41)
rod5 = rendering.make_cuboid(.6, .05)
rod5.set_color(0.5, 0.5, 0.5)
self.colours.append([0.5, 0.5, 0.5])
self.pole_transform5 = rendering.Transform()
self.pole_transform51 = rendering.Transform()
rod5.add_attr(self.pole_transform5)
rod5.add_attr(self.pole_transform51)
rod6 = rendering.make_cuboid(.6, .05)
rod6.set_color(0.5, 0.5, 0.5)
self.colours.append([0.5, 0.5, 0.5])
self.pole_transform6 = rendering.Transform()
self.pole_transform61 = rendering.Transform()
rod6.add_attr(self.pole_transform6)
rod6.add_attr(self.pole_transform61)
rod7 = rendering.make_cuboid(.6, .05)
rod7.set_color(0.5, 0.5, 0.5)
self.colours.append([0.5, 0.5, 0.5])
self.pole_transform7 = rendering.Transform()
self.pole_transform71 = rendering.Transform()
rod7.add_attr(self.pole_transform7)
rod7.add_attr(self.pole_transform71)
axle1 = rendering.make_sphere(0.25)
axle1.set_color(1, 0, 0)
self.colours.append([1.0, 0.0, 0.0])
self.axle_transform1 = rendering.Transform()
axle1.add_attr(self.axle_transform1)
self.axle_transform12 = rendering.Transform()
axle1.add_attr(self.axle_transform12)
axle2 = rendering.make_sphere(.22)
axle2.set_color(0, 1, 0)
self.colours.append([0.0, 1.0, 0.0])
self.axle_transform2 = rendering.Transform()
axle2.add_attr(self.axle_transform2)
self.axle_transform22 = rendering.Transform()
axle2.add_attr(self.axle_transform22)
axle3 = rendering.make_sphere(.2)
axle3.set_color(0, 0, 1)
self.colours.append([0.0, 0.0, 1.0])
self.axle_transform3 = rendering.Transform()
axle3.add_attr(self.axle_transform3)
self.axle_transform32 = rendering.Transform()
axle3.add_attr(self.axle_transform32)
axle4 = rendering.make_sphere(.17)
axle4.set_color(0, 0, 0.5)
self.colours.append([0.0, 0.0, 0.5])
self.axle_transform4 = rendering.Transform()
axle4.add_attr(self.axle_transform4)
self.axle_transform42 = rendering.Transform()
axle4.add_attr(self.axle_transform42)
axle5 = rendering.make_sphere(.14)
axle5.set_color(0, 0.5, 0)
self.colours.append([0.0, 0.5, 0])
self.axle_transform5 = rendering.Transform()
axle5.add_attr(self.axle_transform5)
self.axle_transform52 = rendering.Transform()
axle5.add_attr(self.axle_transform52)
axle6 = rendering.make_sphere(.1)
axle6.set_color(0.5, 0, 0.0)
self.colours.append([0.5, 0.0, 0.0])
self.axle_transform6 = rendering.Transform()
axle6.add_attr(self.axle_transform6)
self.axle_transform62 = rendering.Transform()
axle6.add_attr(self.axle_transform62)
axle7 = rendering.make_sphere(.08)
axle7.set_color(0.5, 0, 0.5)
self.colours.append([0.5, 0.0, 0.5])
self.axle_transform7 = rendering.Transform()
axle7.add_attr(self.axle_transform7)
self.axle_transform72 = rendering.Transform()
axle7.add_attr(self.axle_transform72)
self.viewer.add_geom(rod1)
self.viewer.add_geom(rod2)
self.viewer.add_geom(rod3)
self.viewer.add_geom(rod4)
self.viewer.add_geom(rod5)
self.viewer.add_geom(rod6)
self.viewer.add_geom(rod7)
self.viewer.add_geom(axle1)
self.viewer.add_geom(axle2)
self.viewer.add_geom(axle3)
self.viewer.add_geom(axle4)
self.viewer.add_geom(axle5)
self.viewer.add_geom(axle6)
self.viewer.add_geom(axle7)
self.target.set_color(0.7, 0.7, 0.7)
self.invalid_target.set_color(0.7, 0.7, 0.7)
self.viewer.add_onetime(self.target)
self.viewer.add_onetime(self.invalid_target)
self.transform_link(self.body1, self.pole_transform11,
self.pole_transform1, self.axle_transform12,
self.axle_transform1)
self.transform_link(self.body2, self.pole_transform21,
self.pole_transform2, self.axle_transform22,
self.axle_transform2)
self.transform_link(self.body3, self.pole_transform31,
self.pole_transform3, self.axle_transform32,
self.axle_transform3)
self.transform_link(self.body4, self.pole_transform41,
self.pole_transform4, self.axle_transform42,
self.axle_transform4)
self.transform_link(self.body5, self.pole_transform51,
self.pole_transform5, self.axle_transform52,
self.axle_transform5)
self.transform_link(self.body6, self.pole_transform61,
self.pole_transform6, self.axle_transform62,
self.axle_transform6)
self.transform_link(self.body7, self.pole_transform71,
self.pole_transform7, self.axle_transform72,
self.axle_transform7)
self.targetTrans.set_translation(self.targetPos[0], self.targetPos[1],
self.targetPos[2])
self.invalidTargetTrans.set_translation(self.invalpos[0],
self.invalpos[1],
self.invalpos[2])
self.ground_truth_joint_angles = np.array([
self.j1.getAngle(),
self.j2.getAngle(),
self.j3.getAngle(),
self.j4.getAngle(),
self.j5.getAngle(),
self.j6.getAngle(),
self.j7.getAngle()
])
self.ground_truth_joint_velocities = np.array([
self.j1.getAngleRate(),
self.j2.getAngleRate(),
self.j3.getAngleRate(),
self.j4.getAngleRate(),
self.j5.getAngleRate(),
self.j6.getAngleRate(),
self.j7.getAngleRate()
])
self.ground_truth_valid_target = self.targetPos.T.flatten()
self.ground_truth_invalid_target = self.invalpos.T.flatten()
self.ground_truth_end_effector = self.body8.getPosition()
if self.viewer.time == 0:
if np.random.rand() > 0.5:
self.dir = 1.01
else:
self.dir = 0.99
self.viewer.time = np.random.randint(20) + 1
else:
self.viewer.time -= 1
self.viewer.lightValue *= self.dir
if self.viewer.lightValue > 1:
self.viewer.lightValue = 1
elif self.viewer.lightValue < 0.01:
self.viewer.lightValue = 0.01
for obj in self.viewer.onetime_geoms:
redBright, greenBright, blueBright = self.update_colour(
obj._color.vec4)
obj.set_color(redBright, greenBright, blueBright)
for i in range(np.size(self.viewer.geoms)):
redBright, greenBright, blueBright = self.update_colour(
self.colours[i])
self.viewer.geoms[i].set_color(redBright, greenBright, blueBright)
return self.viewer.render(True)
def _render(self, mode='human', close=False):
if close:
if self.viewer is not None:
self.viewer.close()
self.viewer = None
return
x1,y1,z1 = self.body1.getPosition()
x2,y2,z2 = self.body2.getPosition()
x3,y3,z3 = self.body3.getPosition()
x5,y5,z5 = self.body5.getPosition()
if self.viewer is None:
self.viewer = rendering.Viewer(self.viewerSize,self.viewerSize)
self.viewer.perspective_transform_on=self.perspective_transform_on
self.viewer.set_bounds(-self.spaceSize/2.0,self.spaceSize/2.0,-self.spaceSize/2.0,self.spaceSize/2.0)
rod1 = rendering.make_cuboid(1, .3)
rod1.set_color(0.4, 0.4, 0.4)
self.pole_transform1 = rendering.Transform()
self.pole_transform11 = rendering.Transform()
rod1.add_attr(self.pole_transform1)
rod1.add_attr(self.pole_transform11)
rod2 = rendering.make_cuboid(1, .2)
rod2.set_color(0.2, 0.2, 0.2)
self.pole_transform2 = rendering.Transform()
self.pole_transform21 = rendering.Transform()
rod2.add_attr(self.pole_transform2)
rod2.add_attr(self.pole_transform21)
rod3 = rendering.make_cuboid(1, .1)
rod3.set_color(0,0,0)
self.pole_transform3 = rendering.Transform()
self.pole_transform31 = rendering.Transform()
rod3.add_attr(self.pole_transform3)
rod3.add_attr(self.pole_transform31)
rod5 = rendering.make_cuboid(1, .1)
rod5.set_color(0.5,0.5,0.5)
self.pole_transform5 = rendering.Transform()
self.pole_transform51 = rendering.Transform()
rod5.add_attr(self.pole_transform5)
rod5.add_attr(self.pole_transform51)
axle1 = rendering.make_sphere(0.25)
axle1.set_color(1,0,0)
self.axle_transform1 = rendering.Transform()
axle1.add_attr(self.axle_transform1)
self.axle_transform12 = rendering.Transform()
axle1.add_attr(self.axle_transform12)
axle2 = rendering.make_sphere(.22)
axle2.set_color(0,1,0)
self.axle_transform2 = rendering.Transform()
axle2.add_attr(self.axle_transform2)
self.axle_transform22 = rendering.Transform()
axle2.add_attr(self.axle_transform22)
axle3 = rendering.make_sphere(.18)
axle3.set_color(0,0,1)
self.axle_transform3 = rendering.Transform()
axle3.add_attr(self.axle_transform3)
self.axle_transform32 = rendering.Transform()
axle3.add_attr(self.axle_transform32)
axle5 = rendering.make_sphere(.13)
axle5.set_color(0,0,0.5)
self.axle_transform5 = rendering.Transform()
axle5.add_attr(self.axle_transform5)
self.axle_transform52 = rendering.Transform()
axle5.add_attr(self.axle_transform52)
self.viewer.add_geom(rod1)
self.viewer.add_geom(rod2)
self.viewer.add_geom(axle1)
self.viewer.add_geom(rod3)
self.viewer.add_geom(axle2)
self.viewer.add_geom(rod5)
self.viewer.add_geom(axle3)
self.viewer.add_geom(axle5)
self.target.set_color(0.7,0.7,0.7)
self.invalid_target.set_color(0.7,0.7,0.7)
self.viewer.add_onetime(self.target)
self.viewer.add_onetime(self.invalid_target)
self.transform_link(self.body1, self.pole_transform11, self.pole_transform1, self.axle_transform12, self.axle_transform1)
self.transform_link(self.body2,self.pole_transform21, self.pole_transform2,self.axle_transform22,self.axle_transform2)
self.transform_link(self.body3,self.pole_transform31, self.pole_transform3,self.axle_transform32,self.axle_transform3)
self.transform_link(self.body5,self.pole_transform51, self.pole_transform5,self.axle_transform52,self.axle_transform5)
self.targetTrans.set_translation(self.targetPos[0],self.targetPos[1],self.targetPos[2])
self.invalidTargetTrans.set_translation(self.invalpos[0],self.invalpos[1],self.invalpos[2])
self.ground_truth_joint_angles = np.array([self.j1.getAngle(), self.j2.getAngle(),self.j3.getAngle(),self.j5.getAngle()])
self.ground_truth_joint_velocities = np.array([self.j1.getAngleRate(), self.j2.getAngleRate(),self.j3.getAngleRate(),self.j5.getAngleRate()])
self.ground_truth_valid_target = self.targetPos.T.flatten()
self.ground_truth_end_effector = self.body4.getPosition()
return self.viewer.render(True)
