combining multiple secondary constraint.md

Fantastic question — this is exactly where null‑space control really shines.
When a manipulator is redundant, the null‑space term can hold multiple secondary objectives simultaneously — each “competing” to influence joint motion without breaking the primary end‑effector task.

Let’s go step‑by‑step.

---

🧮 1. General Differential IK Equation

Starting from the differential inverse kinematics solution:

[ \dot{\boldsymbol{\theta}} = J^{+}\dot{\mathbf{x}}

• (I - J^{+}J)\mathbf{z} \tag{77 revisited} ]

• The first term satisfies the primary (task‑space) objective.
• The second term is free null‑space motion, (\mathbf{z}), used for secondary goals.

---

🧩 2. Single Secondary Objective Recap

For a single secondary goal (H(\boldsymbol{\theta})):

[ \mathbf{z} = -\alpha \nabla H(\boldsymbol{\theta}) ]

which yields:

[ \dot{\boldsymbol{\theta}} = J^{+}\dot{\mathbf{x}}

• \alpha (I - J^{+}J)\nabla H(\boldsymbol{\theta}) \tag{A} ]

---

⚙️ 3. Multiple Secondary Objectives

If you have several scalar cost functions:
[ H_1(\theta),; H_2(\theta),; \ldots,; H_k(\theta) ]

each representing one constraint — e.g.

• (H_1): joint‑limit avoidance
• (H_2): preferred posture
• (H_3): manipulability enhancement

You can combine them linearly into a total cost:

[ H_{total} = \sum_{i=1}^k w_i,H_i(\theta) \tag{B} ]

where (w_i > 0) are priority or weight factors (how much each secondary goal matters).

Then:

[ \nabla H_{total} = \sum_{i=1}^k w_i \nabla H_i(\theta) ]

and you simply plug this combined gradient into (A):

[ \boxed{ \dot{\theta} = J^{+}\dot{x}

• \alpha (I - J^{+}J) !! \underbrace{\sum_{i=1}^k w_i, \nabla H_i(\theta) }_{\text{weighted combined gradient}} } \tag{C} ]

---

🧠 4. Conceptual Interpretation

• The weighted sum of gradients defines a “compromise direction” in joint space.
• ( (I - J^{+}J) ) ensures that this secondary motion doesn’t disturb the primary end‑effector command.
• ( w_i ) tune how strongly each objective influences the posture.

You’re effectively performing multi‑objective optimization in the null space, subject to the primary task constraint.

---

💻 5. Example: Joint‑Limit + Posture Control Combined

Below is a direct numerical example.

import numpy as np

def truncated_pinv(J, eps=1e-4):
    U, S, Vt = np.linalg.svd(J)
    S_inv = np.array([1/s if s > eps else 0 for s in S])
    return Vt.T @ np.diag(S_inv) @ U.T

def grad_joint_limit(theta, theta_c, theta_min, theta_max):
    c = 1.0 / (theta_max - theta_min)**2
    return 2 * c * (theta - theta_c)

def grad_posture(theta, theta_ref):
    return (theta - theta_ref)

def diff_ik_multi_secondary(J, x_dot, theta, theta_c, theta_min, theta_max, theta_ref,
                            w_joint=1.0, w_posture=0.3, alpha=0.2):
    J_pinv = truncated_pinv(J)
    I = np.eye(J.shape[1])

    # compute gradients
    grad_H1 = grad_joint_limit(theta, theta_c, theta_min, theta_max)
    grad_H2 = grad_posture(theta, theta_ref)

    # weighted combination
    grad_total = w_joint * grad_H1 + w_posture * grad_H2

    q_dot = J_pinv @ x_dot - alpha * (I - J_pinv @ J) @ grad_total
    return q_dot

# Example setup
theta = np.array([0.8, -1.0, 0.5])
theta_min = np.array([-1.5, -1.0, -1.0])
theta_max = np.array([ 1.5,  1.5,  1.5])
theta_c = (theta_max + theta_min)/2
theta_ref = np.array([0.0, 0.0, 0.0])  # nominal rest pose
x_dot = np.array([0.05, 0, 0, 0, 0, 0])
J = np.random.randn(6,3)

q_dot = diff_ik_multi_secondary(J, x_dot, theta, theta_c, theta_min, theta_max, theta_ref)
print("Joint velocity (combined secondary constraints):\n", q_dot)

