meta_task_posture.py 3.1 KB
Newer Older
Nicolas Mansard's avatar
Nicolas Mansard committed
1
from dynamic_graph import plug
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
2
from dynamic_graph.sot.core import Flags
Guilhem Saurel's avatar
Guilhem Saurel committed
3
4
from dynamic_graph.sot.core.feature_generic import FeatureGeneric
from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
Guilhem Saurel's avatar
Guilhem Saurel committed
5
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple
Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
6
from dynamic_graph.sot.core.meta_task_6d import toFlags  # noqa kept for backward compatibility
Nicolas Mansard's avatar
Nicolas Mansard committed
7
from dynamic_graph.sot.core.meta_tasks import setGain
8
from dynamic_graph.sot.core.task import Task
Guilhem Saurel's avatar
Guilhem Saurel committed
9
from numpy import identity, matrix, zeros
Nicolas Mansard's avatar
Nicolas Mansard committed
10
11
12


class MetaTaskPosture(object):
Guilhem Saurel's avatar
Guilhem Saurel committed
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
    postureRange = {
        "rleg": range(6, 12),
        "rhip": range(6, 9),
        "rknee": [9],
        "rankle": range(10, 12),
        "lleg": range(12, 18),
        "lhip": range(12, 15),
        "lknee": [15],
        "lankle": range(16, 18),
        "chest": range(18, 20),
        "head": range(20, 22),
        "rarm": range(22, 28),
        "rshoulder": range(22, 25),
        "relbow": [25],
        "rwrist": range(26, 28),
        "rhand": [28],
        "larm": range(29, 35),
        "lshoulder": range(29, 32),
        "lelbow": [32],
        "lwrist": range(33, 35),
        "lhand": [35],
    }
Nicolas Mansard's avatar
Nicolas Mansard committed
35
36
    nbDof = None

Guilhem Saurel's avatar
Guilhem Saurel committed
37
38
39
40
41
42
43
    def __init__(self, dyn, name="posture"):
        self.dyn = dyn
        self.name = name
        self.feature = FeatureGeneric('feature' + name)
        self.featureDes = FeatureGeneric('featureDes' + name)
        self.gain = GainAdaptive('gain' + name)
        plug(dyn.position, self.feature.errorIN)
Nicolas Mansard's avatar
Nicolas Mansard committed
44
        robotDim = len(dyn.position.value)
Guilhem Saurel's avatar
Guilhem Saurel committed
45
        self.feature.jacobianIN.value = matrixToTuple(identity(robotDim))
Nicolas Mansard's avatar
Nicolas Mansard committed
46
47
48
49
        self.feature.setReference(self.featureDes.name)

    def plugTask(self):
        self.task.add(self.feature.name)
Guilhem Saurel's avatar
Guilhem Saurel committed
50
51
        plug(self.task.error, self.gain.error)
        plug(self.gain.gain, self.task.controlGain)
Nicolas Mansard's avatar
Nicolas Mansard committed
52
53
54
55
56
57

    @property
    def ref(self):
        return self.featureDes.errorIN.value

    @ref.setter
Guilhem Saurel's avatar
Guilhem Saurel committed
58
    def ref(self, v):
Nicolas Mansard's avatar
Nicolas Mansard committed
59
60
        self.featureDes.errorIN.value = v

Guilhem Saurel's avatar
Guilhem Saurel committed
61
62
63
64
65
    def gotoq(self, gain=None, qdes=None, **kwargs):
        act = list()
        if qdes is not None:
            if isinstance(qdes, tuple):
                qdes = matrix(qdes).T
Nicolas Mansard's avatar
Nicolas Mansard committed
66
        else:
Guilhem Saurel's avatar
Guilhem Saurel committed
67
            if MetaTaskPosture.nbDof is None:
Nicolas Mansard's avatar
Nicolas Mansard committed
68
                MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
Guilhem Saurel's avatar
Guilhem Saurel committed
69
            qdes = zeros((MetaTaskPosture.nbDof, 1))
Nicolas Mansard's avatar
Nicolas Mansard committed
70

Guilhem Saurel's avatar
flake8    
Guilhem Saurel committed
71
72
73
        act = [
            False,
        ] * MetaTaskPosture.nbDof
Guilhem Saurel's avatar
Guilhem Saurel committed
74
        for limbName, jointValues in kwargs.items():
Nicolas Mansard's avatar
Nicolas Mansard committed
75
            limbRange = self.postureRange[limbName]
76
77
            for i in limbRange:
                act[i] = True
Guilhem Saurel's avatar
Guilhem Saurel committed
78
79
80
81
82
            if jointValues != []:
                if isinstance(jointValues, matrix):
                    qdes[limbRange, 0] = vectorToTuple(jointValues)
                else:
                    qdes[limbRange, 0] = jointValues
Nicolas Mansard's avatar
Nicolas Mansard committed
83
        self.ref = vectorToTuple(qdes)
Guilhem Saurel's avatar
Guilhem Saurel committed
84
        if len(act) > 0:
85
            self.feature.selec.value = Flags(act)
Guilhem Saurel's avatar
Guilhem Saurel committed
86
87
        setGain(self.gain, gain)

Nicolas Mansard's avatar
Nicolas Mansard committed
88
89

class MetaTaskKinePosture(MetaTaskPosture):
Guilhem Saurel's avatar
Guilhem Saurel committed
90
91
92
    def __init__(self, dyn, name="posture"):
        MetaTaskPosture.__init__(self, dyn, name)
        self.task = Task('task' + name)
Nicolas Mansard's avatar
Nicolas Mansard committed
93
        self.plugTask()