diff --git a/euscollada/CMakeLists.txt b/euscollada/CMakeLists.txt
index 21a46323c..2db470648 100644
--- a/euscollada/CMakeLists.txt
+++ b/euscollada/CMakeLists.txt
@@ -68,3 +68,4 @@ install(PROGRAMS ${_install_sh_files}
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
add_rostest(test/euscollada-model-conversion.test)
+add_rostest(test/test-mimic.test)
diff --git a/euscollada/src/collada2eus_urdfmodel.cpp b/euscollada/src/collada2eus_urdfmodel.cpp
index db4f96fbf..8a79f80ec 100644
--- a/euscollada/src/collada2eus_urdfmodel.cpp
+++ b/euscollada/src/collada2eus_urdfmodel.cpp
@@ -1030,6 +1030,14 @@ void ModelEuslisp::printMimicJoints () {
fprintf(fp, " (instance mimic-joint-param :init %s_jt :multiplier %f :offset %f)\n", (*joint)->name.c_str(), (*joint)->mimic->multiplier, (*joint)->mimic->offset);
}
fprintf(fp, " ))\n");
+ fprintf(fp, " ;; set offset as default-coords\n");
+ fprintf(fp, " (dolist (j (%s_jt . mimic-joints))\n", mimic->first->name.c_str());
+ fprintf(fp, " (cond ((derivedp (send j :joint) rotational-joint)\n");
+ fprintf(fp, " (send (send j :joint :child-link) :rotate (send j :offset) ((send j :joint) . axis)))\n");
+ fprintf(fp, " ((derivedp (send j :joint) linear-joint)\n");
+ fprintf(fp, " (send (send j :joint :child-link) :translate (scale (* 1000 (send j :offset)) ((send j :joint) . axis))))\n");
+ fprintf(fp, " (t (error \"unsupported mimic joint ~A\" (send j :joint))))\n");
+ fprintf(fp, " (setq ((send j :joint) . default-coords) (send j :joint :child-link :copy-coords)))\n");
}
}
diff --git a/euscollada/src/euscollada-robot.l b/euscollada/src/euscollada-robot.l
index 5d22b6ef6..17d57b87d 100644
--- a/euscollada/src/euscollada-robot.l
+++ b/euscollada/src/euscollada-robot.l
@@ -151,6 +151,47 @@
(:offset (&rest args) (forward-message-to offset args))
)
+(defun calc-jacobian-mimic
+ (mimic-joints
+ fik row column joint paxis child-link world-default-coords child-reverse
+ move-target transform-coords rotation-axis translation-axis
+ tmp-v0 tmp-v1 tmp-v2 tmp-v3 tmp-v3a tmp-v3b tmp-m33)
+ (let* ((fik-tmp (copy-object fik))
+ (dim (* 6 (if (atom rotation-axis) 1 (length rotation-axis)))))
+ (dolist (axis (append (if (atom translation-axis)
+ (list translation-axis) translation-axis)
+ (if (atom rotation-axis)
+ (list rotation-axis) rotation-axis)))
+ (case axis
+ ((:x :y :z :xx :yy :zz) (decf dim 1))
+ ((:xy :yx :yz :zy :zx :xz) (decf dim 2))
+ (nil (decf dim 3))))
+ ;;
+ (dolist (mj mimic-joints)
+ (let* ((j (send mj :joint))
+ (paxis (case (j . axis)
+ (:x #f(1 0 0)) (:y #f(0 1 0)) (:z #f(0 0 1))
+ (:xx #f(1 0 0)) (:yy #f(0 1 0)) (:zz #f(0 0 1))
+ (:-x #f(-1 0 0)) (:-y #f(0 -1 0)) (:-z #f(0 0 -1))
+ (t (j . axis))))
+ (child-link (send j :child-link))
+ (parent-link (send j :parent-link))
+ (default-coords (j . default-coords))
+ (world-default-coords (send (send parent-link :copy-worldcoords)
+ :transform default-coords)))
+ (fill (array-entity fik-tmp) 0)
+ (send j :calc-jacobian
+ fik-tmp row column j paxis child-link world-default-coords child-reverse
+ move-target transform-coords rotation-axis translation-axis
+ tmp-v0 tmp-v1 tmp-v2 tmp-v3 tmp-v3a tmp-v3b tmp-m33)
+ ;;
+ (dotimes (i dim)
+ (setf (aref fik (+ i row) column)
+ (+ (aref fik (+ i row) column)
+ (* (send mj :multiplier) (aref fik-tmp (+ i row) column)))))
+ ))
+ fik))
+
(defclass rotational-mimic-joint
:super rotational-joint
:slots (mimic-joints))
@@ -158,13 +199,27 @@
(:init (&rest args &key ((:mimic-joints mjs)) &allow-other-keys)
(send-super* :init args)
(setq mimic-joints mjs)
+ (dolist (j mimic-joints)
+ (send (send j :joint :child-link) :rotate (send j :offset) ((send j :joint) . axis))
+ (setq ((send j :joint) . default-coords) (send j :joint :child-link :copy-coords)))
self)
(:joint-angle (&rest args)
(prog1
(send-super* :joint-angle args)
(dolist (j mimic-joints)
- (send j :joint :joint-angle (+ (* joint-angle (send j :multiplier)) (send j :offset))))
+ (send j :joint :joint-angle (* joint-angle (send j :multiplier))))
))
+ (:calc-jacobian (fik row column joint paxis child-link world-default-coords child-reverse
+ move-target transform-coords rotation-axis translation-axis
+ tmp-v0 tmp-v1 tmp-v2 tmp-v3 tmp-v3a tmp-v3b tmp-m33)
+ (calc-jacobian-rotational fik row column joint paxis child-link world-default-coords child-reverse
+ move-target transform-coords rotation-axis translation-axis
+ tmp-v0 tmp-v1 tmp-v2 tmp-v3 tmp-v3a tmp-v3b tmp-m33)
+ (calc-jacobian-mimic mimic-joints
+ fik row column joint paxis child-link world-default-coords child-reverse
+ move-target transform-coords rotation-axis translation-axis
+ tmp-v0 tmp-v1 tmp-v2 tmp-v3 tmp-v3a tmp-v3b tmp-m33)
+ ))
)
(defclass linear-mimic-joint
@@ -174,11 +229,25 @@
(:init (&rest args &key ((:mimic-joints mjs)) &allow-other-keys)
(send-super* :init args)
(setq mimic-joints mjs)
+ (dolist (j mimic-joints)
+ (send (send j :joint :child-link) :translate (scale (* 1000 (send j :offset)) ((send j :joint) . axis)))
+ (setq ((send j :joint) . default-coords) (send j :joint :child-link :copy-coords)))
self)
(:joint-angle (&rest args)
(prog1
(send-super* :joint-angle args)
(dolist (j mimic-joints)
- (send j :joint :joint-angle (+ (* joint-angle (send j :multiplier)) (send j :offset))))
+ (send j :joint :joint-angle (* joint-angle (send j :multiplier))))
))
+ (:calc-jacobian (fik row column joint paxis child-link world-default-coords child-reverse
+ move-target transform-coords rotation-axis translation-axis
+ tmp-v0 tmp-v1 tmp-v2 tmp-v3 tmp-v3a tmp-v3b tmp-m33)
+ (calc-jacobian-linear fik row column joint paxis child-link world-default-coords child-reverse
+ move-target transform-coords rotation-axis translation-axis
+ tmp-v0 tmp-v1 tmp-v2 tmp-v3 tmp-v3a tmp-v3b tmp-m33)
+ (calc-jacobian-mimic mimic-joints
+ fik row column joint paxis child-link world-default-coords child-reverse
+ move-target transform-coords rotation-axis translation-axis
+ tmp-v0 tmp-v1 tmp-v2 tmp-v3 tmp-v3a tmp-v3b tmp-m33)
+ ))
)
diff --git a/euscollada/test/robots/test-linear-1.urdf b/euscollada/test/robots/test-linear-1.urdf
new file mode 100644
index 000000000..274944d67
--- /dev/null
+++ b/euscollada/test/robots/test-linear-1.urdf
@@ -0,0 +1,136 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-linear-1.yaml b/euscollada/test/robots/test-linear-1.yaml
new file mode 100644
index 000000000..3104128f6
--- /dev/null
+++ b/euscollada/test/robots/test-linear-1.yaml
@@ -0,0 +1,19 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint3 : rarm-elbow-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-linear-mimic-1.urdf b/euscollada/test/robots/test-linear-mimic-1.urdf
new file mode 100644
index 000000000..372f642a0
--- /dev/null
+++ b/euscollada/test/robots/test-linear-mimic-1.urdf
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-linear-mimic-1.yaml b/euscollada/test/robots/test-linear-mimic-1.yaml
new file mode 100644
index 000000000..71242c63e
--- /dev/null
+++ b/euscollada/test/robots/test-linear-mimic-1.yaml
@@ -0,0 +1,18 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-linear-mimic-2.urdf b/euscollada/test/robots/test-linear-mimic-2.urdf
new file mode 100644
index 000000000..38103748a
--- /dev/null
+++ b/euscollada/test/robots/test-linear-mimic-2.urdf
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-linear-mimic-2.yaml b/euscollada/test/robots/test-linear-mimic-2.yaml
new file mode 100644
index 000000000..71242c63e
--- /dev/null
+++ b/euscollada/test/robots/test-linear-mimic-2.yaml
@@ -0,0 +1,18 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-revolute-1.urdf b/euscollada/test/robots/test-revolute-1.urdf
new file mode 100644
index 000000000..4b5ecd2b4
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-1.urdf
@@ -0,0 +1,136 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-revolute-1.yaml b/euscollada/test/robots/test-revolute-1.yaml
new file mode 100644
index 000000000..3104128f6
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-1.yaml
@@ -0,0 +1,19 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint3 : rarm-elbow-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-revolute-2.urdf b/euscollada/test/robots/test-revolute-2.urdf
new file mode 100644
index 000000000..862d3e970
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-2.urdf
@@ -0,0 +1,136 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-revolute-2.yaml b/euscollada/test/robots/test-revolute-2.yaml
new file mode 100644
index 000000000..3104128f6
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-2.yaml
@@ -0,0 +1,19 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint3 : rarm-elbow-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-revolute-mimic-1.urdf b/euscollada/test/robots/test-revolute-mimic-1.urdf
new file mode 100644
index 000000000..41c459e82
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-1.urdf
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-revolute-mimic-1.yaml b/euscollada/test/robots/test-revolute-mimic-1.yaml
new file mode 100644
index 000000000..71242c63e
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-1.yaml
@@ -0,0 +1,18 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-revolute-mimic-2.urdf b/euscollada/test/robots/test-revolute-mimic-2.urdf
new file mode 100644
index 000000000..d0577d3f9
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-2.urdf
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-revolute-mimic-2.yaml b/euscollada/test/robots/test-revolute-mimic-2.yaml
new file mode 100644
index 000000000..71242c63e
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-2.yaml
@@ -0,0 +1,18 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-revolute-mimic-3.urdf b/euscollada/test/robots/test-revolute-mimic-3.urdf
new file mode 100644
index 000000000..311d1fbc4
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-3.urdf
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-revolute-mimic-3.yaml b/euscollada/test/robots/test-revolute-mimic-3.yaml
new file mode 100644
index 000000000..71242c63e
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-3.yaml
@@ -0,0 +1,18 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-revolute-mimic-4.urdf b/euscollada/test/robots/test-revolute-mimic-4.urdf
new file mode 100644
index 000000000..93f68cbfe
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-4.urdf
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-revolute-mimic-4.yaml b/euscollada/test/robots/test-revolute-mimic-4.yaml
new file mode 100644
index 000000000..71242c63e
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-4.yaml
@@ -0,0 +1,18 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/robots/test-revolute-mimic-5.urdf b/euscollada/test/robots/test-revolute-mimic-5.urdf
new file mode 100644
index 000000000..8ae1b306d
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-5.urdf
@@ -0,0 +1,137 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/euscollada/test/robots/test-revolute-mimic-5.yaml b/euscollada/test/robots/test-revolute-mimic-5.yaml
new file mode 100644
index 000000000..71242c63e
--- /dev/null
+++ b/euscollada/test/robots/test-revolute-mimic-5.yaml
@@ -0,0 +1,18 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+ - joint1 : rarm-shoulder-y
+ - joint2 : rarm-shoulder-p
+ - joint4 : rarm-wrist-p
+ - joint5 : rarm-wrist-r
+ - joint6 : rarm-wrist-y
+
+##
+## end-coords
+##
+rarm-end-coords:
+ translate : [0, 0, 0.2 ]
+ rotate : [0, 0, 1, 0]
+ parent : link6
diff --git a/euscollada/test/test-mimic.l b/euscollada/test/test-mimic.l
new file mode 100644
index 000000000..dcd950258
--- /dev/null
+++ b/euscollada/test/test-mimic.l
@@ -0,0 +1,137 @@
+(require :unittest "lib/llib/unittest.l")
+
+(init-unit-test)
+
+;; copy from euscollada-model-conversion-test.l
+(defun generate-eusmodel-from-collada (col-file eus-file &key (yaml-file))
+ (let ((strm (piped-fork
+ (if yaml-file
+ (format nil "rosrun euscollada collada2eus ~A ~A ~A && echo finished"
+ col-file yaml-file eus-file)
+ (format nil "rosrun euscollada collada2eus ~A ~A && echo finished"
+ col-file eus-file)))))
+ ;; (assert (select-stream (list strm) 240) (format nil ";; faild to generate ~A within 240 second" eus-file)) ;; why return nil immidiately
+ (dotimes (i 240)
+ (when
+ (while (and (select-stream (list strm) 1)
+ (not (eq :eof (read-line strm nil :eof))))
+ (return t))
+ (return))
+ (assert (/= i 239) (format nil ";; faild to generate ~A within 240 second" eus-file))
+ (unix::sleep 1)
+ )
+ (unix::sleep 1)
+ (close strm)
+ ))
+
+(load "package://euscollada/src/euscollada-robot.l")
+(defvar *loop* 0)
+(defmethod euscollada-robot
+ (:inverse-kinematics-loop (&rest args)
+ (incf *loop*)
+ (send-super* :inverse-kinematics-loop args)))
+
+(defun test-mimic-joint (name)
+ (let ((urdf-file (ros::resolve-ros-path (format nil "package://euscollada/test/robots/test-~A.urdf" name)))
+ (yaml-file (ros::resolve-ros-path (format nil "package://euscollada/test/robots/test-~A.yaml" name)))
+ (eus-file (format nil "/tmp/test-~A.l" name))
+ param len av robot end-coords goal ret)
+ (generate-eusmodel-from-collada urdf-file eus-file :yaml-file yaml-file)
+ (load eus-file)
+ (setq robot (funcall (intern (string-upcase (substitute #\_ #\- name)))))
+ (format t "create robot model ~A~%" robot)
+ (setq param (list (cons (send robot :joint1) 45)
+ (cons (send robot :joint2) 45)
+ (cons (send robot :joint3) 90)
+ (cons (send robot :joint4) 45)
+ (cons (send robot :joint5) 0)
+ (cons (send robot :joint6) 45)))
+ (setq len (length (send robot :angle-vector)))
+ (setq av (instantiate float-vector len))
+ (objects (list robot))
+ (dotimes (i len)
+ (let* ((j (elt (send robot :joint-list) i))
+ (p (assoc j param))
+ (joint (car p))
+ (angle (cdr p)))
+ (if (and (string= (send j :name) "joint3") ;; for test-revolute-2
+ (eps= (elt (car (send (j . default-coords) :rpy-angle)) 1) 0.785398))
+ (setq angle 45))
+ (if (derivedp j linear-joint) ;; for test-linear
+ (setq angle (* 10 angle)))
+ (if (or (minusp (elt (j . axis) 0))
+ (minusp (elt (j . axis) 1))
+ (minusp (elt (j . axis) 2)))
+ (setq angle (- angle)))
+ (setf (elt av i) angle)
+ (format t "moving ~A to ~A~%" joint angle)
+ (dotimes (i 10)
+ (send joint :joint-angle (* (+ i 1) 0.1 angle))
+ (send *irtviewer* :draw-objects))
+ (send joint :joint-angle angle)
+ (send *irtviewer* :draw-objects)))
+ ;; check target vector is equal to av
+ (format t "target angle-vector is ~A~%" (send robot :angle-vector))
+ (assert (v= (send robot :angle-vector) av))
+
+ ;; get goal coordinates
+ (setq end-coords (send robot :rarm :end-coords))
+ (send end-coords :draw-on :size 100 :flush t)
+ (setq goal (send end-coords :copy-worldcoords))
+
+ (assert (eps= (elt (send goal :worldpos) 2) 0.0 0.1) "goal should be on the ground ~A" goal)
+ ;; set initial posture
+ (send robot :angle-vector (scale 0.6 av))
+ ;; (send robot :angle-vector (scale 0.0 av))
+
+ ;; start ik
+ (setq *loop* 0)
+ (setq ret (send robot :inverse-kinematics goal :move-target end-coords :stop 20
+ :debug-view t))
+ (assert ret "ik solved")
+ (format t "ik loop was ~A~%" *loop*)
+ (assert (< *loop* 20) "check loop count ()" *loop*)
+ ))
+
+(deftest test-revolute
+ (test-mimic-joint "revolute-1"))
+
+;; joint3: 45 deg offset
+(deftest test-revolute
+ (test-mimic-joint "revolute-2"))
+
+;; mimic joint2 -> joint3, :y->:y multiply: 2, offset: 0
+(deftest test-revolute-mimic-1
+ (test-mimic-joint "revolute-mimic-1"))
+
+;; mimic joint2 -> joint3, :-y->:y multiply: -2, offset: 0
+(deftest test-revolute-mimic-2
+ (test-mimic-joint "revolute-mimic-2"))
+
+;; mimic joint2 -> joint3, :y->:y multiply: 1, offset: 45
+(deftest test-revolute-mimic-3
+ (test-mimic-joint "revolute-mimic-3"))
+
+;; mimic joint2 -> joint3, :-y->:y multiply: -1, offset: -45
+(deftest test-revolute-mimic-4
+ (test-mimic-joint "revolute-mimic-4"))
+
+;; mimic joint1 -> joint3, :y->:y multiply: 2, offset: 0
+(deftest test-revolute-mimic-5
+ (test-mimic-joint "revolute-mimic-5"))
+
+(deftest test-linear
+ (test-mimic-joint "linear-1"))
+
+(deftest test-linear-mimic-1
+ (test-mimic-joint "linear-mimic-1"))
+
+(deftest test-linear-mimic-2
+ (test-mimic-joint "linear-mimic-2"))
+
+(if (sys:list-all-catchers) ;; called from interpreter (progn (load "test-mimic.l") (test-mimic-joint "linear-mimic-1"))
+ (setq lisp::*exit-on-fatal-error* nil)
+ (progn ;; called as roseus argument `roseus test-mimic.l`
+ (run-all-tests)
+ (exit)))
+
diff --git a/euscollada/test/test-mimic.test b/euscollada/test/test-mimic.test
new file mode 100644
index 000000000..7f3572ff1
--- /dev/null
+++ b/euscollada/test/test-mimic.test
@@ -0,0 +1,5 @@
+
+
+