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)