panda robot arm inverse kinematics

[z1x1ang]

Dear Webots Team,

I am experiencing an issue while using a robotic arm in Webots. I would like to use inverse kinematics decomposition to enable the Panda robotic arm to perform a sequence of actions. To do this, I modified one of the sample examples — specifically the IRB example — by replacing the IRB arm with the built‑in Panda arm.

However, I have encountered a puzzling problem that has been troubling me for quite some time: the Panda arm’s end effector is unable to reach the target ball and instead moves farther away from it.

Could you please help me identify the cause of this issue? Your assistance would be greatly appreciated.

Thank you very much.

file:https://1drv.ms/u/c/946a200addca5961/EeNi2dQv-2JEmiV5CG5x23cBri3_i1u57vnVTFwbi1gbFA?e=Rn1Yy4
main code:

Copyright 1996-2024 Cyberbotics Ltd.

Licensed under the Apache License, Version 2.0

Demonstration of inverse kinematics using the "ikpy" Python module.

import sys
import math
import tempfile
from controller import Supervisor

Try importing ikpy

try:
import ikpy
from ikpy.chain import Chain
except ImportError:
sys.exit(
'The "ikpy" Python module is not installed. '
'To run this sample, please upgrade "pip" and install ikpy with this command: "pip install ikpy"'
)

Check ikpy version

if ikpy.version[0] < '3':
sys.exit(
'The "ikpy" Python module version is too old. '
'Please upgrade "ikpy" to version 3.0 or newer with: "pip install --upgrade ikpy"'
)

IKPY_MAX_ITERATIONS = 10 # Increased for better convergence

Initialize the Webots Supervisor

supervisor = Supervisor()
timeStep = int(4 * supervisor.getBasicTimeStep())

Create the arm chain from the URDF

filename = None
with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
filename = file.name
file.write(supervisor.getUrdf().encode('utf-8'))

armChain = Chain.from_urdf_file(
filename,
active_links_mask=[False, False, True, True, True, True, True, True, True, False, False]
)

print("Arm Chain Links:")
print(armChain.links)

Initialize the arm motors and encoders

motors = []
position_sensors = []

for link in armChain.links:
print(f"Link: {link.name}, Bounds: {link.bounds}")
if 'panda_joint' in link.name:
motor = supervisor.getDevice(link.name)
if motor is None:
print(f"Warning: Motor for {link.name} not found!")
continue
motor.setVelocity(1.0)
position_sensor = motor.getPositionSensor()
if position_sensor is None:
print(f"Warning: Position sensor for {link.name} not found!")
else:
position_sensor.enable(timeStep)
position_sensors.append(position_sensor)
motors.append(motor)

print(f"Found {len(motors)} motors.")

Get the arm and target nodes

target = supervisor.getFromDef('TARGET')
if target is None:
sys.exit("Target node 'TARGET' not found!")

arm = supervisor.getSelf()

Main loop: Move the arm hand to the target

print('Move the yellow and black sphere to move the arm...')
iteration = 0

while supervisor.step(timeStep) != -1:
iteration += 1
print(f"\n--- Iteration {iteration} ---")

# Get absolute positions
targetPosition = target.getPosition()
armPosition = arm.getPosition()
print(f"Target absolute: {targetPosition}")
print(f"Arm base absolute: {armPosition}")

# Compute relative position (manual axis swap due to misalignment)
x = -(targetPosition[1] - armPosition[1])
y = targetPosition[0] - armPosition[0]
z = targetPosition[2] - armPosition[2]
print(f"Relative target: [x={x:.4f}, y={y:.4f}, z={z:.4f}]")

# Get current joint positions
current_positions = [ps.getValue() if ps else 0.0 for ps in position_sensors]
print(f"Current joint positions: {current_positions}")

# Prepare initial position for IK
initial_position = [0, 0] + current_positions + [0, 0]
print(f"Initial position for IK: {initial_position}")

# Compute IK
try:
    ikResults = armChain.inverse_kinematics(
        [x, y, z],
        max_iter=IKPY_MAX_ITERATIONS,
        initial_position=initial_position
    )
    print(f"IK Results: {ikResults}")
except Exception as e:
    print(f"IK computation failed: {e}")
    continue

# Compute FK to verify
fk_position = armChain.forward_kinematics(ikResults)
achieved_x = fk_position[0, 3]
achieved_y = fk_position[1, 3]
achieved_z = fk_position[2, 3]
print(f"Achieved position: [x={achieved_x:.4f}, y={achieved_y:.4f}, z={achieved_z:.4f}]")

# Calculate distance
squared_distance = (achieved_x - x)**2 + (achieved_y - y)**2 + (achieved_z - z)**2
distance = math.sqrt(squared_distance)
print(f"Distance to target: {distance:.4f}")

# Retry if not close enough
if distance > 0.03:
    print("Distance > 0.03, retrying IK with higher iterations...")
    ikResults = armChain.inverse_kinematics(
        [x, y, z],
        max_iter=20,
        initial_position=initial_position
    )
    fk_position = armChain.forward_kinematics(ikResults)
    achieved_x = fk_position[0, 3]
    achieved_y = fk_position[1, 3]
    achieved_z = fk_position[2, 3]
    squared_distance = (achieved_x - x)**2 + (achieved_y - y)**2 + (achieved_z - z)**2
    distance = math.sqrt(squared_distance)
    print(f"Retry achieved position: [x={achieved_x:.4f}, y={achieved_y:.4f}, z={achieved_z:.4f}]")
    print(f"Retry distance: {distance:.4f}")
    if distance > 0.03:
        print("Warning: IK did not converge sufficiently. Target may be unreachable.")

# Check joint limits
for i, joint in enumerate(ikResults[2:-2]):
    link = armChain.links[i + 2]
    if link.bounds:
        min_bound, max_bound = link.bounds
        if joint < min_bound or joint > max_bound:
            print(f"Warning: Joint {link.name} out of bounds: {joint:.4f} (bounds: {min_bound}, {max_bound})")

# Actuate motors
for i in range(len(motors)):
    target_pos = ikResults[i + 2]
    motors[i].setPosition(target_pos)
    print(f"Setting {motors[i].getName()} to {target_pos:.4f}")