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
コード例 #3
0
    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)
コード例 #4
0
	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)
コード例 #5
0
    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
'''
'**********************************'