---

🧭 6. Physical Meaning of the Combined Behavior

• The primary term, ( J^{+}\dot{x} ), moves the end‑effector along its commanded direction.
• The joint‑limit term keeps joints away from boundaries.
• The posture term gently pulls the joints back toward their neutral configuration.
• The weights let you balance them (e.g., emphasize safety over aesthetics).

In run‑time behavior, the arm will:

• Reach toward the target,
• Stay clear of joint limits,
• Drift toward a natural pose whenever possible.

---

🧭 7. Advanced Extension: Hierarchical Null‑Space Control

If strict priority (not just weighting) is desired, you can use hierarchical projections.

For example, to ensure the second task doesn’t disturb the first secondary task either:

[ \begin{aligned} \dot{\theta} &= J_1^{+}\dot{x}_1

• (I - J_1^{+}J_1) J_2^{+} \dot{x}_2 \ &+ (I - J_1^{+}J_1)(I - J_2^{+}J_2),\mathbf{z}_3 \end{aligned} ]

This recursive projection builds strict task priorities:

• Task 1 (end‑effector) highest,
• Task 2 (e.g., posture) second,
• Task 3 (e.g., joint‑limit) third, etc.

But for most applications, the weighted‑sum approach (Eq. C) is simpler and effective.

---

✅ Summary

Concept	Formula	Effect
Weighted combination	( \dot{\theta} = J^{+}\dot{x} - \alpha (I-J^{+}J)\sum_i w_i\nabla H_i )	Blends multiple goals
Strict hierarchy	Recursive null‑space projections	Enforces strict priorities (task 1 ≫ task 2 ...)
(w_i) tuning	Adjusts influence of each secondary task	Balance between constraints

---

Would you like me to show a hierarchical two‑level numerical example, demonstrating strict priority (e.g., manipulability optimization secondary to joint‑limit avoidance)?

general differential inverse kinematics solution.md

Excellent — you’re referencing the general differential inverse kinematics solution expressed as

[ \dot{\boldsymbol{\theta}} = J^{+}\dot{\mathbf{x}} + (I - J^{+}J)\mathbf{z} \tag{77} ]

Let’s dissect this carefully — its geometric and algebraic meaning connects SVD, null-space control, and redundant manipulators.

---

🧩 1. Background: The General Solution

The basic differential kinematics equation is

[ \dot{\mathbf{x}} = J(\mathbf{q}),\dot{\boldsymbol{\theta}} ]

where

• ( \dot{\mathbf{x}} \in \mathbb{R}^m ): end-effector velocity
• ( \dot{\boldsymbol{\theta}} \in \mathbb{R}^n ): joint velocity vector
• ( J \in \mathbb{R}^{m\times n} ): manipulator Jacobian

If ( n > m ), the manipulator is redundant, and this system is underdetermined — infinitely many joint velocity vectors can produce the same end-effector velocity.

---

🧮 2. Moore–Penrose Pseudoinverse and Null Space

One particular minimal-norm solution is

[ \dot{\boldsymbol{\theta}} = J^{+}\dot{\mathbf{x}} ]

but the most general solution adds an arbitrary component in the null space of ( J ):

[ \boxed{\dot{\boldsymbol{\theta}} = J^{+}\dot{\mathbf{x}} + (I - J^{+}J)\mathbf{z}} ]

where:

• ( J^{+} ): Moore–Penrose (or truncated SVD) pseudoinverse of ( J )

• ( I ): ( n \times n ) identity

• ( (I - J^{+}J) ): projection matrix onto the null space of ( J )

• ( \mathbf{z} ): any arbitrary joint-space velocity vector (a free term) that influences joint motion but not end-effector motion.

---

📚 3. Derivation Using the SVD of ( J )

Let the Singular Value Decomposition of the Jacobian be:

[ J = U \Sigma V^T ]

where

• ( U \in \mathbb{R}^{m\times m} ): orthogonal (output space)
• ( V \in \mathbb{R}^{n\times n} ): orthogonal (joint space)
• ( \Sigma = \mathrm{diag}(\sigma_1, \dots, \sigma_r, 0, \dots, 0) ), with ( r = \text{rank}(J) )

