示例#1
0
    def gotoq(self, gain=None, qdes=None, **kwargs):
        act = list()
        if qdes is not None:
            if isinstance(qdes, tuple):
                qdes = matrix(qdes).T
        else:
            if MetaTaskPosture.nbDof is None:
                MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
            qdes = zeros((MetaTaskPosture.nbDof, 1))

        act = [
            False,
        ] * MetaTaskPosture.nbDof
        for limbName, jointValues in kwargs.items():
            limbRange = self.postureRange[limbName]
            for i in limbRange:
                act[i] = True
            if jointValues != []:
                if isinstance(jointValues, matrix):
                    qdes[limbRange, 0] = vectorToTuple(jointValues)
                else:
                    qdes[limbRange, 0] = jointValues
        self.ref = vectorToTuple(qdes)
        if len(act) > 0:
            self.feature.selec.value = Flags(act)
        setGain(self.gain, gain)
 def goto3D(self, point3D, gain=None, ref2D=None, selec=None):
     self.target = point3D
     if ref2D is not None:
         self.ref = ref2D
     if selec is not None:
         self.feature.selec.value = selec
     setGain(self.gain, gain)
示例#3
0
def goForTheBall():
    '''The hand goes for the hand.'''
    taskRH.feature.selec.value = '111'
    setGain(taskRH.gain,(5,0.5,0.03,0.9))
    taskWaist.feature.selec.value = '011000'
    ballTraj.elasticXY = 0.0
    ballTraj.gravity = (0,0,-3.0)
    attime(ALWAYS,graspIf)
示例#4
0
def goForTheBall():
    '''The hand goes for the hand.'''
    taskRH.feature.selec.value = '111'
    setGain(taskRH.gain, (5, 0.5, 0.03, 0.9))
    taskWaist.feature.selec.value = '011000'
    ballTraj.elasticXY = 0.0
    ballTraj.gravity = (0, 0, -3.0)
    attime(ALWAYS, graspIf)
def goto6dRel(task,position,positionRef,gain=None,resetJacobian=True):
	M=generic6dReference(position)
	MRef=generic6dReference(positionRef)
	task.featureDes.position.value = matrixToTuple(M)
	task.featureDes.positionRef.value = matrixToTuple(MRef)
	task.feature.selec.value = "111111"
	setGain(task.gain,gain)
	if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
		task.task.resetJacobianDerivative()
示例#6
0
def goto6dRel(task, position, positionRef, gain=None, resetJacobian=True):
    M = generic6dReference(position)
    MRef = generic6dReference(positionRef)
    task.featureDes.position.value = matrixToTuple(M)
    task.featureDes.positionRef.value = matrixToTuple(MRef)
    task.feature.selec.value = Flags("111111")
    setGain(task.gain, gain)
    if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
        task.task.resetJacobianDerivative()
示例#7
0
def grasp():
    '''Ball is in the hand.'''
    del attime.events[ALWAYS]
    attime(ALWAYS,ballInHand)
    sot.rm(taskRH.task.name)
    sot.rm(taskGaze.task.name)
    q0 = halfSittingConfig[robotName]
    taskPosture.gotoq((5,1,0.05,0.9),q0,rarm=[],larm=[],chest=[],head=[],rhand=[0,])
    setGain(taskWaist.gain,(2,0.2,0.03,0.9))
    taskWaist.feature.selec.value = '011100'
def gotoNdRel(task,position,positionRef,selec=None,gain=None,resetJacobian=True):
	M=generic6dReference(position)
	MRef=generic6dReference(positionRef)
	if selec!=None:
		if isinstance(selec,str):   task.feature.selec.value = selec
		else: task.feature.selec.value = toFlags(selec)
	task.featureDes.position.value = matrixToTuple(M)
	task.featureDes.positionRef.value = matrixToTuple(MRef)
	setGain(task.gain,gain)
	if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
		task.task.resetJacobianDerivative()
示例#9
0
def gotoNdRel(task, position, positionRef, selec=None, gain=None, resetJacobian=True):
    M = generic6dReference(position)
    MRef = generic6dReference(positionRef)
    if selec is not None:
        if not isinstance(selec, Flags):
            selec = Flags(selec)
        task.feature.selec.value = selec
    task.featureDes.position.value = matrixToTuple(M)
    task.featureDes.positionRef.value = matrixToTuple(MRef)
    setGain(task.gain, gain)
    if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
        task.task.resetJacobianDerivative()
