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