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()
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()
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)
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)
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
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
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()
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()
