meta_tasks.py
Go to the documentation of this file.
1 from numpy import array, eye, matrix, ndarray
2 
3 from dynamic_graph import plug
4 from dynamic_graph.sot.core import Flags
5 from dynamic_graph.sot.core.feature_generic import FeatureGeneric
6 from dynamic_graph.sot.core.gain_adaptive import GainAdaptive
7 from dynamic_graph.sot.core.matrix_util import rpy2tr
8 
9 
10 class MetaTaskCom(object):
11  def __init__(self, dyn, name="com"):
12  self.dyn = dyn
13  self.name = name
14  # dyn.setProperty('ComputeCoM','true')
15 
16  self.feature = FeatureGeneric("feature" + name)
17  self.featureDes = FeatureGeneric("featureDes" + name)
18  self.gain = GainAdaptive("gain" + name)
19 
20  plug(dyn.com, self.feature.errorIN)
21  plug(dyn.Jcom, self.feature.jacobianIN)
22  self.feature.setReference(self.featureDes.name)
23 
24  def plugTask(self):
25  self.task.add(self.feature.name)
26  plug(self.task.error, self.gain.error)
27  plug(self.gain.gain, self.task.controlGain)
28 
29  @property
30  def ref(self):
31  return self.featureDes.errorIN.value
32 
33  @ref.setter
34  def ref(self, v):
35  self.featureDes.errorIN.value = v
36 
37 
38 # --- HELPER FUNCTIONS --------------------------------------------------------
39 def setGain(gain, val):
40  if val is not None:
41  if isinstance(val, int) or isinstance(val, float):
42  gain.setConstant(val)
43  elif len(val) == 1:
44  gain.setConstant(val[0])
45  elif len(val) == 3:
46  gain.set(val[0], val[1], val[2])
47  elif len(val) == 4:
48  gain.setByPoint(val[0], val[1], val[2], val[3])
49 
50 
52  M = eye(4)
53  if isinstance(p, (matrix, ndarray)) and p.size == 3:
54  M[0:3, 3] = p
55  elif isinstance(p, tuple) and len(p) == 3:
56  M[0:3, 3] = p
57  elif isinstance(p, (matrix, ndarray)) and p.shape == (4, 4):
58  M = p
59  elif isinstance(p, (matrix, tuple)) and len(p) == 4 == len(p[0]) == len(
60  p[1]
61  ) == len(p[2]) == len(p[3]):
62  M = matrix(p)
63  elif isinstance(p, (matrix, ndarray, tuple)) and len(p) == 6:
64  M = array(rpy2tr(*p[3:7]))
65  M[0:3, 3] = p[0:3]
66  else:
67  print("Position with other parameters ... todo")
68  return M
69 
70 
71 def goto6d(task, position, gain=None, resetJacobian=True):
72  M = generic6dReference(position)
73  task.featureDes.position.value = array(M)
74  task.feature.selec.value = Flags("111111")
75  setGain(task.gain, gain)
76  if (
77  "resetJacobianDerivative" in task.task.__class__.__dict__.keys()
78  and resetJacobian
79  ):
80  task.task.resetJacobianDerivative()
81 
82 
83 def gotoNd(task, position, selec=None, gain=None, resetJacobian=True):
84  M = generic6dReference(position)
85  if selec is not None:
86  if not isinstance(selec, Flags):
87  selec = Flags(selec)
88  task.feature.selec.value = selec
89  task.featureDes.position.value = array(M)
90  setGain(task.gain, gain)
91  if (
92  "resetJacobianDerivative" in task.task.__class__.__dict__.keys()
93  and resetJacobian
94  ):
95  task.task.resetJacobianDerivative()
def gotoNd(task, position, selec=None, gain=None, resetJacobian=True)
Definition: meta_tasks.py:83
def __init__(self, dyn, name="com")
Definition: meta_tasks.py:11
def goto6d(task, position, gain=None, resetJacobian=True)
Definition: meta_tasks.py:71


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