Then the pseudoinverse is:

[ J^{+} = V \begin{bmatrix} \Sigma_r^{-1} & 0 \ 0 & 0 \end{bmatrix} U^T ]

---

Compute the Null-Space Projector

[ I - J^{+}J = I - V \begin{bmatrix} I_r & 0 \ 0 & 0 \end{bmatrix} V^T ]

Simplify:

[ \boxed{I - J^{+}J = \sum_{i=r+1}^{n} \hat{v}_i \hat{v}_i^T} ]

This is exactly what you quoted:

[ (I - J^{+}J) = \sum_{i=r+1}^{n} \hat{v}_i \hat{v}_i^T ]

where:

• ( \hat{v}_i ) are the right singular vectors (columns of ( V ))
• those corresponding to zero singular values (( \sigma_i = 0 )) span the null space of ( J )

---

🧭 4. Physical Meaning

This decomposition gives us two orthogonal parts:

Term	Subspace	Meaning
( J^{+}\dot{\mathbf{x}} )	Range of ( J^T )	Moves the end-effector where we want
( (I - J^{+}J)\mathbf{z} )	Null space of ( J )	Adjusts posture without affecting the end-effector

That is, you can superimpose additional motions (for obstacle avoidance, joint limit avoidance, secondary goals) that do not change the end-effector velocity.

---

⚙️ 5. Using Truncated SVD in This Framework

If the manipulator is near a singularity, we replace ( J^{+} ) with the truncated SVD pseudoinverse ( J_{TSVD}^{+} ):

[ J_{TSVD}^{+} = V \begin{bmatrix} \Sigma_r^{+} & 0 \ 0 & 0 \end{bmatrix} U^T ]

where only singular values ( \sigma_i > \varepsilon ) are inverted.
The truncated null-space projection becomes:

[ I - J_{TSVD}^{+}J = \sum_{i=p+1}^{n} \hat{v}_i \hat{v}_i^T ]

where ( p ) is the number of singular values retained ( ≥ threshold).

---

🧠 6. Compact Interpretation

You can visualize this solution as:

[ \boxed{\dot{\boldsymbol{\theta}} = \underbrace{J^{+}\dot{\mathbf{x}}}{\text{end-effector control}} + \underbrace{(I - J^{+}J)\mathbf{z}}{\text{null-space motion}} } ]

• The first term achieves the desired Cartesian velocity.
• The second term adds any velocity pattern that doesn’t affect the end-effector — useful for redundant motion optimization.

---

🔧 7. Implementation Outline (Python + NumPy)

Here’s how you’d implement it numerically:

import numpy as np

def truncated_pinv(J, eps=1e-3):
    U, S, Vt = np.linalg.svd(J)
    S_inv = np.array([1/s if s > eps else 0 for s in S])
    return Vt.T @ np.diag(S_inv) @ U.T, Vt.T, S

def differential_general_solution(J, x_dot, z=None, eps=1e-3):
    J_pinv, V, S = truncated_pinv(J, eps)
    I = np.eye(J.shape[1])
    N = I - J_pinv @ J   # projection matrix onto null space

    if z is None:
        z = np.zeros(J.shape[1])

    q_dot = J_pinv @ x_dot + N @ z
    return q_dot, N

This computes both
✅ the main motion term ( J^{+}\dot{x} ) and
✅ the null-space projection ( N = (I - J^{+}J) ).

You can then choose ( \mathbf{z} ) to perform secondary optimizations (e.g., minimize joint velocities, avoid obstacles).

---

✅ Summary

Symbol	Meaning
( J^{+}\dot{x} )	Primary (end-effector) velocity solution
( \mathbf{z} )	Free velocity parameter in joint space
( (I - J^{+}J) )	Projection operator onto null space
( \sum_{i=r+1}^{n} \hat{v}_i \hat{v}_i^T )	Null-space expansion in SVD basis
TSVD	Stabilizes near singularities by truncating small singular values

---

Would you like me to extend this to a Python + Sympy code example that visually demonstrates how the null-space term ((I - J^{+}J)\mathbf{z}) moves the arm without changing the end-effector position?

secondary constraint: Joint Limit Avoidance.md

Exactly — now you’re touching on redundant manipulator optimization using the null-space term in the general differential inverse kinematics solution.

