search

uw-biorobotics / IKBT

A python package to solve robot arm inverse kinematics in symbolic form
https://www.jair.org/index.php/jair/article/view/11592
Other
177 stars 35 forks source link

dVRK PSM inverse kinematics #26

Closed mrslvg closed 5 years ago

mrslvg commented 5 years ago

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?

blake5634 commented 5 years ago

Hi Mario, I've repeated this result with latest testing branch. This is not an error but rather indicates that none of IKBT's solvers could find any equations they were able to solve right from the start. Right now it keeps futilely trying 6 times when this happens. I'm going to try to hack the BT to detect this and formally give up.
Note that I do not claim that there is no analytic solution to DvrkPSM but rather only that IKBT's existing rules can't find one.
One message that's revealing is that no equations are produced in the initial equation sets with only one unknown. Also, simultaneous equation solver (2eqn's in 2 unknowns) and x2z2 solver couldn't find relevant pairs of equations.

mrslvg commented 5 years ago

Thank you very much for your effort. Probably, a more human-readable message would be helpful in case of no solutions found. Anyway, great work!

blake5634 commented 5 years ago

The latest commit (below) in testing branch has cleaner output for the case of no solvable equations.