#!/bin/sh

set -exu

cd "$AUTOPKGTEST_TMP"

cat > pykdl_smoke.py << 'END'
import PyKDL

chain = PyKDL.Chain()
chain.addSegment(PyKDL.Segment(
    PyKDL.Joint(PyKDL.Joint.RotZ),
    PyKDL.Frame(PyKDL.Vector(1.0, 0.0, 0.0))))
chain.addSegment(PyKDL.Segment(
    PyKDL.Joint(PyKDL.Joint.RotZ),
    PyKDL.Frame(PyKDL.Vector(1.0, 0.0, 0.0))))
assert chain.getNrOfJoints() == 2

q = PyKDL.JntArray(chain.getNrOfJoints())
frame = PyKDL.Frame()
assert PyKDL.ChainFkSolverPos_recursive(chain).JntToCart(q, frame) >= 0
assert abs(frame.p.x() - 2.0) < 1e-9
assert abs(frame.p.y()) < 1e-9
END

python3 pykdl_smoke.py
