Review geometric solution of IK in AsibotSolver

roboticslab-uc3m / kinematics-dynamics

Kinematics and dynamics solvers and controllers.

https://robots.uc3m.es/kinematics-dynamics/

GNU Lesser General Public License v2.1

19 stars 12 forks source link

Review geometric solution of IK in AsibotSolver #139

Closed PeterBowman closed 6 years ago

PeterBowman commented 6 years ago

The following cosine (ct2) in AsibotSolver::invKin may return values larger than 1.0, leading to a NaN in st2 and propagating to subsequent operations:

https://github.com/roboticslab-uc3m/kinematics-dynamics/blob/5913c9f4e02ad52ed6f6095a40e929d89adf533f/libraries/YarpPlugins/AsibotSolver/ICartesianSolverImpl.cpp#L175-L176

Steps to reproduce this (on simulation):

move to cartesian-space position 0.45 0.0 0.8 0.0 1.55 0.0
try movj 0.45 0.0 1.1 0.0 1.55 0.0

PeterBowman commented 6 years ago

Just a mere reachability issue solved at fae5678. Works fine with x = 1.09 (instead of 1.1). Perhaps a previous, apparently successful movl call fooled me to think that the robot actually reached that position, but I never checked the decimals. Idea: check final pose before finalizing a movl sequence?

Comment: singularities will be tracked at #109.