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 ''' '**********************************'