diff --git a/kine_ctrl/rm75_kine_qp.py b/kine_ctrl/rm75_kine_qp.py
index 009d241..65a7e6c 100644
--- a/kine_ctrl/rm75_kine_qp.py
+++ b/kine_ctrl/rm75_kine_qp.py
@@ -26,6 +26,15 @@ class KinematicsSolver():
 
         self.cfg_j_limit()
 
+        q_range = (
+                self.model.upperPositionLimit[:7] -
+                self.model.lowerPositionLimit[:7]
+        )
+
+        self.w_q_limit = np.diag(1.0 / (q_range ** 2))
+
+        self.q_mid = 0.5 * (self.model.lowerPositionLimit[:7] + self.model.upperPositionLimit[:7])
+
         # ----------  for reused qp_solver  ------------------
         self.nv = 7
 
@@ -51,6 +60,12 @@ class KinematicsSolver():
         )
 
         self.W = np.diag([1, 1, 1, 0.4, 0.4, 0.4])
+        # Smaller value => joint moves more actively
+        # Larger value  => joint moves less / more lazy
+        self.joint_motion_weight = np.diag([
+            1.0, 1.0, 1.0, 1.0,
+            0.3, 0.3, 0.2
+        ])
 
     def add_frame(self,frame_name, position, rotationXYZ):
         '''
@@ -255,28 +270,28 @@ class KinematicsSolver():
                 # =========================
                 # QP-based IK
                 # =========================
-                w_posture = 0.0001
+                w_ref = 0.0001
+                w_limit_mid = 0.00002
 
                 J_eff = pin.Jlog6(error_SE3) @ J #J #
 
                 H = J_eff.T @ self.W @ J_eff
 
 
-                # H = J.T @ self.W @ J
-                H += damping * damping * np.eye(7)
-                H += w_posture * np.eye(7)
+                H += damping * damping * self.joint_motion_weight
+                H += w_ref * np.eye(7)
+                H += w_limit_mid * self.w_q_limit
 
                 H_triu = sparse.triu(H).tocsc()
 
                 g = -J_eff.T @ self.W @ error_vec
-                g += w_posture * (q[:7] - q_ref[:7])
-                # g = - J.T @ self.W @ error_vec
+                g += w_ref * (q[:7] - q_ref[:7])
+                g += w_limit_mid * self.w_q_limit @ (q[:7] - self.q_mid)
 
                 # -------------------------
                 # Joint velocity constraints
                 # -------------------------
-
-                dq_limit = 0.05  # rad per iteration
+                dq_limit =  np.array([ 0.05, 0.05, 0.05, 0.05, 0.08, 0.08, 0.10 ])  # rad per iteration
 
                 lb = -dq_limit * np.ones(7)
                 ub = dq_limit * np.ones(7)