Spaces:
Running
Running
File size: 632 Bytes
65da30d |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 |
from reachy_mini.utils.constants import URDF_ROOT_PATH
from reachy_mini.kinematics import PlacoKinematics
from reachy_mini.utils import create_head_pose
def offline_test_collision():
head_kinematics = PlacoKinematics(
urdf_path=URDF_ROOT_PATH, check_collision=True
)
reachable_pose = create_head_pose()
sol = head_kinematics.ik(reachable_pose)
assert sol is not None, "The reachable pose should not cause a collision."
unreachable_pose = create_head_pose(x=20, y=20, mm=True)
sol = head_kinematics.ik(unreachable_pose)
assert sol is None, "The unreachable pose should cause a collision."
|