def createOpPointModif(self):
self.opPointModif = DynamicOppointModifier('dynopmodif' + self.name)
offset = numpy.eye(4)
offset[0:3, 3] = (0.0, 0.0, 0.0)
self.opPointModif.setTransformation(matrixToTuple(offset))
plug(self.dyn.signal(self.opPoint),
self.opPointModif.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPoint),
self.opPointModif.signal('jacobianIN'))
self.opPointModif.activ = True
class MetaTaskOpModVelocityDamping(MetaTaskVelocityDamping):
def createOpPointModif(self):
self.opPointModif = DynamicOppointModifier('dynopmodif' + self.name)
offset = numpy.eye(4)
offset[0:3, 3] = (0.0, 0.0, 0.0)
self.opPointModif.setTransformation(matrixToTuple(offset))
plug(self.dyn.signal(self.opPoint),
self.opPointModif.signal('positionIN'))
plug(self.dyn.signal('J' + self.opPoint),
self.opPointModif.signal('jacobianIN'))
self.opPointModif.activ = True
def plugEverything(self):
plug(self.opPointModif.signal('position'), self.task.p1)
plug(self.opPointModif.signal('jacobian'), self.task.jVel)
self.task.di.value = self._di
self.task.ds.value = self._ds
print type(self.task)
#self.task.controlGain.value = self.controlGain
self.task.dt.value = 0.001
#p1 is the moving point, p2 considered to be static
def __init__(self,
name,
dynamic,
opPoint,
opPointRef,
di,
ds,
controlGain=1,
collisionCenter=None):
self.name = 'taskVelocity' + str(name)
self.dyn = dynamic
self.collisionCenter = collisionCenter
self._ds = ds
self._di = di
self.controlGain = controlGain
self.createTask()
self.createOpPoint(opPoint, opPointRef)
self.createOpPointModif()
self.plugEverything()
@property
def opmodif(self):
if not self.opPointModif.activ:
return False
else:
return self.opPointModif.getTransformation()
@opmodif.setter
def opmodif(self, m):
# print 'opmodif m value = ', m
# opmodif m equals to input matrix
if isinstance(m, bool) and m == False:
plug(self.dyn.signal(self.opPoint), self.task.p1)
plug(self.dyn.signal('J' + self.opPoint), self.task.jVel)
self.opPointModif.activ = False
else:
if not self.opPointModif.activ:
plug(self.opPointModif.signal('position'), self.task.p1)
plug(self.opPointModif.signal('jacobian'), self.task.jVel)
self.opPointModif.setTransformation(m)
self.opPointModif.activ = True
taskRW.task.controlGain.value = gainvalue
taskLW.task.controlGain.value = gainvalue
def avoidMarker():
createRosExport('matrixHomoStamped', taskDAMPstatic.task.p2_arm_right_tool,
'/aruco_single/transform')
def basicStack():
# Basic stack
push(taskJL)
solver.addContact(taskBASE)
dynTest = DynamicOppointModifier('bla')
staticOp = DynamicOppointModifier('static')
robot.dynamic.createOpPoint('arm_right_tool_joint', 'arm_right_tool_joint')
plug(robot.dynamic.arm_right_tool_joint, staticOp.positionIN)
plug(robot.dynamic.Jarm_right_tool_joint, staticOp.jacobianIN)
staticOp.transformationSig.value = (0, 0, 0)
taskDAMPstatic = MetaTaskDynamicVelocityDamping('velDampstatic', 0.1, 0.07)
taskDAMPstatic.task.set_avoiding_objects('arm_right_tool')
plug(robot.dynamic.arm_right_tool_joint, taskDAMPstatic.task.p1_arm_right_tool)
plug(robot.dynamic.Jarm_right_tool_joint,
taskDAMPstatic.task.jVel_arm_right_tool)
collision_point = (0.2, -0.2, 1.1)
p2_point = goalDef(collision_point)
taskDAMPstatic.task.p2_arm_right_tool.value = matrixToTuple(p2_point)
for j in range(len(collision_objects)):
if not(collision_objects[i] == collision_objects[j]):
signal_name = str(collision_objects[i])+str(collision_objects[j])
entFCL.signal(signal_name).recompute(0)
'''
TORSO TEST
DYNOP FOR CLOSEST POINT CALUCLATION AND JACOBIAN TWIST
'''
# just for torso test!
#arm right
arm_right_group = ['arm_right_3_joint', 'arm_right_7_joint', 'arm_right_5_joint']
dynOP_right_collection = []
for arm_right_part in arm_right_group:
dynOPright = DynamicOppointModifier(arm_right_part)
plug(robot.dynamic.getSignal(arm_right_part), dynOPright.positionIN)
plug(robot.dynamic.getSignal('J'+str(arm_right_part)), dynOPright.jacobianIN)
plug(entFCL.getSignal('oppoint_'+str(arm_right_part)+'torso_1_joint'), dynOPright.transformationSig)
dynOP_right_collection.append(dynOPright)
arm_left_group = ['arm_left_3_joint', 'arm_left_7_joint', 'arm_left_5_joint']
dynOP_left_collection = []
for arm_left_part in arm_left_group:
dynOPleft = DynamicOppointModifier(arm_left_part)
plug(robot.dynamic.getSignal(arm_left_part), dynOPleft.positionIN)
plug(robot.dynamic.getSignal('J'+str(arm_left_part)), dynOPleft.jacobianIN)
plug(entFCL.getSignal('oppoint_'+str(arm_left_part)+'torso_1_joint'), dynOPleft.transformationSig)
dynOP_left_collection.append(dynOPleft)
collision_point = (0.2, -0.2, 1.1)
p2_point = goalDef(collision_point)
taskDAMP.task.p2_arm_right_tool.value = matrixToTuple(p2_point)
# necessary for a simple vector creation
dynTest.transformationSig.value = collision_point
createRosImport(
'vector3', dynTest.transformationSig,
'rviz_marker_closest_points/avoid_torso_1_jointarm_right_7_joint')
createRosImport('double', taskDAMP.task.ds,
'rviz_marker_closest_points/ds')
createRosImport('double', taskDAMP.task.di,
'rviz_marker_closest_points/di')
dynTest7 = DynamicOppointModifier('arm7')
dynTest5 = DynamicOppointModifier('arm5')
staticOp = DynamicOppointModifier('static')
taskDAMP = MetaTaskDynamicVelocityDamping('velDamp', 0.1, 0.07)
# Create basic tasks
taskRW = createEqualityTask('rightWrist', 'arm_right_tool_joint')
taskLW = createEqualityTask('leftWrist', 'arm_left_tool_joint')
taskBASE = createEqualityTask('baseContact', 'base_joint', 10)
taskJL = createJointLimitsTask(500)
safePos = safePosition()
diag = (0, 0, 0, 0, 0, 0, 1.36499, 1.49997, 0.677897, 0.636507, 0.170576,
0.234007, 0.120986, 0.0156722, 0.0213592, 0.687228, 0.63189, 0.172151,
0.260947, 0.120986, 0.0156722, 0.0213592, 0.511255, 0.520094)
taskWEIGHTS = createWeightsTask(diag)
basicStack()
'init oppoint modifiers'
''' this is going to be necessary for each active moving collision point (namely link1)
for the collision avoidance task we need a jacobian at the closest point
rather than at the link-origin. This being sad, the entfcl bring out oppoint_<name> signals
which are relative points of the closest point pairs w.r.t the link origin
dyanamic op point modifier takes this relative point as a transformation and outputs the according jacobian
Note that we only need link1->link2 for the jacobian, we don't need the counter part jacobian link2->link1
'''
'**********************************'
dynOP_collection = []
idx = 1
for pair in collision_pairs:
link1 = pair['link1']
link2 = pair['link2']
dynOP = DynamicOppointModifier('dynop' + str(idx))
plug(robot.dynamic.signal(link1), dynOP.positionIN)
plug(robot.dynamic.signal('J' + link1), dynOP.jacobianIN)
plug(entFCL.signal('oppoint_' + link1 + link2), dynOP.transformationSig)
dynOP_collection.append(dynOP)
idx += 1
'**********************************'
'init taskDAMP for actual collision avoidance '
''' for each collision pair we init the following signals
- p1_<link1link2> = considered as the moving part for which the jacobian has to be computed
- p2_<link1link2> = considered as the static part which has to be avoided
- ds_<link1link2> = security distance between p1 and p2 which is not allowed to get violated
- di_<link1link2> = influence distance on which the inequality becomes part of the active search set
'''
'**********************************'
