how to move panda-arm on a cube by ikpy?

[linewon]

Intro

Hi!

I am a undergrad student, I use MuJoCo for my research.

My setup

1. define a scene with a cube on a table
2. download panda.urdf from github
3. delete fingers in urdf and add "hand_tcp" at the end of urdf. (which AI tells me to do.)
4. get cube_pos and calc target_pos by "cube_pos + np.array([0.0, 0.0, 0.1])" (I want panda_tcp stop on the cube.)
5. use ikpy.chain.inverse_kinematics to calc the joint_angles
6. move panda-arms slowly to the joint_angles

My question

the panda-arm stops at a wrong position.

I can't debug this problem by my Qwen3-Max AI which is free.

What's wrong with my codes?

What AI do you guys use?

Minimal model and/or code that explain my question

URDF from https://github.com/StanfordASL/PandaRobot.jl/blob/master/deps/Panda/panda.urdf

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from panda_arm_hand.urdf.xacro      | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
    <link name="panda_link0">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link0.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link0.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>
    <link name="panda_link1">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link1.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link1.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>
    <joint name="panda_joint1" type="revolute">
        <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
        <origin rpy="0 0 0" xyz="0 0 0.333"/>
        <parent link="panda_link0"/>
        <child link="panda_link1"/>
        <axis xyz="0 0 1"/>
        <limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
    </joint>
    <link name="panda_link2">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link2.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link2.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>
    <joint name="panda_joint2" type="revolute">
        <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
        <origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
        <parent link="panda_link1"/>
        <child link="panda_link2"/>
        <axis xyz="0 0 1"/>
        <limit effort="87" lower="-1.8326" upper="1.8326" velocity="2.1750"/>
    </joint>
    <link name="panda_link3">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link3.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link3.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>
    <joint name="panda_joint3" type="revolute">
        <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
        <origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
        <parent link="panda_link2"/>
        <child link="panda_link3"/>
        <axis xyz="0 0 1"/>
        <limit effort="87" lower="-2.9671" upper="2.9671" velocity="2.1750"/>
    </joint>
    <link name="panda_link4">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link4.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link4.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>
    <joint name="panda_joint4" type="revolute">
        <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
        <origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
        <parent link="panda_link3"/>
        <child link="panda_link4"/>
        <axis xyz="0 0 1"/>
        <limit effort="87" lower="-3.1416" upper="0.0" velocity="2.1750"/>
    </joint>
    <link name="panda_link5">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link5.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link5.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>
    <joint name="panda_joint5" type="revolute">
        <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
        <origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
        <parent link="panda_link4"/>
        <child link="panda_link5"/>
        <axis xyz="0 0 1"/>
        <limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
    </joint>
    <link name="panda_link6">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link6.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link6.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>
    <joint name="panda_joint6" type="revolute">
        <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
        <parent link="panda_link5"/>
        <child link="panda_link6"/>
        <axis xyz="0 0 1"/>
        <limit effort="12" lower="-0.0873" upper="3.8223" velocity="2.6100"/>
    </joint>
    <link name="panda_link7">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link7.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/link7.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>
    <joint name="panda_joint7" type="revolute">
        <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
        <origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
        <parent link="panda_link6"/>
        <child link="panda_link7"/>
        <axis xyz="0 0 1"/>
        <limit effort="12" lower="-2.9671" upper="2.9671" velocity="2.6100"/>
    </joint>
    <link name="panda_link8"/>
    <joint name="panda_joint8" type="fixed">
        <origin rpy="0 0 0" xyz="0 0 0.107"/>
        <parent link="panda_link7"/>
        <child link="panda_link8"/>
        <axis xyz="0 0 0"/>
    </joint>
    <joint name="panda_hand_joint" type="fixed">
        <parent link="panda_link8"/>
        <child link="panda_hand"/>
        <origin rpy="0 0 -0.785398163397" xyz="0 0 0"/>
    </joint>
    <link name="panda_hand">
        <visual>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/hand.obj"/>
            </geometry>
            <material name="panda_white"/>
        </visual>
        <collision>
            <geometry>
                <mesh filename="package://Panda/meshes/collision/hand.obj"/>
            </geometry>
            <material name="panda_white"/>
        </collision>
    </link>

    <!-- 添加 TCP 虚拟 link -->
    <link name="panda_tcp"/>
    <joint name="hand_to_tcp" type="fixed">
        <parent link="panda_hand"/>
        <child link="panda_tcp"/>
        <origin xyz="0 0 0.0584" rpy="0 0 0"/>
    </joint>
    <material name="panda_white">
        <color rgba="1. 1. 1. 1."/>
    </material>
