'**********************************' '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)):
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) 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))
createRosExport('vector3Stamped',taskGAZE.proj.point3D,'/aruco_single/position') def gotoMarker(): createRosExport('matrixHomoStamped',taskRW.featureDes.position,'/aruco_single/pose') def speedUp(gainvalue): taskRW.task.controlGain.value = gainvalue taskLW.task.controlGain.value = gainvalue def basicStack(): # Basic stack push(taskJL) solver.addContact(taskBASE) 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', 'torso_2_joint', 'head_1_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))
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))