[euscollada] add generate-pr2-custom-limb-test

euscollada/pr2_custom_limb.yaml

@@ -0,0 +1,56 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+torso:
+  - torso_lift_joint : torso-waist-z
+larm:
+  - l_shoulder_pan_joint   : larm-collar-y
+  - l_shoulder_lift_joint  : larm-shoulder-p
+  - l_upper_arm_roll_joint : larm-shoulder-r
+  - l_elbow_flex_joint     : larm-elbow-p
+  - l_forearm_roll_joint   : larm-elbow-r
+  - l_wrist_flex_joint     : larm-wrist-p
+  - l_wrist_roll_joint     : larm-wrist-r
+rarm:
+  - r_shoulder_pan_joint   : rarm-collar-y
+  - r_shoulder_lift_joint  : rarm-shoulder-p
+  - r_upper_arm_roll_joint : rarm-shoulder-r
+  - r_elbow_flex_joint     : rarm-elbow-p
+  - r_forearm_roll_joint   : rarm-elbow-r
+  - r_wrist_flex_joint     : rarm-wrist-p
+  - r_wrist_roll_joint     : rarm-wrist-r
+head:
+  - head_pan_joint  : head-neck-y
+  - head_tilt_joint : head-neck-p
+rhand:
+  - r_gripper_r_finger_joint : rhand-r-finger
+  - r_gripper_r_finger_tip_joint : rhand-r-finger-tip
+  - r_gripper_l_finger_joint : rhand-l-finger
+  - r_gripper_l_finger_tip_joint : rhand-l-finger-tip
+lhand:
+  - l_gripper_r_finger_joint : lhand-r-finger
+  - l_gripper_r_finger_tip_joint : lhand-r-finger-tip
+  - l_gripper_l_finger_joint : lhand-l-finger
+  - l_gripper_l_finger_tip_joint : lhand-l-finger-tip
+
+##
+## end-coords
+##
+larm-end-coords: 
+  parent : l_gripper_tool_frame
+  rotate : [0, 1, 0, 00]
+rarm-end-coords:
+  parent : r_gripper_tool_frame
+  rotate : [0, 1, 0, 00]
+head-end-coords:
+  translate : [0.08, 0, 0.13]
+  rotate    : [0, 1, 0, 90]
+
+##
+## reset-pose
+##
+angle-vector:
+  reset-manip-pose : [300.0, 75.0, 50.0, 110.0, -110.0, -20.0, -10.0, -10.0, -75.0, 50.0, -110.0, -110.0, 20.0, -10.0, -10.0, 0.0, 50.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+  reset-pose : [50.0, 60.0, 74.0, 70.0, -120.0, 20.0, -30.0, 180.0, -60.0, 74.0, -70.0, -120.0, -20.0, -30.0, 180.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+

euscollada/test/euscollada-model-conversion-test.l

@@ -58,6 +58,18 @@
 (deftest pr2-eusmodel-validity-check
   (eusmodel-validity-check-one *pr2*))
 
+(deftest generate-pr2-custom-limb-test
+  (generate-eusmodel-from-collada "/tmp/pr2.dae" "/tmp/pr2_custom_limb.l"
+                                  :yaml-file "$(rospack find euscollada)/pr2_custom_limb.yaml")
+  (assert (probe-file "/tmp/pr2_custom_limb.l") ";; faild to generate /tmp/pr2_custom_limb.l, file does not exist")
+  (load "/tmp/pr2_custom_limb.l")
+  (pr2)
+  (eusmodel-validity-check-one *pr2*)
+  (send *pr2* :reset-pose)
+  (assert (equal (send-all (send *pr2* :rhand :joint-list) :name) (list "r_gripper_r_finger_joint" "r_gripper_r_finger_tip_joint" "r_gripper_l_finger_joint" "r_gripper_l_finger_tip_joint")))
+  (assert (subsetp (send *pr2* :rhand :joint-list) (send *pr2* :joint-list) :test #'equal))
+  )
+
 (deftest generate-sample-robot-model-test
  (when (ros::rospack-find "openhrp3")
   (generate-eusmodel-from-collada