diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 911c877334d009c8a76f48b8275f3bd6fbe28f52..c067ea49de13b62e85f832ef7ffa711c8eb50946 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 0000000000000000000000000000000000000000..28ed2dcb3aa52911d1373008a12a0cb705226b37 --- /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 + + + + +