Commit 91d36e01 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

[Python 3] Format

parent 4acd0619
from robot_simu import RobotSimu
# flake8: noqa
from constraint import Constraint
from exp_moving_avg import ExpMovingAvg
from feature_generic import FeatureGeneric
from feature_joint_limits import FeatureJointLimits
from feature_point6d import FeaturePoint6d
from feature_position import FeaturePosition
from feature_posture import FeaturePosture
from feature_position_relative import FeaturePositionRelative
from feature_generic import FeatureGeneric
from feature_joint_limits import FeatureJointLimits
from feature_posture import FeaturePosture
from feature_visual_point import FeatureVisualPoint
from task import Task
from task_pd import TaskPD
from constraint import Constraint
from gain_adaptive import GainAdaptive
from gradient_ascent import GradientAscent
from joint_limitator import JointLimitator
from sot import SOT
from op_point_modifier import OpPointModifier
from kalman import Kalman
from math_small_entities import *
from op_point_modifier import OpPointModifier
from robot_simu import RobotSimu
from sot import SOT
from task import Task
from task_pd import TaskPD
from visual_point_projecter import VisualPointProjecter
from feature_visual_point import FeatureVisualPoint
from kalman import Kalman
from exp_moving_avg import ExpMovingAvg
from gradient_ascent import GradientAscent
......@@ -2,11 +2,12 @@
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
from dynamic_graph.sot.core import FeaturePoint6d
from dynamic_graph import plug
from dynamic_graph.entity import Entity
from dynamic_graph.sot.core import FeaturePoint6d
class FeaturePosition (Entity):
class FeaturePosition(Entity):
"""
Position of a rigid-body in space as a feature
......@@ -26,8 +27,7 @@ class FeaturePosition (Entity):
signalMap = dict()
def __init__(self, name, signalPosition=None, signalJacobian = None,
referencePosition = None):
def __init__(self, name, signalPosition=None, signalJacobian=None, referencePosition=None):
self._feature = FeaturePoint6d(name)
self.obj = self._feature.obj
self._reference = FeaturePoint6d(name + '_ref')
......@@ -44,32 +44,34 @@ class FeaturePosition (Entity):
# Signals stored in members
self.position = self._feature.signal('position')
self.reference = self._reference.signal('position')
self.velocity = self._reference.signal ('velocity')
self.velocity = self._reference.signal('velocity')
self.Jq = self._feature.signal('Jq')
self.error = self._feature.signal('error')
self.errordot = self._feature.signal ('errordot')
self.errordot = self._feature.signal('errordot')
self.selec = self._feature.signal('selec')
self.signalMap = {'position':self.position,
'reference':self.reference,
'Jq':self.Jq,
'error':self.error,
'selec':self.selec}
self.signalMap = {
'position': self.position,
'reference': self.reference,
'Jq': self.Jq,
'error': self.error,
'selec': self.selec
}
@property
def name(self) :
def name(self):
return self._feature.name
def signal (self, name):
def signal(self, name):
"""
Get a signal of the entity from signal name
"""
if name in self.signalMap.keys():
return self.signalMap[name]
else:
raise RunTimeError('No signal with this name')
raise RuntimeError('No signal with this name')
def signals(self) :
def signals(self):
"""
Return the list of signals
"""
......@@ -79,13 +81,13 @@ class FeaturePosition (Entity):
"""
Return the list of commands.
"""
return ('frame', 'getFrame', 'keep')
return ('frame', 'getFrame', 'keep')
def frame(self, f):
return self._feature.frame(f)
def getFrame (self):
return self._feature.getFrame ()
def getFrame(self):
return self._feature.getFrame()
def keep (self):
return self._feature.keep ()
def keep(self):
return self._feature.keep()
......@@ -2,16 +2,18 @@
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
from dynamic_graph.sot.core.feature_point6d_relative \
import FeaturePoint6dRelative
from dynamic_graph.signal_base import SignalBase
from functools import reduce
from dynamic_graph import plug
from dynamic_graph.entity import Entity
from dynamic_graph.signal_base import SignalBase
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
# Identity matrix of order 4
I4 = reduce(lambda m, i: m + (i*(0.,)+(1.,)+ (3-i)*(0.,),), range(4), ())
I4 = reduce(lambda m, i: m + (i * (0., ) + (1., ) + (3 - i) * (0., ), ), range(4), ())
class FeaturePositionRelative (Entity):
class FeaturePositionRelative(Entity):
"""
Relative position of two rigid-body frames in space as a feature
......@@ -58,33 +60,43 @@ class FeaturePositionRelative (Entity):
signalMap = dict()
def __init__(self, name, basePosition=None, otherPosition=None,
baseReference = None, otherReference = None,
JqBase = None, JqOther = None):
self._feature = FeaturePoint6dRelative (name)
def __init__(self,
name,
basePosition=None,
otherPosition=None,
baseReference=None,
otherReference=None,
JqBase=None,
JqOther=None):
self._feature = FeaturePoint6dRelative(name)
self.obj = self._feature.obj
self._reference = FeaturePoint6dRelative (name + '_ref')
self._reference = FeaturePoint6dRelative(name + '_ref')
# Set undefined input parameters as identity matrix
if basePosition is None: basePosition = I4
if otherPosition is None: otherPosition = I4
if baseReference is None: baseReference = I4
if otherReference is None: otherReference = I4
if basePosition is None:
basePosition = I4
if otherPosition is None:
otherPosition = I4
if baseReference is None:
baseReference = I4
if otherReference is None:
otherReference = I4
# If input positions are signals, plug them, otherwise set values
for (sout, sin) in \
((basePosition, self._feature.signal ('positionRef')),
(otherPosition, self._feature.signal ('position')),
(baseReference, self._reference.signal ('positionRef')),
(otherReference, self._reference.signal ('position'))):
if isinstance (sout, SignalBase): plug (sout, sin)
else: sin.value = sout
((basePosition, self._feature.signal('positionRef')),
(otherPosition, self._feature.signal('position')),
(baseReference, self._reference.signal('positionRef')),
(otherReference, self._reference.signal('position'))):
if isinstance(sout, SignalBase):
plug(sout, sin)
else:
sin.value = sout
if JqBase:
plug(JqBase, self._feature.signal('JqRef'))
if JqOther:
plug(JqOther, self._feature.signal('Jq'))
self._feature.setReference (self._reference.name)
self._feature.setReference(self._reference.name)
self._feature.signal('selec').value = '111111'
self._feature.frame('current')
......@@ -96,33 +108,35 @@ class FeaturePositionRelative (Entity):
self.JqBase = self._feature.signal('JqRef')
self.JqOther = self._feature.signal('Jq')
self.error = self._feature.signal('error')
self.jacobian = self._feature.signal ('jacobian')
self.jacobian = self._feature.signal('jacobian')
self.selec = self._feature.signal('selec')
self.signalMap = {'basePosition':self.basePosition,
'otherPosition':self.otherPosition,
'baseReference':self.baseReference,
'otherReference':self.otherReference,
'JqBase':self.JqBase,
'JqOther':self.JqOther,
'error':self.error,
'jacobian':self.jacobian,
'selec':self.selec}
self.signalMap = {
'basePosition': self.basePosition,
'otherPosition': self.otherPosition,
'baseReference': self.baseReference,
'otherReference': self.otherReference,
'JqBase': self.JqBase,
'JqOther': self.JqOther,
'error': self.error,
'jacobian': self.jacobian,
'selec': self.selec
}
@property
def name(self) :
def name(self):
return self._feature.name
def signal (self, name):
def signal(self, name):
"""
Get a signal of the entity from signal name
"""
if name in self.signalMap.keys():
return self.signalMap[name]
else:
raise RunTimeError('No signal with this name')
raise RuntimeError('No signal with this name')
def signals(self) :
def signals(self):
"""
Return the list of signals
"""
......@@ -137,5 +151,5 @@ class FeaturePositionRelative (Entity):
def frame(self, f):
return self._feature.frame(f)
def keep (self):
return self._feature.keep ()
def keep(self):
return self._feature.keep()
from derivator import Derivator_of_Matrix
from derivator import Derivator_of_Vector
# flake8: noqa
# from operator import Compose_RPY_and_T
# from operator import EndomorphismBasis
# from operator import ComposeVector_RPY_T
# from operator import WeightAdd_of_vector
# from operator import WeightDir
# from operator import Nullificator
from operator import (
Add_of_double, Add_of_matrix, Add_of_vector, Component_of_vector, Compose_R_and_T, ConvolutionTemporal,
HomoToMatrix, HomoToRotation, HomoToTwist, Inverse_of_matrix, Inverse_of_matrixHomo, Inverse_of_matrixrotation,
Inverse_of_matrixtwist, Inverse_of_unitquat, MatrixDiagonal, MatrixHomoToPose, MatrixHomoToPoseQuaternion,
MatrixHomoToPoseRollPitchYaw, MatrixHomoToPoseUTheta, MatrixToHomo, MatrixToQuaternion, MatrixToRPY,
MatrixToUTheta, MatrixTranspose, Multiply_double_vector, Multiply_matrix_vector, Multiply_matrixHomo_vector,
Multiply_of_double, Multiply_of_matrix, Multiply_of_matrixHomo, Multiply_of_matrixrotation,
Multiply_of_matrixtwist, Multiply_of_quaternion, Multiply_of_vector, PoseRollPitchYawToMatrixHomo,
PoseRollPitchYawToPoseUTheta, PoseUThetaToMatrixHomo, QuaternionToMatrix, RPYToMatrix, Selec_column_of_matrix,
Selec_of_matrix, Selec_of_vector, SkewSymToVector, Stack_of_vector, Substract_of_double, Substract_of_matrix,
Substract_of_vector, UThetaToQuaternion)
from vector_constant import VectorConstant
from derivator import Derivator_of_Matrix, Derivator_of_Vector
from matrix_constant import MatrixConstant
from operator import Inverse_of_matrix
from operator import Inverse_of_matrixHomo
from operator import Inverse_of_matrixtwist
from operator import Inverse_of_matrixrotation
from operator import Inverse_of_unitquat
from operator import Selec_of_vector
from operator import Component_of_vector
from operator import Selec_of_matrix
from operator import Selec_column_of_matrix
from operator import MatrixHomoToPose
from operator import MatrixHomoToPoseUTheta
from operator import SkewSymToVector
from operator import PoseUThetaToMatrixHomo
from operator import MatrixHomoToPoseQuaternion
from operator import MatrixHomoToPoseRollPitchYaw
from operator import PoseRollPitchYawToMatrixHomo
from operator import PoseRollPitchYawToPoseUTheta
from operator import HomoToMatrix
from operator import MatrixToHomo
from operator import HomoToTwist
from operator import HomoToRotation
from operator import RPYToMatrix
from operator import MatrixToRPY
from operator import QuaternionToMatrix
from operator import MatrixToQuaternion
from operator import MatrixToUTheta
from operator import UThetaToQuaternion
from operator import MatrixDiagonal
from operator import MatrixTranspose
from operator import Add_of_matrix
from operator import Add_of_vector
from operator import Add_of_double
from operator import Multiply_of_vector
from operator import Multiply_of_matrix
from operator import Multiply_of_matrixHomo
from operator import Multiply_of_matrixrotation
from operator import Multiply_of_matrixtwist
from operator import Multiply_of_quaternion
from operator import Multiply_of_double
from operator import Substract_of_vector
from operator import Substract_of_matrix
from operator import Substract_of_double
from operator import Stack_of_vector
#from operator import WeightAdd_of_vector
#from operator import WeightDir
#from operator import Nullificator
from operator import Compose_R_and_T
#from operator import ComposeVector_RPY_T
from operator import Multiply_double_vector
from operator import Multiply_matrix_vector
from operator import Multiply_matrixHomo_vector
#from operator import Compose_RPY_and_T
#from operator import EndomorphismBasis
from operator import ConvolutionTemporal
from vector_constant import VectorConstant
'''
Tiny matrix functions, taken from Oscar source code.
'''
#--------------------------------------------------------------------------
#--------------------------------------------------------------------------
#--------------------------------------------------------------------------
from math import *
from numpy import *
from math import atan2
from random import random
from numpy import array, cos, matrix, pi, sin, sqrt
from numpy.linalg import inner, norm
# Convert matrix to tuple
def matrixToTuple(M):
tmp = M.tolist()
......@@ -18,96 +17,111 @@ def matrixToTuple(M):
res.append(tuple(i))
return tuple(res)
def vectorToTuple(M):
if len(M.shape)==1: return tuple(M.tolist())
elif M.shape[0]==1: return tuple(M.tolist()[0])
else: return tuple(M.transpose().tolist()[0])
if len(M.shape) == 1:
return tuple(M.tolist())
elif M.shape[0] == 1:
return tuple(M.tolist()[0])
else:
return tuple(M.transpose().tolist()[0])
# Convert from Roll, Pitch, Yaw to transformation Matrix
def rpy2tr(r,p,y):
mat = matrix(rotate('z',y))*matrix(rotate('y',p))*matrix(rotate('x',r))
def rpy2tr(r, p, y):
mat = matrix(rotate('z', y)) * matrix(rotate('y', p)) * matrix(rotate('x', r))
return matrixToTuple(mat)
# Get the distance btw the position components of 2 transf matrices
def distVector(M2,M1):
def distVector(M2, M1):
px = M1[0][3] - M2[0][3]
py = M1[1][3] - M2[1][3]
pz = M1[2][3] - M2[2][3]
return [px,py,pz]
return [px, py, pz]
# Obtain an orthonormal matrix SO3 using the given vector as its first column
# (it computes Gram Schmidt to obtain an orthonormal basis using the
# first vector and 2 other 'random' vectors)
def generateOrthonormalM(v1):
v2 = matrix([random(),random(),random()])
v3 = matrix([random(),random(),random()])
v2 = matrix([random(), random(), random()])
v3 = matrix([random(), random(), random()])
v1 = matrix(v1)
e1 = v1/linalg.norm(v1)
e1 = v1 / norm(v1)
u2 = v2-inner(v2,e1)*e1
e2 = u2/linalg.norm(u2)
u2 = v2 - inner(v2, e1) * e1
e2 = u2 / norm(u2)
u3 = v3-inner(v3,e1)*e1-inner(v3,e2)*e2
e3 = u3/linalg.norm(u3)
u3 = v3 - inner(v3, e1) * e1 - inner(v3, e2) * e2
e3 = u3 / norm(u3)
e1=e1.tolist(); e2=e2.tolist(); e3=e3.tolist()
M = ( (e1[0][0],e2[0][0],e3[0][0]), (e1[0][1],e2[0][1],e3[0][1]), (e1[0][2],e2[0][2],e3[0][2]) )
e1 = e1.tolist()
e2 = e2.tolist()
e3 = e3.tolist()
M = ((e1[0][0], e2[0][0], e3[0][0]), (e1[0][1], e2[0][1], e3[0][1]), (e1[0][2], e2[0][2], e3[0][2]))
return M
# Convert from Transformation Matrix to Roll,Pitch,Yaw
def tr2rpy(M):
m = sqrt(M[2][1]**2+M[2][2]**2)
p = atan2(-M[2][0],m)
m = sqrt(M[2][1]**2 + M[2][2]**2)
p = atan2(-M[2][0], m)
if abs( p-pi/2 ) < 0.001:
if abs(p - pi / 2) < 0.001:
r = 0
y = atan2(M[0][1],M[1][1])
elif abs( p+pi/2 ) < 0.001:
y = atan2(M[0][1], M[1][1])
elif abs(p + pi / 2) < 0.001:
r = 0
y = -atan2(M[0][1],M[1][1])
y = -atan2(M[0][1], M[1][1])
else:
r = atan2(M[1][0],M[0][0])
y = atan2(M[2][1],M[2][2])
r = atan2(M[1][0], M[0][0])
y = atan2(M[2][1], M[2][2])
return [float(r),float(p),float(y)]
return [float(r), float(p), float(y)]
def matrixToRPY( M ):
def matrixToRPY(M):
'''
Convert a 4x4 homogeneous matrix to a 6x1 rpy pose vector.
'''
rot = tr2rpy(M)
return [ M[0][3], M[1][3], M[2][3], rot[2],rot[1],rot[0]]
return [M[0][3], M[1][3], M[2][3], rot[2], rot[1], rot[0]]
def RPYToMatrix( pr ):
def RPYToMatrix(pr):
'''
Convert a 6x1 rpy pose vector to a 4x4 homogeneous matrix.
'''
M=array(rpy2tr(pr[3],pr[4],pr[5]))
M[0:3,3] = pr[0:3]
M = array(rpy2tr(pr[3], pr[4], pr[5]))
M[0:3, 3] = pr[0:3]
return M
# Transformation Matrix corresponding to a rotation about x,y or z
def rotate(axis,ang):
def rotate(axis, ang):
''' eg. T=rot('x',pi/4): rotate pi/4 rad about x axis
'''
ca=cos(ang); sa=sin(ang)
if axis=='x':
mat = ((1,0,0,0),(0,ca,-sa,0),(0,sa,ca,0),(0,0,0,1))
elif axis=='y':
mat = ((ca,0,sa,0),(0,1,0,0),(-sa,0,ca,0),(0,0,0,1))
elif axis=='z':
mat = ((ca,-sa,0,0),(sa,ca,0,0),(0,0,1,0),(0,0,0,1))
ca = cos(ang)
sa = sin(ang)
if axis == 'x':
mat = ((1, 0, 0, 0), (0, ca, -sa, 0), (0, sa, ca, 0), (0, 0, 0, 1))
elif axis == 'y':
mat = ((ca, 0, sa, 0), (0, 1, 0, 0), (-sa, 0, ca, 0), (0, 0, 0, 1))
elif axis == 'z':
mat = ((ca, -sa, 0, 0), (sa, ca, 0, 0), (0, 0, 1, 0), (0, 0, 0, 1))
else:
print 'Axis should be: x,y or z only'
print('Axis should be: x,y or z only')
return mat
def quaternionToMatrix(q):
[qx,qy,qz,qw]=q
R= [[1-2*qy**2-2*qz**2,2*qx*qy-2*qz*qw,2*qx*qz+2*qy*qw],
[2*qx*qy+2*qz*qw,1-2*qx**2-2*qz**2,2*qy*qz-2*qx*qw],
[2*qx*qz-2*qy*qw,2*qy*qz+2*qx*qw,1-2*qx**2-2*qy**2]]
[qx, qy, qz, qw] = q
R = [[1 - 2 * qy**2 - 2 * qz**2, 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw],
[2 * qx * qy + 2 * qz * qw, 1 - 2 * qx**2 - 2 * qz**2, 2 * qy * qz - 2 * qx * qw],
[2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * qx**2 - 2 * qy**2]]
return R
from dynamic_graph import plug
from dynamic_graph.sot.core import *
from dynamic_graph.sot.core import FeaturePoint6d, GainAdaptive, OpPointModifier, Task
def toFlags(arr):
"""
Convert an array of boolean to a /flag/ format, type 001010110, in little indian
(reverse order, first bool of the list will be the [01] of extrem right).
"""
l=max(arr)+1
lres=[0]*l
lres = [0] * (max(arr) + 1)
for i in arr:
lres[i]=1
lres[i] = 1
lres.reverse()
res=''
res = ''
for i in lres:
res+=str(i)
res += str(i)
return res
class MetaTask6d(object):
name=''
opPoint=''
dyn=0
task=0
feature=0
featureDes=0
def opPointExist(self,opPoint):
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
self.dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J'+opPoint,
self.dyn.signals())
return len(sigsP)==1 & len(sigsJ)==1
def defineDynEntities(self,dyn):