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)
collision_objects = [
'arm_right_3_joint', 'arm_right_7_joint', 'arm_right_5_joint',
'torso_1_joint', 'arm_left_3_joint', 'arm_left_7_joint', 'arm_left_5_joint'
]
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))
