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 _makeRelativeTask(self, sotrobot):
name = self._name(self.gripper.name, "relative",
self.otherGripper.name)
alreadyInit = name + "_feature" in Entity.entities
self.feature = FeaturePose(name + "_feature")
if not alreadyInit:
# Create the operational points
_createOpPoint(sotrobot, self.gripper.link)
_createOpPoint(sotrobot, self.otherGripper.link)
plug(sotrobot.dynamic.signal(self.gripper.link), self.feature.oMja)
plug(sotrobot.dynamic.signal("J" + self.gripper.link),
self.feature.jaJja)
self.feature.jaMfa.value = se3ToTuple(self.gripper.lMf)
plug(sotrobot.dynamic.signal(self.otherGripper.link),
self.feature.oMjb)
plug(sotrobot.dynamic.signal("J" + self.otherGripper.link),
self.feature.jbJjb)
self.feature.jbMfb.value = se3ToTuple(self.otherGripper.lMf)
self._createTaskAndGain(name)
self.tasks = [
self.task,
]
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 _makeAbsoluteTask(self, sotrobot, gripper):
name = self._name(gripper.name)
alreadyInit = name + "_feature" in Entity.entities
self.feature = FeaturePose(name + "_feature")
if not alreadyInit:
# Create the operational points
_createOpPoint(sotrobot, gripper.link)
plug(sotrobot.dynamic.signal(gripper.link), self.feature.oMjb)
plug(sotrobot.dynamic.signal("J" + gripper.link),
self.feature.jbJjb)
self.feature.jbMfb.value = se3ToTuple(gripper.lMf)
self.feature.jaJja.value = np.zeros(
(6, sotrobot.dynamic.getDimension()))
self._createTaskAndGain(name)
self.tasks = [
self.task,
]
def set(oMj, jMf, jJj, g, h):
# Create the operational points
_createOpPoint (sotrobot, g.link)
jMf.value = se3ToTuple(g.lMf * h.lMf.inverse())
plug(sotrobot.dynamic.signal( g.link), oMj)
plug(sotrobot.dynamic.signal('J'+g.link), jJj)
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 ]