Let's unpack every step carefully and make this both theoretically clear and practically computable.

---

🧮 1. The General Differential IK Solution

You already have:

[ \dot{\boldsymbol{\theta}} = J^{+} \dot{\mathbf{x}} + (I - J^{+}J)\mathbf{z} \tag{77} ]

where

• ( J^{+} \dot{\mathbf{x}} ): achieves your end-effector velocity goal,
• ( (I - J^{+}J)\mathbf{z} ): projects arbitrary joint motion into the null space (does not change end-effector motion).

---

🧠 2. Adding a Secondary Optimization: Joint Limit Avoidance

You’d like to also minimize a secondary cost function, namely joint limit avoidance:

[ H(\boldsymbol{\theta}) = \sum_{i=1}^{n} \left( \frac{\theta_i - \theta_{i,c}}{\theta_{i,\max} - \theta_{i,\min}} \right)^2 \tag{79} ]

This function is minimized when each joint ( \theta_i ) is close to its center value ( \theta_{i,c} ).
It grows as joints approach their mechanical limits.

---

🧭 3. Null-Space Gradient Descent

We want to perform gradient descent on ( H(\boldsymbol{\theta}) ), without affecting the end-effector motion.
This is done by setting the null-space motion term (( \mathbf{z} )) to drive the gradient:

[ \boxed{ \mathbf{z} = -\alpha , \nabla H(\boldsymbol{\theta}) } \tag{80} ]

where ( \alpha > 0 ) is a small positive gain.
Substituting into (77):

[ \boxed{ \dot{\boldsymbol{\theta}} = J^{+} \dot{\mathbf{x}} - \alpha (I - J^{+}J)\nabla H(\boldsymbol{\theta}) } \tag{81} ]

The first term achieves the primary task (end-effector control),
the second term uses redundancy to perform secondary optimization (joint limit avoidance).

---

🔍 4. Deriving the Gradient ( \nabla H(\boldsymbol{\theta}) )

From (79):

[ H = \sum_{i=1}^n c_i (\theta_i - \theta_{i,c})^2, \quad c_i = \frac{1}{ (\theta_{i,\max} - \theta_{i,\min})^2 } ]

Therefore:

[ \nabla H(\boldsymbol{\theta}) = \begin{bmatrix} \frac{\partial H}{\partial \theta_1} \ \vdots \ \frac{\partial H}{\partial \theta_n} \end{bmatrix} = 2 \begin{bmatrix} c_1 (\theta_1 - \theta_{1,c}) \ \vdots \ c_n (\theta_n - \theta_{n,c}) \end{bmatrix} ]

---

🧩 5. Full Algorithm (Step-by-Step)

At each time step ( k ):

1. Compute Jacobian ( J(\boldsymbol{\theta}_k) ).

2. Compute pseudoinverse ( J^{+} ) (possibly truncated SVD).

3. Compute gradient ( \nabla H(\boldsymbol{\theta}_k) ).

4. Choose gain ( \alpha > 0 ).

5. Compute the joint velocity:

   [ \dot{\boldsymbol{\theta}} = J^{+} \dot{\mathbf{x}} - \alpha (I - J^{+}J) \nabla H ]

6. Integrate forward: [ \boldsymbol{\theta}_{k+1} = \boldsymbol{\theta}_k + \dot{\boldsymbol{\theta}}, \Delta t ]

---

⚙️ 6. Python Implementation Example

Here’s a numerical demonstration using numpy — for, say, a 3-DOF redundant manipulator (6×3 Jacobian):

import numpy as np

def truncated_pinv(J, eps=1e-4):
    U, S, Vt = np.linalg.svd(J)
    S_inv = np.array([1/s if s > eps else 0 for s in S])
    return Vt.T @ np.diag(S_inv) @ U.T

def joint_limit_gradient(theta, theta_c, theta_min, theta_max):
    c = 1.0 / (theta_max - theta_min)**2
    return 2 * c * (theta - theta_c)

def diff_ik_with_joint_limit(J, x_dot, theta, theta_c, theta_min, theta_max, alpha=0.1):
    J_pinv = truncated_pinv(J)
    I = np.eye(J.shape[1])

    grad_H = joint_limit_gradient(theta, theta_c, theta_min, theta_max)
    q_dot = J_pinv @ x_dot - alpha * (I - J_pinv @ J) @ grad_H
    return q_dot