示例#10
0
    def makeTasks(self, sotrobot):
        from dynamic_graph.sot.core.matrix_util import matrixToTuple
        from dynamic_graph.sot.core import Multiply_of_matrix, Multiply_of_matrixHomo, OpPointModifier
        from dynamic_graph import plug
        if self.relative:
            self.graspTask = MetaTaskKine6d (
                    Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name,
                    sotrobot.dynamic,
                    self.gripper.sotjoint,
                    self.gripper.sotjoint)
            
            # Creates the matrix transforming the goal gripper's position into the desired moving gripper's position:
            #     on the other handle of the object
            #M = ((-1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, -1.0, -0.55), (0.0, 0.0, 0.0, 1.0))
            M = transformToMatrix(self.otherGripper.sotpose * self.otherHandle.sotpose.inverse() * self.handle.sotpose * self.gripper.sotpose.inverse())
            self.graspTask.opmodif = matrixToTuple(M)
            self.graspTask.feature.frame("current")
            setGain(self.graspTask.gain,(100,0.9,0.01,0.9))
            self.graspTask.task.setWithDerivative (False)

            # Sets the position and velocity of the other gripper as the goal of the first gripper pose
            if not self.opPointExist(sotrobot.dynamic, self.otherGripper.sotjoint):
                sotrobot.dynamic.createOpPoint(self.otherGripper.sotjoint, self.otherGripper.sotjoint)
                sotrobot.dynamic.createVelocity("vel_"+self.otherGripper.sotjoint, self.otherGripper.sotjoint)
            plug(sotrobot.dynamic.signal(self.otherGripper.sotjoint), self.graspTask.featureDes.position)
            plug(sotrobot.dynamic.signal("vel_"+self.otherGripper.sotjoint), self.graspTask.featureDes.velocity)
            #plug(sotrobot.dynamic.signal("J"+self.otherGripper.sotjoint), self.graspTask.featureDes.Jq)
            
            # We will need a less barbaric method to do this...
            # Zeroes the jacobian for the joints we don't want to use.
            
            #  - Create the selection matrix for the desired joints
            mat = ()
            for i in range(38):
                mat += ((0,)*i + (1 if i in range(29, 36) else 0,) + (0,)*(37-i),)
            
            # - Multiplies it with the computed jacobian matrix of the gripper
            mult = Multiply_of_matrix('mult')
            mult.sin2.value = mat
            plug(self.graspTask.opPointModif.jacobian, mult.sin1)
            plug(mult.sout, self.graspTask.feature.Jq)

            self.tasks = [ self.graspTask.task ]
            #self.tasks = []
        self += EEPosture (sotrobot, self.gripper, [-0.2 if self.closeGripper else 0])
示例#11
0
def grasp():
    '''Ball is in the hand.'''
    del attime.events[ALWAYS]
    attime(ALWAYS, ballInHand)
    sot.rm(taskRH.task.name)
    sot.rm(taskGaze.task.name)
    q0 = halfSittingConfig[robotName]
    taskPosture.gotoq((5, 1, 0.05, 0.9),
                      q0,
                      rarm=[],
                      larm=[],
                      chest=[],
                      head=[],
                      rhand=[
                          0,
                      ])
    setGain(taskWaist.gain, (2, 0.2, 0.03, 0.9))
    taskWaist.feature.selec.value = '011100'
示例#12
0
    def makePositionControl(self, position):
        q = list(self.tp.feature.state.value)
        # Define the reference
        ip = 0
        for iq, nv in self.jointRanks:
            q[iq:iq + nv] = position[ip:ip + nv]
            ip += nv
        assert ip == len(position)
        self.tp.feature.posture.value = q
        setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))

        from .events import norm_inferior_to
        n, c = norm_inferior_to(self.name + "_positioncmp", self.tp.error,
                                0.001)
        self.events = {
            "done_close": c.sout,
            "done_open": c.sout,
        }
