Hello,
I’m not 100% sure that my issue is related to this topic, but it seems like it to me.
I’m currently struggling to understand how get_inverse_kin and qnear work.
in a script I’ve got this:
textmsg("Drop_Pre_Pt1: ",Drop_Pre_Pt1)
Drop_Pre_Pt1J=get_inverse_kin(pose_trans(Uframe,Drop_Pre_Pt1))
textmsg("Drop_Pre_Pt1J[0]: ",r2d(Drop_Pre_Pt1J[0]))
textmsg("Drop_Pre_Pt1J[1]: ",r2d(Drop_Pre_Pt1J[1]))
textmsg("Drop_Pre_Pt1J[2]: ",r2d(Drop_Pre_Pt1J[2]))
textmsg("Drop_Pre_Pt1J[3]: ",r2d(Drop_Pre_Pt1J[3]))
textmsg("Drop_Pre_Pt1J[4]: ",r2d(Drop_Pre_Pt1J[4]))
textmsg("Drop_Pre_Pt1J[5]: ",r2d(Drop_Pre_Pt1J[5]))
textmsg("Drop_Pre_Pt1J: ",Drop_Pre_Pt1J)
Drop_Pre_Pt1J=get_inverse_kin(pose_trans(Uframe,Drop_Pre_Pt1),[d2r(-100),d2r(-80),d2r(-120),d2r(-70),d2r(90),d2r(-90)])
textmsg("Drop_Pre_Pt1J[0]: ",r2d(Drop_Pre_Pt1J[0]))
textmsg("Drop_Pre_Pt1J[1]: ",r2d(Drop_Pre_Pt1J[1]))
textmsg("Drop_Pre_Pt1J[2]: ",r2d(Drop_Pre_Pt1J[2]))
textmsg("Drop_Pre_Pt1J[3]: ",r2d(Drop_Pre_Pt1J[3]))
textmsg("Drop_Pre_Pt1J[4]: ",r2d(Drop_Pre_Pt1J[4]))
textmsg("Drop_Pre_Pt1J[5]: ",r2d(Drop_Pre_Pt1J[5]))
textmsg("Drop_Pre_Pt1J: ",Drop_Pre_Pt1J)
Drop_Pre_Pt1 is calculated and relative to Uframe.
I log inverse kinematic because time to time I’ve got some “weird” axes configuration, which led to collision.
On the last collision the log return this:
Drop_Pre_Pt1: p[0.308678,0.0377616,0.3,2.16857,2.273,-9.39359e-05]
#get_inverse_kin solution with no qnear parameter
Drop_Pre_Pt1J[0]: 25.3461
Drop_Pre_Pt1J[1]: 13.3889
Drop_Pre_Pt1J[2]: -99.7442
Drop_Pre_Pt1J[3]: -184.133
Drop_Pre_Pt1J[4]: 90.5411
Drop_Pre_Pt1J[5]: 23.6616
Drop_Pre_Pt1J: [0.442372,0.233681,-1.74087,-3.21372,1.58024,0.412974]
#get_inverse_kin solution with qnear parameter
Drop_Pre_Pt1J[0]: 25.3461
Drop_Pre_Pt1J[1]: 13.3889
Drop_Pre_Pt1J[2]: -99.7442
Drop_Pre_Pt1J[3]: -184.133
Drop_Pre_Pt1J[4]: 90.5411
Drop_Pre_Pt1J[5]: 23.6616
Drop_Pre_Pt1J: [0.442372,0.233681,-1.74087,-3.21372,1.58024,0.412974]
How come get_inverse_kin with qnear gives such results?
Drop_Pre_Pt1J[0]: 25.3461 qnear[0]=d2r(-100) -> more than 90° difference
Drop_Pre_Pt1J[1]: 13.3889 qnear[1]=d2r(-80) -> more than 90° difference
Drop_Pre_Pt1J[2]: -99.7442 qnear[2]=d2r(-120) -> acceptable
Drop_Pre_Pt1J[3]: -184.133 qnear[3]=d2r(-70) -> more than 90° difference
Drop_Pre_Pt1J[4]: 90.5411 qnear[4]=d2r(90) -> fine
Drop_Pre_Pt1J[5]: 23.6616 qnear[5]d2r(-90) -> more than 90° difference
Especially when a solution exist with the following axes angle really close to qnear.
Drop_Pre_Pt1J[0]: -117.25
Drop_Pre_Pt1J[1]: -75.15
Drop_Pre_Pt1J[2]: -121.52
Drop_Pre_Pt1J[3]: -73.19
Drop_Pre_Pt1J[4]: 89.15
Drop_Pre_Pt1J[5]: -119.1
Something doesn’t seem to work properly or I’m miss something?