Add interface.py

[iory]

 ~/ros/kxr  rosrun kxr_controller interface.py --viewer trimesh
Python 3.8.10 (default, Nov 22 2023, 10:22:35)
Type 'copyright', 'credits' or 'license' for more information
IPython 8.12.3 -- An enhanced Interactive Python. Type '?' for help.

In [1]: ri.servo_on()

In [2]: ri.angle_vector(robot_model.init_pose(), 1)
Out[2]: array([0., 0., 0., 0., 0., 0., 0., 0., 0.], dtype=float32)

In [3]: robot_model.rarm_joint2_w.joint_angle(0.9)
Out[3]: 0.9

In [4]: ri.angle_vector(robot_model.angle_vector(), 0.1)
Out[4]: array([0. , 0.9, 0. , 0. , 0. , 0. , 0. , 0. , 0. ], dtype=float32)

In [5]: ri.angle_vector(robot_model.init_pose(), 1)
Out[5]: array([0., 0., 0., 0., 0., 0., 0., 0., 0.], dtype=float32)

In [6]: ri.wait_interpolation()
Out[6]: True