def _plugObjectLink (self, sotrobot, linkName, outSignal, withMeasurement):
if withMeasurement:
linkNameMeas = linkName + self.meas_suffix
# Create default value
_createOpPoint (sotrobot, sotrobot.camera_frame)
oMl = matrixHomoProduct(linkNameMeas + "_wrt_world",
sotrobot.dynamic.signal(sotrobot.camera_frame),
None,
check=False,)
name = linkNameMeas + "wrt_world"
if_ = entityIfMatrixHomo (name, condition=None,
value_then=oMl.sout,
value_else=None,
check=False)
self.addTfListenerTopic (linkNameMeas,
frame0 = sotrobot.camera_frame,
frame1 = linkNameMeas,
signalGetters = [(oMl.sin1, if_.condition),],
)
self.addHppJointTopic (linkName, signalGetters = [ if_.else_, ],)
plug(if_.out, outSignal)
else:
print("Plug object link: no measument for " + linkName)
self.extendSignalGetters(linkName, outSignal)
def _plugRobotLink(self, sotrobot, linkName, poseSignal, Jsignal,
withMeasurement):
if withMeasurement:
_createOpPoint(sotrobot, sotrobot.camera_frame)
linkNameMeas = linkName + self.meas_suffix
oMl = matrixHomoProduct(linkNameMeas + "_wrt_world",
sotrobot.dynamic.signal(
sotrobot.camera_frame),
None,
check=False)
if_ = entityIfMatrixHomo(
linkNameMeas + "_wrt_world_safe",
condition=None,
value_then=oMl.sout,
value_else=sotrobot.dynamic.signal(linkName),
check=False)
self.addTfListenerTopic(
linkNameMeas,
frame0=sotrobot.camera_frame,
frame1=linkNameMeas,
signalGetters=[
(oMl.sin(1), if_.condition),
],
)
plug(if_.out, poseSignal)
else:
plug(sotrobot.dynamic.signal(linkName), poseSignal)
print("Plug robot link: no measument for " + linkName)
if Jsignal is not None:
plug(sotrobot.dynamic.signal("J" + linkName), Jsignal)
def test_if_entity(self):
name = "test_if_entity"
self.assertFalse(entityExists(name))
import pinocchio
Id = pinocchio.SE3.Identity()
M0 = pinocchio.SE3.Random()
if_ = entityIfMatrixHomo(name,
condition = None,
value_then = Id,
value_else = M0)
if_.condition.value = 1
if_.out.recompute(0)
self.assertEqual(if_.out.value, se3ToTuple(Id))
if_.condition.value = 0
if_.out.recompute(1)
np.testing.assert_almost_equal(np.array(if_.out.value), M0.homogeneous)
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 ]
