'**********************************'
'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))
'**********************************'
'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)):