Merge pull request #225 from jsk-ros-pkg/add_mimic

add mimic joint support for collada2eus

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)

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)                                                                \
+  )									\
+"

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

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,58 @@ void ModelEuslisp::printJoint (boost::shared_ptr<const Joint> joint) {
   fprintf(fp, "                     ))\n");
 }
 
+void ModelEuslisp::printMimicJoints () {
+  fprintf(fp, "\n     ;; mimic joint re-definition\n");
+#if URDFDOM_1_0_0_API
+  map<JointSharedPtr, list<JointSharedPtr> > mimic_joint_list;
+#else
+  map<boost::shared_ptr<Joint>, list<boost::shared_ptr<Joint> > > mimic_joint_list;
+#endif
+#if URDFDOM_1_0_0_API
+  for (map<string, JointSharedPtr>::iterator joint = robot->joints_.begin();
+#else
+  for (map<string, boost::shared_ptr<Joint> >::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<JointSharedPtr, list<JointSharedPtr> >::iterator mimic = mimic_joint_list.begin();
+#else
+  for (map<boost::shared_ptr<Joint>, list<boost::shared_ptr<Joint> > >::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<JointSharedPtr>::iterator joint = mimic->second.begin();
+#else
+    for (list<boost::shared_ptr<Joint> >::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");
+    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");
+  }
+}
+
+
 void ModelEuslisp::printEndCoords () {
   // TODO: end coords from collada ...
   fprintf(fp, "     ;; end coords from yaml file\n");

euscollada/src/euscollada-robot.l

@@ -136,3 +136,118 @@
           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))
+  )
+
+(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))
+(defmethod rotational-mimic-joint
+  (: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))))
+           ))
+  (: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
+       :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)
+         (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))))
+           ))
+  (: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)
+         ))
+  )

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))
     )