# Example setup
np.set_printoptions(precision=3, suppress=True)
theta = np.array([0.5, -0.9, 1.1])       # current joint angles
theta_min = np.array([-1.5, -1.5, -1.5])
theta_max = np.array([ 1.5,  1.5,  1.5])
theta_c = np.array([0.0, 0.0, 0.0])       # centered configuration

# Random 6x3 Jacobian
J = np.random.randn(6, 3)
x_dot = np.array([0.05, 0, 0, 0, 0, 0])  # desired Cartesian motion

q_dot = diff_ik_with_joint_limit(J, x_dot, theta, theta_c, theta_min, theta_max, alpha=0.2)

print("Joint velocity (q_dot):")
print(q_dot)

---

📈 7. Interpretation

This command:

[ q̇ = J^{+}\dot{x} - \alpha(I - J^{+}J)\nabla H ]

causes two phenomena simultaneously:

1. Primary behavior:
   Moves the end-effector in the desired Cartesian direction (x_dot).
2. Secondary behavior (null-space motion):
   Drives joints away from their physical limits toward the center of their ranges.

---

🧭 8. Notes & Practical Tips

• The secondary term acts only in the null space of ( J ) — end-effector motion is unaffected.
• You can extend ( \nabla H ) to more complex secondary goals (energy, manipulability, obstacle avoidance).
• Proper tuning of ( \alpha ) ensures smooth prioritization: too high → disrupts primary goal; too low → ineffective.
• In near-singular configurations, use truncated SVD for a stable pseudoinverse.

---

✅ Summary

Equation	Description
( H(\theta) = \sum_i \left( \frac{\theta_i - \theta_{ic}}{\theta_{i\max} - \theta_{i\min}} \right)^2 )	Joint limit cost function
( \nabla H = 2, c_i, (\theta_i - \theta_{ic}) )	Gradient (repels from limits)
( z = -\alpha \nabla H(\theta) )	Drives secondary goal
( \dot{\theta} = J^{+}\dot{x} - \alpha (I - J^{+}J)\nabla H )	Combined prioritized differential IK
( I - J^{+}J )	Projection to null space (does not affect end-effector)

---

Would you like me to extend that Python example into a simulation loop that shows joints converging toward their center positions while tracking an end-effector path?

secondary constraint: maximize manipulability .md

Excellent — that’s the natural next step.
Once you understand how joint limit avoidance works via the null-space term, you can easily add other secondary objectives that optimize robot behavior without disturbing the primary task.

Let’s look at another classic secondary constraint:

---

⚙️ Example: Maximizing Manipulability

This one’s common in robotics and humanoid control — it tries to keep the manipulator far from singularities to maintain dexterity and numerical stability.

---

🧮 1. Manipulability Measure

Yoshikawa’s manipulability index is defined as:

[ w(\boldsymbol{\theta}) = \sqrt{\det(J J^T)} ]

where ( J ) is the manipulator’s Jacobian.
Intuitively:

• ( w ) is large when the Jacobian’s singular values are all similar (i.e., nonsingular).
• ( w ) → 0 near singularities (poor dexterity).

The goal is to maximize ( w ), or equivalently minimize ( H = -w ) in the null space.

---

🧑‍🏫 2. Secondary Objective Definition

We construct an auxiliary scalar function:

[ H(\boldsymbol{\theta}) = -w(\boldsymbol{\theta}) = -\sqrt{ \det \left(J J^T \right) } ]

Then its gradient (the direction of steepest manipulability increase) is:

[ \nabla H = -\frac{1}{2w} , \nabla \det(JJ^T) ]

Computing the full analytical gradient can be complex,
but in practice we can use finite differences to approximate it numerically — which works well in control loops.

---

🧭 3. Null-Space Controller

We can plug this into the general redundant solution:

[ \boxed{ \dot{\boldsymbol{\theta}} = J^{+} \dot{\mathbf{x}}

• \alpha (I - J^{+}J) \nabla H(\boldsymbol{\theta}) } \tag{82} ]

so that the robot tracks the desired end-effector velocity while maximizing manipulability by null-space motion.

---

💻 4. Python Implementation Example

Here’s a numerical way to implement this concept:

