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')
# 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 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
if JqBase:
plug(JqBase, self._feature.signal('JqRef'))
if JqOther:
plug(JqOther, self._feature.signal('Jq'))
self._feature.setReference(self._reference.name)
self._feature.signal('selec').value = '111111'
self._feature.frame('current')
# Signals stored in members
self.basePosition = self._feature.signal('positionRef')
self.otherPosition = self._feature.signal('position')
self.baseReference = self._reference.signal('positionRef')
self.otherReference = self._reference.signal('position')
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.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
}
def createFeatures(self):
self.feature = FeaturePoint6dRelative('featureRel' + self.name)
self.featureDes = FeaturePoint6dRelative('featureRel' + self.name +
'_ref')
self.feature.selec.value = Flags('111111')
self.feature.frame('current')
jac[10][16] = 1.
jac[11][17] = 1.
jac = tuple(map(tuple, jac))
jacobianLegs.set(jac)
plug(jacobianLegs.signal('out'), featureLegs.signal('jacobianIN'))
vectorLegs = VectorConstant('vectorLegs')
vectorLegs.set(7 * (0., ))
plug(vectorLegs.signal('out'), featureLegs.signal('errorIN'))
# set featureLegs.errorIN [12](0,0,0,0,0,0,0,0,0,0,0,0)
taskLegs = Task('taskLegs')
taskLegs.add('featureLegs')
taskLegs.signal('controlGain').value = 1.
# --- TWOFEET
featureTwofeet = FeaturePoint6dRelative('featureTwofeet')
plug(dyn.signal('Jrleg'), featureTwofeet.signal('Jq'))
plug(dyn.signal('Jlleg'), featureTwofeet.signal('JqRef'))
plug(dyn.signal('rleg'), featureTwofeet.signal('position'))
plug(dyn.signal('lleg'), featureTwofeet.signal('positionRef'))
taskTwofeet = Task('taskTwofeet')
taskTwofeet.add('featureTwofeet')
taskTwofeet.signal('controlGain').value = 0.
# --- CONTACT CONSTRAINT
legs = Constraint('legs')
legs.addJacobian('dyn.Jlleg')
# --- SOT
sot = SOT('sot')