def Rb2q(Rb):
global s
init_matlab()
pymat.put(s, 'Rb', array(Rb))
pymat.eval(s, 'q = Rb2q(Rb)')
q = pymat.get(s, 'q')
return q
def q2Rb(r, p, y):
global s
init_matlab()
pymat.put(s, 'R_glob', array([r, p, y]))
pymat.eval(s, 'Rb = q2Rb(R_glob)')
Rb = pymat.get(s, 'Rb')
return Rb
def xrel2xabs(q, xbody):
global s
init_matlab()
pymat.put(s, 'q', array(q))
pymat.put(s, 'xbody', array(xbody))
pymat.eval(s, 'x_curr = xrel2xabs(q, xbody)')
x_curr = pymat.get(s, 'x_curr')
return x_curr
def footpos(encoders):
global s
init_matlab()
arry = encoders.toarray()
pymat.put(s, 'encoders', arry)
pymat.eval(s, 'encoders')
pymat.eval(s, 'fp = footpos(encoders)')
feet_pos_rel = pymat.get(s, 'fp')
return feet_pos_rel
def kin_body_qa(encoder_angles, x_st, swing_leg):
global s
init_matlab()
x_st = transpose(x_st).tolist()
x_st.pop(swing_leg - 1)
x_st = transpose(x_st)
enc_ary = encoder_angles.toarray()
pymat.put(s, 'enc_ary', array(enc_ary))
pymat.put(s, 'x_st', array(x_st))
pymat.put(s, 'swing_leg', [swing_leg])
pymat.eval(s, 'swing_leg=swing_leg(1)')
pymat.eval(
s, '[x_body, R_body, valid] = kin_body_qa(enc_ary, x_st, swing_leg)')
x_body = pymat.get(s, 'x_body')
R_body = pymat.get(s, 'R_body')
valid = pymat.get(s, 'valid')
return x_body, R_body, valid
def get(self, name): return pymat.get(self.H, name)
#!/usr/bin/env python
from Numeric import *
import os, sys
# plot NumPy arrays in Matlab, if pymat is present:
try:
import pymat
except:
print "pymat module is not available..."; sys.exit(1)
x = arrayrange(0,4*math.pi,0.1)
m = pymat.open()
pymat.put(m, 'x', x);
pymat.eval(m, 'y = sin(x)')
pymat.eval(m, 'plot(x,y)')
y = pymat.get(m, 'y')
import time; time.sleep(4) # wait before killing the plot...
pymat.close(m)
# compare sin(x) in Matlab and Python:
print "Matlab: sin(%g)=%g. Python: sin(%g)=%g" % \
(x[1], sin(x[1]), x[1], y[1])
#!/usr/bin/python
# Simple test file for the PyMat interface
import pymat
from Numeric import *
x = array([1, 2, 3, 4, 5, 6])
H = pymat.open()
pymat.put(H, 'x', x)
pymat.eval(H, 'y = dct(x)')
y = pymat.get(H, 'y')
pymat.close(H)
expect = asarray([8.573214099741124e+000,
-4.162561795878958e+000,
-1.785561006679121e-015,
-4.082482904638622e-001,
-1.638955159695808e-015,
-8.007889124033019e-002])
err = sum((y-expect)*(y-expect))
print 'Norm of the error is:', err
if err < 1e-15:
print "(That's pretty small -- the test passes)"
else:
print "UMMM...THAT'S A BIT BIG! The test has failed."
