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.
JointSpace	fsb::joint_limit_avoidance_objective (const JointSpacePosition &joint_positions, const JointLimits &joint_limits, size_t dofs, double gain=0.1)
	Joint limit avoidance objective function.

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

joint_limit_avoidance_objective()

JointSpace fsb::joint_limit_avoidance_objective	(	const JointSpacePosition &	joint_positions,
		const JointLimits &	joint_limits,
		size_t	dofs,
		double	gain = 0.1
	)

Joint limit avoidance objective function.

Computes a joint velocity vector that drives joints away from their limits using a potential field approach. The objective is to maximize distance from joint limits.

The joint limit avoidance velocity for each joint is computed as:

\[ \dot{q}_i = k \left( \frac{1}{q_i - q_{i,min}} - \frac{1}{q_{i,max} - q_i} \right) \]

where:

• \( \dot{q}_i \) is the joint limit avoidance velocity for joint i
• \( k \) is a gain factor
• \( q_i \) is the current joint position
• \( q_{i,min} \) and \( q_{i,max} \) are the minimum and maximum joint limits

Parameters

[in]	joint_positions	Current joint positions
[in]	joint_limits	Joint limits for each joint
[in]	dofs	Number of degrees of freedom (joints)
[in]	gain	Gain factor for scaling the avoidance velocity (default: 0.1)

Returns

Joint velocity vector for joint limit avoidance