示例#13
0
    def __init__ (self, sotrobot, gripper, position):
        from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
        from dynamic_graph.sot.core.matrix_util import matrixToTuple
        from dynamic_graph import plug
        from numpy import identity, hstack, zeros

        super(EEPosture, self).__init__()
        self.gripper = gripper

        # Get joint position in posture
        pinmodel = sotrobot.dynamic.model
        idJ = pinmodel.getJointId(gripper.sotjoint)
        assert idJ < pinmodel.njoints
        joint = sotrobot.dynamic.model.joints[idJ]
        assert joint.nq == len(position)

        idx_q = joint.idx_q + 1
        idx_v = joint.idx_v + 1

        n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(position)

        self.tp = Task ('task' + n)
        self.tp.dyn = sotrobot.dynamic
        self.tp.feature = FeaturePosture ('feature_' + n)

        plug(sotrobot.dynamic.position, self.tp.feature.state)
        q = list(sotrobot.dynamic.position.value)
        q[idx_v:idx_v + joint.nv] = position
        self.tp.feature.posture.value = q

        self.tp.gain = GainAdaptive("gain_"+n)
        robotDim = sotrobot.dynamic.getDimension()
        # for i in range (6, robotDim):
            # self.tp.feature.selectDof (i, False)
        # print idx_q, idx_v
        for i in range(joint.nv):
            self.tp.feature.selectDof (idx_v + i, True)
        self.tp.add(self.tp.feature.name)

        # Set the gain of the posture task
        setGain(self.tp.gain,(4.9,0.9,0.01,0.9))
        plug(self.tp.gain.gain, self.tp.controlGain)
        plug(self.tp.error, self.tp.gain.error)
        self.tasks = [ self.tp ]
示例#14
0
文件: tools.py 项目: nim65s/sot-hpp
    def __init__(self, sotrobot, gripper, position):
        from dynamic_graph.sot.core import Task, GainAdaptive

        super(EEPosture, self).__init__()
        self.gripper = gripper
        self.jointNames = gripper.joints
        pinmodel = sotrobot.dynamic.model

        n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(
            list(position))

        self.tp = Task('task' + n)
        self.tp.dyn = sotrobot.dynamic
        self.tp.feature = FeaturePosture('feature_' + n)

        plug(sotrobot.dynamic.position, self.tp.feature.state)
        q = list(sotrobot.dynamic.position.value)
        # Define the reference and the selected DoF
        rank = 0
        ranks = []
        for name in self.jointNames:
            idJ = pinmodel.getJointId(name)
            assert idJ < pinmodel.njoints
            joint = pinmodel.joints[idJ]
            idx_v = joint.idx_v
            nv = joint.nv
            ranks += range(idx_v, idx_v + nv)
            q[idx_v:idx_v + nv] = position[rank:rank + nv]
            rank += nv
        assert rank == len(position)

        self.tp.feature.posture.value = q
        for i in ranks:
            self.tp.feature.selectDof(i, True)

        self.tp.gain = GainAdaptive("gain_" + n)
        self.tp.add(self.tp.feature.name)

        # Set the gain of the posture task
        setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
        plug(self.tp.gain.gain, self.tp.controlGain)
        plug(self.tp.error, self.tp.gain.error)
        if len(self.jointNames) > 0:
            self.tasks = [self.tp]
    def gotoq(self,gain=None,qdes=None,**kwargs):
        act=list()
        if qdes!=None:
            if isinstance(qdes,tuple): qdes=matrix(qdes).T
        else:
            if MetaTaskPosture.nbDof==None:
                MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
            qdes = zeros((MetaTaskPosture.nbDof,1))

        for limbName,jointValues in kwargs.items():
            limbRange = self.postureRange[limbName]
            act += limbRange
            if jointValues!=[]:
                if isinstance(jointValues,matrix):
                    qdes[limbRange,0] = vectorToTuple(jointValues)
                else: qdes[limbRange,0] = jointValues
        self.ref = vectorToTuple(qdes)
        if(len(act)>0): self.feature.selec.value = toFlags(act)
        setGain(self.gain,gain)
