Merge pull request #605 from YuyaNagamatsu/nagamatsu/A0B

[hrpsys_ros_bridge_tutorials/A0B] add A0B interface

hrpsys_ros_bridge_tutorials/CMakeLists.txt

@@ -444,6 +444,14 @@ compile_rbrain_model_for_closed_robots(TQLEG0 TQLEG0 TQLEG0
   --conf-file-option "seq_optional_data_dim: 4"
   )
 
+# A0B
+compile_rbrain_model_for_closed_robots(A0B A0B A0B
+  --robothardware-conf-file-option "pdgains.file_name: ${PROJECT_SOURCE_DIR}/models/PDgains.sav"
+  --conf-file-option "end_effectors: rarm,RARM_JOINT5,WAIST,0.25375,0.0,0.0,0.0,0.0,0.0,0.0,"
+  --conf-dt-option "0.002"
+  --simulation-timestep-option "0.002"
+  )
+
 if(EXISTS ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl)
   compile_openhrp_model(
     ${PROJECT_SOURCE_DIR}/models/TESTMDOFARM.wrl
@@ -502,6 +510,7 @@ generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(URATALEG
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(YSTLEG YSTLEG "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(CHIDORI CHIDORI "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TQLEG0 TQLEG0 "--use-robot-hrpsys-config")
+generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(A0B A0B "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files_for_jsk_closed_rbrain_robots(TABLIS TABLIS "--use-robot-hrpsys-config")
 generate_default_launch_eusinterface_files(
   "$(find hrpsys_ros_bridge_tutorials)/models/TESTMDOFARM.wrl"

hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l

@@ -0,0 +1,15 @@
+(load "package://hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l")
+(require :a0b "package://hrpsys_ros_bridge_tutorials/models/a0b.l")
+
+(defclass a0b-interface
+  :super rtm-ros-robot-interface
+  :slots ())
+(defmethod a0b-interface
+  (:init (&rest args &key ((:controller-timeout ct) nil))
+         (send-super* :init :robot a0b-robot :controller-timeout ct args)))
+
+(defun a0b-init (&rest args)
+  (if (not (boundp '*ri*))
+      (setq *ri* (instance* a0b-interface :init args)))
+  (if (not (boundp '*a0b*))
+      (setq *a0b* (instance a0b-robot :init))))

hrpsys_ros_bridge_tutorials/models/a0b.yaml

@@ -0,0 +1,25 @@
+##
+## - collada_joint_name : euslisp_joint_name (start with :)
+##
+
+rarm:
+  - RARM_JOINT0   : rarm-joint0
+  - RARM_JOINT1   : rarm-joint1
+  - RARM_JOINT2   : rarm-joint2
+  - RARM_JOINT3   : rarm-joint3
+  - RARM_JOINT4   : rarm-joint4
+  - RARM_JOINT5   : rarm-joint5
+
+##
+## end-coords
+##
+rarm-end-coords:
+  translate : [0.25375, 0, 0]
+
+##
+## reset-pose
+##
+angle-vector:
+  zero-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+  reset-pose : [60.0, -90.0, 90.0, -30.0, -90.0, 90.0]
+  reverse-reset-pose : [60.0, 90.0, -90.0, -30.0, 90.0, -90.0]

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/a0b_hrpsys_config.py

@@ -0,0 +1,12 @@
+#!/usr/bin/env python
+
+from urata_hrpsys_config import *
+
+if __name__ == '__main__':
+    hcf = URATAHrpsysConfigurator("A0B")
+    if len(sys.argv) > 2 :
+        hcf.init(sys.argv[1], sys.argv[2])
+    elif len(sys.argv) > 1 :
+        hcf.init(sys.argv[1])
+    else :
+        hcf.init()

hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py

@@ -56,6 +56,9 @@ def defJointGroups (self):
             head_group = ['head', ['HEAD_JOINT0', 'HEAD_JOINT1']]
             torso_group = ['torso', ['CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2']]
             self.Groups = [rarm_group, larm_group, rleg_group, lleg_group, head_group, torso_group]
+        elif self.ROBOT_NAME == "A0B":
+            rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5']]
+            self.Groups = [rarm_group]
         else:
             rarm_group = ['rarm', ['RARM_JOINT0', 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6', 'RARM_JOINT7']]
             larm_group = ['larm', ['LARM_JOINT0', 'LARM_JOINT1', 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6', 'LARM_JOINT7']]
@@ -804,6 +807,12 @@ def tablisLoadTestPose (self):
     def tablisInitPose (self):
         return [0]*len(self.tablisResetPose())
 
+    def a0bResetPose(self):
+        return [math.radians(60), math.radians(-90), math.radians(90), math.radians(-30), math.radians(-90), math.radians(90)]
+
+    def a0bInitPose (self):
+        return [0]*len(self.a0bResetPose())
+
     # (mapcar #'deg2rad (concatenate cons (send *robot* :reset-landing-pose)))
     def jaxonResetLandingPose (self):
         return [0.004318,0.005074,-0.134838,1.18092,-0.803855,-0.001463,0.004313,0.005079,-0.133569,1.18206,-0.806262,-0.001469,0.003782,-0.034907,0.004684,0.0,0.0,0.0,0.698132,-0.349066,-0.087266,-1.39626,0.0,0.0,-0.349066,0.0,0.698132,0.349066,0.087266,-1.39626,0.0,0.0,-0.349066]
@@ -830,6 +839,8 @@ def setResetPose(self):
             self.seq_svc.setJointAngles(self.tqleg0ResetPose(), 5.0)
         elif self.ROBOT_NAME == "TABLIS":
             self.seq_svc.setJointAngles(self.tablisResetPose(), 5.0)
+        elif self.ROBOT_NAME == "A0B":
+            self.seq_svc.setJointAngles(self.a0bResetPose(), 5.0)
 
     def setLoadTestPose(self):
         if self.ROBOT_NAME == "TABLIS":
@@ -858,6 +869,8 @@ def setInitPose(self):
             self.seq_svc.setJointAngles(self.tqleg0InitPose(), 10.0)
         elif self.ROBOT_NAME == "TABLIS":
             self.seq_svc.setJointAngles(self.tablisInitPose(), 10.0)
+        elif self.ROBOT_NAME == "A0B":
+            self.seq_svc.setJointAngles(self.a0bInitPose(), 10.0)
 
     def setCollisionFreeInitPose(self):
         if self.ROBOT_NAME == "JAXON_BLUE":