suport calc-jacobian for mimnic joints, with tests

jsk-ros-pkg · Jan 5, 2020 · 03f515c · 03f515c
1 parent ac6166d
commit 03f515c
Show file tree

Hide file tree

Showing 25 changed files with 1,772 additions and 2 deletions.
diff --git 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
@@ -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
@@ -151,20 +151,75 @@
   (: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)) (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
@@ -0,0 +1,136 @@
+<?xml version="1.0"?>
+<robot name="linear_1">
+  <link name="base_link">
+    <visual>
+      <origin xyz="0 0 0.5" rpy="0 0 0" />
+      <geometry>
+        <box size="0.2 0.2 1.0" />
+      </geometry>
+      <material name="red">
+        <color rgba="1 0 0 1" />
+      </material>
+    </visual>
+  </link>
+
+  <link name="link1">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0" />
+      <geometry>
+        <box size="0.2 0.2 0.2" />
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1" />
+      </material>
+    </visual>
+  </link>
+
+  <link name="link2">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 1.5708 0" />
+      <geometry>
+        <box size="0.2 0.2 1.0" />
+      </geometry>
+      <material name="blue">
+        <color rgba="0 0 1 1" />
+      </material>
+    </visual>
+  </link>
+
+  <link name="link3">
+    <visual>
+      <origin xyz="0 0 -0.5" rpy="0 0 0" />
+      <geometry>
+        <box size="0.2 0.2 1.0" />
+      </geometry>
+      <material name="red">
+        <color rgba="1 0 0 1" />
+      </material>
+    </visual>
+  </link>
+
+  <link name="link4">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0" />
+      <geometry>
+        <box size="0.2 0.2 0.2" />
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1" />
+      </material>
+    </visual>
+  </link>
+
+  <link name="link5">
+    <visual>
+      <origin xyz="0 0 0.0828" rpy="0 0 0" />
+      <geometry>
+        <box size="0.2 0.2 0.1656" />
+      </geometry>
+      <material name="blue">
+        <color rgba="0 0 1 1" />
+      </material>
+    </visual>
+  </link>
+
+  <link name="link6">
+    <visual>
+      <origin xyz="0 0 0.1" rpy="0 0 0" />
+      <geometry>
+        <box size="0.2 0.2 0.2" />
+      </geometry>
+      <material name="red">
+        <color rgba="1 0 0 1" />
+      </material>
+    </visual>
+  </link>
+
+  <joint name="joint1" type="revolute">
+    <parent link="base_link"/>
+    <child link="link1"/>
+    <origin xyz="0 0 1.0" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.36" upper="2.36" effort="1" velocity="1" />
+  </joint>
+
+  <joint name="joint2" type="prismatic">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin xyz="0 0 0.3" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2.36" upper="2.36" effort="1" velocity="1" />
+  </joint>
+
+  <joint name="joint3" type="prismatic">
+    <parent link="link2"/>
+    <child link="link3"/>
+    <origin xyz="0.6 0 0" rpy="3.14159 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.36" upper="2.36" effort="1" velocity="1" />
+  </joint>
+
+  <joint name="joint4" type="revolute">
+    <parent link="link3"/>
+    <child link="link4"/>
+    <origin xyz="0 0 0" rpy="0 0 0" />
+    <axis xyz="0 1 0" />
+    <limit lower="-2.36" upper="2.36" effort="1" velocity="1" />
+  </joint>
+
+  <joint name="joint5" type="revolute">
+    <parent link="link4"/>
+    <child link="link5"/>
+    <origin xyz="0 0 0.2" rpy="0 0 0" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2.36" upper="2.36" effort="1" velocity="1" />
+  </joint>
+
+  <joint name="joint6" type="revolute">
+    <parent link="link5"/>
+    <child link="link6"/>
+    <origin xyz="0 0 0.1656" rpy="0 0 0" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.36" upper="2.36" effort="1" velocity="1" />
+  </joint>
+
+
+</robot>
diff --git a/euscollada/test/robots/test-linear-1.yaml 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
Original file line number	Diff line number	Diff line change
Expand Up		@@ -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)