'read out yaml file '
'**********************************'
collision_objects = yaml_parser['collision_objects']
collision_pairs = yaml_parser['collision_pairs']
'**********************************'
'init entfcl and all pointers for possible collision pairs'
''' This is a design choice here, that the complete collision matrix
based on ALL collision_objects will be initialized.
Later, only the necessary collision pairs are plugged in the taskDAMP
This is valid, because the DG only computes the pointers which are actually
requested by some other entities.'''
'**********************************'
entFCL = DynamicGraphFCL('entFCL')
for collision in collision_objects:
entFCL.addCollisionJoint(collision)
robot.dynamic.createOpPoint(collision, collision)
robot.dynamic.signal(collision).recompute(0)
entFCL.finalizeSignals()
# enable a rostopic publisher for capusle information
#entFCL.enableCapsuleTopic(True)
# plug the forward kinematics into the input signals
for collision in collision_objects:
plug(robot.dynamic.signal(collision), entFCL.signal(collision))
# cross init signals/matrix
for i in range(len(collision_objects)):
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])
'**********************************'
collision_objects = yaml_parser['collision_objects']
collision_pairs = yaml_parser['collision_pairs']
'**********************************'
'init entfcl and all pointers for possible collision pairs'
''' This is a design choice here, that the complete collision matrix
based on ALL collision_objects will be initialized.
Later, only the necessary collision pairs are plugged in the taskDAMP
This is valid, because the DG only computes the pointers which are actually
requested by some other entities.'''
'**********************************'
entFCL = DynamicGraphFCL('entFCL')
for collision in collision_objects:
entFCL.addCollisionJoint(collision)
robot.dynamic.createOpPoint(collision, collision)
robot.dynamic.signal(collision).recompute(0)
entFCL.finalizeSignals()
# enable a rostopic publisher for capusle information
#entFCL.enableCapsuleTopic(True)
# plug the forward kinematics into the input signals
for collision in collision_objects:
plug(robot.dynamic.signal(collision), entFCL.signal(collision))
# cross init signals/matrix
for i in range(len(collision_objects)):
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])