示例#16
0
文件: tools.py 项目: nim65s/sot-hpp
    def __init__(self, name, sotrobot):
        super(Posture, self).__init__()
        from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
        from dynamic_graph.sot.core.matrix_util import matrixToTuple
        from numpy import identity

        n = Posture.sep + name
        self.tp = Task('task' + n)
        self.tp.dyn = sotrobot.dynamic
        self.tp.feature = FeaturePosture('feature_' + n)

        q = list(sotrobot.dynamic.position.value)
        self.tp.feature.state.value = q
        self.tp.feature.posture.value = q

        robotDim = sotrobot.dynamic.getDimension()
        for i in range(6, robotDim):
            self.tp.feature.selectDof(i, True)
        self.tp.gain = GainAdaptive("gain_" + n)
        self.tp.add(self.tp.feature.name)

        # Connects the dynamics to the current feature of the posture task
        plug(sotrobot.dynamic.position, self.tp.feature.state)

        self.tp.setWithDerivative(True)

        # Set the gain of the posture task
        setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
        plug(self.tp.gain.gain, self.tp.controlGain)
        plug(self.tp.error, self.tp.gain.error)
        self.tasks = [self.tp]
        self.topics = {
            name: {
                "type": "vector",
                "topic": "/hpp/target/position",
                "signalGetters": [self._signalPositionRef]
            },
            "vel_" + name: {
                "type": "vector",
                "topic": "/hpp/target/velocity",
                "signalGetters": [self._signalVelocityRef]
            },
        }
示例#17
0
    def __init__(self,
                 footname,
                 sotrobot,
                 selec='111111',
                 withDerivative=False):
        super(Foot, self).__init__()

        robotname, sotjoint = parseHppName(footname)
        basename = self._name(footname)

        # Create the FeaturePose
        self.feature = FeaturePose('pose' + basename)

        dyn = sotrobot.dynamic
        if sotjoint not in dyn.signals():
            dyn.createOpPoint(sotjoint, sotjoint)
        plug(dyn.signal(sotjoint), self.feature.oMjb)
        plug(dyn.signal('J' + sotjoint), self.feature.jbJjb)

        # Wrap it into a Task
        self.task = SotTask('task' + basename)
        self.gain = GainAdaptive('gain' + basename)

        self.task.add(self.feature.name)
        plug(self.task.error, self.gain.error)
        plug(self.gain.gain, self.task.controlGain)

        setGain(self.gain, (4.9, 0.9, 0.01, 0.9))
        self.task.setWithDerivative(withDerivative)

        if selec != '111111':
            self.feature.selec.value = selec

        self.tasks = [
            self.task,
        ]
        self.addHppJointTopic(footname,
                              signalGetters=[self._signalPositionRef])
        if withDerivative:
            self.addHppJointTopic("vel_" + footname,
                                  footname,
                                  velocity=True,
                                  signalGetters=[self._signalVelocityRef])
示例#18
0
    def __init__(self, sotrobot, gripper, name_suffix):
        from dynamic_graph.sot.core import Task, GainAdaptive

        super(EndEffector, self).__init__()
        self.gripper = gripper
        self.jointNames = gripper.joints
        self.robot = sotrobot
        pinmodel = sotrobot.dynamic.model

        self.name = Posture.sep.join(["ee", gripper.name, name_suffix])

        self.tp = Task('task' + self.name)
        self.tp.dyn = sotrobot.dynamic
        self.tp.feature = FeaturePosture('feature_' + self.name)

        plug(sotrobot.dynamic.position, self.tp.feature.state)

        # Select the dofs
        self.tp.feature.posture.value = sotrobot.dynamic.position.value
        # Define the reference and the selected DoF
        self.jointRanks = []
        for name in self.jointNames:
            idJ = pinmodel.getJointId(name)
            assert idJ < pinmodel.njoints
            joint = pinmodel.joints[idJ]
            idx_v = joint.idx_v
            nv = joint.nv
            self.jointRanks.append((idx_v, nv))
        for idx_v, nv in self.jointRanks:
            for i in range(idx_v, idx_v + nv):
                self.tp.feature.selectDof(i, True)

        self.tp.gain = GainAdaptive("gain_" + self.name)
        self.tp.add(self.tp.feature.name)

        # Set the gain of the posture task
        setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9))
        plug(self.tp.gain.gain, self.tp.controlGain)
        plug(self.tp.error, self.tp.gain.error)
        if len(self.jointNames) > 0:
            self.tasks = [self.tp]
