diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 95f358eafb94c083d1f7cc6dabc826360dabb0cb..6d4481429b58d4babe58ae64cdafd6e30859b2e9 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -123,6 +123,102 @@ def invKinmQP(J, err_vector, lb=None, ub=None):
     qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
     return qd
 
+
+# TODO: calculate nice q (in QP) as the secondary objective
+# this requires getting the forward kinematics hessian,
+# a.k.a jacobian derivative w.r.t. joint positions  dJ/dq .
+# the ways to do it are as follows:
+#   1) shitty and sad way to do it by computing dJ/dq \cdot \dot{q}
+#      with unit velocities (qd) and then stacking that (very easy tho)
+#   2) there is a function in c++ pinocchio for it getKinematicsHessian and you could write the pybind
+#   3) you can write it yourself following peter corke's quide (he has a tutorial on fwd kinm derivatives)
+#   4) figure out what pin.computeForwardKinematicDerivatives and pin.getJointAccelerationDerivatives
+#      actually do and use that
+# HA! found it in a winter school
+# use this
+# and test it with finite differencing!
+"""
+class CostManipulability:
+    def __init__(self,jointIndex=None,frameIndex=None):
+        if frameIndex is not None:
+            jointIndex = robot.model.frames[frameIndex].parent
+        self.jointIndex = jointIndex if jointIndex is not None else robot.model.njoints-1
+    def calc(self,q):
+        J = self.J=pin.computeJointJacobian(robot.model,robot.data,q,self.jointIndex)
+        return np.sqrt(det(J@J.T))
+    def calcDiff(self,q):
+        Jp = pinv(pin.computeJointJacobian(robot.model,robot.data,q,self.jointIndex))
+        res = np.zeros(robot.model.nv)
+        v0 = np.zeros(robot.model.nv)
+        for k in range(6):
+            pin.computeForwardKinematicsDerivatives(robot.model,robot.data,q,Jp[:,k],v0)
+            JqJpk = pin.getJointVelocityDerivatives(robot.model,robot.data,self.jointIndex,pin.LOCAL)[0]
+            res += JqJpk[k,:]
+        res *= self.calc(q)
+        return res
+"""
+
+# use this as a starting point for finite differencing
+"""
+def numdiff(func, x, eps=1e-6):
+    f0 = copy.copy(func(x))
+    xe = x.copy()
+    fs = []
+    for k in range(len(x)):
+        xe[k] += eps
+        fs.append((func(xe) - f0) / eps)
+        xe[k] -= eps
+    if isinstance(f0, np.ndarray) and len(f0) > 1:
+        return np.stack(fs,axis=1)
+    else:
+        return np.matrix(fs)
+"""
+
+# and here's example usage
+"""
+# Tdiffq is used to compute the tangent application in the configuration space.
+Tdiffq = lambda f,q: Tdiff1(f,lambda q,v:pin.integrate(robot.model,q,v),robot.model.nv,q)
+c=costManipulability
+Tg = costManipulability.calcDiff(q)
+Tgn = Tdiffq(costManipulability.calc,q)
+#assert( norm(Tg-Tgn)<1e-4)
+"""
+
+
+
+def QPManipMax(J, err_vector, lb=None, ub=None):
+    """
+    invKinmQP
+    ---------
+    generic QP:
+        minimize 1/2 x^T P x + q^T x 
+        subject to
+                 G x \leq h                
+                 A x = b                   
+                 lb <= x <= ub
+    inverse kinematics QP:
+        minimize 1/2 qd^T P qd 
+                    + q^T qd (optional secondary objective)
+        subject to
+                 G qd \leq h (optional)
+                 J qd = b    (mandatory)      
+                 lb <= qd <= ub (optional)
+    """
+    P = np.eye(J.shape[1], dtype="double")
+    # secondary objective is given via q
+    # we set it to 0 here, but we should give a sane default here
+    q = np.array([0] * J.shape[1], dtype="double")
+    G = None
+    b = err_vector
+    A = J
+    # TODO: you probably want limits here
+    #lb = None
+    #ub = None
+    h = None
+    #qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos")
+    qd = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog")
+    return qd
+
 def getClikController(args):
     """
     getClikController