uw-biorobotics / ikbt Goto Github PK
View Code? Open in Web Editor NEWA python package to solve robot arm inverse kinematics in symbolic form
Home Page: https://www.jair.org/index.php/jair/article/view/11592
License: Other
ikbt's People
Contributors
Stargazers
Watchers
Forkers
dianmuz riddicksky jstnhuang st-le iory ericyao2013 wright4thejob kishannagi levi-armstrong 3dprintingdoctor linsats aiyi2099 sreekanthmadhav longjie0723 boogiecan yoonlee-lab capaulson boelnasr ammydolphin mugiro justjosias mdecourse ttthoma laplacekorea 1127378627 yucnet tbazina markusbuchholz python-repository-hub gsubramani weizx208 echeng22 ochichichi lingdomuwikbt's Issues
Closed-chain kinematics
Hi, I was just wondering, if your library supports closed-chain kinematic systems? I have been looking for an automated way to calculate kinematics of a closed chain system, but cannot seem to find one.
Computation won't finish
Hi,
Thank you for your work, I really appreciate it.
Unfortunatelly, I experience issues while computing the Inverse Kinematics solver. I tried several robot which worked well as the Wrist but for the Khat6DOF, the computation stays stuck. It seems to stop computing here :
`
set_solved: th_23 by: simultaneous eqn
th_23 = atan2((-Pz + l_1)(-a_2cos(th_3) - a_3) - (a_2sin(th_3) + l_4)(-Pxcos(th_1) - Pysin(th_1) + a_1cos(th_1)), (-Pz + l_1)(a_2sin(th_3) + l_4) + (-a_2cos(th_3) - a_3)(-Pxcos(th_1) - Pysin(th_1) + a_1cos(th_1)))
atan2((-Pz + l_1)(-a_2cos(th_3) - a_3) - (a_2sin(th_3) + l_4)(-Pxcos(th_1) - Pysin(th_1) + a_1cos(th_1)), (-Pz + l_1)(a_2sin(th_3) + l_4) + (-a_2cos(th_3) - a_3)(-Pxcos(th_1) - Pysin(th_1) + a_1cos(th_1)))
`
It's still consuming about 10% of my CPU but it stayed so overnight. Do you know about it ?
Thank you.
Marc
Failed to solve UR5
I tried the ikSolver.py with UR5 and it encountered an error
NameError: global name 'find_xy' is not defined
is this something known? I did not touch the solutions just tried the Wrist and then wanted to do UR5 but it stopped after finishing FK.
Error getting index of unknown variable
Hi,
Im trying to get a custom robot's IK solution and I am running into the following error.
Starting Forward Kinematics
Completed Forward Kinematics
Starting Sum of Angles scan (slow!)
ik_classes: length Robot.mequation_list: 8
Starting sum-of-angles scan. Please be patient
[ 1%] [................................................] Sum of AnglesTraceback (most recent call last):
File "ikSolver.py", line 111, in <module>
[M, R, unknowns] = kinematics_pickle(robot, dh, params, pvals, vv, unknowns, testing)
File "/home/smaghsoudi/IKBT/ikbtbasics/ik_classes.py", line 104, in kinematics_pickle
R.sum_of_angles_transform(unks) # find sum of angles
File "/home/smaghsoudi/IKBT/ikbtbasics/ik_classes.py", line 350, in sum_of_angles_transform
rhs, newj, newe= sum_of_angles_sub(self, rhs, variables)
File "/home/smaghsoudi/IKBT/ikbtbasics/ik_classes.py", line 411, in sum_of_angles_sub
nil.append( str(get_variable_index(variables, v)) )
File "/home/smaghsoudi/IKBT/ikbtbasics/ik_classes.py", line 474, in get_variable_index
assert found, 'Error: trying to get index of an unknown joint variable' + str(symb)
AssertionError: Error: trying to get index of an unknown joint variable-th_3
I'm not sure what the issue is, it doesnt take long before this error shows up after running the solver.
Here are my dh parameters.
dh = sp.Matrix([
[ 0, 0, d_1, th_1 ],
[-sp.pi/2, a_1, 0, th_2 ],
[ sp.pi, a_2, 0, th_3 ],
[ sp.pi/2 , a_3, d_2, th_4 ],
[ sp.pi/2 , 0, d_3, th_5 ],
[-sp.pi/2 , 0, d_4, th_6 ]
])
vv = [1,1,1,1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5), unknown(th_6)]
params = [d_1, a_1, a_2, a_3, d_2, d_3, d_4]
pvals = {d_1:0.112891 , a_1:0.12893 , a_2:0.8382, a_3:0.064 , d_2:0.4615 , d_3:0.029, d_4:0.418}
Thanks!
RRR articulated manipulator
I was trying to solve a simple 3 dof arm but was coming into an issue very similar to the one with the 3 link planar arm where it wouldn't solve for theta_3
these are the DH values I used to create it in the robots file.
` if(name == 'Brad'): # 3-RRR articulated manipulator
# Brad 2019
# DOF: 3
dh = sp.Matrix([
[sp.pi/2, 0., d_1, th_1],
[0., l_2, 0., th_2],
[0., l_3, 0., th_3],
[0., 0., 0., 0.],
[0., 0., 0., 0.],
[0., 0., 0., 0.]
])
vv = [1, 1, 1, 0, 0, 0]
variables = [unknown(th_1), unknown(th_2), unknown(th_3)]
params = [d_1, l_2, l_3]
pvals = {d_1: 0.06, l_2: 0.15, l_3: 0.15}`
I'm probably missing something really simple but didn't know how else to ask for your help. thank you very much in advance.
Kinematics with wrist displacement
I am trying to find a solution for the Fanuc CRX series.
The kinematics of these robot is shown in the image below. The frames in the image are from a URDF file, not those resulting from the DH formalism.
Notable for this kinematics (like other, Kinova Jaco, GMF P150, Fanuc P250iB and Yaskawa Motoman HC10) is the wrist displacement/offset to avoid the wrist singularity. This also makes the Fanuc CRX an cuspidal robot (https://arxiv.org/pdf/2210.05204.pdf, Sec. 5.4.2).
I've made several attempts to find a solution with IKBT, playing with the modified DH parameters where it is possible, but without success.
Is it in general possible for IKBT to find a solution for this kind of kinematics?
UR5 self tests fail
When I run the self test for the UR5, I get a math domain error on line 128 coming from the asin function.
Index out of range
I try to create a simple robot but I got error
if(name == 'Contoh'): #Testing robot
dh = sp.Matrix([
[ 0 , 0 , 0 , th_1 ],
[ 0 , a_1, 0 , th_2 ],
[ 0 , a_2, 0 , th_3 ]
])
vv = [1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3)]
params = [a_1,a_2]
pvals = {a_1: .425, a_2: .392} # metersError message
kinematics pickle: trying to open fk_eqns/Contoh_pickle.p in D:\Source-Code\Robotics\IKBT
Did not find VALID stored pickle file: fk_eqns/Contoh_pickle.p
Starting Forward Kinematics
Traceback (most recent call last):
File "ikSolver.py", line 108, in <module>
[M, R, unknowns] = kinematics_pickle(robot, dh, params, pvals, vv, unknowns, testing)
File "D:\Source-Code\Robotics\IKBT\ikbtbasics\ik_classes.py", line 91, in kinematics_pickle
m.forward_kinematics()
File "D:\Source-Code\Robotics\IKBT\ikbtbasics\kin_cl.py", line 155, in forward_kinematics
self.T_34 = Link_S(self.DH[3,al], self.DH[3,a], self.DH[3,d], self.DH[3,th])
File "C:\Users\ekoru\Anaconda3\envs\ikbt\lib\site-packages\sympy\matrices\dense.py", line 122, in __getitem__
return self.extract(i, j)
File "C:\Users\ekoru\Anaconda3\envs\ikbt\lib\site-packages\sympy\matrices\common.py", line 333, in extract
rowsList = [a2idx(k, self.rows) for k in rowsList]
File "C:\Users\ekoru\Anaconda3\envs\ikbt\lib\site-packages\sympy\matrices\common.py", line 2304, in a2idx
raise IndexError("Index out of range: a[%s]" % (j,))
IndexError: Index out of range: a[3]SumOfAngles (KeyError) for custom DH table
I've defined my robot as follows :
if(name == 'WRO2018'):
sp.var('th_1 th_2 th_3 th_4 th_5')
### These somewhat wierd DH params give you the ZYX Euler Angles
# matrix of example 4.7 (don't ask how I got this!)
dh = sp.Matrix([
[ sp.pi/2, 0, l_1, th_1 ],
[ 0, l_2, 0, -(th_2 + sp.pi/2) ],
[ 0, l_3, 0, (th_3 + sp.pi/2) ], # must fill remaining rows with zeros
[ -sp.pi/2, l_4, 0, th_4 ],
[ 0 , 0, l_5, th_5 ],
[ 0 , 0, 0, 0 ]
])
vv = [1,1,1,1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5)]
params = [l_1, l_2, l_3, l_4, l_5]
pvals = {l_1:1, l_2:1, l_3:1, l_4:1, l_5:1}
But I got the error as follows
GOT HERE: robot name: WRO2018
found new 'joint' (sumofangle) variable: th_45
sumofanglesT: appending th_45 = th_4 + th_5
th_4 + th_5
Traceback (most recent call last):
File "ikSolver.py", line 324, in <module>
R.sum_of_angles_transform(unknowns) #get the sum of angle
File "/home/aniruddh/wro_2018/inverse_kinematics/IKBT/ikbtbasics/ik_classes.py", line 306, in sum_of_angles_transform
th_xy = find_xy(d[thx], d[thy])
File "/home/aniruddh/wro_2018/inverse_kinematics/IKBT/ikbtbasics/ik_classes.py", line 132, in find_xy
thx_s = set(thxy_lookup[thx])
KeyError: th_1 + th_3
I checked the #4 , but couldn't help myself.
Sum of angles cos(x-y) error
Hi @blake5634, great to see this project on github.
I tried to use it for one of the Kinova arms but got this failure:
Starting sum-of-angles scan. Please be patient [ 1%] [................................................] Sum of AnglesTraceback (most recent call last): File "ikSolver.py", line 108, in <module> [M, R, unknowns] = kinematics_pickle(robot, dh, params, pvals, vv, unknowns, testing) File "/home/fredrik/src/IKBT/ikbtbasics/ik_classes.py", line 101, in kinematics_pickle R.sum_of_angles_transform(unks) # find sum of angles File "/home/fredrik/src/IKBT/ikbtbasics/ik_classes.py", line 320, in sum_of_angles_transform rhs, newj, newe= sum_of_angles_sub(self, rhs, variables) File "/home/fredrik/src/IKBT/ikbtbasics/ik_classes.py", line 387, in sum_of_angles_sub nil.append( str(get_variable_index(variables, v)) ) File "/home/fredrik/src/IKBT/ikbtbasics/ik_classes.py", line 442, in get_variable_index assert found, 'Error: trying to get index of an unknown joint variable: ' + str(symb) AssertionError: Error: trying to get index of an unknown joint variable: -th_3
I assume it has to do with the fact the equations (which I partially solved by hand) involve cos(x-y) and thus it tries to find '-th_3' instead of 'th_3'. Would love to help fix this problem but not sure exactly where that fix would go.
if(name == 'Jaco'): # j2s6s300
dh = sp.Matrix([
[sp.pi, 0, d_0, th_1],
[sp.pi/2, 0, 0, th_2],
[sp.pi, a_2, d_2, th_3],
[sp.pi/2, 0, d_3, th_4],
[-sp.pi/2, 0, 0, th_5],
[-sp.pi/2, 0, d_5, th_6]
])
vv = [1, 1, 1, 1, 1, 1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5), unknown(th_6)]
params = [d_0, d_2, a_2, d_3, d_5]
pvals = {d_0: -0.27545, d_2: -0.0114, a_2: 0.410,
d_3: -0.31105, d_5: .10375} # meters
Interpreting outputs
I managed to get a custom robot working on branch 2dea0c6. However, I'm having a difficult time understanding the output. It seems that for a 6 DoF arm, I'm getting solution sets that contain nine parameters. For example ( a line from the file in the code gen folder):
solution_list.append( [ th_1s2, th_234s5, th_23s4, th_2s5, th_34s32, th_3s8, th_4s36, th_5s3, th_6s4, ] )
I assume th_1 refers to the variable to be solved and the s2 is solution 2 for that variable. If so, can you help me understand what variable(s) is(are) represented by th_234?
Cannot solve my 6DoF robot case
Hello, great project first! however when I try to test the code with my Dummy Robot, I cannot get the correct result, and here are my DH parameters：
if (name == 'Dummy'):
dh = sp.Matrix([
[-sp.pi / 2, 0.035, 0.109, th_1],
[0, 0.146, 0, (th_2 - sp.pi / 2)],
[sp.pi / 2, 0, 0.052, (th_3 + sp.pi / 2)],
[-sp.pi / 2, 0, 0.115, th_4],
[sp.pi / 2, 0, 0, th_5],
[0, 0, 0.072, th_6]
])
vv = [1, 1, 1, 1, 1, 1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5), unknown(th_6)]
params = []
pvals = {} # meters
which represent for:
It takes too long to come up with result output, any suggestion please?
Python 3.6 error
In Python 3.6 we get this error "working on AttributeError: module 'ikbtbasics' has no attribute 'kin_cl' ". However, with Python 3.7 and Python 3.8, I don't see this issue. Looks like there's some dependencies missing here. Thank you!
IkSolver infinite loop / hang for Khat6DOF
Running 'python ikSolver.py Khat6DOF', script appears to enter an infinite loop.
Last output:
set_solved: th_23 by: simultaneous eqn
th_23 = atan2((-Pz + l_1)*(-a_2*cos(th_3) - a_3) - (a_2*sin(th_3) + l_4)*(-Px*cos(th_1) - Py*sin(th_1) + a_1*cos(th_1)), (-Pz + l_1)*(a_2*sin(th_3) + l_4) + (-a_2*cos(th_3) - a_3)*(-Px*cos(th_1) - Py*sin(th_1) + a_1*cos(th_1)))
atan2((-Pz + l_1)*(-a_2*cos(th_3) - a_3) - (a_2*sin(th_3) + l_4)*(-Px*cos(th_1) - Py*sin(th_1) + a_1*cos(th_1)), (-Pz + l_1)*(a_2*sin(th_3) + l_4) + (-a_2*cos(th_3) - a_3)*(-Px*cos(th_1) - Py*sin(th_1) + a_1*cos(th_1)))
Followed by forty minutes of waiting until I Ctrl-C'ed out of there.
Stack trace (following Ctrl-C):
Traceback (most recent call last):
File "ikSolver.py", line 339, in <module>
ikbt.tick("Test a full solver", bb)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/behaviortree.py", line 120, in tick
state = self.root._execute(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/decorators/repeatuntilsuccess.py", line 20, in tick
status = self.child._execute(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/composites/sequence.py", line 14, in tick
status = node._execute(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/decorators/repeatuntilsuccess.py", line 20, in tick
status = self.child._execute(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/composites/sequence.py", line 14, in tick
status = node._execute(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/composites/priority.py", line 13, in tick
status = node._execute(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/composites/sequence.py", line 14, in tick
status = node._execute(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/ikbtleaves/two_eqn_m7.py", line 161, in tick
curr_unk.set_solved(R, unknowns)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/ikbtbasics/kin_cl.py", line 190, in set_solved
curr_node.detect_parent(R)
File "/Users/philippd/Google Drive/projects/flexo/IKBT/ikbtbasics/solution_graph_v2.py", line 130, in detect_parent
if par != other_par and related(par, other_par):
File "/Users/philippd/Google Drive/projects/flexo/IKBT/ikbtbasics/solution_graph_v2.py", line 75, in related
if curr == end_node:
File "/Users/philippd/Google Drive/projects/flexo/IKBT/ikbtbasics/solution_graph_v2.py", line 106, in __eq__
return self.symbol == other.symbol
File "/Users/philippd/Google Drive/projects/flexo/IKBT/lib/python2.7/site-packages/sympy/core/basic.py", line 341, in __eq__
return self._hashable_content() == other._hashable_content()
File "/Users/philippd/Google Drive/projects/flexo/IKBT/lib/python2.7/site-packages/sympy/core/symbol.py", line 252, in _hashable_content
return (self.name,) + tuple(sorted(self.assumptions0.items()))
KeyboardInterrupt
SyntaxError: Missing parentheses in call to 'print'. Did you mean print(int "")?
running code python ikSolver.py wrist throw error:
File "ikSolver.py", line 50 print "" ^ SyntaxError: Missing parentheses in call to 'print'. Did you mean print(int "")?
I'm running Win10 64bit , Python 3.6.4
Stuck in a loop while solving th_5
I tried to use the ik_solver for a 5DOF robot with given D-H parameters:
[-pi/2 0 l1 th_1]
[0 l_2 0 th_2]
[pi/2 0 0 th_3]
[-pi/2 0 0 th_4]
[0 l_4 0 th_5]
[0 0 0 0 ]
The solver finds 2 variables and then get stuck in a loop finding th_5.
Is it possible that there is simply no analytical solutions ?
Tried the program on differents robots, it works very well and output the solutions in a very pleasant way to use them.
Python 3 support?
What is the dependency stopping IKBT from moving to Python 3?
Unable to solve simple 3-link planar arm
I wanted to test the solver so I entered a basic 3-link planar arm as a custom robot:
`
if(name == 'Arm_3'):
#
dh = sp.Matrix([
[ 0 , a_1 , 0 , th_1 ],
[ 0 , a_2 , 0 , th_2 ],
[ 0 , a_3 , 0 , th_3 ],
[ 0 , 0 , 0 , 0 ],
[ 0 , 0 , 0 , 0 ],
[ 0 , 0 , 0 , 0 ]
])
vv = [1,1,1,0,0,0]
variables = [unknown(th_1), unknown(th_2), unknown(th_3)]
params = [a_1, a_2, a_3]
pvals = {a_1:1, a_2:1, a_3:1}
`
ikSolver is unable to solve for this arm. It solves for th_1 and th_2 and then gets stuck while solving for th_3. I get no error message, nothing, the terminal is just stuck. This is the output:
Running IK solution
Working on Arm_3
kinematics pickle: trying to open fk_eqns/Arm_3_pickle.p in /home/drfastolfe/IKBT
Did not find VALID stored pickle file: fk_eqns/Arm_3_pickle.p
Starting Forward Kinematics
Completed Forward Kinematics
Starting Sum of Angles scan (slow!)
ik_classes: length Robot.mequation_list: 8
Starting sum-of-angles scan. Please be patient
[ 1%] [................................................] Sum of Anglesfound new 'joint' (sumofangle) variable: (k= 0 ) th_123
sumofanglesT: appending new equation: th_123 = th_1 + th_2 + th_3
[ 4%] [==..............................................] Sum of Anglesfound new 'joint' (sumofangle) variable: (k= 0 ) th_12
sumofanglesT: appending new equation: th_12 = th_1 + th_2
[ 13%] [======..........................................] Sum of Anglesfound new 'joint' (sumofangle) variable: (k= 1 ) th_23
sumofanglesT: appending new equation: th_23 = th_2 + th_3
[100%] [================================================] Sum of Angles
Completed sum-of-angles scan.
[th_1, th_2, th_3, th_123, th_12, th_23]
[th_1, th_2, th_3, th_123, th_12, th_23]
Storing kinematics pickle for Arm_3(fk_eqns/Arm_3_pickle.p)
GOT HERE: robot name: Arm_3
Ticking IK BT for Arm_3 -------------------------
variable on blackboard: th_1
basenode: TanID+Solv ticked
tan_id: ( th_1 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: a_2 Bw: -Py + a_3*sin(th_1 + th_2)
Cw: a_2 Dw: -Px + a_1 + a_3*cos(th_12)
basenode: TanID+Solv FAIL
Simultaneous Eqn Unsuccessful: divded by 0
Looking for unknown: th_1 in equation: r_11 = cos(th_123)
Looking for unknown: th_1 in equation: r_13*cos(th_1) + r_23*sin(th_1) = 0
set_solved: th_1 by: sinANDcos
th_1 = atan2(r_13, -r_23)
- - - - -
[th_1, th_2, th_3, th_123, th_12, th_23]
Trying to find: th_1
- - - - -
atan2(r_13, -r_23)
//////////////////////// > 1 sol
curr: th_1s1
a*b
//////////////////////// > 1 sol
curr: th_1s2
a*b
finish set_solved th_1
basenode: Completion Detect ticked
Completion Detector: 6 variables, 1 are solved.
solved: th_1 (sinANDcos);
basenode: Completion Detect FAIL
variable on blackboard: th_2
set_solved: th_2 by: algebra
th_2 = zoo*(Py - a_2*sin(th_1) - a_3*sin(th_1 + th_2))
- - - - -
[th_1, th_2, th_3, th_123, th_12, th_23]
Trying to find: th_2
- - - - -
zoo*(Py - a_2*sin(th_1) - a_3*sin(th_1 + th_2))
finish set_solved th_2
basenode: Completion Detect ticked
Completion Detector: 6 variables, 2 are solved.
solved: th_1 (sinANDcos); th_2 (algebra);
basenode: Completion Detect FAIL
variable on blackboard: th_3
set_solved: th_3 by: algebra
th_3 = r_21*th_3/sin(th_1 + th_2 + th_3)
- - - - -
[th_1, th_2, th_3, th_123, th_12, th_23]
Trying to find: th_3
- - - - -
r_21*th_3/sin(th_1 + th_2 + th_3)
Syntax error in generated cpp code.
output_cpp.py contains the following code on line 106:
tmp += 'double '+str(p) + ' = XXXXX ; \\ deliberate undeclared error! USER needs to give numerical value\n'
This should instead read:
tmp += 'double '+str(p) + ' = XXXXX ; // deliberate undeclared error! USER needs to give numerical value\n'
As it is, the generated cpp code will not parse, because of the backslashes. The above fix retains the intentional undeclared error.
Implementing my own arm
Hi,
I'd like to use your inverse kinematic solver for my own robot. Here is the parameters in DH formalism.
DHRobot: KINOVA, 6 joints (PPRRRR), standard DH parameters
┏━━━━━━━┳━━━━━━━━━━┳━━━━━━┳━━━━━━━━━━━━━━━━━━━━━┳━━━━━━━━━┳━━━━━━━━┓
┃ θⱼ ┃ dⱼ ┃ aⱼ ┃ ⍺ⱼ ┃ q⁻ ┃ q⁺ ┃
┣━━━━━━━╋━━━━━━━━━━╋━━━━━━╋━━━━━━━━━━━━━━━━━━━━━╋━━━━━━━━━╋━━━━━━━━┫
┃0.0° ┃ q1 ┃ 0 ┃ -90.0° ┃ -0.33 ┃ 0.33 ┃
┃-90.0° ┃ q2 ┃ 0 ┃ 90.0° ┃ -0.33 ┃ 0.33 ┃
┃ q3 ┃ 0.1 ┃ 0 ┃ 90.0° ┃ -180.0° ┃ 180.0° ┃
┃ q4 ┃ 0.01 ┃ 0.25 ┃ 29.999999999999996° ┃ -30.0° ┃ 90.0° ┃
┃ q5 ┃ 0.07 ┃ 0 ┃ 239.99999999999997° ┃ -180.0° ┃ 180.0° ┃
┃ q6 ┃ -0.05831 ┃ 0.05 ┃ 0.0° ┃ -60.0° ┃ 30.0° ┃
┗━━━━━━━┻━━━━━━━━━━┻━━━━━━┻━━━━━━━━━━━━━━━━━━━━━┻━━━━━━━━━┻━━━━━━━━┛
│base │ t = 0, 0, 0; rpy/xyz = 0°, 90°, 0° │
│tool │ t = 0, -0.005, 0; rpy/xyz = 0°, 0°, 0° │
Is it possible to include pre-rotated bases ? Here mine is pre-rotated of Ry(90°), and is it possible to include an extra tool location ?
Finally, I tried to run it without these extra settings and it return an error:
[ 1%] [................................................] Sum of Angles: found new 'joint' (sumofangle) variable: th_56
sum_of_angles_sub: created new equation: th_56 = th_4 + th_5
: found new 'joint' (sumofangle) variable: th_456
sum_of_angles_sub: created new equation: th_456 = th_3 + th_4 + th_5
Traceback (most recent call last):
File "/home/puchaud/Projets_Python/IKBT-main/ikSolver.py", line 108, in
[M, R, unknowns] = kinematics_pickle(robot, dh, params, pvals, vv, unknowns, testing)
File "/home/puchaud/Projets_Python/IKBT-main/ikbtbasics/ik_classes.py", line 100, in kinematics_pickle
R.sum_of_angles_transform(unks) # find sum of angles
File "/home/puchaud/Projets_Python/IKBT-main/ikbtbasics/ik_classes.py", line 320, in sum_of_angles_transform
rhs, newj, newe= sum_of_angles_sub(self, rhs, variables)
File "/home/puchaud/Projets_Python/IKBT-main/ikbtbasics/ik_classes.py", line 387, in sum_of_angles_sub
nil.append( str(get_variable_index(variables, v)) )
File "/home/puchaud/Projets_Python/IKBT-main/ikbtbasics/ik_classes.py", line 442, in get_variable_index
assert found, 'Error: trying to get index of an unknown joint variable' + str(symb)
AssertionError: Error: trying to get index of an unknown joint variable-th_3
Process finished with exit code 1
Any idea on what I have done wrong ?
Regards,
URDF Files
Is there any way to initiate kinematic parameters from urdf instead of dh parameters?
Or is there any convenient way to extract dh parameters from urdf file?
find_xy error
I've tried to implement IKBT on my personal robot, but I found the following error:
Assigner: variable on blackboard: th_1
Traceback (most recent call last):
File "ikSolver.py", line 346, in <module>
ikbt.tick("Test a full solver", bb)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/behaviortree.py", line 120, in tick
state = self.root._execute(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/decorators/repeatuntilsuccess.py", line 20, in tick
status = self.child._execute(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/composites/sequence.py", line 14, in tick
status = node._execute(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/decorators/repeatuntilsuccess.py", line 20, in tick
status = self.child._execute(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/composites/sequence.py", line 14, in tick
status = node._execute(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 128, in _execute
status = self._tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/b3/core/basenode.py", line 153, in _tick
status = self.tick(tick)
File "/Users/seunghyeonbang/Repository/IKBT/ikbtleaves/sum_transform.py", line 95, in tick
th_xy = find_xy(d[thx], d[thy])
File "/Users/seunghyeonbang/Repository/IKBT/ikbtbasics/ik_classes.py", line 137, in find_xy
thx_s = set(thxy_lookup[thx])
KeyError: th_34
Before I got this error message, IBKT package did not find the function find_xy and I comment in the function definition in ik_classes.py. But, I believe it still has an issue inside of that find_xy function. Do you have any suggestion?
Solution finder does not work
Greetings,
i tried using your IKBT solver but no matter what robot example i use, i do not get any valid results.
i tried three different robots, with their DH parameters. One was from the book from our University with the following DH parameters, the robot described was supposed to be Puma-like robot.
if(name == 'PumaV2'):
dh = sp.Matrix([
[ sp.pi/2 , 0 , d_1 , th_1 ], # Note: Puma is used for tests so mods to this table
[ 0 , a_2 , 0 , th_2 ], # may break ikbtleaves.updateL.TestSolver007
[-sp.pi/2 , 0 , 0 , th_3 ],
[ sp.pi/2 , 0 , d_4 , th_4 ],
[-sp.pi/2 , 0 , 0 , th_5 ],
[ 0 , 0 , d_6 , th_6 ] ])
vv = [1,1,1,1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5), unknown(th_6)]
params = [d_1, a_2, d_4, d_6]
pvals = {d_1:0.6,a_2:0.432,d_4:0.432, d_6:0.4} # meters
this just came to the line:
Trying to find: th_12
atan2(-r_13cos(th_4) + r_33(2r_13cos(th_12) + 2r_33sin(th_12) + sin(th_4)cos(th_3)), r_13 (2r_13cos(th_12) + 2r_33sin(th_12) + sin(th_4)cos(th_3)) + r_33cos(th_4))
and then stops there (after 20+ min i stoped the code).
I also tried one of my own robots with the following DH parameters:
if(name == 'SainSmart'):
dh = sp.Matrix([
[ sp.pi/2 , a_1, d_1 , th_1 ],
[ sp.pi , a_2, 0 , th_2 ],
[sp.pi/2 , a_3, 0 , th_3 ],
[-sp.pi/2 , 0, d_4, th_4 ],
[-sp.pi/2 , 0, d_5, th_5 ],
[ 0 , 0, 0 , th_6 ] ])
vv = [1,1,1,1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5), unknown(th_6)]
params = [a_1, a_2, a_3, d_1, d_4, d_5]
pvals = {a_1:0.3, a_2:1.22, a_3:0.26, d_1:1.1, d_4:1.33, d_5:0.3} # meters `
and this does not find the solution for the theta6 joint.
Am i doing something wrong?
error on running custom Robot arm kinematics
I added my Robot DH parameters as below:
# The Amin
#
if(name == 'Amin'):
dh = sp.Matrix([
[ th_1 , sp.pi/2 , 0 , d_1 ],
[(sp.pi/2+th_2), 0 , d_2 , 0 ],
[ th_3 , 0, d_3 , 0 ],
[ (sp.pi/2+th_4) , sp.pi/2, 0 , 0 ],
[ th_5 , 0 , 0, (d_4+d_5) ],
[ 0 , 0 , 0, 0 ]
])
vv = [1,1,1,1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5)]
params = [d_1, d_2, d_3, d_4, d_5]
pvals = {d_1:1, d_2:1, d_3:1, d_4:1, d_5:1} # meters
after running python ikSolver.py Amin
C:\Users\lion\Desktop\IKBT-master>python ikSolver.py Amin
Running IK solution
Working on Amin
kinematics pickle: trying to open fk_eqns/Amin_pickle.p in C:\Users\lion\Desktop\IKBT-master
Did not find VALID stored pickle file: fk_eqns/Amin_pickle.p
Starting Forward Kinematics
Completed Forward Kinematics
Starting Sum of Angles scan (slow!)
ik_classes: length Robot.mequation_list: 6
[th_1, th_2, th_3, th_4, th_5]
[th_1, th_2, th_3, th_4, th_5]
Storing results
GOT HERE: robot name: Amin
Traceback (most recent call last):
File "ikSolver.py", line 324, in <module>
R.sum_of_angles_transform(unknowns) #get the sum of angle
File "C:\Users\lion\Desktop\IKBT-master\ikbtbasics\ik_classes.py", line 306, in sum_of_angles_transform
th_xy = find_xy(d[thx], d[thy])
File "C:\Users\lion\Desktop\IKBT-master\ikbtbasics\ik_classes.py", line 132, in find_xy
thx_s = set(thxy_lookup[thx])
KeyError: d_1 + d_4 + d_5 + th_2 + th_3 + th_4
Panda Robot IK Solutions
Thanks for a really sweet package. I have been trying to compute the analytical solution for the first 6 joints of the Franka Panda robot with DH Parameters listed here.
Adding the following to ik_robots.py:
if(name == 'Panda'):
dh = sp.Matrix([ ## This one requires sum-of-angles.
[ 0, 0, l_1, th_1],
[-sp.pi/2, 0, 0, th_2],
[ sp.pi/2, 0, l_3, th_3],
[ sp.pi/2, a_1, 0, th_4],
[-sp.pi/2, a_2, l_5, th_5],
[ sp.pi/2, 0, 0, th_6]
])
vv = [1,1,1,1,1,1]
sp.var('a_1 a_2 l_1 l_3 l_5')
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5), unknown(th_6)]
params = [a_1, a_2, l_1, l_3, l_5]
pvals = {a_1:0.0825, a_2:-0.0825, l_1:0.333, l_3:0.316, l_5:0.384}and running python3 ikSolver.py Panda results in the error:
Running IK solution
Working on Panda
kinematics pickle: trying to open fk_eqns/Panda_pickle.p in /home/alexander/work/SymbolicIK/IKBT
\Trying to read pre-computed forward kinematics from fk_eqns/Panda_pickle.p
Successfully read pre-computed forward kinematics
pickle contained 6 unknowns
GOT HERE: robot name: Panda
Ticking IK BT for Panda -------------------------
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
Assigner: variable on blackboard: th_5
Assigner: variable on blackboard: th_6
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
Assigner: variable on blackboard: th_5
Assigner: variable on blackboard: th_6
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
Assigner: variable on blackboard: th_5
Assigner: variable on blackboard: th_6
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
Assigner: variable on blackboard: th_5
Assigner: variable on blackboard: th_6
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
Assigner: variable on blackboard: th_5
Assigner: variable on blackboard: th_6
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
Assigner: variable on blackboard: th_5
Assigner: variable on blackboard: th_6
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
Assigner: variable on blackboard: th_5
Assigner: variable on blackboard: th_6
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
Assigner: variable on blackboard: th_5
Assigner: variable on blackboard: th_6
Assigner: variable on blackboard: th_1
Assigner: variable on blackboard: th_2
Assigner: variable on blackboard: th_3
x2y2 did not find suitable eqns
Assigner: variable on blackboard: th_4
[]
matching.py: bad notation collection
From a previous issue I figured this matching.py error ultimately means the solver is unable to compute a solution however I just wanted to check. Thanks!
detect_parent in set_solved
# standardize on the order "alpha N-1, a N-1, d N, theta N' for the DH table columns.
dh = sp.Matrix([
[sp.pi/2 , 0., d_1, th_1],
[0 , a_1, 0., th_2],
[0 , a_2, 0., th_3],
[-sp.pi/2, a_3, 0., th_4],
[0 , a_4, 0., th_5],
[0 , 0, 0, 0]
])
vv = [1, 1, 1, 1, 1, 0]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5)]
params = [d_1, a_1, a_2, a_3, a_4]
pvals = {d_1: 0.05, a_1: 0.247, a_2: 0.177, a_3:.085, a_4: 0.}```
bad notation collection
Hi there,
I tried to solve my own robots configuration for months and keep failing.
And i found this. Really interesting and helpful.
When i tried to solved with this, it gave a results like this:
variable on blackboard: th_1
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_2
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_3
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: -sin(th_4) Dw: r_23
basenode: TanID+Solv FAIL
x2y2 did not find suitable eqns
variable on blackboard: th_4
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_5
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: d_6
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_12
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_1
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_2
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_3
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: -sin(th_4) Dw: r_23
basenode: TanID+Solv FAIL
x2y2 did not find suitable eqns
variable on blackboard: th_4
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_5
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: d_6
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_12
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_1
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_2
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_3
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: -sin(th_4) Dw: r_23
basenode: TanID+Solv FAIL
x2y2 did not find suitable eqns
variable on blackboard: th_4
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_5
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: d_6
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_12
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_1
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_2
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_3
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: -sin(th_4) Dw: r_23
basenode: TanID+Solv FAIL
x2y2 did not find suitable eqns
variable on blackboard: th_4
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_5
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: d_6
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_12
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_1
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_2
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_3
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: -l_3 Bw: -Py + d_6*r_23 - l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: l_3 Bw: Py - d_6*r_23 + l_2
Cw: -sin(th_4) Dw: r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: sin(th_4) Dw: -r_23
tan_id: ( th_3 ) 0 = Aw*sin(th)+Bw , 0 = Cw*cos(th) + Dw
Aw: 1 Bw: r_21*sin(th_5) + r_22*cos(th_5)
Cw: -sin(th_4) Dw: r_23
basenode: TanID+Solv FAIL
x2y2 did not find suitable eqns
variable on blackboard: th_4
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_5
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: d_6
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_12
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_1
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
variable on blackboard: th_2
basenode: Algebra ID ticked
running: Algebra ID
eqns w/ one unk: 0
eqns w/ two unk: 5
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
[]
matching.py: bad notation collection
here is my robots configuration:
if(name == 'ArmRobo'):
# standardize on the order "alpha N-1, a N-1, d N, theta N' for the DH table columns.
# Olson 2013
# DOF: 6
# methods to test: m5, m3,
# Yb = d_1, Xb = d_2, L1 = l3, L2 = l4, L3 = l5
dh = sp.Matrix([
[sp.pi/2, 0., l_2, th_1],
[0, l_1, 0., th_2],
[sp.pi/2, 0., 0., (th_3-(sp.pi/2))],
[-sp.pi/2, 0., l_3, th_4],
[sp.pi/2, 0., 0., th_5],
[0, 0., d_6, 0.]
])
vv = [1, 1, 1, 1, 1, 0]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5), unknown(d_6)]
params = [l_1, l_2, l_3]
pvals = {l_1: 0.19681, l_2: 0.251, l_3:0.145423}
Big thanks for your help, really appreciate it.
SyntaxError: Missing parentheses in call to 'print' in generated Python code
A remnant of python2 syntax is in the generated code file \IKBT\CodeGen\Python\IK_equations<robot_name>.py .
[NewRobot]For ABB CRB collaborative robots, is there an analytical inverse solution?
I learned about your project from the blog. This is a very meaningful project. I tried the sample code and the output is very explanatory.
I am manually sorting out the analytical inverse kinematics recently. For the ABB CRB 15000 robot, it seems that no analytical inverse solution can be found. Did you use your script to get the solution? Does the robot have no analytical inverse solution?
https://new.abb.com/products/robotics/en/cobot/crb-15000
I used a modified DH model, I am not sure if you use this model
Thank you very much for your contribution!
th_* variables undeclared, "argument" variable multi-assigned in generated cpp code
Generated cpp code appears to assign results of multiple solutions to the same "argument" variable, and uses undeclared th_1, th_2, th_3 (etc) variables in solution calculations. The code won't compile, and is unusable.
The generated python code doesn't appear to have these issues.
My robot definition:
if(name == 'Flexo'):
# standardize on the order "alpha N-1, a N-1, d N, theta N' for the DH table columns.
a_1 = sp.symbols("a_1")
dh = sp.Matrix([
[ 0, 0, d_1 , th_1 ],
[ -sp.pi/2, a_1, 0, th_2-sp.pi/2 ],
[ 0, a_2, 0, th_3 ],
[ -sp.pi/2, a_3, d_4, th_4 ],
[ sp.pi/2, 0, 0, th_5 ],
[ -sp.pi/2, 0, 0, th_6 ]
])
vv = [1,1,1,1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5), unknown(th_6)]
params = [a_1, d_1, a_2, a_3, d_4]
# all values in mm.
pvals = {a_1: 90.11,
d_1: 140.431,
a_2: 250,
a_3: -66.306,
d_4: 342.451}
Python 3.x support?
Python 2.7 is being deprecated. Do you plan to migrate to 3.x any time soon?
calculation stuck at first unknown
The calculation was stuck in the first variable of my 5 dof robot.
the dh is as follows,
if(name == 'Robo'):
dh = sp.Matrix([
[sp.pi/2 , 0 , d_1, th_1 ],
[0 , a_1, 0, th_2 ],
[0 , a_2, 0, th_3 ],
[-sp.pi/2 , a_3, 0, th_4 ],
[0 , a_4, 0, th_5 ],
[0 , 0, 0, 0 ]
])
vv = [1,1,1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5)]
params = [d_1, a_1, a_2, a_3, a_4]
pvals = {d_1:0.05,a_1:0.247, a_2:0.177, a_3:0.085, a_4:0} # meters
7dof Support
Exciting project. I was curious if 7dof robots (or, really anything greater than 6) are supported.
I built a robot in the ik_robots.py with a theta_7 variable and d_7 parameter which both caused errors in variables being not defined. It looks like you have global variable lists in both ik_classes.py (line 40) and ikSolver.py (line 64). Those duplicated globals are probably something worth addressing in the future.
In the meantime, I added my variables and the code was able to run, but then it failed with the following error:
currently looking at: th_2 basenode: X2Y2 id and solver FAIL Traceback (most recent call last): File "ikSolver.py", line 358, in <module> final_groups = matching.matching_func(R.notation_collections, R.solution_nodes) File "/Users/mdicicco/IKBT/ikbtbasics/matching.py", line 128, in matching_func start_list = notation_d[max_len] # get lists with most variables KeyError: -1
Is this maybe another error related to the code, or is the kind of error I would expect from trying to solve a redundant manipulator?
Help: Am I doing this right?
I've defined what I think is my 6DOF robot arm (Freebody diagram here), but not 100% sure I'm putting the right DH Params in the right place. I'm also not sure that the signs are correct on all my params. For example:
- running this through ik_solver, there is no solution for θ6.
- The "Solution Sets" section of the resulting pdf/LaTeX report is empty. Not sure what this means. (There are no solutions?)
- Not sure how to specify the tool frame (As an offset from J6 frame). Or world frame (as an offset from J0 frame).
The Code:
if(name == 'Flexo'):
# standardize on the order "alpha N-1, a N-1, d N, theta N' for the DH table columns.
dh = sp.Matrix([
[-sp.pi/2, l_1, d_1, th_1],
[0, l_2, 0., th_2],
[sp.pi/2, l_3, 0., th_3],
[-sp.pi/2, 0., d_4, th_4],
[sp.pi/2, 0., 0., th_5],
[0, 0., d_6, th_6]
])
vv = [1, 1, 1, 1, 1, 1]
variables = [unknown(th_1),
unknown(th_2),
unknown(th_3),
unknown(th_4),
unknown(th_5),
unknown(th_6)]
params = [l_1, l_2, l_3, d_1, d_4, d_6]
pvals = {l_1: 90, l_2: 250, l_3: 66, d_1: 140, d_4: -340, d_6: -60} #in millimeters
dVRK PSM inverse kinematics
I am trying to get the dVRK PSM ik. This is how I defined my robot:
if(name == 'DvrkPSM'):
dh = sp.Matrix([
[ -sp.pi/2, 0 , 0 , th_1 ],
[ -sp.pi/2, 0 , 0 , th_2 ],
[ 0 , 0 , d_3 , 0 ],
[ sp.pi/2, 0 , 0 , th_4 ],
[ -sp.pi/2, a_5, 0 , th_5 ],
[ -sp.pi/2 , 0 , 0 , th_6 ]
])
vv = [1,1,0,1,1,1]
variables = [unknown(th_1), unknown(th_2), unknown(d_3), unknown(th_4), unknown(th_5), unknown(th_6)]
params = [a_5]
pvals = {a_5: .0091} # meters
This is how I run my program:
python ikSolver.py DvrkPSM
And this is the output I get:
Running IK solution
Working on DvrkPSM
kinematics pickle: trying to open fk_eqns/DvrkPSM_pickle.p in /data/work/repositories/IKBT
Did not find VALID stored pickle file: fk_eqns/DvrkPSM_pickle.p
Starting Forward Kinematics
Completed Forward Kinematics
Starting Sum of Angles scan (slow!)
ik_classes: length Robot.mequation_list: 8
Starting sum-of-angles scan. Please be patient
[100%] [================================================] Sum of Angles
Completed sum-of-angles scan.
[th_1, th_2, d_3, th_4, th_5, th_6]
[th_1, th_2, d_3, th_4, th_5, th_6]
Storing kinematics pickle for DvrkPSM(fk_eqns/DvrkPSM_pickle.p)
GOT HERE: robot name: DvrkPSM
Ticking IK BT for DvrkPSM -------------------------
Assigner: variable on blackboard: th_1
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_2
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: d_3
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_4
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_5
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_6
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_1
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_2
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: d_3
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_4
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_5
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_6
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_1
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_2
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: d_3
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_4
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_5
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_6
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_1
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_2
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: d_3
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_4
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_5
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_6
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_1
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_2
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: d_3
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_4
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_5
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_6
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_1
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_2
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: d_3
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_4
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_5
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
Assigner: variable on blackboard: th_6
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
basenode: Algebra ID ticked
algebra_id: there are no one-unknown equations
basenode: Algebra ID FAIL
basenode: TanID+Solv ticked
basenode: TanID+Solv FAIL
[]
matching.py: bad notation collection
What is wrong?
Error in Python CodeGen: global name 'acos' is not defined
If the set of solution methods contains the arccos or arcsin methods, the correct imports are not generated into the resulting python code, resulting in the below errors:
- global name 'acos' is not defined
- global name 'asin' is not defined
Sum of Angles error
I've added the following DH params to the robots file:
if(name == 'UR3'):
dh = sp.Matrix([
[0, sp.pi/2, 0, sp.pi/2],
[0, sp.pi / 2, l_1, -sp.pi / 2 + th_1],
[l_2, 0, 0, sp.pi / 2 - th_2],
[l_3, 0, 0, -th_3],
[0, sp.pi / 2, l_4, sp.pi / 2 - th_4],
[0, -sp.pi / 2, l_5, sp.pi + th_5],
])
vv = [1, 1, 1, 1, 1, 1]
variables = [unknown(th_1), unknown(th_2), unknown(th_3), unknown(th_4), unknown(th_5)]
params = [l_1, l_2, l_3, l_4, l_5]I'm not 100% sure these params are correct (and they're skipping the last joint of the real UR3 at the moment), but I figured I would give it a shot. When I run it, I get the following error:
% python ikSolver.py UR3 1 ↵ ✹ ✭
Running IK solution
Working on UR3
kinematics pickle: trying to open fk_eqns/UR3_pickle.p in /home/zplizzi/temp/IKBT
\Trying to read pre-computed forward kinematics from fk_eqns/UR3_pickle.p
Successfully read pre-computed forward kinematics
GOT HERE: robot name: UR3
SoA: found sin(th_3 + th_4 - th_5) in -sin(l_2)*sin(l_3)*sin(th_1)*cos(th_3 + th_4 - th_5) + sin(th_1)*sin(th_2)*cos(l_2)*cos(l_3)*cos(th_3 + th_4 - th_5) + sin(th_1)*sin(th_3 + th_4 - th_5)*cos(l_2)*cos(th_2) - sin(th_2)*sin(th_3 + th_4 - th_5)*cos(th_1) + cos(l_3)*cos(th_1)*cos(th_2)*cos(th_3 + th_4 - th_5)
Traceback (most recent call last):
File "ikSolver.py", line 324, in <module>
R.sum_of_angles_transform(unknowns) #get the sum of angle
File "/home/zplizzi/temp/IKBT/ikbtbasics/ik_classes.py", line 306, in sum_of_angles_transform
th_xy = find_xy(d[thx], d[thy])
File "/home/zplizzi/temp/IKBT/ikbtbasics/ik_classes.py", line 132, in find_xy
thx_s = set(thxy_lookup[thx])
KeyError: th_3 + th_4
It looks like the sum of angles rule is trying to match something like sin(x + y) but is finding instead sin(x + y - z) or something along those lines, although that's just a wild guess.
BTW this project looks awesome!
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.