meta_tasks_kine_relative.py 4.86 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
from dynamic_graph.sot.core.meta_task_6d import MetaTask6d
from dynamic_graph.sot.core.meta_tasks import setGain,generic6dReference
from dynamic_graph.sot.core import *
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core.feature_point6d_relative import FeaturePoint6dRelative
from dynamic_graph import plug

class MetaTaskKine6dRel( MetaTask6d ):

	opPointBase=''


	def createOpPointBase(self,opPointBase,opPointRefBase = 'left-wrist'):
		self.opPointBase=opPointBase
		if self.opPointExist(opPointBase): return
		self.dyn.createOpPoint(opPointBase,opPointRefBase)
	def createOpPointModifBase(self):
		self.opPointModifBase = OpPointModifier('opmodifBase'+self.name)
		plug(self.dyn.signal(self.opPointBase),self.opPointModifBase.signal('positionIN'))
		plug(self.dyn.signal('J'+self.opPointBase),self.opPointModifBase.signal('jacobianIN'))
		self.opPointModifBase.activ = False

	def createFeatures(self):
		self.feature    = FeaturePoint6dRelative('featureRel'+self.name)
		self.featureDes = FeaturePoint6dRelative('featureRel'+self.name+'_ref')
		self.feature.selec.value = '111111'
		self.feature.frame('current')

	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.task.error,self.gain.error)
		plug(self.gain.gain,self.task.controlGain)
	def keep(self):
		self.feature.position.recompute(self.dyn.position.time)
		self.feature.keep()

	def __init__(self,name,dyn,opPoint,opPointBase,opPointRef='right-wrist',opPointRefBase='left-wrist'):
		self.name=name
		self.defineDynEntities(dyn)
		self.createOpPoint(opPoint,opPointRef)
		self.createOpPointBase(opPointBase,opPointRefBase)
		self.createOpPointModif()
		self.createOpPointModifBase()
		self.createFeatures()
		self.createTask()
		self.createGain()
		self.plugEverything()




	@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

# --- HELPER FUNCTIONS ---------------------------------------------------------

def goto6dRel(task,position,positionRef,gain=None,resetJacobian=True):
	M=generic6dReference(position)
	MRef=generic6dReference(positionRef)
	task.featureDes.position.value = matrixToTuple(M)
	task.featureDes.positionRef.value = matrixToTuple(MRef)
	task.feature.selec.value = "111111"
	setGain(task.gain,gain)
	if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
		task.task.resetJacobianDerivative()

def gotoNdRel(task,position,positionRef,selec=None,gain=None,resetJacobian=True):
	M=generic6dReference(position)
	MRef=generic6dReference(positionRef)
	if selec!=None:
		if isinstance(selec,str):   task.feature.selec.value = selec
		else: task.feature.selec.value = toFlags(selec)
	task.featureDes.position.value = matrixToTuple(M)
	task.featureDes.positionRef.value = matrixToTuple(MRef)
	setGain(task.gain,gain)
	if 'resetJacobianDerivative' in task.task.__class__.__dict__.keys() and resetJacobian:
		task.task.resetJacobianDerivative()


"""

Documentation

Inherited from MetaTask6d.

The aim of this MetaTask is to give a simple and immediate interface to implement a relative task between two operational points of the robot. The new variable "opPointBase" represents in fact the second operational point (the first is inherited from the father class).

It's been decided to reuse (so not to redefine) all methodes from MetaTask6d related to the opPoint to implement the behaviour of one of the two points (called "Other" from now on) and to reimplement in a intuitive way the same functions for the second point ("Ref").


Utilization

It should be noticed that both feature and reference are defined as a couple of signals, while normally it would be enough define the reference as one signal that represents the diplacement between the two positions. Nevertheless this redundant approach allows to a very intuitive and safe usage of the class because the references can be set just using the current position of the two operational points. 
For this reason all the goTo functions have been redefined.

"""