From eb73e8e9fc95aa028a11bcfdd341b8a6bd677b9e Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Fri, 6 Dec 2019 20:11:56 +0900 Subject: [PATCH 1/3] support mimic joints introduce mimic-joint-parmas, rotational-mimic-joint, linear-mimic-joint class --- euscollada/pr2-mimic.sh | 51 ++++++++++++++++++++++++ euscollada/pr2-mimic.yaml | 8 ++++ euscollada/src/collada2eus_urdfmodel.cpp | 47 ++++++++++++++++++++++ euscollada/src/euscollada-robot.l | 46 +++++++++++++++++++++ 4 files changed, 152 insertions(+) create mode 100755 euscollada/pr2-mimic.sh create mode 100644 euscollada/pr2-mimic.yaml diff --git a/euscollada/pr2-mimic.sh b/euscollada/pr2-mimic.sh new file mode 100755 index 000000000..5a4fe4c63 --- /dev/null +++ b/euscollada/pr2-mimic.sh @@ -0,0 +1,51 @@ +#!/bin/bash + +if [ -w `rospack find euscollada` ]; then + cd `rospack find euscollada` +else + echo "euscollada direcotry is not writable, so write to current directory" +fi + +#rosrun collada_urdf_jsk_patch urdf_to_collada `rospack find pr2_mechanism_model`/pr2.urdf pr2.dae +if [ -e `rospack find pr2_mechanism_model`/pr2.urdf ]; +then + rosrun collada_urdf urdf_to_collada `rospack find pr2_mechanism_model`/pr2.urdf pr2.dae +else + rosrun xacro xacro.py `rospack find pr2_description`/robots/pr2.urdf.xacro > /tmp/pr2.$$.urdf + rosrun collada_urdf urdf_to_collada /tmp/pr2.$$.urdf pr2.dae +fi +if [ "$?" != 0 ] ; then exit ; fi + +rosrun euscollada collada2eus pr2.dae `rospack find euscollada`/pr2-mimic.yaml pr2-mimic.l.$$.tmp; mv pr2-mimic.l.$$.tmp pr2-mimic.l +if [ "$?" != 0 ] ; then exit ; fi + +rosrun roseus roseus lib/llib/unittest.l "(init-unit-test)" "\ +(progn \ + (load \"pr2-mimic.l\") \ + (if (and x::*display* (> x::*display* 0) (not (boundp '*irtviewer*))) (make-irtviewer :title \"pr2.sh\")) \ + (if (not (boundp '*pr2*)) (pr2)) \ + \ + (when (boundp '*irtviewer*) (objects (list *pr2*))) \ + (setq i 0.0) \ + (do-until-key \ + (send *pr2* :r_gripper :joint-angle i) \ + (print (list i (send *pr2* :r_gripper_l_finger_joint :joint-angle) \ + (send *pr2* :r_gripper_r_finger_joint :joint-angle) \ + (send *pr2* :r_gripper_l_finger_tip_joint :joint-angle) \ + (send *pr2* :r_gripper_r_finger_tip_joint :joint-angle))) \ + (assert (eps= (send *pr2* :r_gripper_r_finger_joint :joint-angle) \ + (send *pr2* :r_gripper_l_finger_joint :joint-angle))) \ + (assert (eps= (send *pr2* :r_gripper_r_finger_joint :joint-angle) \ + (send *pr2* :r_gripper_l_finger_tip_joint :joint-angle))) \ + (assert (eps= (send *pr2* :r_gripper_r_finger_joint :joint-angle) \ + (send *pr2* :r_gripper_r_finger_tip_joint :joint-angle))) \ + (when (boundp '*irtviewer*) \ + (send *irtviewer* :draw-objects) \ + ) \ + (incf i) \ + (when (> i 30) (exit 0)) \ + ) \ + (if (boundp '*irtviewer*) (send-all (send *pr2* :links) :draw-on :flush t))\ + (exit) \ + ) \ +" diff --git a/euscollada/pr2-mimic.yaml b/euscollada/pr2-mimic.yaml new file mode 100644 index 000000000..1089a5f8a --- /dev/null +++ b/euscollada/pr2-mimic.yaml @@ -0,0 +1,8 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +larm: + - r_gripper_l_finger_joint : r_gripper +rarm: + - l_gripper_l_finger_joint : l_gripper diff --git a/euscollada/src/collada2eus_urdfmodel.cpp b/euscollada/src/collada2eus_urdfmodel.cpp index c2c78746e..db4f96fbf 100644 --- a/euscollada/src/collada2eus_urdfmodel.cpp +++ b/euscollada/src/collada2eus_urdfmodel.cpp @@ -252,6 +252,7 @@ class ModelEuslisp { #endif void printLinks (); void printJoints (); + void printMimicJoints (); void printEndCoords(); void printSensors(); void printSensorLists(); @@ -761,6 +762,8 @@ void ModelEuslisp::printRobotMethods() { printJoints(); + printMimicJoints(); + printEndCoords(); printSensors(); @@ -987,6 +990,50 @@ void ModelEuslisp::printJoint (boost::shared_ptr joint) { fprintf(fp, " ))\n"); } +void ModelEuslisp::printMimicJoints () { + fprintf(fp, "\n ;; mimic joint re-definition\n"); +#if URDFDOM_1_0_0_API + map > mimic_joint_list; +#else + map, list > > mimic_joint_list; +#endif +#if URDFDOM_1_0_0_API + for (map::iterator joint = robot->joints_.begin(); +#else + for (map >::iterator joint = robot->joints_.begin(); +#endif + joint != robot->joints_.end(); joint++) { + if (joint->second->mimic) { + mimic_joint_list[robot->joints_[joint->second->mimic->joint_name]].push_back(joint->second); + } + } + +#if URDFDOM_1_0_0_API + for (map >::iterator mimic = mimic_joint_list.begin(); +#else + for (map, list > >::iterator mimic = mimic_joint_list.begin(); +#endif + mimic != mimic_joint_list.end(); mimic++){ + bool linear = (mimic->first->type==Joint::PRISMATIC); + fprintf(fp, " ;; re-define %s as mimic-joint\n", mimic->first->name.c_str()); + fprintf(fp, " (let (tmp-mimic-joint)\n"); + fprintf(fp, " (setq tmp-mimic-joint (replace-object (instance %s :init :parent-link (make-cascoords) :child-link (make-cascoords) :max-joint-velocity 0 :max-joint-torque 0) %s_jt))\n", linear?"linear-mimic-joint":"rotational-mimic-joint", mimic->first->name.c_str()); + fprintf(fp, " (setq %s_jt tmp-mimic-joint))\n", mimic->first->name.c_str()); + fprintf(fp, " (setq (%s_jt . mimic-joints)\n", mimic->first->name.c_str()); + fprintf(fp, " (list\n"); +#if URDFDOM_1_0_0_API + for (list::iterator joint = mimic->second.begin(); +#else + for (list >::iterator joint = mimic->second.begin(); +#endif + joint != mimic->second.end(); joint++){ + 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"); + } +} + + void ModelEuslisp::printEndCoords () { // TODO: end coords from collada ... fprintf(fp, " ;; end coords from yaml file\n"); diff --git a/euscollada/src/euscollada-robot.l b/euscollada/src/euscollada-robot.l index 13850780c..5d22b6ef6 100644 --- a/euscollada/src/euscollada-robot.l +++ b/euscollada/src/euscollada-robot.l @@ -136,3 +136,49 @@ org-mesh))) )) ) + +;; copy mimic-joint class definition from euscollada/src/euscollada-robot.l +;; This mimic-joint class is for mimic-joints in URDF file +(defclass mimic-joint-param + :super propertied-object + :slots (joint multiplier offset)) +(defmethod mimic-joint-param + (:init (j &key ((:multiplier m) 1) ((:offset o) 0)) + (setq joint j multiplier m offset o) + self) + (:joint (&rest args) (forward-message-to joint args)) + (:multiplier (&rest args) (forward-message-to multiplier args)) + (:offset (&rest args) (forward-message-to offset args)) + ) + +(defclass rotational-mimic-joint + :super rotational-joint + :slots (mimic-joints)) +(defmethod rotational-mimic-joint + (:init (&rest args &key ((:mimic-joints mjs)) &allow-other-keys) + (send-super* :init args) + (setq mimic-joints mjs) + 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)))) + )) + ) + +(defclass linear-mimic-joint + :super linear-joint + :slots (mimic-joints)) +(defmethod linear-mimic-joint + (:init (&rest args &key ((:mimic-joints mjs)) &allow-other-keys) + (send-super* :init args) + (setq mimic-joints mjs) + 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)))) + )) + ) From 03f515c97bd54e67d89c2999e4366056bafce184 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sun, 5 Jan 2020 16:39:15 +0900 Subject: [PATCH 2/3] suport calc-jacobian for mimnic joints, with tests --- euscollada/CMakeLists.txt | 1 + euscollada/src/collada2eus_urdfmodel.cpp | 8 + euscollada/src/euscollada-robot.l | 73 +++++++++- euscollada/test/robots/test-linear-1.urdf | 136 +++++++++++++++++ euscollada/test/robots/test-linear-1.yaml | 19 +++ .../test/robots/test-linear-mimic-1.urdf | 137 ++++++++++++++++++ .../test/robots/test-linear-mimic-1.yaml | 18 +++ .../test/robots/test-linear-mimic-2.urdf | 137 ++++++++++++++++++ .../test/robots/test-linear-mimic-2.yaml | 18 +++ euscollada/test/robots/test-revolute-1.urdf | 136 +++++++++++++++++ euscollada/test/robots/test-revolute-1.yaml | 19 +++ euscollada/test/robots/test-revolute-2.urdf | 136 +++++++++++++++++ euscollada/test/robots/test-revolute-2.yaml | 19 +++ .../test/robots/test-revolute-mimic-1.urdf | 137 ++++++++++++++++++ .../test/robots/test-revolute-mimic-1.yaml | 18 +++ .../test/robots/test-revolute-mimic-2.urdf | 137 ++++++++++++++++++ .../test/robots/test-revolute-mimic-2.yaml | 18 +++ .../test/robots/test-revolute-mimic-3.urdf | 137 ++++++++++++++++++ .../test/robots/test-revolute-mimic-3.yaml | 18 +++ .../test/robots/test-revolute-mimic-4.urdf | 137 ++++++++++++++++++ .../test/robots/test-revolute-mimic-4.yaml | 18 +++ .../test/robots/test-revolute-mimic-5.urdf | 137 ++++++++++++++++++ .../test/robots/test-revolute-mimic-5.yaml | 18 +++ euscollada/test/test-mimic.l | 137 ++++++++++++++++++ euscollada/test/test-mimic.test | 5 + 25 files changed, 1772 insertions(+), 2 deletions(-) create mode 100644 euscollada/test/robots/test-linear-1.urdf create mode 100644 euscollada/test/robots/test-linear-1.yaml create mode 100644 euscollada/test/robots/test-linear-mimic-1.urdf create mode 100644 euscollada/test/robots/test-linear-mimic-1.yaml create mode 100644 euscollada/test/robots/test-linear-mimic-2.urdf create mode 100644 euscollada/test/robots/test-linear-mimic-2.yaml create mode 100644 euscollada/test/robots/test-revolute-1.urdf create mode 100644 euscollada/test/robots/test-revolute-1.yaml create mode 100644 euscollada/test/robots/test-revolute-2.urdf create mode 100644 euscollada/test/robots/test-revolute-2.yaml create mode 100644 euscollada/test/robots/test-revolute-mimic-1.urdf create mode 100644 euscollada/test/robots/test-revolute-mimic-1.yaml create mode 100644 euscollada/test/robots/test-revolute-mimic-2.urdf create mode 100644 euscollada/test/robots/test-revolute-mimic-2.yaml create mode 100644 euscollada/test/robots/test-revolute-mimic-3.urdf create mode 100644 euscollada/test/robots/test-revolute-mimic-3.yaml create mode 100644 euscollada/test/robots/test-revolute-mimic-4.urdf create mode 100644 euscollada/test/robots/test-revolute-mimic-4.yaml create mode 100644 euscollada/test/robots/test-revolute-mimic-5.urdf create mode 100644 euscollada/test/robots/test-revolute-mimic-5.yaml create mode 100644 euscollada/test/test-mimic.l create mode 100644 euscollada/test/test-mimic.test 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 @@ + + + From 628e63d31213b6d73ee5899b7d91f317554a3198 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Mon, 6 Jan 2020 10:07:11 +0900 Subject: [PATCH 3/3] not sure why but we need double check 'strm' since https://travis-ci.org/jsk-ros-pkg/jsk_model_tools/builds/632871892 --- euscollada/test/euscollada-model-conversion-test.l | 1 + 1 file changed, 1 insertion(+) diff --git a/euscollada/test/euscollada-model-conversion-test.l b/euscollada/test/euscollada-model-conversion-test.l index bb1195173..20e95c459 100644 --- a/euscollada/test/euscollada-model-conversion-test.l +++ b/euscollada/test/euscollada-model-conversion-test.l @@ -33,6 +33,7 @@ (assert (select-stream (list strm) 120) ";; ;; faild to generate /tmp/pr2.dae within 120 second") (close strm)) (let ((strm (piped-fork "rosrun xacro xacro.py `rospack find pr2_description`/robots/pr2.urdf.xacro > /tmp/pr2.$$.urdf && rosrun collada_urdf urdf_to_collada /tmp/pr2.$$.urdf /tmp/pr2.dae"))) + (format *error-output* ";; check /tmp/pr2.dae file is generetad within 120 second .. ~A~%" (select-stream (list strm) 120)) ;; not sure why but we need double check since https://travis-ci.org/jsk-ros-pkg/jsk_model_tools/builds/632871892 (assert (select-stream (list strm) 120) ";; faild to generate /tmp/pr2.dae within 120 second") (close strm)) )