From bc335bf3c637260dc9e320b8e322816d0d82adfa Mon Sep 17 00:00:00 2001
From: Francesco Morsillo <fmorsill@laas.fr>
Date: Wed, 5 Jun 2013 18:48:41 +0200
Subject: [PATCH] Added Relative MetaTaskDyn and modified CMakeLists
---
src/CMakeLists.txt | 1 +
.../sot/dyninv/meta_tasks_dyn_relative.py | 51 +++++++++++++++++++
2 files changed, 52 insertions(+)
create mode 100644 src/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 911c877..c067ea4 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -74,6 +74,7 @@ INSTALL(FILES
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/__init__.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_task_dyn_6d.py
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn.py
+ ${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py
${CMAKE_CURRENT_BINARY_DIR}/../python/robot_specific.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/dyninv
)
diff --git a/src/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py b/src/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py
new file mode 100644
index 0000000..28ed2dc
--- /dev/null
+++ b/src/dynamic_graph/sot/dyninv/meta_tasks_dyn_relative.py
@@ -0,0 +1,51 @@
+from dynamic_graph import plug
+from dynamic_graph.sot.core.meta_tasks import setGain,generic6dReference
+from dynamic_graph.sot.core import GainAdaptive, OpPointModifier
+from dynamic_graph.sot.dyninv import TaskDynPD
+from dynamic_graph.sot.core.meta_tasks_kine_relative import MetaTaskKine6dRel, goto6dRel, gotoNdRel
+from dynamic_graph.sot.core.matrix_util import *
+
+class MetaTaskDyn6dRel(MetaTaskKine6dRel):
+ def createTask(self):
+ self.task = TaskDynPD('task'+self.name)
+ self.task.dt.value = 1e-3
+ def createGain(self):
+ self.gain = GainAdaptive('gain'+self.name)
+ self.gain.set(1050,45,125e3)
+ def plugEverything(self):
+ self.feature.setReference(self.featureDes.name)
+ plug(self.dyn.signal(self.opPoint),self.feature.signal('position'))
+ plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq'))
+ plug(self.dyn.signal(self.opPointBase),self.feature.signal('positionRef'))
+ plug(self.dyn.signal('J'+self.opPointBase),self.feature.signal('JqRef'))
+ self.task.add(self.feature.name)
+ plug(self.dyn.velocity,self.task.qdot)
+ plug(self.task.error,self.gain.error)
+ plug(self.gain.gain,self.task.controlGain)
+
+ def __init__(self,*args):
+ MetaTaskKine6dRel.__init__(self,*args)
+
+
+ @property
+ def opmodifBase(self):
+ if not self.opPointModifBase.activ: return False
+ else: return self.opPointModifBase.getTransformation()
+
+ @opmodifBase.setter
+ def opmodifBase(self,m):
+ if isinstance(m,bool) and m==False:
+ plug(self.dyn.signal(self.opPointBase),self.feature.signal('positionRef'))
+ plug(self.dyn.signal('J'+self.opPointBase),self.feature.signal('JqRef'))
+ self.opPointModifBase.activ = False
+ else:
+ if not self.opPointModifBase.activ:
+ plug(self.opPointModifBase.signal('position'),self.feature.positionRef )
+ plug(self.opPointModifBase.signal('jacobian'),self.feature.JqRef)
+ self.opPointModifBase.setTransformation(m)
+ self.opPointModifBase.activ = True
+
+
+
+
+
--
GitLab