PR2 virtual force

pr2-virtual-force.l

(setq *robot* (instance pr2-robot :init))

(setq *J*
      (send *robot* :calc-jacobian-from-link-list
            (send *robot* :link-list (send (send *robot* :larm :end-coords) :parent)
                  (send (car (send *robot* :larm :joint-list)) :child-link))
            :move-target (send *robot* :larm :end-coords)))
(setq *torque-vector* (float-vector 5 5 5 5 5 5 5))
;; (send *ri* :state :torque-vector)から左腕だけ取ってくる                                                                                                              

(setq *force* (transform (pseudo-inverse2 (transpose *J*)) *torque-vector*))