import numpy as np

def truncated_pinv(J, eps=1e-4):
    U, S, Vt = np.linalg.svd(J)
    S_inv = np.array([1/s if s > eps else 0 for s in S])
    return Vt.T @ np.diag(S_inv) @ U.T

def manipulability(J):
    return np.sqrt(np.linalg.det(J @ J.T))

def manipulability_gradient(J_func, theta, delta=1e-5):
    """Approximate ∇w(θ) using finite differences."""
    w0 = manipulability(J_func(theta))
    grad = np.zeros_like(theta)
    for i in range(len(theta)):
        theta_pert = theta.copy()
        theta_pert[i] += delta
        w_i = manipulability(J_func(theta_pert))
        grad[i] = (w_i - w0) / delta
    return grad

def diff_ik_with_manipulability(J_func, theta, x_dot, alpha=0.2):
    J = J_func(theta)
    J_pinv = truncated_pinv(J)
    I = np.eye(J.shape[1])
    
    grad_w = manipulability_gradient(J_func, theta)
    # Minimize -w → move along +∇w
    z = alpha * grad_w
    q_dot = J_pinv @ x_dot + (I - J_pinv @ J) @ z
    return q_dot, grad_w, manipulability(J)

# ---- Example setup ----
def simple_2r_jacobian(theta):
    """Jacobian for planar 2R arm."""
    L1, L2 = 1.0, 1.0
    t1, t2 = theta
    J = np.array([
        [-L1*np.sin(t1) - L2*np.sin(t1+t2), -L2*np.sin(t1+t2)],
        [ L1*np.cos(t1) + L2*np.cos(t1+t2),  L2*np.cos(t1+t2)]
    ])
    return J

theta = np.array([0.2, 1.5])      # current joint angles
x_dot = np.array([0.0, 0.1])      # desired end-effector motion (y direction)

q_dot, grad_w, w = diff_ik_with_manipulability(simple_2r_jacobian, theta, x_dot, alpha=0.1)

print("Manipulability w:", w)
print("Gradient of w:", grad_w)
print("Joint velocity (q_dot):", q_dot)

---

🧠 5. Interpretation of the Results

• J_pinv @ x_dot drives the end-effector in the desired direction.
• (I - J_pinv @ J) @ z adds motion that increases manipulability — pushing the robot away from singularities.
• The finite-difference gradient ∇w points in the joint-space direction where manipulability improves most.

In a control loop:

1. Compute ( J(\theta) ).
2. Compute manipulability ( w ) and its gradient.
3. Compute ( \dot{\theta} ) from (82).
4. Integrate forward ( \theta_{k+1} = \theta_k + \dot{\theta} \Delta t ).

Over time, the manipulator remains dexterous without compromising the task.

---

🧭 6. Other Common Secondary Constraints

