diff --git a/hrpsys_ros_bridge_tutorials/CMakeLists.txt b/hrpsys_ros_bridge_tutorials/CMakeLists.txt index 5e5d8a00..a1e5b88f 100644 --- a/hrpsys_ros_bridge_tutorials/CMakeLists.txt +++ b/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" diff --git a/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l b/hrpsys_ros_bridge_tutorials/euslisp/a0b-interface.l new file mode 100644 index 00000000..d42f2650 --- /dev/null +++ b/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)))) diff --git a/hrpsys_ros_bridge_tutorials/models/a0b.yaml b/hrpsys_ros_bridge_tutorials/models/a0b.yaml new file mode 100644 index 00000000..ca3e2c9f --- /dev/null +++ b/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] diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/a0b_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/a0b_hrpsys_config.py new file mode 100755 index 00000000..fe9534ec --- /dev/null +++ b/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() diff --git a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py b/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py index 91eb4d0f..a64c7eb8 100755 --- a/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/urata_hrpsys_config.py +++ b/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":