示例#19
0
    def __init__(self, name, sotrobot, withDerivative=False):
        super(Posture, self).__init__()

        n = self._name(name)
        self._task = SotTask(n + '_task')
        self._task.dyn = sotrobot.dynamic
        self._feature = FeaturePosture(n + '_feature_')

        q = list(sotrobot.dynamic.position.value)
        self._feature.state.value = q
        self._feature.posture.value = q

        robotDim = sotrobot.dynamic.getDimension()
        for i in range(6, robotDim):
            self._feature.selectDof(i, True)
        self._gain = GainAdaptive(n + "_gain")
        self._task.add(self._feature.name)

        # Connects the dynamics to the current feature of the posture task
        plug(sotrobot.dynamic.position, self._feature.state)

        self._task.setWithDerivative(withDerivative)

        # Set the gain of the posture task
        setGain(self._gain, (4.9, 0.9, 0.01, 0.9))
        plug(self._gain.gain, self._task.controlGain)
        plug(self._task.error, self._gain.error)
        self.tasks = [self._task]
        self.topics = {
            name: {
                "type": "vector",
                "topic": "/hpp/target/position",
                "signalGetters": frozenset([self._signalPositionRef])
            },
        }
        if withDerivative:
            self.topics["vel_" + name] = {
                "type": "vector",
                "topic": "/hpp/target/velocity",
                "signalGetters": frozenset([self._signalVelocityRef])
            }
示例#20
0
    def __init__ (self, name, sotrobot):
        super(Posture, self).__init__()
        from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector
        from dynamic_graph.sot.core.matrix_util import matrixToTuple
        from dynamic_graph import plug
        from numpy import identity, hstack, zeros

        n = Posture.sep + name
        self.tp = Task ('task' + n)
        self.tp.dyn = sotrobot.dynamic
        self.tp.feature = FeatureGeneric('feature_'+n)
        self.tp.featureDes = FeatureGeneric('feature_des_'+n)
        self.tp.gain = GainAdaptive("gain_"+n)
        robotDim = sotrobot.dynamic.getDimension()
        self.tp.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
        self.tp.feature.setReference(self.tp.featureDes.name)
        self.tp.add(self.tp.feature.name)

        # Connects the dynamics to the current feature of the posture task
        plug(sotrobot.dynamic.position, self.tp.feature.errorIN)

        self.tp.setWithDerivative (True)

        # Set the gain of the posture task
        setGain(self.tp.gain,10)
        plug(self.tp.gain.gain, self.tp.controlGain)
        plug(self.tp.error, self.tp.gain.error)
        self.tasks = [ self.tp ]
        self.topics = {
                    name: {
                        "type": "vector",
                        "topic": "/hpp/target/position",
                        "signalGetters": [ self._signalPositionRef ] },
                    "vel_" + name: {
                        "type": "vector",
                        "topic": "/hpp/target/velocity",
                        "signalGetters": [ self._signalVelocityRef ] },
                }
示例#21
0
文件: tools.py 项目: nim65s/sot-hpp
    def makeTasks(self, sotrobot):
        if self.relative:
            self.graspTask = MetaTaskKine6dRel(
                Grasp.sep + self.gripper.name + Grasp.sep +
                self.otherGripper.name, sotrobot.dynamic, self.gripper.joint,
                self.otherGripper.joint, self.gripper.joint,
                self.otherGripper.joint)

            M = se3ToTuple(self.gripper.pose * self.handle.pose.inverse())
            self.graspTask.opPointModif.activ = True
            self.graspTask.opPointModif.setTransformation(M)
            # Express the velocities in local frame.
            # This is the default.
            # self.graspTask.opPointModif.setEndEffector(True)

            M = se3ToTuple(self.otherGripper.pose *
                           self.otherHandle.pose.inverse())
            self.graspTask.opPointModifBase.activ = True
            self.graspTask.opPointModifBase.setTransformation(M)
            # Express the velocities in local frame.
            # This is the default.
            # self.graspTask.opPointModif.setEndEffector(True)

            # Plug the signals
            plug(self.graspTask.opPointModif.signal('position'),
                 self.graspTask.feature.position)
            plug(self.graspTask.opPointModif.signal('jacobian'),
                 self.graspTask.feature.Jq)
            plug(self.graspTask.opPointModifBase.signal('position'),
                 self.graspTask.feature.positionRef)
            plug(self.graspTask.opPointModifBase.signal('jacobian'),
                 self.graspTask.feature.JqRef)

            setGain(self.graspTask.gain, (4.9, 0.9, 0.01, 0.9))
            self.graspTask.task.setWithDerivative(False)

            self.tasks = [self.graspTask.task]
