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 ]