search

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

IK through infinitesimal displacement twists #171

Closed jgvictores closed 5 years ago

jgvictores commented 5 years ago

Proposed here by @PeterBowman: IK through infinitesimal displacement twists (Bruyninckx, Robot Kinematics and Dynamics, 4.1.8), which somehow resemble virtual displacements, and then solve the usual delta_q = J^-1 * delta_x equation (note the deltas instead of time derivatives).

PeterBowman commented 5 years ago

Closing as invalid. Take a look at the implementation of KDL::ChainIkSolverPos_NR_JL::CartToJnt: what I wanted to achieve can be already done by instantiating our KdlSolver device with --ik nrjl --maxIter 1.

PeterBowman commented 5 years ago

My bad, the KDL implementation of NR is going to return a E_MAX_ITERATIONS_EXCEEDED error with that setup (ref). It has to be reimplemented, then, without that for-loop (and with no epsilon check). Also, the IK velocity solver might throw an error (ref) due to exceeding the maximum number of iterations in the SVD step. I wonder if this could be avoided no mather what. Beware of singular Jacobian pseudoinverses, too.

PeterBowman commented 5 years ago

@jgvictores BTW OK to rename nrjl to simply nr? (jl stands for "joint limits", which I deem an implementation detail; KDL provides another Newton-Raphson solver without taking those into account, whereas we use the full version)

jgvictores commented 5 years ago

Perfectly agree!

jgvictores commented 5 years ago

Samuel R. Buss "Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods" https://www.math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf

PeterBowman commented 5 years ago

Implemented with a Jacobian transpose per https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/171#issuecomment-453642345. The behavior is far from ideal: TEO's hands are kinda shaky in simulation, the TCP deviates 1-2 cm along a "linear" path (tested with keyboardController), and TCP-frame motion is mostly broken. @jgvictores not sure we can get any better, shall we merge this?

jgvictores commented 5 years ago

@jgvictores not sure we can get any better, shall we merge this?

Let's go!

PeterBowman commented 5 years ago

BTW OK to rename nrjl to simply nr? (jl stands for "joint limits", which I deem an implementation detail; KDL provides another Newton-Raphson solver without taking those into account, whereas we use the full version)

Reverted in https://github.com/roboticslab-uc3m/kinematics-dynamics/commit/8d7a353fbb1e585db72fdb6f16f87cae195f71df. I forgot the lmajl thing we spoke about at https://github.com/roboticslab-uc3m/kinematics-dynamics/issues/128, thus keeping the jl suffix actually makes sense.