def matrixHomoProduct(name, *args, **kwargs):
from dynamic_graph.sot.core.operator import Multiply_of_matrixHomo
if kwargs.get('check', True): assertEntityDoesNotExist(name)
ent = Multiply_of_matrixHomo(name)
ent.setSignalNumber(len(args))
for i, valueOrSignal in enumerate(args):
if valueOrSignal is None: continue
plugMatrixHomo(valueOrSignal, ent.signal('sin' + str(i)))
return ent
def test_matrix_homo(self):
name_a = "test_homo_entity_a"
name_b = "test_homo_entity_b"
self.assertFalse(entityExists(name_a))
self.assertFalse(entityExists(name_b))
import pinocchio
Id = pinocchio.SE3.Identity()
a = matrixHomoProduct(name_a, None, Id)
b = matrixHomoInverse(name_b, a.sout)
plugMatrixHomo(b.sout, a.sin0)
def makeTfListenerDefaultValue(self, name, value, outputs=None):
from dynamic_graph.sot.core.switch import SwitchMatrixHomogeneous as Switch
from agimus_sot.tools import plugMatrixHomo, assertEntityDoesNotExist
from dynamic_graph.signal_base import SignalBase
from dynamic_graph import plug
assertEntityDoesNotExist(name)
switch = Switch(name)
switch.setSignalNumber(2)
plugMatrixHomo(value, switch.sin(0))
if outputs is not None:
if isinstance(outputs, SignalBase):
plug(switch.sout, outputs)
elif isinstance(outputs, (list, tuple)):
for output in outputs:
plug(switch.sout, output)
return switch, (switch.sin(1), switch.boolSelection)
def entityIfMatrixHomo(name, condition, value_then, value_else, check=True):
"""
- name: the If entity name,
- condition: None, a boolean constant or a boolean signal.
- value_then, value_else: None, a constant MatrixHomo or a MatrixHomo signal.
"""
from dynamic_graph.sot.core.switch import SwitchMatrixHomogeneous as Switch
from agimus_sot.tools import plugMatrixHomo, assertEntityDoesNotExist
from dynamic_graph.signal_base import SignalBase
from dynamic_graph import plug
if check: assertEntityDoesNotExist(name)
switch = Switch(name)
switch.setSignalNumber(2)
if_ = IfEntity(switch)
if value_then is not None: plugMatrixHomo(value_then, if_.then_)
if value_else is not None: plugMatrixHomo(value_else, if_.else_)
if condition is not None:
if isinstance(condition, bool):
if_.condition.value = condition
else:
plug(condition, if_.condition)
return if_
def matrixHomoInverse(name, valueOrSignal=None, check=True):
from dynamic_graph.sot.core.operator import Inverse_of_matrixHomo
if check: assertEntityDoesNotExist(name)
ent = Inverse_of_matrixHomo(name)
plugMatrixHomo(valueOrSignal, ent.sin)
return ent