update spot-utils.l, spot.yml for spot-with arm. This requires jsk-ro…

…s-pkg/jsk_model_tools#249
k-okada · Sep 4, 2022 · a55f7d3 · a55f7d3
1 parent 81ce316
commit a55f7d3
Show file tree

Hide file tree

Showing 2 changed files with 52 additions and 23 deletions.
diff --git a/jsk_spot_robot/spoteus/spot-utils.l b/jsk_spot_robot/spoteus/spot-utils.l
@@ -17,7 +17,21 @@
                     :_make_instance_rear_right_lower_leg_geom0))
      (rplacd (assoc b (send (class self) :methods))
              (cdr (subst '(list :diffuse #f(1.00 0.84 0.32 0))
-                         '(list :diffuse (float-vector 1.0 1.0 1.0 0.0))
+                         '(list :diffuse (float-vector 1.000000 1.000000 1.000000 1.000000))
+                         (assoc b (send  (class self) :methods)))))
+     )
+   (dolist (b (list :_make_instance_arm0.link_sh0_geom0
+                    :_make_instance_arm0.link_sh1_geom0
+                    :_make_instance_arm0.link_hr0_geom0
+;                    :_make_instance_arm0.link_el0_geom0
+                    :_make_instance_arm0.link_el1_geom0
+                    :_make_instance_arm0.link_wr0_geom0
+                    :_make_instance_arm0.link_wr1_geom0
+                    :_make_instance_arm0.link_fngr_geom0
+                    ))
+     (rplacd (assoc b (send (class self) :methods))
+             (cdr (subst '(list :diffuse #f(1.00 0.84 0.32 0))
+                         '(list :diffuse (float-vector 0.200000 0.200000 0.200000 1.000000))
                          (assoc b (send  (class self) :methods)))))
      )
    (dolist (b (list :_make_instance_front_left_hip_geom0
@@ -26,7 +40,7 @@
                     :_make_instance_rear_right_hip_geom0))
      (rplacd (assoc b (send (class self) :methods))
              (cdr (subst '(list :diffuse #f(0.1 0.1 0.1 0))
-                         '(list :diffuse (float-vector 1.0 1.0 1.0 0.0))
+                         '(list :diffuse (float-vector 1.000000 1.000000 1.000000 1.000000))
                          (assoc b (send  (class self) :methods)))))
      )
    (send* self :init-orig args))

diff --git a/jsk_spot_robot/spoteus/spot.yaml b/jsk_spot_robot/spoteus/spot.yaml
@@ -2,39 +2,54 @@
 ## - collada_joint_name : euslisp_joint_name (start with :)
 ##
 
-larm:
-  - front_left_hip_x : larm-shoulder-r
-  - front_left_hip_y : larm-shoulder-p
-  - front_left_knee  : larm-elbow-p
-rarm:
-  - front_right_hip_x : rarm-shoulder-r
-  - front_right_hip_y : rarm-shoulder-p
-  - front_right_knee  : rarm-elbow-p
-lleg:
-  - rear_left_hip_x : lleg-crotch-r
-  - rear_left_hip_y : lleg-crotch-p
-  - rear_left_knee  : lleg-knee-p
-rleg:
-  - rear_right_hip_x : rleg-crotch-r
-  - rear_right_hip_y : rleg-crotch-p
-  - rear_right_knee  : rleg-knee-p
+lfleg:
+  - front_left_hip_x : lfleg-crotch-r
+  - front_left_hip_y : lfleg-crotch-p
+  - front_left_knee  : lfleg-knee-p
+rfleg:
+  - front_right_hip_x : rfleg-crotch-r
+  - front_right_hip_y : rfleg-crotch-p
+  - front_right_knee  : rfleg-knee-p
+lrleg:
+  - rear_left_hip_x : lrleg-crotch-r
+  - rear_left_hip_y : lrleg-crotch-p
+  - rear_left_knee  : lrleg-knee-p
+rrleg:
+  - rear_right_hip_x : rrleg-crotch-r
+  - rear_right_hip_y : rrleg-crotch-p
+  - rear_right_knee  : rrleg-knee-p
+arm:
+  - arm0.sh0 : arm-shoulder-y
+  - arm0.sh1 : arm-shoulder-p
+  - arm0.hr0 : arm-shoulder-r
+  - arm0.el0 : arm-elbow-p
+  - arm0.el1 : arm-elbow-r
+  - arm0.wr0 : arm-wrist-p
+  - arm0.wr1 : arm-wrist-r
+hand:
+  - arm0.f1x : hand-gripper-p
+
 
 angle-vector:
-  reset-pose : [0.0, 45.0, -90.0, 0.0, 45.0, -90.0, 0.0, 45.0, -90.0, 0.0, 45.0, -90.0]
+  reset-pose : [0.0, 45.0, -90.0, 0.0, 45.0, -90.0, 0.0, 45.0, -90.0, 0.0, 45.0, -90.0, 0.0,  -170.0, 0.0, 160.0, 0.0, 10.0, 0.0,  0]
 
-larm-end-coords:
+lfleg-end-coords:
   parent : front_left_lower_leg
   translate : [0, 0, -0.38]
   rotate : [0, 1, 0, 0]
-rarm-end-coords:
+rfleg-end-coords:
   parent : front_right_lower_leg
   translate : [0, 0, -0.38]
   rotate : [0, 1, 0, 0]
-lleg-end-coords:
+lrleg-end-coords:
   parent : rear_left_lower_leg
   translate : [0, 0, -0.38]
   rotate : [0, 1, 0, 0]
-rleg-end-coords:
+rrleg-end-coords:
   parent : rear_right_lower_leg
   translate : [0, 0, -0.38]
   rotate : [0, 1, 0, 0]
+arm-end-coords:
+  parent: arm0.link_wr1
+  translate : [0.175, 0, -0.03]
+  rotate : [1, 0, 0, 90]