meta_task_posture.py
Go to the documentation of this file.
1 from dynamic_graph import plug
2 from dynamic_graph.sot.core import Flags
3 from dynamic_graph.sot.core.feature_generic import FeatureGeneric
4 from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
5 from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple
6 from dynamic_graph.sot.core.meta_tasks import setGain
7 from dynamic_graph.sot.core.task import Task
8 from numpy import identity, matrix, zeros
9 
10 
11 class MetaTaskPosture(object):
12  postureRange = {
13  "rleg": range(6, 12),
14  "rhip": range(6, 9),
15  "rknee": [9],
16  "rankle": range(10, 12),
17  "lleg": range(12, 18),
18  "lhip": range(12, 15),
19  "lknee": [15],
20  "lankle": range(16, 18),
21  "chest": range(18, 20),
22  "head": range(20, 22),
23  "rarm": range(22, 28),
24  "rshoulder": range(22, 25),
25  "relbow": [25],
26  "rwrist": range(26, 28),
27  "rhand": [28],
28  "larm": range(29, 35),
29  "lshoulder": range(29, 32),
30  "lelbow": [32],
31  "lwrist": range(33, 35),
32  "lhand": [35],
33  }
34  nbDof = None
35 
36  def __init__(self, dyn, name="posture"):
37  self.dyn = dyn
38  self.name = name
39  self.feature = FeatureGeneric("feature" + name)
40  self.featureDes = FeatureGeneric("featureDes" + name)
41  self.gain = GainAdaptive("gain" + name)
42  plug(dyn.position, self.feature.errorIN)
43  robotDim = len(dyn.position.value)
44  self.feature.jacobianIN.value = matrixToTuple(identity(robotDim))
45  self.feature.setReference(self.featureDes.name)
46 
47  def plugTask(self):
48  self.task.add(self.feature.name)
49  plug(self.task.error, self.gain.error)
50  plug(self.gain.gain, self.task.controlGain)
51 
52  @property
53  def ref(self):
54  return self.featureDes.errorIN.value
55 
56  @ref.setter
57  def ref(self, v):
58  self.featureDes.errorIN.value = v
59 
60  def gotoq(self, gain=None, qdes=None, **kwargs):
61  act = list()
62  if qdes is not None:
63  if isinstance(qdes, tuple):
64  qdes = matrix(qdes).T
65  else:
66  if MetaTaskPosture.nbDof is None:
67  MetaTaskPosture.nbDof = len(self.feature.errorIN.value)
68  qdes = zeros((MetaTaskPosture.nbDof, 1))
69 
70  act = [
71  False,
72  ] * MetaTaskPosture.nbDof
73  for limbName, jointValues in kwargs.items():
74  limbRange = self.postureRange[limbName]
75  for i in limbRange:
76  act[i] = True
77  if jointValues != []:
78  if isinstance(jointValues, matrix):
79  qdes[limbRange, 0] = vectorToTuple(jointValues)
80  else:
81  qdes[limbRange, 0] = jointValues
82  self.ref = vectorToTuple(qdes)
83  if len(act) > 0:
84  self.feature.selec.value = Flags(act)
85  setGain(self.gain, gain)
86 
87 
89  def __init__(self, dyn, name="posture"):
90  MetaTaskPosture.__init__(self, dyn, name)
91  self.task = Task("task" + name)
92  self.plugTask()
def gotoq(self, gain=None, qdes=None, kwargs)


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26