示例#22
0
    def makeTasks(self, sotrobot, withDerivative = False):
        if self.relative:
            basename = self._name(self.gripper.name,
                    self.handle.name,
                    self.otherGripper.name,
                    self.otherHandle.name)


            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)

            # Create the FeaturePose
            self.feature = FeaturePose (basename+"_feature")
            set(self.feature.oMja, self.feature.jaMfa, self.feature.jaJja,
                    self.otherGripper, self.otherHandle)
            set(self.feature.oMjb, self.feature.jbMfb, self.feature.jbJjb,
                    self.gripper, self.handle)

            # Wrap it into a Task
            self.task = SotTask (basename+"_task")
            self.gain = GainAdaptive (basename+"_gain")

            self.task.add (self.feature.name)
            plug (self.task.error, self.gain.error)
            plug (self.gain.gain , self.task.controlGain)

            setGain(self.gain,(4.9,0.9,0.01,0.9))
            if withDerivative:
                print("Grasp constraint with derivative is not implemented yet.")
            self.task.setWithDerivative (False)

            self.tasks = [ self.task ]
示例#23
0
    "/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz"
)
aSimpleSeqPlay.setTimeToStart(10.0)
# Connects the sequence player to the posture task
from dynamic_graph.sot.core import Selec_of_vector
from dynamic_graph import plug

plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN)

# Connects the dynamics to the current feature of the posture task
getPostureValue = Selec_of_vector("current_posture")
getPostureValue.selec(6, robotDim)
plug(robot.dynamic.position, getPostureValue.sin)
plug(getPostureValue.sout, taskPosture.feature.errorIN)
plug(getPostureValue.sout, aSimpleSeqPlay.currentPosture)

# Set the gain of the posture task
setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9))
plug(taskPosture.gain.gain, taskPosture.controlGain)
plug(taskPosture.error, taskPosture.gain.error)

# Create the solver
from dynamic_graph.sot.core import SOT
sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)

taskPosture.featureDes.errorIN.recompute(0)

# Push the posture task in the solver
示例#24
0
featureVectorRH.positionRef.value = (1,0,0)
taskVectorRH = Task('taskVectorRH')
taskVectorRH.add(featureVectorRH.name)
taskVectorRH.controlGain.value = 10

featureVectorLH = FeatureVector3('fv3lh')
plug(dyn.lh,featureVectorLH.position)
plug(dyn.Jlh,featureVectorLH.Jq)
featureVectorLH.vector.value = (1,0,0)
featureVectorLH.positionRef.value = (1,0,0)
taskVectorLH = Task('taskVectorLH')
taskVectorLH.add(featureVectorLH.name)
gainVectorLH = GainAdaptive("gainVectorLH")
plug(taskVectorLH.error,gainVectorLH.error)
plug(gainVectorLH.gain,taskVectorLH.controlGain)
setGain(gainVectorLH,(10,1.5,0.01,0.9))

# Control the orientation of the gripper toward the continuous handle.
taskRHOrient=MetaTaskKine6d('rhorient',dyn,'rh','right-wrist')
taskRHOrient.opmodif = matrixToTuple(handMgrip)
taskRHOrient.feature.frame('current')
taskRHOrient.feature.selec.value = '011'
taskRHOrient.featureVector = FeatureVector3('feature-rhorient-v3')
plug(taskRHOrient.feature.position,taskRHOrient.featureVector.position)
plug(taskRHOrient.feature.Jq,taskRHOrient.featureVector.Jq)
taskRHOrient.featureVector.vector.value = (1,0,0)
taskRHOrient.task.add(taskRHOrient.featureVector.name)
setGain(taskRHOrient.gain,1)

taskLHOrient=MetaTaskKine6d('lhorient',dyn,'lh','left-wrist')
taskLHOrient.opmodif = matrixToTuple(handMgrip)
示例#25
0
    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]
示例#26
0
    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]
示例#27
0
    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]
