def initialize(self):
m = np.matrix([
[-0.8530, 0.5208, -0.0360, 0.0049],
[-0.5206, -0.8537, -0.0146, -0.0078],
[-0.0384, 0.0063, 0.9993, 0.2158], [0, 0, 0, 1.0000]
]) #transformation from the chest-markers frame to the chest frame
self.chestMarkersSot = OpPointModifier(
'chestMarkersSot'
) # gets the position of the chest-markers frame in sot frame
self.chestMarkersSot.setEndEffector(False)
self.chestMarkersSot.setTransformation(matrixToTuple(m))
plug(self.robot.dynamic.chest, self.chestMarkersSot.positionIN)
self.chestMarkersSot.position.recompute(1)
self.mchestsot = np.matrix(self.chestMarkersSot.position.value)
self.mchestmocap = np.matrix(self.ros.signal('chest').value)
self.mtransformMocap2ISot = np.linalg.inv(
self.mchestsot
) * self.mchestmocap #transforms the mocap frame to the initial-sot frame
self.chestMarkerMocapInISot = Multiply_of_matrixHomo(
"chestMarkerMocap"
) #gets position of chest-markers frame in sot-initial frame
self.chestMarkerMocapInISot.sin1.value = matrixToTuple(
self.mtransformMocap2ISot)
plug(self.mocapSignal, self.chestMarkerMocapInISot.sin2)
self.chestMarkersSotInverse = Inverse_of_matrixHomo(
"chestMarkersSotInverse"
) #inverse of the homomatrix for position of markers in local sot
plug(self.chestMarkersSot.position, self.chestMarkersSotInverse.sin)
self.robotPositionISot = Multiply_of_matrixHomo("robotPositionInSot")
plug(self.chestMarkerMocapInISot.sout, self.robotPositionISot.sin1)
plug(self.chestMarkersSotInverse.sout, self.robotPositionISot.sin2)
self.robotPositionInMocap = Multiply_of_matrixHomo(
"robotPositionInMocap")
plug(self.mocapSignal, self.robotPositionInMocap.sin1)
plug(self.chestMarkersSotInverse.sout, self.robotPositionInMocap.sin2)
# Initial position of the ankles.
rightAnklePosition = ((1, 0, 0, 0.010260575628), (0, 1, 0, -0.096000000000),
(0, 0, 1, 0.066999500724), (0, 0, 0, 1.000000000000))
leftAnklePosition = ((1, 0, 0, 0.010260575628), (0, 1, 0, 0.096000000000),
(0, 0, 1, 0.066999500724), (0, 0, 0, 1.000000000000))
# Define the distance to the reference frame
expg.positionRef.value = 0
# Remove the task corresponding to the right ankle.
solver.sot.remove('romeo_task_right-ankle')
# add the task corresponding to the expression graph.
# note that the contacts have already been added (to have having a flying robot).
solver.push(taskExpg)
from dynamic_graph.sot.core import Multiply_of_matrixHomo
# Express the desired position in the chosen frame.
invRefFrame = Inverse_of_matrixHomo("invRefFrame")
plug(robot.leftAnkle.position, invRefFrame.sin)
multHomo = Multiply_of_matrixHomo("multHomo")
plug(invRefFrame.sout, multHomo.sin1)
multHomo.sin2.value = rightAnklePosition
plug(multHomo.sout, expg.position_obj)
expg.setChain("b2l", "r_ankle", "l_ankle")
def __init__(self, robot, name='flextimatorEncoders'):
DGIMUModelBaseFlexEstimation.__init__(self, name)
self.setSamplingPeriod(0.005)
self.robot = robot
# Covariances
self.setProcessNoiseCovariance(
matrixToTuple(
np.diag((1e-8, ) * 12 + (1e-4, ) * 3 + (1e-4, ) * 3 +
(1e-4, ) * 3 + (1e-4, ) * 3 + (1.e-2, ) * 6 +
(1e-15, ) * 2 + (1.e-8, ) * 3)))
self.setMeasurementNoiseCovariance(
matrixToTuple(np.diag((1e-3, ) * 3 + (1e-6, ) * 3)))
self.setUnmodeledForceVariance(1e-13)
self.setForceVariance(1e-4)
self.setAbsolutePosVariance(1e-4)
# Contact model definition
self.setContactModel(1)
self.setKfe(matrixToTuple(np.diag((40000, 40000, 40000))))
self.setKfv(matrixToTuple(np.diag((600, 600, 600))))
self.setKte(matrixToTuple(np.diag((600, 600, 600))))
self.setKtv(matrixToTuple(np.diag((60, 60, 60))))
#Estimator interface
self.interface = EstimatorInterface(name + "EstimatorInterface")
self.interface.setLeftHandSensorTransformation((0., 0., 1.57))
self.interface.setRightHandSensorTransformation((0., 0., 1.57))
self.interface.setFDInertiaDot(True)
# State and measurement definition
self.interface.setWithUnmodeledMeasurements(False)
self.interface.setWithModeledForces(True)
self.interface.setWithAbsolutePose(False)
self.setWithComBias(False)
# Contacts forces
plug(self.robot.device.forceLLEG, self.interface.force_lf)
plug(self.robot.device.forceRLEG, self.interface.force_rf)
plug(self.robot.device.forceLARM, self.interface.force_lh)
plug(self.robot.device.forceRARM, self.interface.force_rh)
# Selecting robotState
self.robot.device.robotState.value = 46 * (0., )
self.robotState = Selec_of_vector('robotState')
plug(self.robot.device.robotState, self.robotState.sin)
self.robotState.selec(0, 36)
# Reconstruction of the position of the free flyer from encoders
# Create dynamic with the free flyer at the origin of the control frame
self.robot.dynamicOdo = self.createDynamic(self.robotState.sout,
'_dynamicOdo')
self.robot.dynamicOdo.inertia.recompute(1)
self.robot.dynamicOdo.waist.recompute(1)
# Reconstruction of the position of the contacts in dynamicOdo
self.leftFootPosOdo = Multiply_of_matrixHomo(name + "leftFootPosOdo")
plug(self.robot.dynamicOdo.signal('left-ankle'),
self.leftFootPosOdo.sin1)
self.leftFootPosOdo.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPosOdo = Multiply_of_matrixHomo(name + "rightFootPosOdo")
plug(self.robot.dynamicOdo.signal('right-ankle'),
self.rightFootPosOdo.sin1)
self.rightFootPosOdo.sin2.value = self.robot.forceSensorInRightAnkle
# Odometry
self.odometry = Odometry(name + 'odometry')
plug(self.robot.frames['leftFootForceSensor'].position,
self.odometry.leftFootPositionRef)
plug(self.robot.frames['rightFootForceSensor'].position,
self.odometry.rightFootPositionRef)
plug(self.rightFootPosOdo.sout, self.odometry.rightFootPositionIn)
plug(self.leftFootPosOdo.sout, self.odometry.leftFootPositionIn)
plug(self.robot.device.forceLLEG, self.odometry.force_lf)
plug(self.robot.device.forceRLEG, self.odometry.force_rf)
self.odometry.setLeftFootPosition(
self.robot.frames['leftFootForceSensor'].position.value)
self.odometry.setRightFootPosition(
self.robot.frames['rightFootForceSensor'].position.value)
plug(self.interface.stackOfSupportContacts,
self.odometry.stackOfSupportContacts)
# Create dynamicEncoders
self.robotStateWoFF = Selec_of_vector('robotStateWoFF')
plug(self.robot.device.robotState, self.robotStateWoFF.sin)
self.robotStateWoFF.selec(6, 36)
self.stateEncoders = Stack_of_vector(name + 'stateEncoders')
plug(self.odometry.freeFlyer, self.stateEncoders.sin1)
plug(self.robotStateWoFF.sout, self.stateEncoders.sin2)
self.stateEncoders.selec1(0, 6)
self.stateEncoders.selec2(0, 30)
self.robot.dynamicEncoders = self.createDynamic(
self.stateEncoders.sout, '_dynamicEncoders')
# self.robot.dynamicEncoders=self.createDynamic(self.robotState.sout,'_dynamicEncoders')
# plug(self.odometry.freeFlyer,self.robot.dynamicEncoders.ffposition)
# self.robot.dynamicEncoders=self.createDynamic(self.robot.device.state,'_dynamicEncoders')
# Reconstruction of the position of the contacts in dynamicEncoders
self.leftFootPos = Multiply_of_matrixHomo("leftFootPos")
plug(self.robot.dynamicEncoders.signal('left-ankle'),
self.leftFootPos.sin1)
self.leftFootPos.sin2.value = self.robot.forceSensorInLeftAnkle
self.rightFootPos = Multiply_of_matrixHomo("rightFootPos")
plug(self.robot.dynamicEncoders.signal('right-ankle'),
self.rightFootPos.sin1)
self.rightFootPos.sin2.value = self.robot.forceSensorInRightAnkle
# Contacts velocities
self.leftFootVelocity = Multiply_matrix_vector('leftFootVelocity')
plug(self.robot.frames['leftFootForceSensor'].jacobian,
self.leftFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.leftFootVelocity.sin2)
self.rightFootVelocity = Multiply_matrix_vector('rightFootVelocity')
plug(self.robot.frames['rightFootForceSensor'].jacobian,
self.rightFootVelocity.sin1)
plug(self.robot.dynamicEncoders.velocity, self.rightFootVelocity.sin2)
# Contacts positions and velocities
plug(self.leftFootPos.sout, self.interface.position_lf)
plug(self.rightFootPos.sout, self.interface.position_rf)
plug(self.leftFootVelocity.sout, self.interface.velocity_lf)
plug(self.rightFootVelocity.sout, self.interface.velocity_rf)
plug(self.robot.dynamicEncoders.signal('right-wrist'),
self.interface.position_lh)
plug(self.robot.dynamicEncoders.signal('left-wrist'),
self.interface.position_rh)
# Compute contacts number
plug(self.interface.supportContactsNbr, self.contactNbr)
# Contacts model and config
plug(self.interface.contactsModel, self.contactsModel)
self.setWithConfigSignal(True)
plug(self.interface.config, self.config)
# Drift
self.drift = DriftFromMocap(name + 'Drift')
# Compute measurement vector
plug(self.robot.device.accelerometer, self.interface.accelerometer)
plug(self.robot.device.gyrometer, self.interface.gyrometer)
plug(self.drift.driftVector, self.interface.drift)
plug(self.interface.measurement, self.measurement)
# Input reconstruction
# IMU Vector
# Creating an operational point for the IMU
self.robot.dynamicEncoders.createJacobian(name + 'ChestJ_OpPoint',
'chest')
self.imuOpPoint = OpPointModifier(name + 'IMU_oppoint')
self.imuOpPoint.setTransformation(
matrixToTuple(
np.linalg.inv(np.matrix(
self.robot.dynamicEncoders.chest.value)) *
np.matrix(self.robot.frames['accelerometer'].position.value)))
self.imuOpPoint.setEndEffector(False)
plug(self.robot.dynamicEncoders.chest, self.imuOpPoint.positionIN)
plug(self.robot.dynamicEncoders.signal(name + 'ChestJ_OpPoint'),
self.imuOpPoint.jacobianIN)
# IMU position
self.PosAccelerometer = Multiply_of_matrixHomo(name +
"PosAccelerometer")
plug(self.robot.dynamicEncoders.chest, self.PosAccelerometer.sin1)
self.PosAccelerometer.sin2.value = matrixToTuple(
self.robot.accelerometerPosition)
self.inputPos = MatrixHomoToPoseUTheta(name + 'InputPosition')
plug(self.PosAccelerometer.sout, self.inputPos.sin)
# IMU velocity
self.inputVel = Multiply_matrix_vector(name + 'InputVelocity')
plug(self.imuOpPoint.jacobian, self.inputVel.sin1)
plug(self.robot.dynamicEncoders.velocity, self.inputVel.sin2)
# Concatenate
self.inputPosVel = Stack_of_vector(name + 'InputPosVel')
plug(self.inputPos.sout, self.inputPosVel.sin1)
plug(self.inputVel.sout, self.inputPosVel.sin2)
self.inputPosVel.selec1(0, 6)
self.inputPosVel.selec2(0, 6)
# IMU Vector
self.IMUVector = PositionStateReconstructor(name + 'EstimatorInput')
plug(self.inputPosVel.sout, self.IMUVector.sin)
self.IMUVector.inputFormat.value = '001111'
self.IMUVector.outputFormat.value = '011111'
self.IMUVector.setFiniteDifferencesInterval(2)
# CoM and derivatives
self.comIn = self.robot.dynamicEncoders.com
self.comVector = PositionStateReconstructor(name + 'ComVector')
plug(self.comIn, self.comVector.sin)
self.comVector.inputFormat.value = '000001'
self.comVector.outputFormat.value = '010101'
self.comVector.setFiniteDifferencesInterval(20)
# Compute derivative of Angular Momentum
self.angMomDerivator = Derivator_of_Vector(name + 'angMomDerivator')
plug(self.robot.dynamicEncoders.angularmomentum,
self.angMomDerivator.sin)
self.angMomDerivator.dt.value = self.robot.timeStep
# Concatenate with interace estimator
plug(self.comVector.sout, self.interface.comVector)
plug(self.robot.dynamicEncoders.inertia, self.interface.inertia)
plug(self.robot.dynamicEncoders.angularmomentum,
self.interface.angMomentum)
plug(self.angMomDerivator.sout, self.interface.dangMomentum)
self.interface.dinertia.value = (0, 0, 0, 0, 0, 0)
plug(self.robot.dynamicEncoders.waist, self.interface.positionWaist)
plug(self.IMUVector.sout, self.interface.imuVector)
plug(self.interface.input, self.input)
self.robot.flextimator = self
def _makeAbsoluteBasedOnOther(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = PreGrasp.sep.join([
"", "pregrasp", self.gripper.fullName, self.handle.fullName,
"based", self.otherGripper.name, self.otherHandle.fullName
])
self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic,
self.otherGripper.joint,
self.otherGripper.joint)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
# Current gripper position
self.graspTask.opmodif = se3ToTuple(self.otherGripper.pose)
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Desired gripper position:
# Displacement gripper1: M = G1_p^-1 * G1_r
# The derised position of handle1 is
# H1*_r = H1_p * M = H1_p * G1_p^-1 * G1_r
#
# Moreover, the relative position of handle2 wrt to gripper2 may not be perfect so
# h2_r = O_r^-1 * G2_r
#
# G2*_r = O*_r * h2_r = H1_p * G1_p^-1 * G1_r * h1^-1 * h2_r
# O_p * h1 * G1_p^-1 * G1_r * h1^-1 * h2_r
# O_p * h1 * g1^-1 * J1_p^-1 * J1_r * g1 * h1^-1 * h2_r
# where h2_r can be a constant value of the expression above.
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired")
if withMeasurementOfObjectPos:
# TODO Integrate measurement of h1_r and h2_r: position error
# O_r^-1 * G1_r and O_r^-1 * G2_r
# (for the release phase and computed only at time 0)
self.gripper_desired_pose.setSignalNumber(5)
## self.gripper_desired_pose.setSignalNumber (7)
# self.gripper_desired_pose.sin0 -> plug to object1 planning pose
# self.gripper_desired_pose.sin1.value = se3ToTuple(self.handle.pose)
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.handle.pose * self.gripper.pose.inverse())
self._invert_planning_pose = Inverse_of_matrixHomo(
self.gripper.fullLink + "_invert_planning_pose")
# self._invert_planning_pose.sin -> plug to link1 planning pose
plug(self._invert_planning_pose.sout,
self.gripper_desired_pose.sin2)
# self.gripper_desired_pose.sin3 -> plug to link1 real pose
# self.gripper_desired_pose.sin4.value = se3ToTuple (self.handle.pose.inverse() * self.otherHandle.pose)
self.gripper_desired_pose.sin4.value = se3ToTuple(
self.gripper.pose * self.handle.pose.inverse() *
self.otherHandle.pose)
## self.gripper_desired_pose.sin4.value = se3ToTuple (self.gripper.pose * self.handle.pose.inverse())
## self.gripper_desired_pose.sin5 -> plug to object real pose
## if self.otherGripper.joint not in sotrobot.dyn.signals():
## sotrobot.dyn.createOpPoint (self.otherGripper.joint, self.otherGripper.joint)
## plug(sotrobot.dyn.signal(self.otherGripper.joint), self.gripper_desired_pose.sin6)
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.topics = {
self.handle.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.handle.fullLink,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
self.gripper.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullLink,
"signalGetters": [
lambda: self._invert_planning_pose.sin,
]
},
"measurement_" + self.gripper.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": "world",
"frame1": self.gripper.fullLink,
"signalGetters": [
lambda: self.gripper_desired_pose.sin3,
]
},
## "measurement_" + self.otherHandle.fullLink: {
## "velocity": False,
## "type": "matrixHomo",
## "handler": "tf_listener",
## "frame0": "world",
## "frame1": self.otherHandle.fullLink,
## "signalGetters": [ self.gripper_desired_pose.sin5 ] },
}
else:
# G2*_r = H1_p * h1^-1 * h2 = G2_p = J2_p * G
self.gripper_desired_pose.setSignalNumber(2)
# self.gripper_desired_pose.sin0 -> plug to joint planning pose
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.otherGripper.pose)
self.topics = {
self.otherGripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.otherGripper.fullJoint,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
}
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.tasks = [self.graspTask.task]
def _makeRelativeTask(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = PreGrasp.sep.join([
"", "pregrasp", self.gripper.name, self.handle.fullName, "based",
self.otherGripper.name, self.handle.fullName
])
self.graspTask = MetaTaskKine6dRel(name, sotrobot.dynamic,
self.gripper.joint,
self.otherGripper.joint,
self.gripper.joint,
self.otherGripper.joint)
self.graspTask.opmodif = se3ToTuple(self.gripper.pose *
self.handle.pose.inverse())
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
self.graspTask.opmodifBase = se3ToTuple(
self.otherGripper.pose * self.otherHandle.pose.inverse())
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired1")
self.otherGripper_desired_pose = Multiply_of_matrixHomo(name +
"_desired2")
if withMeasurementOfObjectPos:
# TODO Integrate measurement of h1_r and h2_r: position error
# O_r^-1 * G1_r and O_r^-1 * G2_r
# (for the release phase and computed only at time 0)
print("Relative grasp with measurement is NOT IMPLEMENTED")
# else:
# G2*_r = H1_p * h1^-1 * h2 = G2_p = J2_p * G
self.gripper_desired_pose.setSignalNumber(2)
# self.gripper_desired_pose.sin0 -> plug to joint planning pose
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.gripper.pose * self.handle.pose.inverse())
self.otherGripper_desired_pose.setSignalNumber(2)
# self.otherGripper_desired_pose.sin0 -> plug to otherJoint planning pose
self.otherGripper_desired_pose.sin1.value = se3ToTuple(
self.otherGripper.pose * self.otherHandle.pose.inverse())
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
plug(self.otherGripper_desired_pose.sout,
self.graspTask.featureDes.positionRef)
self.topics = {
self.gripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullJoint,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
self.otherGripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.otherGripper.fullJoint,
"signalGetters": [
lambda: self.otherGripper_desired_pose.sin0,
]
},
}
self.tasks = [self.graspTask.task]
def _makeAbsolute(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
name = PreGrasp.sep.join(
["", "pregrasp", self.gripper.name, self.handle.fullName])
self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic,
self.gripper.joint, self.gripper.joint)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
# Current gripper position
self.graspTask.opmodif = se3ToTuple(self.gripper.pose)
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Desired gripper position:
# Planned handle pose H_p = object_planned_pose * self.handle.pose
# Real handle pose H_r = object_real_pose * self.handle.pose
# Displacement M = H_p^-1 * H_r
# planned gripper pose G_p= joint_planned_pose * self.gripper.pose
# The derised position is
# G*_r = G_p * M = G_p * h^-1 * O_p^-1 * O_r * h
# = J_p * g * h^-1 * O_p^-1 * O_r * h
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired")
nsignals = 2
if withMeasurementOfGripperPos: nsignals += 1
if withMeasurementOfObjectPos: nsignals += 5
self.gripper_desired_pose.setSignalNumber(nsignals)
isignal = 0
def sig(i):
return self.gripper_desired_pose.signal("sin" + str(i))
# Object calibration
if withMeasurementOfObjectPos:
h = self.handle.pose
# TODO Integrate measurement of h_r: position error of O_r^-1 * G_r
# (for the release phase and computed only at time 0)
# self.gripper_desired_pose.sin0 -> plug to handle link planning pose
self.topics[self.handle.fullLink] = {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.handle.fullLink,
"signalGetters": [lambda: self.gripper_desired_pose.sin0],
}
isignal += 1
sig(isignal).value = se3ToTuple(h.inverse())
isignal += 1
delta_h = sig(isignal)
self.topics["delta_" + self.handle.fullLink] = {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": self.handle.fullLink,
"frame1": self.handle.fullLink + "_estimated",
"signalGetters": [lambda: delta_h],
}
isignal += 1
sig(isignal).value = se3ToTuple(h)
isignal += 1
self._oMh_p_inv = Inverse_of_matrixHomo(self.handle.fullLink +
"_invert_planning_pose")
self.topics[self.handle.fullLink]["signalGetters"] \
.append (lambda: self._oMh_p_inv.sin)
plug(self._oMh_p_inv.sout, sig(isignal))
isignal += 1
# Joint planning pose
joint_planning_pose = sig(isignal)
self.topics[self.gripper.fullJoint] = {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullJoint,
"signalGetters": [lambda: joint_planning_pose],
}
isignal += 1
# Joint calibration
if withMeasurementOfGripperPos:
self._delta_g_inv = Inverse_of_matrixHomo(self.gripper.fullJoint +
"_delta_inv")
self.topics["delta_" + self.gripper.fullJoint] = {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": self.gripper.fullJoint,
"frame1": self.gripper.fullJoint + "_estimated",
"signalGetters": [lambda: self._delta_g_inv.sin],
}
plug(self._delta_g_inv.sout, self.gripper_desired_pose.sin6)
isignal += 1
sig(isignal).value = se3ToTuple(self.gripper.pose)
isignal += 1
assert isignal == nsignals
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.tasks = [self.graspTask.task]
class PreGrasp(Manifold):
def __init__(self, gripper, handle, otherGraspOnObject=None):
super(PreGrasp, self).__init__()
self.gripper = gripper
self.handle = handle
if otherGraspOnObject is not None:
self.otherGripper = otherGraspOnObject[0]
self.otherHandle = otherGraspOnObject[1]
else:
self.otherGripper = None
self.otherHandle = None
def makeTasks(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
if self.gripper.enabled:
if self.otherGripper is not None and self.otherGripper.enabled:
self._makeRelativeTask(sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos)
else:
self._makeAbsolute(sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos)
else:
if self.otherGripper is not None and self.otherGripper.enabled:
self._makeAbsoluteBasedOnOther(sotrobot,
withMeasurementOfObjectPos,
withMeasurementOfGripperPos)
else:
# TODO Both grippers are disabled so nothing can be done...
# add a warning ?
pass
def _makeAbsolute(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
name = PreGrasp.sep.join(
["", "pregrasp", self.gripper.name, self.handle.fullName])
self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic,
self.gripper.joint, self.gripper.joint)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
# Current gripper position
self.graspTask.opmodif = se3ToTuple(self.gripper.pose)
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Desired gripper position:
# Planned handle pose H_p = object_planned_pose * self.handle.pose
# Real handle pose H_r = object_real_pose * self.handle.pose
# Displacement M = H_p^-1 * H_r
# planned gripper pose G_p= joint_planned_pose * self.gripper.pose
# The derised position is
# G*_r = G_p * M = G_p * h^-1 * O_p^-1 * O_r * h
# = J_p * g * h^-1 * O_p^-1 * O_r * h
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired")
nsignals = 2
if withMeasurementOfGripperPos: nsignals += 1
if withMeasurementOfObjectPos: nsignals += 5
self.gripper_desired_pose.setSignalNumber(nsignals)
isignal = 0
def sig(i):
return self.gripper_desired_pose.signal("sin" + str(i))
# Object calibration
if withMeasurementOfObjectPos:
h = self.handle.pose
# TODO Integrate measurement of h_r: position error of O_r^-1 * G_r
# (for the release phase and computed only at time 0)
# self.gripper_desired_pose.sin0 -> plug to handle link planning pose
self.topics[self.handle.fullLink] = {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.handle.fullLink,
"signalGetters": [lambda: self.gripper_desired_pose.sin0],
}
isignal += 1
sig(isignal).value = se3ToTuple(h.inverse())
isignal += 1
delta_h = sig(isignal)
self.topics["delta_" + self.handle.fullLink] = {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": self.handle.fullLink,
"frame1": self.handle.fullLink + "_estimated",
"signalGetters": [lambda: delta_h],
}
isignal += 1
sig(isignal).value = se3ToTuple(h)
isignal += 1
self._oMh_p_inv = Inverse_of_matrixHomo(self.handle.fullLink +
"_invert_planning_pose")
self.topics[self.handle.fullLink]["signalGetters"] \
.append (lambda: self._oMh_p_inv.sin)
plug(self._oMh_p_inv.sout, sig(isignal))
isignal += 1
# Joint planning pose
joint_planning_pose = sig(isignal)
self.topics[self.gripper.fullJoint] = {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullJoint,
"signalGetters": [lambda: joint_planning_pose],
}
isignal += 1
# Joint calibration
if withMeasurementOfGripperPos:
self._delta_g_inv = Inverse_of_matrixHomo(self.gripper.fullJoint +
"_delta_inv")
self.topics["delta_" + self.gripper.fullJoint] = {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": self.gripper.fullJoint,
"frame1": self.gripper.fullJoint + "_estimated",
"signalGetters": [lambda: self._delta_g_inv.sin],
}
plug(self._delta_g_inv.sout, self.gripper_desired_pose.sin6)
isignal += 1
sig(isignal).value = se3ToTuple(self.gripper.pose)
isignal += 1
assert isignal == nsignals
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.tasks = [self.graspTask.task]
# TODO Add velocity
def _makeRelativeTask(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = PreGrasp.sep.join([
"", "pregrasp", self.gripper.name, self.handle.fullName, "based",
self.otherGripper.name, self.handle.fullName
])
self.graspTask = MetaTaskKine6dRel(name, sotrobot.dynamic,
self.gripper.joint,
self.otherGripper.joint,
self.gripper.joint,
self.otherGripper.joint)
self.graspTask.opmodif = se3ToTuple(self.gripper.pose *
self.handle.pose.inverse())
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
self.graspTask.opmodifBase = se3ToTuple(
self.otherGripper.pose * self.otherHandle.pose.inverse())
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired1")
self.otherGripper_desired_pose = Multiply_of_matrixHomo(name +
"_desired2")
if withMeasurementOfObjectPos:
# TODO Integrate measurement of h1_r and h2_r: position error
# O_r^-1 * G1_r and O_r^-1 * G2_r
# (for the release phase and computed only at time 0)
print("Relative grasp with measurement is NOT IMPLEMENTED")
# else:
# G2*_r = H1_p * h1^-1 * h2 = G2_p = J2_p * G
self.gripper_desired_pose.setSignalNumber(2)
# self.gripper_desired_pose.sin0 -> plug to joint planning pose
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.gripper.pose * self.handle.pose.inverse())
self.otherGripper_desired_pose.setSignalNumber(2)
# self.otherGripper_desired_pose.sin0 -> plug to otherJoint planning pose
self.otherGripper_desired_pose.sin1.value = se3ToTuple(
self.otherGripper.pose * self.otherHandle.pose.inverse())
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
plug(self.otherGripper_desired_pose.sout,
self.graspTask.featureDes.positionRef)
self.topics = {
self.gripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullJoint,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
self.otherGripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.otherGripper.fullJoint,
"signalGetters": [
lambda: self.otherGripper_desired_pose.sin0,
]
},
}
self.tasks = [self.graspTask.task]
# TODO Add velocity
def _makeAbsoluteBasedOnOther(self, sotrobot, withMeasurementOfObjectPos,
withMeasurementOfGripperPos):
assert self.handle.robotName == self.otherHandle.robotName
assert self.handle.link == self.otherHandle.link
name = PreGrasp.sep.join([
"", "pregrasp", self.gripper.fullName, self.handle.fullName,
"based", self.otherGripper.name, self.otherHandle.fullName
])
self.graspTask = MetaTaskKine6d(name, sotrobot.dynamic,
self.otherGripper.joint,
self.otherGripper.joint)
setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
self.graspTask.task.setWithDerivative(False)
# Current gripper position
self.graspTask.opmodif = se3ToTuple(self.otherGripper.pose)
# Express the velocities in local frame. This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
# Desired gripper position:
# Displacement gripper1: M = G1_p^-1 * G1_r
# The derised position of handle1 is
# H1*_r = H1_p * M = H1_p * G1_p^-1 * G1_r
#
# Moreover, the relative position of handle2 wrt to gripper2 may not be perfect so
# h2_r = O_r^-1 * G2_r
#
# G2*_r = O*_r * h2_r = H1_p * G1_p^-1 * G1_r * h1^-1 * h2_r
# O_p * h1 * G1_p^-1 * G1_r * h1^-1 * h2_r
# O_p * h1 * g1^-1 * J1_p^-1 * J1_r * g1 * h1^-1 * h2_r
# where h2_r can be a constant value of the expression above.
self.gripper_desired_pose = Multiply_of_matrixHomo(name + "_desired")
if withMeasurementOfObjectPos:
# TODO Integrate measurement of h1_r and h2_r: position error
# O_r^-1 * G1_r and O_r^-1 * G2_r
# (for the release phase and computed only at time 0)
self.gripper_desired_pose.setSignalNumber(5)
## self.gripper_desired_pose.setSignalNumber (7)
# self.gripper_desired_pose.sin0 -> plug to object1 planning pose
# self.gripper_desired_pose.sin1.value = se3ToTuple(self.handle.pose)
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.handle.pose * self.gripper.pose.inverse())
self._invert_planning_pose = Inverse_of_matrixHomo(
self.gripper.fullLink + "_invert_planning_pose")
# self._invert_planning_pose.sin -> plug to link1 planning pose
plug(self._invert_planning_pose.sout,
self.gripper_desired_pose.sin2)
# self.gripper_desired_pose.sin3 -> plug to link1 real pose
# self.gripper_desired_pose.sin4.value = se3ToTuple (self.handle.pose.inverse() * self.otherHandle.pose)
self.gripper_desired_pose.sin4.value = se3ToTuple(
self.gripper.pose * self.handle.pose.inverse() *
self.otherHandle.pose)
## self.gripper_desired_pose.sin4.value = se3ToTuple (self.gripper.pose * self.handle.pose.inverse())
## self.gripper_desired_pose.sin5 -> plug to object real pose
## if self.otherGripper.joint not in sotrobot.dyn.signals():
## sotrobot.dyn.createOpPoint (self.otherGripper.joint, self.otherGripper.joint)
## plug(sotrobot.dyn.signal(self.otherGripper.joint), self.gripper_desired_pose.sin6)
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.topics = {
self.handle.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.handle.fullLink,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
self.gripper.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.fullLink,
"signalGetters": [
lambda: self._invert_planning_pose.sin,
]
},
"measurement_" + self.gripper.fullLink: {
"velocity": False,
"type": "matrixHomo",
"handler": "tf_listener",
"frame0": "world",
"frame1": self.gripper.fullLink,
"signalGetters": [
lambda: self.gripper_desired_pose.sin3,
]
},
## "measurement_" + self.otherHandle.fullLink: {
## "velocity": False,
## "type": "matrixHomo",
## "handler": "tf_listener",
## "frame0": "world",
## "frame1": self.otherHandle.fullLink,
## "signalGetters": [ self.gripper_desired_pose.sin5 ] },
}
else:
# G2*_r = H1_p * h1^-1 * h2 = G2_p = J2_p * G
self.gripper_desired_pose.setSignalNumber(2)
# self.gripper_desired_pose.sin0 -> plug to joint planning pose
self.gripper_desired_pose.sin1.value = se3ToTuple(
self.otherGripper.pose)
self.topics = {
self.otherGripper.fullJoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.otherGripper.fullJoint,
"signalGetters": [
lambda: self.gripper_desired_pose.sin0,
]
},
}
plug(self.gripper_desired_pose.sout,
self.graspTask.featureDes.position)
self.tasks = [self.graspTask.task]