]
entFCL = DynamicGraphFCL('entFCL')
# setup entity for FCL
collision_string = ''
for collision in collision_objects:
collision_string += collision + str(':')
collision_string = collision_string[:-1]
print('setting collision objects: ' + str(collision_string))
entFCL.set_collision_joints(collision_string)
# plugging FK into FCL
for collision in collision_objects:
robot.dynamic.createOpPoint(collision, collision)
robot.dynamic.signal(collision).recompute(0)
plug(robot.dynamic.signal(collision), entFCL.signal(collision))
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])
entFCL.signal(signal_name).recompute(0)
'''
TORSO TEST
DYNOP FOR CLOSEST POINT CALUCLATION AND JACOBIAN TWIST
'''
# just for torso test!
#arm right
dynOP7right = DynamicOppointModifier('arm_right_7_joint')
plug(robot.dynamic.arm_right_7_joint, dynOP7right.positionIN)
plug(robot.dynamic.Jarm_right_7_joint, dynOP7right.jacobianIN)
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])
entFCL.signal(signal_name).recompute(0)
'**********************************'
'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
