Commit 626a16ca authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Python clean, mostly pep8

https://www.python.org/dev/peps/pep-0008/

A few hints:
    + About Spaces:
        - always after a comma
        - never after [, (, {
        - never before ], ), }
        - always around -, +, *, **, %, ==, >=, <=
        - always around = for an assignation
        - never around = for a default value of an argument
        - never at the end of a line
    + One statement by line:
        - nothing after : unless inside []
        - no ;
    + No useless (), like in a simple if or assert
    + Indentation uses four spaces
    + Blanklines:
        - 2 before class or function declaration
        - 1 before class method declaration
        - never more than 2
    + Line length:
        - We said 79. This is far from beeign respected. Maybe 119 is more reasonable
    + Align:
        - at the begining of a line continuation
        - not multiple spaces before = to align multiple assignements

Of course, in our case, it would be tricky to follow some other rules, like:
    N802 function name should be lowercase
    N803 argument name should be lowercase
    N806 variable in function should be lowercase

Other issues may be addressed:
    F401 '<something>' imported but unused
    F403 'from <some_lib> import *' used; unable to detect undefined names

To check all of this:
    flake8 --ignore=N802,N803,N806 --max-line-length=119 <root_folder>

Other things changed:
    - import order (thanks isort)
    - useless elif changed to if
    - useless else removed
    - usage of ValueError, when an argument does not fit the expected type / size
    - issubclass(x.__class__, X) / x.__class__ == X → isinstance(x, X)
    - isinstance accepts a list or a tuple of classes (eg: isinstance(x, (int, float)))
    - print "" → print
