Function pfield::franka_ik_swivel

Function Documentation

unsigned int pfield::franka_ik_swivel(const array<double, 3> &r, const array<double, 9> &ROE, const double theta, array<array<double, 7>, 8> &qsols, const double q1_sing = M_PI_2, const unsigned int n_points = 500, const unsigned int n_fine_search = 3)

IK with swivel angle as free variable (numerical).

Parameters:

  • r – position of frame E with respect to frame O.

  • ROE – rotation matrix of frame E with respect to frame O (row-first format).

  • theta – swivel angle in radians (see paper for geometric defninition)

  • qsols – array to store 8 solutions

  • q1_sing – [optional] emergency value of q1 in case of singularity at shoulder joints (type-1 singularity).

  • n_points – [optional] number of points to discretise the range of q7.

Returns:

number of solutions found.