def initiateGraphics(self, graphics=Graphics.Basic(), id_frame=0):
self.graphics = graphics # Use this canvas for plotting etc
if graphics != 0:
self.id_frame = id_frame
self.displace = np.array(graphics.get_canvas_displacement(
id_frame)) # This is where the plotting starts
self.wall_displace = np.append(self.displace, np.array([0, 0]))
self.screen = graphics.screen
self.frameside = graphics.frameside * graphics.canvas_prop
def __init__(self, graphics=Graphics.Basic(), id_frame=0):
# STATE
self.q = None
self.x = None
self.q_goal = None
self.x_goal = None
self.x_Glob = None
self.walls = []
# GRAPHICS
# To use graphics you need a module 'Graphics.py', to not use graphics, use graphics = 0
self.initiateGraphics(graphics, id_frame)
def __init__(self,
graphics=Graphics.Basic(),
id_frame=0,
ip_address=None,
stiffHS=None,
stiffReach=None):
super(Nao, self).__init__(graphics, id_frame)
if ip_address is None:
ip_address = str(raw_input('Enter IP_ADDRESS: '))
IP_ADRESS = ip_address
PORT = 9559
prox = qi.Session()
prox.connect("tcp://" + IP_ADRESS + ":" + str(PORT))
self.body = BodyNao.Body(prox)
self.body.relax()
time.sleep(4)
x = self.body.get_x()
q = self.body.get_q()
self.x = x
self.q = q
self.x_old = copy.deepcopy(x)
self.q_old = copy.deepcopy(q)
self.q_goal = copy.deepcopy(q)
if stiffHS is None:
stiffHS = body.STIFFNESS
if stiffReach is None:
stiffReach = body.STIFFNESS
self.stiffReach = stiffReach
self.stiffHS = stiffHS
def __init__(self,
graphics=Graphics.Basic(),
id_frame=0,
random_start=False,
armLength=1.0,
dof=100,
q_start=None,
wall_setting=-1,
nSteps=20,
maxStep=1.):
super(Arm, self).__init__(graphics, id_frame)
# THE AGENT
# General settings
self.dof = dof # Degrees of freedom
self.dim = [2, dof] # [dim(task), dim(posture)]
self.segLen = armLength / dof # Length of an arm segment
self.aMax = np.pi # Max angle on joint (<= pi)
self.wall_setting = wall_setting
self.armLength = armLength
# Start conditions - this is overwritten if 'random_start' or 'm_start' is given.
aStart = 1. * 4 * np.pi / dof
self.angles = np.linspace(aStart, aStart / 4, dof) # Start in a spiral
self.angle_old = self.angles
self.angle_drawn = self.angles # For graphics
self.root = np.array([0.5, 0.5]) # Position of arm's base
self.angleStart = copy.deepcopy(self.angles)
self.angleGoal = np.zeros(dof) # start by unfolding
# THE UPDATES
self.firstRound = True
self.updateSteps = nSteps
self.stepsLeft = nSteps # Number of steps of motion
self.maxStep = maxStep # How far a joint can travel in one transition (speedlimit)
# THE ENVIRONMENT
cW = 0.2 # corridor width
wW = 0.02 # wall thickness
self.walls = np.array([[0, 0, 0, 0]])
choice = wall_setting # Add walls
if choice == 0:
self.walls = np.array([[.2, .2, .2, .2]])
elif choice == 1:
self.walls = np.array([[1 - cW - wW, 2 * cW, wW, 2 * cW],
[0., 4 * cW, 1 - cW, wW]])
elif choice == 2:
cW = 1. / 6
self.walls = np.array([[cW, 1.5 * cW, wW, 1 - 3 * cW],
[1 - wW - cW, 1.5 * cW, wW, 1 - 3 * cW]])
elif choice == 3:
self.walls = np.array([[1.5 * cW, cW, 1 - 3 * cW, wW],
[1.5 * cW, 1 - wW - cW, 1 - 3 * cW, wW],
[cW, 1.5 * cW, wW, 1 - 3 * cW],
[1 - wW - cW, 1.5 * cW, wW, 1 - 3 * cW]])
elif choice == 4: # One u-shaped wall above
self.walls = np.array([[1 - cW - wW, cW, wW, cW],
[cW, cW, 1 - 2 * cW, wW], [cW, cW, wW, cW]])
elif choice == 5: # One u-shaped wall bellow
self.walls = np.array([[cW, 1 - cW - wW, 1 - 2 * cW, wW],
[1 - cW - wW, 1 - 2 * cW, wW, cW],
[cW, 1 - 2 * cW, wW, cW]])
elif choice == 6: # Simple wall bellow
self.walls = np.array([[2 * cW, 1 - cW - wW, cW, wW]])
elif choice == 7: # Long wall above
self.walls = np.array([[cW, 1 - cW, 1 - 2 * cW, wW]])
elif choice == 8: # cross
self.walls = np.array([[.5 - .5 * wW, 0, wW, cW],
[.5 - .5 * wW, 1 - cW, wW, cW],
[0, .5 - .5 * wW, cW, wW],
[1 - cW, .5 - .5 * wW, cW, wW]])
# INITIAL UPDATE
if random_start:
q_start = self.get_random_posture()
self.angles = self.q2ang(q_start)
elif q_start is not None:
self.angles = self.q2ang(q_start)
else: # Move arm until it is stopped
self.step()
self.firstRound = False
