sample-arm.l

(load "irteus/demo/sample-arm-model.l")

(setq *robot* (instance sarmclass :init))
(setq *cube* (make-cube 30 1000 30))
(send *cube* :locate #f(300 0 200))
(setq *target-coords* (send (send *cube* :worldcoords) :copy-worldcoords))
; (send *target-coords* :translate #f(0 0 -50))
(send *target-coords* :rotate -1.57 :y)
(objects (list *robot* *cube* *target-coords*))
(send *robot* :open-hand)
(send *robot* :inverse-kinematics *target-coords*
      :link-list (send *robot* :link-list (send *robot* :end-coords :parent))
      :move-target (send *robot* :end-coords)
      :rotation-axis :y)
(send *robot* :inverse-kinematics *target-coords*
      :link-list (send *robot* :link-list (send *robot* :end-coords :parent))
      :move-target (send *robot* :end-coords)
      :rotation-axis :y)
(send *robot* :close-hand *cube*)