</robot>

<mujoco model="panda_table_cube">
    <option gravity="0 0 -9.81" timestep="0.005"/>

    <!-- 引入 Panda 机器人（自带夹爪） -->
    <include file="panda.xml"/>

    <worldbody>
        <!-- 地面 -->
        <geom name="floor" type="plane" size="2 2 0.1" rgba=".9 .9 .9 1" friction="1 0.1 0.01"/>

        <!-- Table: 不挡 Panda -->
        <body name="table_right" pos="0.5 0 0.5">
            <geom type="box" size="0.1 0.1 0.02" rgba=".3 .3 .3 1" friction="1 0.1 0.01"/>
        </body>
        <body name="table_left" pos="-0.5 0 0.5">
            <geom type="box" size="0.1 0.1 0.02" rgba=".3 .3 .3 1" friction="1 0.1 0.01"/>
        </body>

        <!-- Cube: on table -->
        <body name="cube" pos="0.5 0 0.525">
            <geom name="cube_geom" type="box" size="0.025 0.025 0.025" rgba="1 0 0 1" friction="1 0.1 0.01"/>
            <joint type="free"/>
        </body>
    </worldbody>
</mujoco>

Code:

import mujoco.viewer
import numpy as np
from ikpy.chain import Chain
import time

from utils.moveUtils import move2TargetSlowly
from utils.viewUtils import initViewerSight
from scipy.spatial.transform import Rotation as R

tarScene = 'mujoco_menagerie/franka_emika_panda/scene_panda_table_cube.xml'
tarUrdf = 'urdf/my_panda_arm_only.urdf'

model = mujoco.MjModel.from_xml_path(tarScene)
data = mujoco.MjData(model)

for i in range(model.nu):
    name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, i)
    print(f"Actuators:   {i} : {name}")

chain = Chain.from_urdf_file(
    tarUrdf,
    base_elements=["panda_link0"],
    active_links_mask=[False] + [True] * 7 + [False] * 3,
)
linkLen = len(chain.links)
print("Chain links length: ", linkLen)
for i, link in enumerate(chain.links):
    print(f"Chain link:   {i} : {link.name}")

print("\n--------------------------------\n")

with mujoco.viewer.launch_passive(model, data) as viewer:
    initViewerSight(viewer)

    # 获取 cube 位置
    cube_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'cube')
    cube_pos = data.body(cube_id).xpos.copy()
    print("Cube position:", cube_pos)

    # --- 设置目标 ---
    tar_pos = cube_pos + np.array([0.0, 0.0, 0.1])
    print("Target pos:", tar_pos)

    # 构造初始猜测
    initial_guess = np.zeros(linkLen)  # [0, 0, 0, ..., 0] (10,)
    initial_guess[2] = 0.5 # 不赋一个初值，计算结果就一直不正确
    print("Initial guess:", initial_guess)

    # r = R.from_euler('x', 180, degrees=True)
    # target_rot = r.as_matrix() # 保证夹爪向下，对应orientation_mode="all"
    target_rot = np.array([0, 0, -1])

    # 调用 IK
    joint_angles = chain.inverse_kinematics(
        target_position=tar_pos,
        target_orientation=target_rot,
        orientation_mode="Z",
        initial_position=initial_guess,
        max_iter=2000
    )
    print("IK result:", joint_angles[1:8])
    print("IK EE pos:", chain.forward_kinematics(joint_angles)[:3, 3])
    print("IK EE rot:\n", chain.forward_kinematics(joint_angles)[:3, :3])

    # ✅ 应用到控制器（前7个 actuator）
    # data.ctrl[:7] = joint_angles[1:8] # 后面尝试从初始位姿，慢慢移动到目标位姿
    # data.ctrl[:7] = [0.14, 1.01, -0.61, 0, -0.56, -0.087, 0.0007]
    move2TargetSlowly(model, data, viewer, joint_angles[1:8])

    # 验证结果
    urdf_result_ee = chain.forward_kinematics(joint_angles)[:3, 3]
    print("✅ IK joint angles (rad):", joint_angles[1:8])
    current_ctrl = data.ctrl[:7].copy()
    print("current control:", current_ctrl)

    # 仿真
    while viewer.is_running():
        mujoco.mj_step(model, data)
        viewer.sync()
        time.sleep(0.01)

Confirmations

• I searched the latest documentation thoroughly before posting.
• I searched previous Issues and Discussions, I am certain this has not been raised before.