Secondary Goal	Typical Function ( H(\theta) )	Behavior
Joint limit avoidance	( \sum_i (\frac{\theta_i - \theta_{ic}}{\theta_{i\max} - \theta_{i\min}})^2 )	Keeps joints near center
Manipulability maximization	( -\sqrt{\det(JJ^T)} )	Avoids singularities
Energy minimization	( \tfrac{1}{2}\dot{\theta}^T M(\theta)\dot{\theta} )	Reduces actuator effort
Obstacle avoidance	( \sum_i 1/d_i^2 ) (using distances to obstacles (d_i))	Keeps links away from obstacles
Preferred posture maintenance	(

All of these fit into the same general framework:

[ \dot{\theta} = J^{+}\dot{x} - \alpha (I - J^{+}J)\nabla H(\theta) ]

---

Would you like me to extend this manipulability example into a multi-step simulation that shows how the manipulator keeps its dexterity while tracking a trajectory?

secondary constraint: Posture or Rest Configuration Control.md

⚙️ Example: Posture or Rest Configuration Control

This constraint tries to keep the robot’s joints close to a comfortable or nominal reference configuration, such as a home pose or energy-efficient posture—unless they need to move away to perform the primary task.

---

🧩 1. Motivation

When performing a task (like reaching for an object), a redundant robot has infinite feasible joint configurations to place its end-effector at a desired position.

Among those, you typically prefer one that is:

• Near a reference posture ( \boldsymbol{\theta}_{ref} )
• Avoids unnecessary joint bending or extreme positions
• Smoothly returns to its neutral posture when the task is finished

This can be posed as a quadratic secondary objective on joint positions.

---

🧮 2. Secondary Objective Function

Define the cost function:

[ H(\boldsymbol{\theta}) = \frac{1}{2} (\boldsymbol{\theta} - \boldsymbol{\theta}{ref})^T W (\boldsymbol{\theta} - \boldsymbol{\theta}{ref}) \tag{83} ]

Where:

• ( \boldsymbol{\theta}_{ref} ) = desired reference configuration
• ( W ) = diagonal weight matrix (determines priority for each joint; identity for uniform preference)

This ( H ) penalizes deviations from the nominal configuration.

---

🔍 3. Gradient of ( H )

[ \nabla H = W(\boldsymbol{\theta} - \boldsymbol{\theta}_{ref}) \tag{84} ]

---

⚙️ 4. Plug into the General Redundant Solution

As always, insert this into the null-space term of Eq. (77):

[ \boxed{ \dot{\boldsymbol{\theta}} = J^{+} \dot{\mathbf{x}}

• \alpha (I - J^{+}J) W (\boldsymbol{\theta} - \boldsymbol{\theta}_{ref}) } \tag{85} ]

This simultaneously drives:

• the end-effector toward the desired Cartesian motion ( \dot{\mathbf{x}} ), and
• the joints toward their preferred configuration ( \boldsymbol{\theta}_{ref} ),
  without disturbing the end-effector trajectory.

---

💻 5. Example Implementation (Python)

import numpy as np

def truncated_pinv(J, eps=1e-4):
    U, S, Vt = np.linalg.svd(J)
    S_inv = np.array([1/s if s > eps else 0 for s in S])
    return Vt.T @ np.diag(S_inv) @ U.T

def diff_ik_with_posture(J, x_dot, theta, theta_ref, alpha=0.1):
    J_pinv = truncated_pinv(J)
    I = np.eye(J.shape[1])
    W = np.eye(J.shape[1])  # Uniform weights here

    grad_H = W @ (theta - theta_ref)
    q_dot = J_pinv @ x_dot - alpha * (I - J_pinv @ J) @ grad_H
    return q_dot

# Example setup
theta = np.array([0.8, 0.2, -0.4])
theta_ref = np.array([0.0, 0.0, 0.0])
x_dot = np.array([0.05, 0.0, 0.0, 0, 0, 0])  # desired end-effector Cartesian motion
J = np.random.randn(6, 3)

q_dot = diff_ik_with_posture(J, x_dot, theta, theta_ref, alpha=0.2)
print("Joint velocity (q_dot):\n", q_dot)

---

🧠 6. Physical Interpretation

Term	Meaning
( J^{+}\dot{x} )	Moves the end-effector along a desired trajectory.
( (I - J^{+}J)W(\theta - \theta_{ref}) )	Pushes the posture toward a nominal configuration without affecting the end-effector.
( \alpha )	Balances between primary and secondary goals (small → prioritize end-effector, large → prioritize posture).

Over time, the robot performs its task while staying near its preferred pose.
This is essential for humanoid balance, repetitive industrial motions, and ergonomic path planning.

---

🚀 7. Summary of Common Secondary Goals

Secondary Constraint	Cost Function ( H(\theta) )	Behavior / Effect
Joint limit avoidance	( \sum_i \left(\frac{\theta_i - \theta_{i,c}}{\theta_{i,\max}-\theta_{i,\min}}\right)^2 )	Prevents joints from nearing their mechanical limits
Manipulability maximization	( -\sqrt{\det(JJ^T)} )	Keeps robot far from singularities
Posture maintenance (this example)	( \frac{1}{2}(\theta - \theta_{ref})^T W (\theta - \theta_{ref}) )	Keeps robot near its nominal "rest" pose
Obstacle avoidance	( \sum_i \frac{1}{d_i^2} )	Steers joints away from obstacles
Energy minimization	( \frac{1}{2}\dot{\theta}^T M(\theta)\dot{\theta} )	Minimizes kinetic or actuator effort

---

Would you like me to give a combined example, where two secondary constraints (for instance, joint limit avoidance and posture control) are both active using weighting in the null-space term?