diff --git a/hrpsys_ros_bridge_tutorials/test/test-kf/check-kf-precision.l b/hrpsys_ros_bridge_tutorials/test/test-kf/check-kf-precision.l new file mode 100644 index 00000000..725aca4a --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/test/test-kf/check-kf-precision.l @@ -0,0 +1,136 @@ +(load "package://hrpsys_ros_bridge_tutorials/euslisp/testmdofarm-interface.l") +(load "package://hrpsys_ros_bridge/euslisp/datalogger-log-parser.l") + +(defun setup () + (if (not (boundp '*ri*)) + (testmdofarm-init)) + (if (not (boundp '*robot*)) + (setq *robot* *testmdofarm*)) + (send *robot* :init-pose) + (send *robot* :newcoords (make-coords)) + (send *robot* :rarm :move-end-pos #f(100 100 -150) :world) + (send *irtviewer* :draw-objects) + (send *ri* :angle-vector (send *robot* :angle-vector) 100) + (send *ri* :wait-interpolation)) + +(defun shake-imu (&key (init-ypr-angle (float-vector 0 0 0)) + (deflection-ypr-angle (float-vector 0 0 0)) (wrt :world) + (divisor 10) (time 1500)) + ;; initial setting + (let* ((_y (send (send *robot* :rarm :end-coords :copy-worldcoords) :rotate (elt init-ypr-angle 0) :z)) + (_p (send _y :rotate (elt init-ypr-angle 1) :y)) + (init-mat (send _p :rotate (elt init-ypr-angle 2) :x))) + (send *robot* :rarm :inverse-kinematics init-mat) + (send *robot* :newcoords (make-coords)) + (send *irtviewer* :draw-objects) + (print "move initial pose") + (send *ri* :angle-vector (send *robot* :angle-vector) 1000) + (send *ri* :wait-interpolation)) + ;; shake + (let* ((start-coords (send *robot* :rarm :end-coords :copy-worldcoords)) + (dist-coords-A + (send + (send + (send (copy-object start-coords) :rotate (elt deflection-ypr-angle 0) :z wrt) + :rotate (elt deflection-ypr-angle 1) :y wrt) + :rotate (elt deflection-ypr-angle 2) :x wrt)) + (dist-coords-B (midcoords -1 start-coords dist-coords-A)) + avs tms) + (let (tc) + (dotimes (i (* divisor 4)) + (format t "~A / ~A~%" (+ i 1) (* divisor 4)) + (cond + ((< i divisor) + (setq tc (midcoords (* (mod i divisor) (/ 1.0 divisor)) start-coords dist-coords-A))) + ((< i (* divisor 2)) + (setq tc (midcoords (* (mod i divisor) (/ 1.0 divisor)) dist-coords-A start-coords))) + ((< i (* divisor 3)) + (setq tc (midcoords (* (mod i divisor) (/ 1.0 divisor)) start-coords dist-coords-B))) + ((< i (* divisor 4)) + (setq tc (midcoords (* (mod i divisor) (/ 1.0 divisor)) dist-coords-B start-coords)))) + (send *robot* :newcoords (make-coords)) + (send *robot* :rarm :inverse-kinematics tc) + (send *irtviewer* :draw-objects) + (unix:sleep (/ (/ time divisor) 1000)) + (push (send *robot* :angle-vector) avs) + (push (/ time divisor) tms))) + (print "finish to calculate avs") + (list (reverse avs) (reverse tms)))) + +(defun take-log (avs tms &key (fname "/tmp/test-mdof-arm")) + (send *ri* :start-log) + (unix:sleep 1) ;wait for sensor value to converge + (send *ri* :angle-vector-sequence avs tms) + (send *ri* :wait-interpolation) + (unix:sleep 1) ;wait for sensor value to converge + (send *ri* :save-log fname)) + + +(defun parse-log (&key (fname "/tmp/test-mdof-arm") (output "/tmp/kuroiwa.dat")) + (setq *log* (instance datalogger-log-parser-controller :init fname :robot *robot*)) + (with-open-file + (f (format nil output) :direction :output) + (let (tm observed-rpy-kf observed-rpy-msg observed-rpy-imucoords true-rpy imu-msg start-tm acc gyro ef-pos) + (do-until-key-with-check + (progn + (send *log* :state) + (if (find-method *log* :eof-p) + (not (send *log* :eof-p)))) + (unless start-tm + (setq start-tm (+ (send (send *log* :time) :sec) + (* 1e-9 (send (send *log* :time) :nsec))))) + (setq tm + (- (+ (send (send *log* :time) :sec) + (* 1e-9 (send (send *log* :time) :nsec))) + start-tm)) + (setq imu-msg (cdr (car (*log* . robot-state)))) + (setq observed-rpy-kf + (reverse + (mapcar #'(lambda (x) (rad2deg x)) + (concatenate cons (send (elt (send *log* :parser-list) 4) :read-state))))) + (setq observed-rpy-msg + (mapcar #'(lambda (x) (rad2deg x)) + (car + (rpy-angle + (ros::tf-quaternion->rot (send imu-msg :orientation)))))) + (setq observed-rpy-imucoords + (car (rpy-angle (send (send *log* :imucoords) :worldrot)))) + (setq acc (send (elt (send *log* :parser-list) 0) :read-state)) + (setq gyro (send (elt (send *log* :parser-list) 2) :read-state)) + (send *robot* :angle-vector (send *log* :potentio-vector)) + (send *robot* :newcoords (make-coords)) + ;; (send *irtviewer* :draw-objects) + (setq true-rpy + (mapcar #'(lambda (x) (rad2deg x)) + (car + (rpy-angle + (send (copy-object (car (send *robot* :imu-sensors))) :worldrot))))) + (setq ef-pos (send (send *robot* :rarm :end-coords :copy-worldcoords) :worldpos)) + (format f "~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A ~A~%" + tm + (elt observed-rpy-kf 0) (elt observed-rpy-kf 1) (elt observed-rpy-kf 2) + (elt true-rpy 0) (elt true-rpy 1) (elt true-rpy 2) + (elt observed-rpy-msg 0) (elt observed-rpy-msg 1) (elt observed-rpy-msg 2) + (elt observed-rpy-imucoords 0) (elt observed-rpy-imucoords 1) (elt observed-rpy-imucoords 2) + (elt acc 0) (elt acc 1) (elt acc 2) + (elt gyro 0) (elt gyro 1) (elt gyro 2) + (elt ef-pos 0) (elt ef-pos 1) (elt ef-pos 2) + ))))) + +(warn "(main :deflection-ypr-angle (float-vector 0 (/ pi 6) 0) :wrt :world :init-ypr-angle (float-vector 0 (/ pi 2) 0))~%") +(defun main (&key (deflection-ypr-angle (float-vector 0 0.5 0)) + (wrt :world) + (init-ypr-angle (float-vector 0.5 0 0)) + (divisor 10) + (time 1500) + (fname "/tmp/test-mdof-arm")) + (setup) + (let ((avs-and-tms (shake-imu :init-ypr-angle init-ypr-angle + :deflection-ypr-angle deflection-ypr-angle :wrt wrt + :divisor divisor :time time))) + (print 'start-take-log) + (bench (take-log (car avs-and-tms) (cadr avs-and-tms) :fname fname))) + (print 'start-parse-log) + (bench (parse-log :fname fname)) + (print "plot...") + (unix:system "./plot.py")) diff --git a/hrpsys_ros_bridge_tutorials/test/test-kf/plot.py b/hrpsys_ros_bridge_tutorials/test/test-kf/plot.py new file mode 100755 index 00000000..551660a0 --- /dev/null +++ b/hrpsys_ros_bridge_tutorials/test/test-kf/plot.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import argparse +import matplotlib.pyplot, numpy + +def parse_args(): + parser = argparse.ArgumentParser(description='plot data from rosbag') + parser.add_argument('-f', type=str, help='input file', metavar='file', default='/tmp/kuroiwa.dat') + return parser.parse_args() + +def process(args): + tm = [] + ypr = [[], [], []] + raw = [[], [], []] + order = ['yaw', 'pitch', 'roll'] + raw_order = ['acceleration', 'gyro', 'position'] + fig, (axes_array) = matplotlib.pyplot.subplots(3, 2, sharex=True, sharey=False) + with open(args.f) as f: + for line in f: + tmp = [float(x) for x in line.rstrip().split(' ')] + tm.append(tmp[0]) + ypr[0].append(tmp[1:(10+1):3]) # 1, 4, 7, 10 + ypr[1].append(tmp[2:(11+1):3]) # 2, 5, 8, 11 + ypr[2].append(tmp[3:(12+1):3]) # 3, 6, 9, 12 + raw[0].append(tmp[13:(15+1)]) # 13, 14, 15 + raw[1].append(tmp[16:(18+1)]) # 16, 17, 18 + raw[2].append(tmp[19:(21+1)]) # 19, 20, 21 + for i in range(len(ypr)): + axes_array[i][0].plot(tm, [hoge[0] for hoge in ypr[i]], 'g.', label='kf_rpy') + axes_array[i][0].plot(tm, [hoge[1] for hoge in ypr[i]], 'r.', label='FowardKinematics') + # axes_array[i].plot(tm, [hoge[1] for hoge in ypr[i]], 'b.', label='imumsg') + # axes_array[i].plot(tm, [hoge[1] for hoge in ypr[i]], 'y.', label='imucoords') + axes_array[i][0].set_title(order[i]) + axes_array[i][0].legend(loc=0) + for i in range(len(raw)): + axes_array[i][1].plot(tm, [hoge[0] for hoge in raw[i]], 'r.', label='x') + axes_array[i][1].plot(tm, [hoge[1] for hoge in raw[i]], 'g.', label='y') + axes_array[i][1].plot(tm, [hoge[2] for hoge in raw[i]], 'b.', label='z') + axes_array[i][1].set_title(raw_order[i]) + axes_array[i][1].legend(loc=0) + axes_array[2][0].set_xlabel('t [s]') + axes_array[1][0].set_ylabel('[deg]') + matplotlib.pyplot.show() + + +def main(): + args = parse_args() + process(args) + +if __name__ == '__main__': + main()