diff --git a/ikbtfunctions/ik_robots.py b/ikbtfunctions/ik_robots.py index 023cc91..be12922 100644 --- a/ikbtfunctions/ik_robots.py +++ b/ikbtfunctions/ik_robots.py @@ -34,7 +34,7 @@ def robot_params(name): pvals = {} # null for most robots List = ['ICP5p5_A21','KR16', 'UR5', - 'Puma', + 'Puma', 'Pumaoffset', 'Chair_Helper', 'Brad', 'Sims11', @@ -221,7 +221,7 @@ def robot_params(name): [ 0 , a_2, d_3 , 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 ] + [ sp.pi/2 , 0, 0, th_6 ] # d_6= 0 simplifies the math ]) vv = [1,1,1,1,1,1] @@ -230,6 +230,24 @@ def robot_params(name): pvals = {d_1:0.6,a_2:0.432, a_3:0.0203, d_3:0.1245, d_4:0.432} # meters +# The famous Puma 560 (solved in Craig) +# + if(name == 'Pumaoffset'): + dh = sp.Matrix([ + [ 0 , 0 , d_1 , th_1 ], # Note: Puma is used for tests so mods to this table + [-sp.pi/2 , 0 , 0 , th_2 ], # may break ikbtleaves.updateL.TestSolver007 + [ 0 , a_2, d_3 , th_3 ], + [-sp.pi/2 , a_3, d_4, th_4 ], + [-sp.pi/2 , 0, 0 , th_5 ], + [ sp.pi/2 , 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, a_3, d_3, d_4,d_6] + pvals = {d_1:0.6,a_2:0.432, a_3:0.0203, d_3:0.1245, d_4:0.432,d_6:0.25} # meters + + if(name == 'Chair_Helper'): vv = [0,1,1,1,1,1] # must be length 5 since 5dof and 5 unks