Inverse kinematics UR10 example not working after switching to a different robot

[lucasjinreal]

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
#
# SPDX-License-Identifier: Apache-2.0
#
# /// script
# dependencies = ["daqp", "pin-pink", "qpsolvers", "robot_descriptions"]
# ///

"""Move the end-effector of a UR10 arm to a prescribed target."""

import numpy as np
import pinocchio
import pinocchio as pin
import qpsolvers
from robot_descriptions.loaders.pinocchio import load_robot_description

import pink
from pink.tasks import FrameTask
from scipy.spatial.transform import Rotation as R

# IK parameters
dt = 1e-2
stop_thres = 1e-8

if __name__ == "__main__":
    # robot = load_robot_description("ur10_description")
    # urdf_path = "assets/piper_description_v100_test2.urdf"
    urdf_path = "data/rynn_models/3.robot_arm/22.piper/urdf/piper.urdf"
    model = pin.buildModelFromUrdf(urdf_path)

    # Frame details
    joint_name = model.names[-1]
    parent_joint = model.getJointId(joint_name)
    # parent_frame = model.getFrameId(joint_name)
    print(f"parent: {parent_joint}")
    # placement = pin.SE3.Identity()

    FRAME_NAME = "joint6"
    # ee_frame = model.addFrame(
    #     pin.Frame(
    #         FRAME_NAME,
    #         parent_joint,
    #         parent_frame,
    #         placement,
    #         pin.FrameType.OP_FRAME,
    #     )
    # )
    data = pin.Data(model)
    low = model.lowerPositionLimit
    high = model.upperPositionLimit
    model.q0 = pin.neutral(model)

    print(model)

    # Task details
    np.random.seed(0)
    q_final = np.array(
        # [np.random.uniform(low=low[i], high=high[i], size=(1,))[0] for i in range(6)]
        [
            np.random.uniform(low=low[i], high=high[i], size=(1,))[0]
            for i in range(model.nq)
        ]
    )
    pin.forwardKinematics(model, data, q_final)
    target_pose = data.oMi[parent_joint]
    ee_task = FrameTask(FRAME_NAME, [1.0, 1.0, 1.0], [1.0, 1.0, 1.0])

    target = pin.SE3.Identity()
    target.translation = np.array([0.1, 0.2, 0.3])
    target.rotation = R.from_euler("xyz", [0, 0, 0]).as_matrix()
    # ee_task.set_target(target_pose)
    ee_task.set_target(target)

    configuration = pink.Configuration(model, data, model.q0)
    error_norm = np.linalg.norm(ee_task.compute_error(configuration))
    print(f"Starting from {error_norm = :.2}")
    print(f"Desired precision is error_norm < {stop_thres}")

    nb_steps = 0
    while error_norm > stop_thres:
        dv = pink.solve_ik(
            configuration,
            tasks=[ee_task],
            dt=dt,
            damping=1e-8,
            solver=(
                "daqp"
                if "daqp" in qpsolvers.available_solvers
                else qpsolvers.available_solvers[0]
            ),
        )
        q_out = pin.integrate(model, configuration.q, dv * dt)
        q_out = np.clip(q_out, low, high)
        configuration = pink.Configuration(model, data, q_out)
        pin.updateFramePlacements(model, data)
        error_norm = np.linalg.norm(ee_task.compute_error(configuration))
        nb_steps += 1

    print(f"Terminated after {nb_steps} steps with {error_norm = :.2}")

Hi, above is a simimple ik example, but the program runs forever and not working at all, can not meet minimal error forever.

I think the target position i set is normal.

Any suggestion would be appreciated!

[stephane-caron]

I see that you have modified the example to use a different robot. However, I can't reproduce it on my machine as I don't have the same local files as you:

ValueError: The file data/rynn_models/3.robot_arm/22.piper/urdf/piper.urdf does not contain a valid URDF model.

I've updated your example to use the piper_description from robot_descriptions.py, and added visualization to it based on the other examples: inverse_kinematics_piper_viz.py.

From the visualization we can see that the arm is stuck on its path to its target because it hits a joint limit:

The reason why this happens is detailed in the subsection "Differential IK is a local algorithm" of #66. I've prepared the example so that it accepts two targets:

• A feasible target by default: this one converges as follows:

$ uv run inverse_kinematics_piper_viz.py
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7000/static/
Starting from error_norm = 1.6
Desired precision is error_norm < 1e-08
Terminated after 64 steps with error_norm = 4.4e-11

• The unfeasible target from your question: this one does not converge, you can then open MeshCat and see the visualization as in the above screenshot:

$ uv run inverse_kinematics_piper_viz.py --hit-limit
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7000/static/
Starting from error_norm = 1.5
Desired precision is error_norm < 1e-08
...

Hoping this helps!

[lucasjinreal]

Hi, i found using robot_descriptions had a bug on macOS:

Commit f2ec6a67e1f404bcb478529e89861ccdf43fa298 not found, let's fetch origin and try again...
stderr: 'error: Your local changes to the following files would be overwritten by checkout:
	src/piper_x_description/meshes/dae/base_Link.dae
Please commit your changes or stash them before you switch branches.
Aborting'

Fixed manually.

I have replicated the hit limit with your kind code.

Actually, I found things getting very wired now!

I tried with:

target.translation = (
        # np.array([0.1, 0.2, 0.3]) if hit_limit else np.array([0.0, 0.2, 0.6])
        # np.array([0.0, 0.2, 0.3]) if hit_limit else np.array([0.0, 0.2, 0.6])
        # np.array([0.0, 0.0, 0.3]) if hit_limit else np.array([0.0, 0.2, 0.6])
        # np.array([0.0, 0.0, 0.1]) if hit_limit else np.array([0.0, 0.2, 0.6])
        np.array([0.0, 0.0, 0.0]) if hit_limit else np.array([0.0, 0.2, 0.6])
    )

all above point, they all relatively looks like hit limit and freezed:

I got some Question now:

1. Doesn't 0,0,0 means rest & natural position?
2. I have a robotwin recorded data, it contains qpos and endpose, for qpos in rest position, the endpose is 0,0,0, int such case, how could i perfectly align them? (making endpose cauclated joints exactly same as qpos in GT?), from the exp above, the 0,0,0 doesn't works at all.
3. Can u teach me how to write a universal Piper ik endpose -> joints class? Currently it doesn't works at all.

Anyhelp would be very appreciated!