Applying Pink to solve IK rather than differential IK

[lucasjinreal]

Hi, i compare with ikpy‘'s result, i found that the pink result is not same, and visualize seems wrong as well:

where ikpy relatively right.

this is code:

class RobotIK_Pink:
    def __init__(
        self,
        urdf_path: str,
        start_joint_positions=None,
        base_elements=["world"],
        end_link: str = None,
    ):

        # Load model
        self.model = pin.buildModelFromUrdf(urdf_path)
        self.data = self.model.createData()

        # Default initial joint positions
        if start_joint_positions is None:
            start_joint_positions = np.zeros(self.model.nq)
        self.start_q = np.array(start_joint_positions)

        # If user does not explicitly give end-effector link → auto-select last link
        if end_link is None:
            end_link = self.model.names[-1]

        self.end_link = end_link
        self.frame_id = self.model.getFrameId(end_link)

        # Pink requires a "task" to match a target pose
        frame_name = self.model.frames[self.frame_id].name
        self.task = FrameTask(
            frame=frame_name,  # ← string name of the frame
            position_cost=1.0,  # or [1.0, 1.0, 1.0] for isotropic
            orientation_cost=1.0,  # or [0.1, 0.1, 0.1] if you care less about orientation
            lm_damping=0.0,  # optional, default is fine
            gain=1.0,  # optional, default 1.0 = no filtering
        )

        self.configuration = Configuration(
            self.model,  # your pin.Model
            self.data,  # usually the same as model if no separate collision
            # visual_model,  # for visualization (Meshcat, Gepetto, etc.)
            q=pin.neutral(self.model),
        )

this is how i calculate ik:

def compute_joint_angles(
        self,
        end_effector_position,
        end_effector_orientation=None,
        initial_joint_positions=None,
        trim_base_link=False,
    ):

        if initial_joint_positions is None:
            q0 = self.start_q.copy()
        else:
            q0 = np.array(initial_joint_positions)
            if len(q0) < self.model.nq:
                q0 = np.pad(q0, (self.model.nq - len(q0), 0), "constant")
            q0 = q0[: self.model.nq]

        # Orientation handling
        if end_effector_orientation is None:
            rot = R.from_euler("xyz", [0, 0, 0]).as_matrix()
        else:
            if len(end_effector_orientation) == 3:
                rot = R.from_euler("xyz", end_effector_orientation).as_matrix()
            else:
                rot = R.from_quat(end_effector_orientation).as_matrix()

        # Compose target SE3
        target = pin.SE3(rot, np.array(end_effector_position))

        # Update IK target
        self.task.set_target(target)

        # Solve IK
        # result = solve_ik(
        #     self.model,
        #     self.task,
        #     q0,
        #     dt=1.0,  # stable optimization step
        #     max_iters=20,  # adjustable
        #     tol=1e-4,
        # )
        result = solve_ik(
            configuration=self.configuration,  # or whatever your Configuration object is called
            tasks=[self.task],  # or your list of tasks
            dt=0.001,  # pick one reasonable value, e.g. 0.005 or 0.001
            solver="osqp",  # or "quadprog" if you prefer
        )
        # print(result)
        # q_sol = result.tolist()
        q_sol = result

        if trim_base_link:
            return q_sol[1:]

        return q_sol

what am missing, thanks for any response.

[stephane-caron]

There is one problem here:

        result = solve_ik(...)
        q_sol = result

Because solve_ik solves differential IK, it returns a velocity rather than a configuration q. You can then integrate velocities over time to converge to a configuration that satisfies the task as much as possible (and it will be a local solution).

Closing this but feel free to open a follow-up discussion. (Issues are if you find a bug in the library, and discussions for everything else.)

[lucasjinreal]

@stephane-caron Hi, can u teach how to solve ik with pink, i got this code workable with pin:

def compute_joint_angles(
        self,
        end_effector_position,
        end_effector_orientation=None,
        initial_joint_positions=None,
        trim_base_link=False,
        max_iter=530,
        dt=1e-2,
        fixed_joint_indices=[0],
    ):
        q = self._ensure_start_joint_pos_right(initial_joint_positions)
        pos = np.array(end_effector_position, dtype=float)
        if end_effector_orientation is None:
            orientation_matrix = np.eye(3)
        else:
            if len(end_effector_orientation) == 3:
                orientation_matrix = R.from_euler(
                    "xyz", end_effector_orientation
                ).as_matrix()
            elif len(end_effector_orientation) == 4:
                orientation_matrix = R.from_quat(end_effector_orientation).as_matrix()
            else:
                raise ValueError(
                    "end_effector_orientation must be length 3 (euler) or 4 (quat)"
                )

        oMdes = pin.SE3(orientation_matrix, np.array(end_effector_position))

        eps = 1e-4
        IT_MAX = max_iter
        DT = dt
        damp = 1e-6

        i = 0
        while True:
            pin.forwardKinematics(self.model, self.data, q)
            iMd = self.data.oMi[self.end_link_id].actInv(oMdes)

            err = pin.log(iMd).vector

            if norm(err) < eps:
                success = True
                break
            if i >= IT_MAX:
                success = False
                break

            J = pin.computeJointJacobian(self.model, self.data, q, self.end_link_id)
            J = -np.dot(pin.Jlog6(iMd.inverse()), J)

            v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err))

            q = pin.integrate(self.model, q, v * DT)

            if not i % 80:
                print(f"{i}: err = {[round(x, 6) for x in err.T.tolist()]}")
            i += 1

        if success:
            logger.success("Convergence achieved!")
        else:
            logger.warning(
                "\n"
                "Warning: the iterative algorithm has not reached convergence "
                "to the desired precision"
            )
            
        return np.array(q.flatten())

But seems it doesn't consider the constraints.

[stephane-caron]

You can take a look at the Inverse kinematics UR10 example.

On the same topic, see also the readme paragraph on global inverse kinematics, and the related discussion here: #66

Hoping this helps!

[lucasjinreal]

Hi!

Can u tell me which qp solver is faster? I found default quadprog is relatively slow.

And when i change to other qp, it will hit limit.

[stephane-caron]

I've provided more details in this reply, but for convenience here is the key part:

When the IK QP benchmark results are ready I can share a proper quantitative comparison of QP solvers, but for the time being, in my experience those that work best for differential IK are Clarabel, ProxQP or quadprog.

[lucasjinreal]

thanks for the reply, i change the solver to clarabel in solve_ik, seems not support this solver yet.

[stephane-caron]

Clarabel should be supported. What error do you get?