示例#28
0
    def makeAdmittanceControl(self,
                              affordance,
                              type,
                              period,
                              simulateTorqueFeedback=False,
                              filterCurrents=True):
        # Make the admittance controller
        from agimus_sot.control.gripper import AdmittanceControl, PositionAndAdmittanceControl
        from .events import norm_superior_to
        # type = "open" or "close"
        desired_torque = affordance.ref["torque"]
        estimated_theta_close = affordance.ref["angle_close"]

        wn, z, nums, denoms = affordance.getControlParameter()

        if affordance.controlType[type] == "torque":
            self.ac = AdmittanceControl("AC_" + self.name + "_" + type,
                                        estimated_theta_close, desired_torque,
                                        period, nums, denoms)
        elif affordance.controlType[type] == "position_torque":
            theta_open = affordance.ref["angle_open"]
            threshold_up = tuple([x / 10. for x in desired_torque])
            threshold_down = tuple([x / 100. for x in desired_torque])
            self.ac = PositionAndAdmittanceControl(
                "AC_" + self.name + "_" + type, theta_open,
                estimated_theta_close, desired_torque, period, threshold_up,
                threshold_down, wn, z, nums, denoms)
        else:
            raise NotImplementedError("Control type " + type +
                                      " is not implemented for gripper.")

        if simulateTorqueFeedback:
            # Get torque from an internal simulation
            M, d, k, x0 = affordance.getSimulationParameters()
            self.ac.setupFeedbackSimulation(M, d, k, x0)
            # Must be done after setupFeedbackSimulation
            self.ac.readPositionsFromRobot(self.robot, self.jointNames)
        else:
            self.ac.readPositionsFromRobot(self.robot, self.jointNames)
            # Get torque from robot sensors (or external simulation)
            # TODO allows to switch between current and torque sensors
            self.ac.readCurrentsFromRobot(self.robot, self.jointNames,
                                          (self.gripper.torque_constant, ),
                                          filterCurrents)
            # self.ac.readTorquesFromRobot(self.robot, self.jointNames)

        from dynamic_graph.sot.core.operator import Mix_of_vector
        mix_of_vector = Mix_of_vector(self.name + "_control_to_robot_control")
        mix_of_vector.default.value = tuple([
            0,
        ] * len(self.tp.feature.posture.value))
        mix_of_vector.setSignalNumber(2)
        for idx_v, nv in self.jointRanks:
            mix_of_vector.addSelec(1, idx_v, nv)

        # Plug the admittance controller to the posture task
        setGain(self.tp.gain, 1.)
        plug(self.ac.outputPosition, mix_of_vector.signal("sin1"))
        plug(mix_of_vector.sout, self.tp.feature.posture)
        # TODO plug posture dot ?
        # I do not think it is necessary.
        # TODO should we send to the posture task
        # - the current robot posture
        # - the postureDot from self.ac.outputDerivative
        # This would avoid the last integration in self.ac.
        # This integration does not know the initial point so
        # there might be some drift (removed the position controller at the beginning)

        n, c = norm_superior_to(self.name + "_torquecmp",
                                self.ac.currentTorqueIn,
                                0.9 * np.linalg.norm(desired_torque))
        self.events = {
            "done_close": c.sout,
        }
 def goto3D(self,point3D,gain=None,ref2D=None,selec=None):
     self.target = point3D
     if ref2D!=None: self.ref=ref2D
     if selec!=None: self.feature.selec.value = selec
     setGain(self.gain,gain)
示例#30
0
robotDim = robot.dynamic.getDimension()
assert robotDim == len(q)

with_wheels = robotDim == 19

for i in range(8 if with_wheels else 6, robotDim):
    tp.feature.selectDof(i, True)

tp.gain = GainAdaptive("gain_" + n)
tp.add(tp.feature.name)

# Connects the dynamics to the current feature of the posture task
plug(robot.dynamic.position, tp.feature.state)

# Set the gain of the posture task
setGain(tp.gain, (4.9, 0.9, 0.01, 0.9))
plug(tp.gain.gain, tp.controlGain)
plug(tp.error, tp.gain.error)


def getJointIdxQ(name):
    model = robot.pinocchioModel
    jid = model.getJointId(name)
    return model.joints[jid].idx_q - 1


# Set initial state.
# q[getJointIdxQ("torso_lift_joint")] = 0.25
# q[getJointIdxQ("arm_1_joint")] = 0.2
# q[getJointIdxQ("arm_2_joint")] = 0.35
# q[getJointIdxQ("arm_3_joint")] = -0.2