def _makeAbsolute(self, sotrobot, withMeasurementOfObjectPos, withMeasurementOfGripperPos, withDerivative):
name = self._name(self.gripper.name, self.handle.fullName)
assertEntityDoesNotExist(name+"_feature")
self.feature = FeaturePose (name + "_feature")
# Create the operational points
_createOpPoint (sotrobot, self.gripper.link)
self._plugRobotLink (sotrobot, self.gripper.link,
self.feature.oMja, self.feature.jaJja,
withMeasurementOfGripperPos)
self.feature.jaMfa.value = se3ToTuple(self.gripper.lMf)
self.addHppJointTopic (self.handle.fullLink)
self._plugObjectLink (sotrobot, self.handle.fullLink,
self.feature.oMjb, withMeasurementOfObjectPos)
self.feature.jbMfb.value = se3ToTuple(self.handle.lMf)
self.feature.jbJjb.value = np.zeros((6, sotrobot.dynamic.getDimension()))
# Compute desired pose between gripper and handle.
# Creates the entity faMfbDes
self._referenceSignal (name, self.gripper, self.handle)
plug(self.faMfbDes.sout, self.feature.faMfbDes)
# Create a task and gain
self._createTaskAndGain (name)
if withDerivative:
print("Relative pose constraint with derivative is not implemented yet.")
self.task.setWithDerivative (False)
self.tasks = [ self.task, ]
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 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 _makeAbsoluteBasedOnOther (self, sotrobot,
withMeasurementOfObjectPos, withMeasurementOfGripperPos,
withMeasurementOfOtherGripperPos, withDerivative):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = self._name(self.gripper.fullName, self.handle.fullName,
"based", self.otherGripper.name, self.otherHandle.fullName)
assertEntityDoesNotExist(name+"_feature")
self.feature = FeaturePose (name + "_feature")
# Create the operational point
_createOpPoint (sotrobot, self.otherGripper.link)
# Joint A is the gripper link
self.addHppJointTopic (self.gripper.fullLink)
self._plugObjectLink (sotrobot, self.gripper.fullLink,
self.feature.oMja, withMeasurementOfGripperPos)
# Frame A is the gripper frame
self.feature.jaMfa.value = se3ToTuple(self.gripper.lMf)
self.feature.jaJja.value = np.zeros((6, sotrobot.dynamic.getDimension()))
# Joint B is the other gripper link
self._plugRobotLink (sotrobot, self.otherGripper.link,
self.feature.oMjb, self.feature.jbJjb,
withMeasurementOfOtherGripperPos)
# Frame B is the handle frame
method = 1
if method == 0: # Does not work
# jbMfb = ogMh = ogMw * wMo * oMh
# wMog^-1
self.wMog_inv = matrixHomoInverse (name + "_wMog_inv")
self._plugRobotLink (sotrobot, self.otherGripper.link,
self.wMog_inv.sin, None,
withMeasurementOfOtherGripperPos)
self.jbMfb = matrixHomoProduct (name + "_jbMfb",
self.wMog_inv.sout, # wMog^-1 -> HPP joint
None, # wMo -> HPP joint
self.handle.lMf, # oMh
)
# wMo
self.addHppJointTopic (self.handle.fullLink)
self._plugObjectLink (self.handle.fullLink,
self.jbMfb.sin1, withMeasurementOfObjectPos)
# Plug it to FeaturePose
plug(self.jbMfb.sout, self.feature.jbMfb)
elif method == 1:
# jbMfb = ogMo * oMh
# Two options for ogMo measured:
if withMeasurementOfOtherGripperPos:
self.jbMfb = matrixHomoProduct (name + "_jbMfb",
None, # ogMo -> TF
self.handle.lMf, # oMh
)
plug(self.jbMfb.sout, self.feature.jbMfb)
# We use TF to get the position of the otherHandle wrt to the otherGripper
self._defaultValue, signals = \
self.makeTfListenerDefaultValue(name+"_defaultValue",
self.otherGripper.lMf * self.otherHandle.lMf.inverse(),
outputs = self.jbMfb.sin0)
self.addTfListenerTopic (
self.otherHandle.fullLink + self.meas_suffix + "_wrt_" + self.otherGripper.link + self.meas_suffix,
frame0 = self.otherGripper.link + self.meas_suffix,
frame1 = self.otherHandle.fullLink + self.meas_suffix,
signalGetters = [ signals, ],
)
else:
ogMo = matrixHomoProduct(name + "_jbMfb_meas",
matrixHomoInverse (self.otherGripper.link + "_inv", sotrobot.dynamic.signal(self.otherGripper.link)).sout,
sotrobot.dynamic.signal(sotrobot.camera_frame),
None, # Tf
self.handle.lMf,
check=True,)
if_ = entityIfMatrixHomo (name + "_jbMfb_cond",
condition=None,
value_then=ogMo.sout,
value_else=self.otherGripper.lMf * self.otherHandle.lMf.inverse() * self.handle.lMf,
check=True)
plug(if_.out, self.feature.jbMfb)
# We use TF to get the position of the otherHandle wrt to the camera
# and then we compute
# Who should we trust ?
# - other grasp: i.e. wMjb = wMog = wMc * cMo (measured) * oMog, jbMfb = ogMo (constant) * oMh
# - kinematics: i.e. wMjb = wMog(q), jbMfb = ogMw(q) * wMc(q) * cMo (measured) * oMh (needs good localisation)
# Below we trust kinematics
self.addTfListenerTopic (
self.otherHandle.fullLink + self.meas_suffix,
frame0 = sotrobot.camera_frame,
frame1 = self.handle.fullLink + self.meas_suffix,
signalGetters = [ (ogMo.sin2, if_.condition), ],
)
self.addHppJointTopic (self.handle.fullLink)
# Compute desired pose between gripper and handle.
# Creates the entity faMfbDes
self._referenceSignal (name, self.gripper, self.handle)
plug(self.faMfbDes.sout, self.feature.faMfbDes)
# Create a task and gain
self._createTaskAndGain (name)
if withDerivative:
print("Relative pose constraint with derivative is not implemented yet.")
self.task.setWithDerivative (False)
self.tasks = [ self.task ]
def _makeRelativeTask (self, sotrobot,
withMeasurementOfObjectPos, withMeasurementOfGripperPos,
withMeasurementOfOtherGripperPos,
withDerivative):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = self._name(self.gripper.name, self.handle.fullName,
"relative", self.otherGripper.name, self.otherHandle.fullName)
assertEntityDoesNotExist(name+"_feature")
self.feature = FeaturePose (name + "_feature")
# Create the operational points
_createOpPoint (sotrobot, self. gripper.link)
_createOpPoint (sotrobot, self.otherGripper.link)
# Joint A is the gripper link
self._plugRobotLink (sotrobot, self. gripper.link,
self.feature.oMja, self.feature.jaJja,
withMeasurementOfGripperPos)
# Frame A is the gripper frame
# self.feature.jaJja.value = np.zeros((6, sotrobot.dynamic.getDimension()))
self.feature.jaMfa.value = se3ToTuple(self.gripper.lMf)
# Joint B is the other gripper link
self._plugRobotLink (sotrobot, self.otherGripper.link,
self.feature.oMjb, self.feature.jbJjb,
withMeasurementOfOtherGripperPos)
# self.feature.jbJjb.value = np.zeros((6, sotrobot.dynamic.getDimension()))
# Frame B is the handle frame
# jbMfb = ogMh = ogMo(t) * oMh
method = 3
if method == 0: # Works
# jbMfb = ogMoh * ohMo * oMh
self.feature.jbMfb.value = se3ToTuple (
self.otherGripper.lMf
* self.otherHandle.lMf.inverse()
* self.handle.lMf)
self.addHppJointTopic (self.handle.fullLink)
elif method == 1: # Does not work
# Above, it is assumed that ogMoh = Id, which must be corrected.
# Instead, we compute
# jbMfb = ogMh = ogMo(t) * oMh
# = wMog(t)^-1 * wMo(t) * oMh
# wMog^-1
self.wMog_inv = matrixHomoInverse (name + "_wMog_inv")
self._plugRobotLink (sotrobot, self.otherGripper.link,
self.wMog_inv.sin, None,
withMeasurementOfOtherGripperPos)
self.jbMfb = matrixHomoProduct (name + "_jbMfb",
self.wMog_inv.sout, # wMog^-1 -> HPP joint
None, # wMo -> HPP joint
self.handle.lMf, # oMh
)
# wMo
self.addHppJointTopic (self.handle.fullLink)
self._plugObjectLink (sotrobot, self.handle.fullLink,
self.jbMfb.sin1, withMeasurementOfObjectPos)
# Plug it to FeaturePose
plug(self.jbMfb.sout, self.feature.jbMfb)
elif method == 2: # Seems to work
# jbMfb = ogMo * oMh
self.jbMfb = matrixHomoProduct (name + "_jbMfb",
None, # ogMo -> TF
self.handle.lMf, # oMh
)
plug(self.jbMfb.sout, self.feature.jbMfb)
# ogMo
self._defaultValue, signals = \
self.makeTfListenerDefaultValue(name+"_defaultValue",
self.otherGripper.lMf * self.otherHandle.lMf.inverse(),
outputs = self.jbMfb.sin0)
self.addTfListenerTopic (
self.otherHandle.fullLink + self.meas_suffix + "_wrt_" + self.otherGripper.link + self.meas_suffix,
frame0 = self.otherGripper.link + self.meas_suffix,
frame1 = self.otherHandle.fullLink + self.meas_suffix,
signalGetters = [ signals, ],
)
self.addHppJointTopic (self.handle.fullLink)
elif method == 3:
self.feature.jbMfb.value = se3ToTuple (self.otherGripper.lMf
* self.otherHandle.lMf.inverse() * self.handle.lMf)
self.addHppJointTopic (self.handle.fullLink)
elif method == 4:
self.jbMfb = matrixHomoProduct (name + "_jbMfb",
None, # ogMo -> TF
self.handle.lMf, # oMh
)
plug(self.jbMfb.sout, self.feature.jbMfb)
self.g2Mobj_measured = matrixHomoProduct (name + "_g2Mobj_measured",
matrixHomoInverse(self.otherGripper.link + "_inverse",
sotrobot.dynamic.signal\
(self.otherGripper.link)),
sotrobot.dynamic.signal(sotrobot.camera_frame),
None, # cMobj -> TF
)
# This creates an If.
# The condition signal is whether the transform is available in TF.
# If not available, use the default value, otherwise, use the one
# computed from TF.
self._defaultValue, signals = \
self.makeTfListenerDefaultValue(name+"_defaultValue",
self.otherGripper.lMf * self.otherHandle.lMf.inverse(), # default value for g2Mobj
outputs = self.jbMfb.sin0)
# g2Mobj_measured_sin will be read iif TF transform is available.
g2Mobj_measured_sin, condition_signal_in = signals
plug(self.g2Mobj_measured.sout, g2Mobj_measured_sin)
self.addTfListenerTopic (
self.otherHandle.fullLink + self.meas_suffix + "_wrt_" + sotrobot.camera_frame,
frame0 = self.otherHandle.fullLink + self.meas_suffix,
frame1 = sotrobot.camera_frame,
signalGetters = [ (self.g2Mobj_measured.sin2, condition_signal_in), ],
)
self.addHppJointTopic (self.handle.fullLink)
# Compute desired pose between gripper and handle.
# Creates the entity faMfbDes
self._referenceSignal (name, self.gripper, self.handle)
plug(self.faMfbDes.sout, self.feature.faMfbDes)
# Create a task and gain
self._createTaskAndGain (name)
if withDerivative:
print("Relative pose constraint with derivative is not implemented yet.")
self.task.setWithDerivative (False)
self.tasks = [ self.task ]
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
