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 _referenceSignal (self, name, gripper, handle):
# oMjg^-1 -> HPP joint
self.oMjaDes_inv = matrixHomoInverse (name + "_oMjaDes_inv")
self.addHppJointTopic (gripper.fullLink, signalGetters = [ self.oMjaDes_inv.sin, ],)
# Plug it to FeaturePose
self.faMfbDes = matrixHomoProduct (name + "_faMfbDes",
gripper.lMf.inverse(), # jgMg^-1
self.oMjaDes_inv.sout, # oMjg^-1 -> HPP joint
None, # oMlh -> HPP joint
handle.lMf, # lhMh
)
# oMlh -> HPP joint
self.extendSignalGetters(handle.fullLink, self.faMfbDes.sin2)
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 ]