Ejemplo n.º 1
0
Archivo: arm.py Proyecto: eejd/blog
    def __init__(self, **kwargs):

        Arm.__init__(self, singularity_thresh=1e-5, **kwargs)

        # length of arm links
        self.l1 = 0.31
        self.l2 = 0.27
        self.L = np.array([self.l1, self.l2])
        # mass of links
        m1 = 1.98
        m2 = 1.32
        # z axis inertia moment of links
        izz1 = 15.0
        izz2 = 8.0
        # create mass matrices at COM for each link
        self.M1 = np.zeros((6, 6))
        self.M2 = np.zeros((6, 6))
        self.M1[0:3, 0:3] = np.eye(3) * m1
        self.M1[3:, 3:] = np.eye(3) * izz1
        self.M2[0:3, 0:3] = np.eye(3) * m2
        self.M2[3:, 3:] = np.eye(3) * izz2

        self.rest_angles = np.array([np.pi / 4.0, np.pi / 4.0])

        # stores information returned from maplesim
        self.state = np.zeros(7)
        # maplesim arm simulation
        self.sim = py2LinkArm.pySim(dt=self.dt)
        self.sim.reset(self.state)
        self.update_state()
Ejemplo n.º 2
0
    def __init__(self, **kwargs):

        Arm.__init__(self, **kwargs)

        # length of arm links
        self.l1 = .31
        self.l2 = .27
        self.L = np.array([self.l1, self.l2])
        # mass of links
        m1 = 1.98
        m2 = 1.32
        # z axis inertia moment of links
        izz1 = 15.
        izz2 = 8.
        # create mass matrices at COM for each link
        self.M1 = np.zeros((6, 6))
        self.M2 = np.zeros((6, 6))
        self.M1[0:3, 0:3] = np.eye(3) * m1
        self.M1[3:, 3:] = np.eye(3) * izz1
        self.M2[0:3, 0:3] = np.eye(3) * m2
        self.M2[3:, 3:] = np.eye(3) * izz2

        self.rest_angles = np.array([np.pi / 4.0, np.pi / 4.0])

        # stores information returned from maplesim
        self.state = np.zeros(7)
        # maplesim arm simulation
        self.sim = py2LinkArm.pySim(dt=self.dt)
        self.sim.reset(self.state)
        self.update_state()
Ejemplo n.º 3
0
    def __init__(self, u=[.1, 0]):
        self.u = np.asarray(u, dtype='float')  # control signal
        self.state = np.zeros(3)  # vector for current state
        self.L1 = 0.37  # length of arm link 1 in m
        self.L2 = 0.27  # length of arm link 2 in m
        self.time_elapsed = 0

        self.sim = py2LinkArm.pySim()
        self.sim.reset(self.state)
Ejemplo n.º 4
0
    def __init__(self, u = [.1, 0]): 
        self.u = np.asarray(u, dtype='float') # control signal
        self.state = np.zeros(3) # vector for current state
        self.L1=0.37 # length of arm link 1 in m
        self.L2=0.27 # length of arm link 2 in m
        self.time_elapsed = 0

        self.sim = py2LinkArm.pySim()
        self.sim.reset(self.state)
Ejemplo n.º 5
0
    def __init__(self, **kwargs):

        Arm2Base.__init__(self, **kwargs)

        # stores information returned from maplesim
        self.state = np.zeros(7) 
        # maplesim arm simulation
        self.sim = py2LinkArm.pySim(dt=1e-5)
        self.sim.reset(self.state)
        self.reset() # set to init_q and init_dq
Ejemplo n.º 6
0
    def __init__(self, **kwargs):

        Arm2Base.__init__(self, **kwargs)

        # stores information returned from maplesim
        self.state = np.zeros(7)
        # maplesim arm simulation
        self.sim = py2LinkArm.pySim(dt=1e-5)
        self.sim.reset(self.state)
        self.reset()  # set to init_q and init_dq
Ejemplo n.º 7
0
Archivo: arm.py Proyecto: liuyepku/blog
    def __init__(self, dt=1e-5):
        self.l1 = .31
        self.l2 = .27

        # mass of links
        m1=1.98; m2=1.32
        # z axis inertia moment of links
        izz1=15.; izz2=8.
        # create mass matrices at COM for each link
        self.M1 = np.zeros((6,6)); self.M2 = np.zeros((6,6)) 
        self.M1[0:3,0:3] = np.eye(3)*m1; self.M1[3:,3:] = np.eye(3)*izz1
        self.M2[0:3,0:3] = np.eye(3)*m2; self.M2[3:,3:] = np.eye(3)*izz2
        
        # stores information returned from maplesim
        self.state = np.zeros(7) 
        # maplesim arm simulation
        self.dt = dt
        self.sim = py2LinkArm.pySim(dt=dt)
        self.sim.reset(self.state)
        self.update_state()
Ejemplo n.º 8
0
def run(): 
    """Runs and plots a 2 link arm simulation generated by MapleSim."""

    # set up input control signal to simulation
    u = np.ones(2) * .1
    # set up array to receive output from simulation
    out = np.zeros(3) 

    # create MapleSim sim class 
    sim = py2LinkArm.pySim() 
    # set up initial conditions of the system
    sim.reset(out) 

    ###### Plotting things

    # segment lengths = (.31, .27)m (defined in MapleSim)
    L = np.array([31, 27]) * 3

    # make our window for drawin'
    window = pyglet.window.Window()

    # set up some labels to display time 
    label_time = pyglet.text.Label('time', font_name='Times New Roman', 
        font_size=36, x=window.width//2, y=window.height - 50,
        anchor_x='center', anchor_y='center')
    # and joint angles
    label_values = pyglet.text.Label('values', font_name='Times New Roman', 
        font_size=36, x=window.width//2, y=window.height - 100,
        anchor_x='center', anchor_y='center')

    def update_arm(dt):
        """ Simulate the arm ahead a chunk of time, then find the 
        (x,y) coordinates of each joint, and update labels."""
   
        # simulate arm forward 15ms
        for i in range(1000):
            sim.step(out, u)

        # find new joint (x,y) coordinates, offset to center of screen-ish
        x = np.array([ 0, 
            L[0]*np.cos(out[1]),
            L[0]*np.cos(out[1]) + L[1]*np.cos(out[1]+out[2])]) + window.width/2
        y = np.array([ 0, 
            L[0]*np.sin(out[1]),
            L[0]*np.sin(out[1]) + L[1]*np.sin(out[1]+out[2])]) + 100

        # update line endpoint positions for drawing
        window.jps = np.array([x, y]).astype('int')
        label_time.text = 'time: %.3f'%out[0]
        label_values.text = '(01, 02) = (%.2f, %.2f)'%(out[1], out[2])
 
    update_arm(0) # call once to set up

    @window.event
    def on_draw():
        window.clear()
        label_time.draw()
        label_values.draw()
        for i in range(2): 
            pyglet.graphics.draw(2, pyglet.gl.GL_LINES, ('v2i', 
                (window.jps[0][i], window.jps[1][i], 
                 window.jps[0][i+1], window.jps[1][i+1])))

    # set up update_arm function to be called super often
    pyglet.clock.schedule_interval(update_arm, 0.00001)
    pyglet.app.run()