Redundant Robotics Kinematics

Redundancy resolution with joint limit avoidance and singularity avoidance. More...

Functions
FsbLinalgErrorType	fsb::jacobian_pseudoinverse (const Jacobian &jacobian, Jacobian &inverse_jacobian, size_t dofs)
	Computes the pseudoinverse of a Jacobian matrix.
JointSpace	fsb::compute_nullspace_motion (const Jacobian &jacobian, const Jacobian &inverse_jacobian, const JointSpace &joint_motion, size_t dofs)
	Computes nullspace motion using the Jacobian and its pseudoinverse.
JointSpace	fsb::compute_nullspace_motion (const Jacobian &jacobian, const JointSpace &joint_motion, size_t dofs)
	Computes nullspace motion by least squares solution without explicit pseudoinverse.

Detailed Description

Redundancy resolution with joint limit avoidance and singularity avoidance.

Function Documentation

compute_nullspace_motion() [1/2]

JointSpace fsb::compute_nullspace_motion	(	const Jacobian &	jacobian,
		const Jacobian &	inverse_jacobian,
		const JointSpace &	joint_motion,
		size_t	dofs
	)

Computes nullspace motion using the Jacobian and its pseudoinverse.

Projects joint motion into the nullspace, resulting in motion that doesn't affect the end-effector pose. This is useful for secondary objectives like joint limit avoidance while maintaining a primary task.

The nullspace projection is computed as:

\[ \dot{\mathbf{q}}_{null} = (\mathbf{I} - \mathbf{J}^{+} \mathbf{J}) \dot{\mathbf{q}} \]

Where:

• \( \dot{\mathbf{q}}_{null} \) is the nullspace component of joint motion
• \( \mathbf{I} \) is the identity matrix
• \( \mathbf{J}^{+} \) is the pseudoinverse of the Jacobian
• \( \mathbf{J} \) is the Jacobian matrix
• \( \dot{\mathbf{q}} \) is the input joint motion vector

Parameters

[in]	jacobian	Input Jacobian matrix \( \mathbf{J} \)
[in]	inverse_jacobian	Pseudoinverse of the Jacobian matrix \( \mathbf{J}^{+} \)
[in]	joint_motion	Input joint motion vector \( \dot{\mathbf{q}} \) to project into nullspace
[in]	dofs	Number of degrees of freedom (elements in joint velocity vector)

Returns

Joint motion vector in the nullspace \( \dot{\mathbf{q}}_{null} \)

compute_nullspace_motion() [2/2]

JointSpace fsb::compute_nullspace_motion	(	const Jacobian &	jacobian,
		const JointSpace &	joint_motion,
		size_t	dofs
	)

Computes nullspace motion by least squares solution without explicit pseudoinverse.

Parameters

[in]	jacobian	Input Jacobian matrix \( \mathbf{J} \)
[in]	joint_motion	Input joint motion vector \( \dot{\mathbf{q}} \) to project into nullspace
[in]	dofs	Number of degrees of freedom (elements in joint velocity vector)

Returns

Joint motion vector in the nullspace \( \dot{\mathbf{q}}_{null} \)

jacobian_pseudoinverse()

FsbLinalgErrorType fsb::jacobian_pseudoinverse	(	const Jacobian &	jacobian,
		Jacobian &	inverse_jacobian,
		size_t	dofs
	)

Computes the pseudoinverse of a Jacobian matrix.

Calculates the Moore-Penrose pseudoinverse \( \mathbf{J}^{+} \) of the provided Jacobian matrix using SVD decomposition. The pseudoinverse is necessary for handling redundant or underconstrained manipulators.

For a Jacobian matrix \( \mathbf{J} \), the pseudoinverse \( \mathbf{J}^{+} \) satisfies:

\[ \mathbf{J}^{+} = \mathbf{V} \mathbf{\Sigma}^{+} \mathbf{U}^T \]

where \( \mathbf{J} = \mathbf{U} \mathbf{\Sigma} \mathbf{V}^T \) is the SVD decomposition.

Parameters

[in]	jacobian	Input Jacobian matrix \( \mathbf{J} \)
[out]	inverse_jacobian	Output pseudoinverse of the Jacobian \( \mathbf{J}^{+} \)
[in]	dofs	Number of degrees of freedom (columns in Jacobian)

Returns

Linear algebra error code