Bonjour,
J’aimerais réalisé un programme de calibration du cobot avec l’aide d’un repère physique.
J’ai réalisé un programme d’essai en utilisant les fonctions “direction” jusqu’à “contact outils”, ensuite je récupère les coordonnées des points pour ensuite effectuer un offset avec la fonction “pose_trans”.
Mais une fois que j’effectue l’offset, le plan est hors porté du cobot et je ne comprends pas où se trouve mon erreur.
You can translate if you want with google extensions
def test_force():
global _hidden_verificationVariable=0
set_target_payload(0.950000, [0.005000, -0.039000, 0.040000], [0.001413, 0.001413, 0.001413, 0.000000, 0.000000, 0.000000])
set_tcp(p[0.0,-0.055,0.14,0.0,0.0,0.0])
set_gravity([0.0, 0.0, 9.82])
set_safety_mode_transition_hardness(1)
set_standard_analog_input_domain(0, 1)
set_standard_analog_input_domain(1, 1)
set_tool_analog_input_domain(0, 1)
set_tool_analog_input_domain(1, 1)
set_analog_outputdomain(0, 0)
set_analog_outputdomain(1, 0)
set_input_actions_to_default()
set_tool_communication(False, 115200, 0, 1, 1.5, 3.5)
set_tool_output_mode(0)
set_tool_digital_output_mode(0, 1)
set_tool_digital_output_mode(1, 1)
set_tool_voltage(0)
global Pallet_1_cnt=6
global Plan_1=p[0.1331296435580705,-0.868169226499013,0.5414466022040523,-0.026350403751349852,-2.2100886491352374,2.2097378874972122]
global Plan_2=p[-0.5811237050703881,-0.11303890891523599,0.7503348138735667,-1.1538081375598757,1.2916246071485196,-1.2916445484211498]
global Plan_3=p[-0.1074386792596403,0.872619415032094,0.470772857903887,-8.391431435361943E-4,2.220723223384258,-2.220742709807844]
global Pose_plato_cart=p[0.4499628973496344,0.33541456507623196,0.047476952042385,0.011788520595514045,-0.14123614456418684,-1.5867235264270372]
global recup_carte=p[0.7198780491177688,0.061841770833425624,0.1260950803757087,4.0819941219672964E-5,2.3367727536255763E-5,-1.6087545549689768]
global recup_plato=p[0.21124180094928877,0.4522677928147516,-0.09171934518012236,-1.2110157265630829E-5,3.141565806848833,-5.8237517913496376E-5]
step_count_beb89c4e_fe97_4a74_8a82_fca6fab741e2 = 0.0
thread Step_Counter_Thread_28341fa3_f2d7_4e3d_afb8_39291027116d():
while (True):
step_count_beb89c4e_fe97_4a74_8a82_fca6fab741e2 = step_count_beb89c4e_fe97_4a74_8a82_fca6fab741e2 + 1.0
sync()
end
end
run Step_Counter_Thread_28341fa3_f2d7_4e3d_afb8_39291027116d()
global PointPassage_1_p=p[.485490348113, .106408567100, .345591807965, 2.239394263184, -2.187618073974, -.035902492588]
global PointPassage_1_q=[3.0712695121765137, -1.7217604122557582, 1.7059066931353968, -1.5291317229023953, -1.5612366835223597, -0.09381324449648076]
def calculate_point_to_move_towards(feature, direction, position_distance):
local posDir=[direction[0], direction[1], direction[2]]
if (norm(posDir) < 1e-6):
return get_target_waypoint()
end
local direction_vector_normalized=normalize(posDir)
local displacement_pose=p[direction_vector_normalized[0] * position_distance,direction_vector_normalized[1] * position_distance,direction_vector_normalized[2] * position_distance,0,0,0]
local wanted_displacement_in_base_frame=pose_sub(pose_trans(feature, displacement_pose), feature)
return pose_add(get_target_waypoint(), wanted_displacement_in_base_frame)
end
global PointPassage_2_p=p[.787139381863, .106395379473, .276564414617, 2.239293112206, -2.187606334181, -.036011811018]
global PointPassage_2_q=[3.1005823612213135, -0.9678192895701905, 0.9489725271808069, -1.5262337413481255, -1.5601580778705042, -0.06493932405580694]
global PointPassage_4_p=p[.787158245562, .106393106350, .038050253400, 2.239326347726, -2.187628157979, -.035947396437]
global PointPassage_4_q=[3.100332260131836, -0.8173595827868958, 1.3257868925677698, -2.053578039208883, -1.5602543989764612, -0.06343347231020147]
global PointPassage_5_p=p[.787136672904, .106397065853, .276559470999, 2.239281266224, -2.187601923975, -.036014634583]
global PointPassage_5_q=[3.1005871295928955, -0.9678192895701905, 0.9489815870868128, -1.5262337413481255, -1.5601652304278772, -0.06493121782411748]
$ 1 "AvantDémarrage"
$ 2 "Départ"
movej([3.1611130237579346, -1.943632265130514, 1.6774948279010218, -1.3376277548125763, -1.5664294401751917, 0.016863178461790085], a=1.3962634015954636, v=1.0471975511965976)
$ 3 "DéplacementA"
$ 4 "PointPassage_1" "breakAfter"
movej(get_inverse_kin(PointPassage_1_p, qnear=PointPassage_1_q), a=1.3962634015954636, v=1.0471975511965976)
$ 5 "DéplacementL"
$ 6 "Direction: Base Z-"
global move_thread_flag_6=0
thread move_thread_6():
enter_critical
move_thread_flag_6 = 1
local towardsPos=calculate_point_to_move_towards(p[0.0,0.0,0.0,0.0,0.0,0.0], [0.0,0.0,-1.0], 1000.0)
movel(towardsPos, a=1.2, v=0.08)
move_thread_flag_6 = 2
exit_critical
end
move_thread_flag_6 = 0
move_thread_han_6 = run move_thread_6()
while (True):
local targetTcpDirection=get_target_tcp_speed()
local stepsToRetract=tool_contact(direction=targetTcpDirection)
if (stepsToRetract > 0):
kill move_thread_han_6
stopl(3.0)
local backTrackMovement=get_actual_joint_positions_history(stepsToRetract)
local contactPose=get_forward_kin(backTrackMovement)
local posDir=[targetTcpDirection[0],targetTcpDirection[1],targetTcpDirection[2]]
local retractTo=contactPose
if (norm(posDir) > 1e-6):
local normalizedPosDir=normalize(posDir)
local additionalRetraction=p[normalizedPosDir[0] * 0.0, normalizedPosDir[1] * 0.0, normalizedPosDir[2] * 0.0, 0, 0, 0]
retractTo = pose_sub(contactPose, additionalRetraction)
end
movel(retractTo, a=3.0, v=0.1)
$ 7 "Until (tool_contact_detection)"
break
end
sync()
end
$ 8 "Direction: Base Y+"
global move_thread_flag_8=0
thread move_thread_8():
enter_critical
move_thread_flag_8 = 1
local towardsPos=calculate_point_to_move_towards(p[0.0,0.0,0.0,0.0,0.0,0.0], [0.0,1.0,0.0], 1000.0)
movel(towardsPos, a=1.2, v=0.08)
move_thread_flag_8 = 2
exit_critical
end
move_thread_flag_8 = 0
move_thread_han_8 = run move_thread_8()
while (True):
local targetTcpDirection=get_target_tcp_speed()
local stepsToRetract=tool_contact(direction=targetTcpDirection)
if (stepsToRetract > 0):
kill move_thread_han_8
stopl(3.0)
local backTrackMovement=get_actual_joint_positions_history(stepsToRetract)
local contactPose=get_forward_kin(backTrackMovement)
local posDir=[targetTcpDirection[0],targetTcpDirection[1],targetTcpDirection[2]]
local retractTo=contactPose
if (norm(posDir) > 1e-6):
local normalizedPosDir=normalize(posDir)
local additionalRetraction=p[normalizedPosDir[0] * 0.0, normalizedPosDir[1] * 0.0, normalizedPosDir[2] * 0.0, 0, 0, 0]
retractTo = pose_sub(contactPose, additionalRetraction)
end
movel(retractTo, a=3.0, v=0.1)
$ 9 "Until (tool_contact_detection)"
break
end
sync()
end
$ 10 "Direction: Base X+"
global move_thread_flag_10=0
thread move_thread_10():
enter_critical
move_thread_flag_10 = 1
local towardsPos=calculate_point_to_move_towards(p[0.0,0.0,0.0,0.0,0.0,0.0], [1.0,0.0,0.0], 1000.0)
movel(towardsPos, a=1.2, v=0.08)
move_thread_flag_10 = 2
exit_critical
end
move_thread_flag_10 = 0
move_thread_han_10 = run move_thread_10()
while (True):
local targetTcpDirection=get_target_tcp_speed()
local stepsToRetract=tool_contact(direction=targetTcpDirection)
if (stepsToRetract > 0):
kill move_thread_han_10
stopl(3.0)
local backTrackMovement=get_actual_joint_positions_history(stepsToRetract)
local contactPose=get_forward_kin(backTrackMovement)
local posDir=[targetTcpDirection[0],targetTcpDirection[1],targetTcpDirection[2]]
local retractTo=contactPose
if (norm(posDir) > 1e-6):
local normalizedPosDir=normalize(posDir)
local additionalRetraction=p[normalizedPosDir[0] * 0.0, normalizedPosDir[1] * 0.0, normalizedPosDir[2] * 0.0, 0, 0, 0]
retractTo = pose_sub(contactPose, additionalRetraction)
end
movel(retractTo, a=3.0, v=0.1)
$ 11 "Until (tool_contact_detection)"
break
end
sync()
end
$ 12 "posi≔get_actual_tcp_pose()"
global posi= get_actual_tcp_pose ()
$ 17 "pos_calibre≔p[posi[0],posi[1],posi[2],0,0,0]"
global pos_calibre=p[posi[0],posi[1],posi[2],0,0,0]
$ 18 "Pop-up"
popup(str_cat("",pos_calibre), "Message", False, False, blocking=True)
$ 19 "recup_carte≔pose_trans(recup_carte, pos_calibre)"
global recup_carte= pose_trans (recup_carte, pos_calibre)
$ 20 "DéplacementA"
$ 21 "recup_carte" "breakAfter"
movej(recup_carte, a=1.3962634015954636, v=1.0471975511965976)
$ 36 "Thread_1"
thread Thread_1():
while (True):
$ 37 "capt_force≔force()"
global capt_force= force ()
$ 38 "position_actuel≔get_actual_tcp_pose()"
global position_actuel= get_actual_tcp_pose ()
$ 39 "sync()"
sync()
end
end
threadId_Thread_1 = run Thread_1()
while (True):
$ 22 "Programme de robot"
$ 23 "DéplacementL"
$ 24 "PointPassage_2" "breakAfter"
movel(pose_trans(recup_carte, pose_trans(p[.089121558906, -.717014070189, -.137084069729, -.000040819941, -.000023367728, 1.608754554969], PointPassage_2_p)), a=1.2, v=0.25)
$ 25 "PointPassage_4" "breakAfter"
movel(pose_trans(recup_carte, pose_trans(p[.089121558906, -.717014070189, -.137084069729, -.000040819941, -.000023367728, 1.608754554969], PointPassage_4_p)), a=1.2, v=0.25)
$ 27 "Attendre: 0.5"
sleep(0.5)
$ 28 "Pop-up: recup carte"
popup("recup carte", "Message", False, False, blocking=True)
$ 29 "DéplacementL"
$ 30 "PointPassage_5" "breakAfter"
movel(PointPassage_5_p, a=1.2, v=0.25)
$ 31 "Régler Aspiration=Off"
set_standard_digital_out(1, False)
$ 32 "Régler Souflette=HI:Pulse 0.1"
thread SinglePulse50():
set_standard_digital_out(0, True)
sleep(0.1)
set_standard_digital_out(0, False)
end
singlePulse50 = run SinglePulse50()
$ 34 "Départ"
movej([3.1611130237579346, -1.943632265130514, 1.6774948279010218, -1.3376277548125763, -1.5664294401751917, 0.016863178461790085], a=1.3962634015954636, v=1.0471975511965976)
$ 35 "Stop"
halt
end
end
Bonjour,
Actuellement, je recherche un moyen de calibrer mon robot après un changement d’articulation.
Je voulais savoir si vous avez obtenu des résultats concluants concernant votre programme lié à un repère physique ?
Merci