Skip to content

Commit ab444aa

Browse files
committed
Fixes index error in inverse kinematics
Corrects an index error in the inverse kinematics calculation that occurred when resetting joint states after each iteration. The loop variable `i` was incorrectly reused from the outer loop, leading to out-of-bounds access when `len(joint_ids_sorted)` differed from `max_iter`. This change ensures correct joint state updates during inverse kinematics solving.
1 parent a3317ba commit ab444aa

1 file changed

Lines changed: 2 additions & 2 deletions

File tree

src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -733,8 +733,8 @@ def _accurate_inverse_kinematics(
733733

734734
for i in range(max_iter):
735735
joint_poses = pybullet.calculateInverseKinematics(**kwargs)
736-
for i in range(len(joint_ids_sorted)):
737-
pybullet.resetJointState(body_id, joint_ids_sorted[i], joint_poses[i], physicsClientId=client_id)
736+
for j in range(len(joint_ids_sorted)):
737+
pybullet.resetJointState(body_id, joint_ids_sorted[j], joint_poses[j], physicsClientId=client_id)
738738
# Retrieve the last link state that contains the last link's position (index 4) and orientation (index 5)
739739
link_state = pybullet.getLinkState(body_id, link_id, physicsClientId=client_id)
740740

0 commit comments

Comments
 (0)