parent c2bc2f56
......@@ -5,11 +5,12 @@ interpolating SE(3) movements.
'''
import robotviewer
viewer=robotviewer.client('XML-RPC')
viewer = robotviewer.client('XML-RPC')
try:
viewer.updateElementConfig('RomeoTrunkYaw',[1,0,0,0,0,0])
except:
viewer.updateElementConfig = lambda a,b: a
viewer.updateElementConfig('RomeoTrunkYaw', [1, 0, 0, 0, 0, 0])
except:
viewer.updateElementConfig = lambda a, b: a
def se3ToRpy(m):
return M.translation.T.tolist()[0] + matrixToRpy(M.rotation).T.tolist()[0]
......@@ -23,62 +24,63 @@ N = 1000
M = se3.SE3.Identity()
# Integrate a constant body velocity.
v = zero(3); v[2] = 1.0 / N
w = zero(3); w[1] = 1.0 / N
nu = se3.Motion( v, w)
v = zero(3)
v[2] = 1.0 / N
w = zero(3)
w[1] = 1.0 / N
nu = se3.Motion(v, w)
for i in range(N):
M = se3.exp(nu)*M
M = se3.exp(nu) * M
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a velocity of the body that is constant in the world frame.
for i in range(N):
Mc = se3.SE3(M.rotation,zero(3))
M = M*se3.exp(Mc.actInv(nu))
Mc = se3.SE3(M.rotation, zero(3))
M = M * se3.exp(Mc.actInv(nu))
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
time.sleep(1)
# Integrate a constant "log" velocity in body frame.
ME = se3.SE3( se3.Quaternion(0.7, -0.6, 0.1, 0.4).normalized().matrix(),
np.matrix([1,-1,2],np.double).T )
nu = se3.Motion(se3.log(M.inverse()*ME).vector()/N)
ME = se3.SE3(se3.Quaternion(0.7, -0.6, 0.1, 0.4).normalized().matrix(),
np.matrix([1, -1, 2], np.double).T)
nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
for i in range(N):
M = M*se3.exp(nu)
M = M * se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm( se3.log(M.inverse()*ME).vector() )
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
time.sleep(1)
# Integrate a constant "log" velocity in reference frame.
ME = se3.SE3( se3.Quaternion(0.3, -0.2, 0.6, 0.5).normalized().matrix(),
np.matrix([-1,1,0.6],np.double).T )
nu = se3.Motion(se3.log(M.inverse()*ME).vector()/N)
ME = se3.SE3(se3.Quaternion(0.3, -0.2, 0.6, 0.5).normalized().matrix(),
np.matrix([-1, 1, 0.6], np.double).T)
nu = se3.Motion(se3.log(M.inverse() * ME).vector() / N)
for i in range(N):
M = M*se3.exp(nu)
M = M * se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm( se3.log(M.inverse()*ME).vector() )
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
time.sleep(1)
# Integrate an exponential decay vector field toward ME.
ME = se3.SE3( se3.Quaternion(0.9, -0.1, 0.1, 0.1).normalized().matrix(),
np.matrix([1,0.2,1.9],np.double).T )
ME = se3.SE3(se3.Quaternion(0.9, -0.1, 0.1, 0.1).normalized().matrix(),
np.matrix([1, 0.2, 1.9], np.double).T)
for i in range(N):
nu = se3.log(M.inverse()*ME).vector() * 1e-2
M = M*se3.exp(nu)
nu = se3.log(M.inverse() * ME).vector() * 1e-2
M = M * se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm( se3.log(M.inverse()*ME).vector() )
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
time.sleep(1)
# Integrate a straight-line vector field toward ME.
ME = se3.SE3( se3.Quaternion(0.1, -0.6, 0.6, 0.1).normalized().matrix(),
np.matrix([.1,-1.2,0.3],np.double).T )
ME = se3.SE3(se3.Quaternion(0.1, -0.6, 0.6, 0.1).normalized().matrix(),
np.matrix([.1, -1.2, 0.3], np.double).T)
for i in range(N):
ERR = ME.inverse()*M
v = ERR.rotation.T*ERR.translation * -9e-3
w = ERR.rotation.T*se3.log(ERR.rotation) * -9e-3
nu = se3.Motion(v,w)
M = M*se3.exp(nu)
ERR = ME.inverse() * M
v = ERR.rotation.T * ERR.translation * -9e-3
w = ERR.rotation.T * se3.log(ERR.rotation) * -9e-3
nu = se3.Motion(v, w)
M = M * se3.exp(nu)
viewer.updateElementConfig('RomeoTrunkYaw', se3ToRpy(M))
print "Residuals = ", norm( se3.log(M.inverse()*ME).vector() )
print "Residuals = ", norm(se3.log(M.inverse() * ME).vector())
time.sleep(1)
......@@ -4,7 +4,8 @@ abscissia), dimension 3 (x,y,z coordinates), dimension 6 (SE3 exp integration)
and dimension 6 (R3xSO3 integration, straightline in the cartesian space).
'''
def kineinv_dim1(q0,xdes,N=100):
def kineinv_dim1(q0, xdes, N=100):
'''
Bring the robot hand to X=xdes, using a gradient descent (which is also a
Gauss-Newton descent in this trivial case).
......@@ -13,17 +14,18 @@ def kineinv_dim1(q0,xdes,N=100):
for i in range(N):
Mrh = robot.Mrh(q)
e = Mrh.translation[0,0] - xdes
J = Mrh.rotation * robot.Jrh(q)[:3,:]
J = J[0,:]
e = Mrh.translation[0, 0] - xdes
J = Mrh.rotation * robot.Jrh(q)[:3, :]
J = J[0, :]
qdot = -J.T * e
qdot = -J.T*e
robot.increment(q,qdot*5e-2)
robot.increment(q, qdot * 5e-2)
robot.display(q)
print "Residuals = ",robot.Mrh(q).translation[0,0] - xdes
print "Residuals = ", robot.Mrh(q).translation[0, 0] - xdes
def kineinv_dim3(q0,xdes,N=100):
def kineinv_dim3(q0, xdes, N=100):
'''
Bring the robot hand to [x,y,z]=xdes using Gauss-Newton descent. Arg q0 is
the initial robot position (np.matrix Nx1); xdes is the desired position
......@@ -33,16 +35,17 @@ def kineinv_dim3(q0,xdes,N=100):
for i in range(N):
Mrh = robot.Mrh(q)
e = Mrh.translation[0:3,0] - xdes
J = Mrh.rotation * robot.Jrh(q)[:3,:]
e = Mrh.translation[:3, 0] - xdes
J = Mrh.rotation * robot.Jrh(q)[:3, :]
qdot = -npl.pinv(J) * e
qdot = -npl.pinv(J)*e
robot.increment(q,qdot*5e-2)
robot.increment(q, qdot * 5e-2)
robot.display(q)
print "Residuals = ",npl.norm(robot.Mrh(q).translation[:3] - xdes)
print "Residuals = ", npl.norm(robot.Mrh(q).translation[:3] - xdes)
def kineinv_dim6(q0,Mdes,straight=True,N=100):
def kineinv_dim6(q0, Mdes, straight=True, N=100):
'''
Bring the robot hand to the reference placement (position+orientation) Mdes
using Gauss-Newton descent. Arg q0 is the initial robot position (np.matrix
......@@ -55,30 +58,31 @@ def kineinv_dim6(q0,Mdes,straight=True,N=100):
q = np.copy(q0)
for i in range(100):
Mrh = robot.Mrh(q)
if straight:
ERR = Mdes.inverse()*Mrh
v = ERR.rotation.T*ERR.translation * -1
w = ERR.rotation.T*se3.log(ERR.rotation) * -1
nu = se3.Motion(v,w)
ERR = Mdes.inverse() * Mrh
v = -ERR.rotation.T * ERR.translation
w = -ERR.rotation.T * se3.log(ERR.rotation)
nu = se3.Motion(v, w)
else:
nu = se3.log( Mrh.inverse()*Mdes )
nu = se3.log(Mrh.inverse() * Mdes)
Jrh = robot.Jrh(q)
qdot = npl.pinv(Jrh)*nu.vector()
robot.increment(q,qdot*5e-2)
qdot = npl.pinv(Jrh) * nu.vector()
robot.increment(q, qdot * 5e-2)
robot.display(q)
print "Residuals = ",npl.norm(se3.log(robot.Mrh(q).inverse()*Mdes).vector())
print "Residuals = ", npl.norm(se3.log(robot.Mrh(q).inverse() * Mdes).vector())
# --- MAIN ------------------------------------------------------
import pinocchio as se3
from pinocchio.utils import *
if __name__ == '__main__':
import pinocchio as se3
from pinocchio.utils import *
robot = se3.RobotWrapper('../../models/romeo.urdf')
robot.initDisplay()
robot = se3.RobotWrapper('../../models/romeo.urdf')
robot.initDisplay()
kineinv_dim1(robot.q0, 2.0)
kineinv_dim3(robot.q0, np.matrix([2.0,0.5,1.0]).T)
kineinv_dim6(robot.q0, se3.SE3(eye(3),np.matrix([2.0,0.5,1.0]).T),straight=False)
kineinv_dim6(robot.q0, se3.SE3(eye(3),np.matrix([2.0,0.5,1.0]).T),straight=True)
kineinv_dim1(robot.q0, 2.0)
kineinv_dim3(robot.q0, np.matrix([2.0, 0.5, 1.0]).T)
kineinv_dim6(robot.q0, se3.SE3(eye(3), np.matrix([2.0, 0.5, 1.0]).T), straight=False)
kineinv_dim6(robot.q0, se3.SE3(eye(3), np.matrix([2.0, 0.5, 1.0]).T), straight=True)
import pinocchio as se3
from pinocchio.utils import *
from pinocchio.utils import isapprox, np, npl, rand, skew, zero
# --- SE3
R = rand([3,3]); [R,tmp,tmp] = npl.svd(R)
R = rand([3, 3])
[R, tmp, tmp] = npl.svd(R)
p = rand(3)
m = se3.SE3(R,p)
X = np.vstack( [ np.hstack([ R, skew(p)*R ]), np.hstack([ zero([3,3]), R ]) ])
assert( isapprox(m.action(),X))
M = np.vstack( [ np.hstack([R,p]), np.matrix('0 0 0 1',np.double) ] )
assert( isapprox(m.homogeneous(),M) )
m = se3.SE3(R, p)
X = np.vstack([np.hstack([R, skew(p) * R]), np.hstack([zero([3, 3]), R])])
assert isapprox(m.action(), X)
M = np.vstack([np.hstack([R, p]), np.matrix('0 0 0 1', np.double)])
assert isapprox(m.homogeneous(), M)
m2 = se3.SE3.Random()
assert(isapprox( (m*m2).homogeneous(),m.homogeneous()*m2.homogeneous() ))
assert(isapprox( (~m).homogeneous(),npl.inv(m.homogeneous()) ))
assert isapprox((m * m2).homogeneous(), m.homogeneous() * m2.homogeneous())
assert isapprox((~m).homogeneous(), npl.inv(m.homogeneous()))
p = rand(3)
assert(isapprox(m*p,m.rotation*p+m.translation))
assert(isapprox(m.actInv(p),m.rotation.T*p-m.rotation.T*m.translation))
assert isapprox(m * p, m.rotation * p + m.translation)
assert isapprox(m.actInv(p), m.rotation.T * p - m.rotation.T * m.translation)
p = np.vstack([p,1])
assert(isapprox(m*p,m.homogeneous()*p))
assert(isapprox(m.actInv(p),npl.inv(m.homogeneous())*p))
p = np.vstack([p, 1])
assert isapprox(m * p, m.homogeneous() * p)
assert isapprox(m.actInv(p), npl.inv(m.homogeneous()) * p)
p = rand(6)
assert(isapprox(m*p,m.action()*p))
assert(isapprox(m.actInv(p),npl.inv(m.action())*p))
assert isapprox(m * p, m.action() * p)
assert isapprox(m.actInv(p), npl.inv(m.action()) * p)
# --- Motion
assert(isapprox(se3.Motion.Zero().vector(),zero(6)))
assert isapprox(se3.Motion.Zero().vector(), zero(6))
v = se3.Motion.Random()
assert(isapprox((m*v).vector(),m.action()*v.vector()))
assert(isapprox((m.actInv(v)).vector(),npl.inv(m.action())*v.vector()))
vv = v.linear; vw = v.angular
assert(isapprox( v.vector(),np.vstack([vv,vw]) ))
assert(isapprox((v**v).vector(),zero(6)))
assert isapprox((m * v).vector(), m.action() * v.vector())
assert isapprox((m.actInv(v)).vector(), npl.inv(m.action()) * v.vector())
vv = v.linear
vw = v.angular
assert isapprox(v.vector(), np.vstack([vv, vw]))
assert isapprox((v ** v).vector(), zero(6))
# --- Force ---
assert(isapprox(se3.Force.Zero().vector(),zero(6)))
assert isapprox(se3.Force.Zero().vector(), zero(6))
f = se3.Force.Random()
ff = f.linear; ft = f.angular
assert(isapprox( f.vector(),np.vstack([ff,ft]) ))
ff = f.linear
ft = f.angular
assert isapprox(f.vector(), np.vstack([ff, ft]))
assert(isapprox((m*f).vector(),npl.inv(m.action().T)*f.vector()))
assert(isapprox((m.actInv(f)).vector(),m.action().T*f.vector()))
f = se3.Force( np.vstack([ v.vector()[3:], v.vector()[:3] ]) )
assert(isapprox( (v**f).vector(),zero(6) ))
assert isapprox((m * f).vector(), npl.inv(m.action().T) * f.vector())
assert isapprox((m.actInv(f)).vector(), m.action().T * f.vector())
f = se3.Force(np.vstack([v.vector()[3:], v.vector()[:3]]))
assert isapprox((v ** f).vector(), zero(6))
# --- Inertia ---
Y1 = se3.Inertia.Random()
Y2 = se3.Inertia.Random()
Y=Y1+Y2
assert(isapprox(Y1.matrix()+Y2.matrix(),Y.matrix()))
assert(isapprox((Y*v).vector(),Y.matrix()*v.vector()))
assert(isapprox( (m*Y).matrix(),m.inverse().action().T*Y.matrix()*m.inverse().action()))
assert(isapprox( (m.actInv(Y)).matrix(),m.action().T*Y.matrix()*m.action()))
Y = Y1 + Y2
assert isapprox(Y1.matrix() + Y2.matrix(), Y.matrix())
assert isapprox((Y * v).vector(), Y.matrix() * v.vector())
assert isapprox((m * Y).matrix(), m.inverse().action().T * Y.matrix() * m.inverse().action())
assert isapprox((m.actInv(Y)).matrix(), m.action().T * Y.matrix() * m.action())
import pinocchio as se3
from pinocchio.utils import *
from pinocchio.utils import isapprox, np, zero
model = se3.Model.BuildEmptyModel()
assert(model.nbody==1 and model.nq==0 and model.nv==0)
assert model.nbody == 1 and model.nq == 0 and model.nv == 0
model = se3.Model.BuildHumanoidSimple()
nb=28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
assert(model.nbody==nb and model.nq==nb-1+6 and model.nv==nb-1+5)
nb = 28 # We should have 28 bodies, thus 27 joints, one of them a free-flyer.
assert model.nbody == nb and model.nq == nb - 1 + 6 and model.nv == nb - 1 + 5
model.inertias[1] = model.inertias[2]
assert( isapprox(model.inertias[1].np,model.inertias[2].np) )
assert isapprox(model.inertias[1].np, model.inertias[2].np)
model.jointPlacements[1] = model.jointPlacements[2]
assert( isapprox(model.jointPlacements[1].np,model.jointPlacements[2].np) )
assert(model.parents[0]==0 and model.parents[1] == 0)
assert isapprox(model.jointPlacements[1].np, model.jointPlacements[2].np)
assert model.parents[0] == 0 and model.parents[1] == 0
model.parents[2] = model.parents[1]
assert( model.parents[2] == model.parents[1] )
assert(model.names[0] == "universe" )
assert( isapprox(model.gravity.np,np.matrix('0; 0; -9.81; 0; 0; 0')) )
assert model.parents[2] == model.parents[1]
assert model.names[0] == "universe"
assert isapprox(model.gravity.np, np.matrix('0; 0; -9.81; 0; 0; 0'))
data = model.createData()
......@@ -23,10 +23,11 @@ data = model.createData()
q = zero(model.nq)
qdot = zero(model.nv)
qddot = zero(model.nv)
for i in range(model.nbody): data.a[i] = se3.Motion.Zero()
for i in range(model.nbody):
data.a[i] = se3.Motion.Zero()
se3.rnea(model,data,q,qdot,qddot)
se3.rnea(model, data, q, qdot, qddot)
for i in range(model.nbody):
assert( isapprox(data.v[i].np,zero(6)) )
assert( isapprox(data.a[0].np,-model.gravity.np) )
assert( isapprox(data.f[-1],model.inertias[-1]*data.a[-1]) )
assert isapprox(data.v[i].np, zero(6))
assert isapprox(data.a[0].np, -model.gravity.np)
assert isapprox(data.f[-1], model.inertias[-1] * data.a[-1])
......@@ -15,61 +15,63 @@
# <http://www.gnu.org/licenses/>.
import numpy as np
from pinocchio.robot_wrapper import RobotWrapper
import libpinocchio_pywrap as se3
import utils
from explog import exp, log
from libpinocchio_pywrap import *
se3.SE3.__repr__ = se3.SE3.__str__
se3.Motion.__repr__ = se3.Motion.__str__
se3.AngleAxis.__repr__ = lambda s: 'AngleAxis('+s.vector().__str__()+')'
se3.AngleAxis.__repr__ = lambda s: 'AngleAxis(%s)' % s.vector()
# --- SE3 action ---
def SE3act(m,x):
assert(isinstance(m,se3.SE3))
if isinstance(x,np.ndarray):
if x.shape[0]==3:
return m.rotation*x+m.translation
elif x.shape[0] == 4:
return m.homogeneous()*x
elif x.shape[0] == 6:
return m.action()*x
else: raise Exception('Error: m can only act on linear object of size 3, 4 and 6.')
elif 'se3Action' in x.__class__.__dict__:
def SE3act(m, x):
assert isinstance(m, se3.SE3)
if isinstance(x, np.ndarray):
if x.shape[0] == 3:
return m.rotation * x + m.translation
if x.shape[0] == 4:
return m.homogeneous() * x
if x.shape[0] == 6:
return m.action() * x
raise ValueError('Error: m can only act on linear object of size 3, 4 and 6.')
if 'se3Action' in x.__class__.__dict__:
return x.se3Action(m)
else:
#print 'Error: SE3 cannot act on the given object'
return m.oldmult(x)
setattr(se3.SE3,'oldmult',se3.SE3.__mul__)
setattr(se3.SE3,'__mul__',SE3act)
setattr(se3.SE3,'act',SE3act)
return m.oldmult(x)
setattr(se3.SE3, 'oldmult', se3.SE3.__mul__)
setattr(se3.SE3, '__mul__', SE3act)
setattr(se3.SE3, 'act', SE3act)
def SE3actinv(m,x):
assert(isinstance(m,se3.SE3))
if isinstance(x,np.ndarray):
if x.shape[0]==3:
return m.rotation.T*x-m.rotation.T*m.translation
elif x.shape[0] == 4:
return m.inverse().homogeneous()*x
elif x.shape[0] == 6:
return m.inverse().action()*x
else: raise Exception('Error: m can only act on linear object of size 3, 4 and 6.')
elif 'se3Action' in x.__class__.__dict__:
def SE3actinv(m, x):
assert isinstance(m, se3.SE3)
if isinstance(x, np.ndarray):
if x.shape[0] == 3:
return m.rotation.T * x - m.rotation.T * m.translation
if x.shape[0] == 4:
return m.inverse().homogeneous() * x
if x.shape[0] == 6:
return m.inverse().action() * x
raise ValueError('Error: m can only act on linear object of size 3, 4 and 6.')
if 'se3Action' in x.__class__.__dict__:
return x.se3ActionInverse(m)
else:
print 'Error: SE3 cannot act on the given object'
setattr(se3.SE3,'actInv',SE3actinv)
raise ValueError('Error: SE3 cannot act on the given object')
setattr(se3.SE3, 'actInv', SE3actinv)
# --- M6/F6 cross product --
def SE3cross(self,y):
assert(isinstance(self,se3.Motion))
if isinstance(y,se3.Motion):
def SE3cross(self, y):
assert isinstance(self, se3.Motion)
if isinstance(y, se3.Motion):
return self.cross_motion(y)
elif isinstance(y,se3.Force):
if isinstance(y, se3.Force):
return self.cross_force(y)
else: raise Exception('Error: SE3 cross product only apply on M6xM6 or M6xF6.')
setattr(se3.Motion,'__pow__',SE3cross)
setattr(se3.Motion,'cross',SE3cross)
from libpinocchio_pywrap import *
from pinocchio.robot_wrapper import RobotWrapper
from explog import exp,log
raise ValueException('Error: SE3 cross product only apply on M6xM6 or M6xF6.')
setattr(se3.Motion, '__pow__', SE3cross)
setattr(se3.Motion, 'cross', SE3cross)
......@@ -19,23 +19,32 @@ import libpinocchio_pywrap as se3
import numpy as np
import math
def exp(x):
if isinstance(x,se3.Motion): return se3.exp6FromMotion(x)
elif np.isscalar(x): return math.exp(x)
elif isinstance(x,np.matrix):
if len(x)==6: return se3.exp6FromVector(x)
elif len(x)==3: return se3.exp3(x)
else: print 'Error only 3 and 6 vectors are allowed.'
else: print 'Error exp is only defined for real, vector3, vector6 and se3.Motion objects.'
if isinstance(x, se3.Motion):
return se3.exp6FromMotion(x)
if np.isscalar(x):
return math.exp(x)
if isinstance(x, np.matrix):
if len(x) == 6:
return se3.exp6FromVector(x)
if len(x) == 3:
return se3.exp3(x)
raise ValueError('Error only 3 and 6 vectors are allowed.')
raise ValueError('Error exp is only defined for real, vector3, vector6 and se3.Motion objects.')
def log(x):
if isinstance(x,se3.SE3): return se3.log6FromSE3(x)
elif np.isscalar(x): return math.log(x)
elif isinstance(x,np.matrix):
if len(x)==4: return se3.log6FromMatrix(x)
elif len(x)==3: return se3.log3(x)
else: print 'Error only 3 and 4 matrices are allowed.'
else: print 'Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.'
def log(x):
if isinstance(x, se3.SE3):
return se3.log6FromSE3(x)
if np.isscalar(x):
return math.log(x)
if isinstance(x, np.matrix):
if len(x) == 4:
return se3.log6FromMatrix(x)
if len(x) == 3:
return se3.log3(x)
raise ValueError('Error only 3 and 4 matrices are allowed.')
raise ValueError('Error log is only defined for real, matrix3, matrix4 and se3.SE3 objects.')
__all__ = [ 'exp','log' ]
__all__ = ['exp', 'log']
......@@ -14,43 +14,44 @@
# Pinocchio If not, see
# <http://www.gnu.org/licenses/>.
import numpy as np
import time
import libpinocchio_pywrap as se3
import utils
from explog import exp
import time
class RobotWrapper:
def __init__(self, filename, package_dirs = None, root_joint = None):
if(root_joint is None):
self.model = se3.buildModelFromUrdf(filename)
class RobotWrapper(object):
def __init__(self, filename, package_dirs=None, root_joint=None):
if root_joint is None:
self.model = se3.buildModelFromUrdf(filename)
else:
self.model = se3.buildModelFromUrdf(filename, root_joint)
self.model = se3.buildModelFromUrdf(filename, root_joint)
self.data = self.model.createData()
if not "buildGeomFromUrdf" in dir(se3):
raise Exception('It seems that the Geometry Module has not been compiled with Pinocchio. No geometry model')
if "buildGeomFromUrdf" not in dir(se3):
raise Exception('the Geometry Module has not been compiled with Pinocchio. No geometry model')
else:
if (package_dirs is None):
if package_dirs is None:
self.geometry_model = se3.buildGeomFromUrdf(self.model, filename)