[hrpsys_choreonoid] add SampleRobot

Author: Naoki-Hiraoka

README.md

@@ -43,9 +43,35 @@ source devel/setup.bash
 ```
 
 ### run test
+If you get error, try `export ORBgiopMaxMsgSize=2097152000`.
+
+#### SampleRobot
+```
+rtmlaunch hrpsys_choreonoid samplerobot_choreonoid.launch
+```
+Launch another terminal and send command to robot. (python)
+```
+ipython -i `rospack find hrpsys_choreonoid`/scripts/samplerobot_rh_setup.py "SampleRobot(Robot)0"
+hcf.abc_svc.goPos(1,0,0)
+```
+Launch another terminal and send command to robot. (euslisp)
+```
+roseus `rospack find hrpsys_ros_bridge`/euslisp/samplerobot-interface.l
+(samplerobot-init)
+(send *ri* :go-pos 1 0 0)
+(setq *robot* *sr*)
+(objects *robot*)
+(send *robot* :reset-manip-pose)
+(send *ri* :angle-vector (send *robot* :angle-vector) 5000)
+```
+
+#### JAXON
+Build hrpsys_choreonoid_tutorials
 ```
 catkin build hrpsys_choreonoid_tutorials
 source devel/setup.bash
+```
+```
 rtmlaunch hrpsys_choreonoid_tutorials jaxon_jvrc_choreonoid.launch
 ```
 Launch another terminal and send command to robot. (python)
@@ -66,8 +92,6 @@ roseus `rospack find hrpsys_choreonoid_tutorials`/euslisp/jaxon_jvrc-interface.l
 (send *ri* :angle-vector (send *robot* :angle-vector) 5000)
 ```
 
-If you get error, try `export ORBgiopMaxMsgSize=2097152000`.
-
 See also [hrpsys_choreonoid_tutorials/README.md](/hrpsys_choreonoid_tutorials/README.md)
 
 ---

hrpsys_choreonoid/CMakeLists.txt

@@ -96,3 +96,13 @@ endif()
 add_subdirectory(iob)
 
 add_custom_target(hrpsys_choreonoid_iob ALL DEPENDS RobotHardware_choreonoid)
+
+
+set(HRPSYS_CHOREONOID_DIRECTORY ${PROJECT_SOURCE_DIR})
+set(OPENHRP_SAMPLE_DIR ${openhrp3_PREFIX}/share/OpenHRP-3.1/sample)
+
+###
+#SampleRobot conid
+###
+configure_file(${PROJECT_SOURCE_DIR}/config/SampleRobot_RH_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/SampleRobot_RH_LOAD_OBJ.cnoid @ONLY)
+configure_file(${PROJECT_SOURCE_DIR}/launch/samplerobot_choreonoid.launch.in ${PROJECT_SOURCE_DIR}/launch/samplerobot_choreonoid.launch @ONLY) #OPENHRP_SAMPLE_DIR is difficult to find in roslaunch api

hrpsys_choreonoid/config/BodyRTC_SampleRobot.RH.conf

@@ -0,0 +1,35 @@
+##
+name-server = localhost:15005
+##
+## PD Controller
+##     in: angleRef, angle
+##     out: torque
+##
+in-port = tauIn:JOINT_TORQUE
+out-port = angleOut:JOINT_VALUE
+out-port = qvel:JOINT_VELOCITY
+out-port = torque:JOINT_TORQUE
+# out-port = ddq:JOINT_ACCELERATION
+connection = tauIn:RobotHardware_choreonoid0:torqueOut
+connection = angleOut:RobotHardware_choreonoid0:angleIn
+connection = qvel:RobotHardware_choreonoid0:qvel_sim
+connection = torque:RobotHardware_choreonoid0:torque_sim
+###
+# debug ## ground truth robot potition
+###
+out-port = WAIST:WAIST:ABS_TRANSFORM
+####
+# sensors
+####
+out-port = rfsensor_sim:rfsensor:FORCE_SENSOR
+out-port = lfsensor_sim:lfsensor:FORCE_SENSOR
+out-port = rhsensor_sim:rhsensor:FORCE_SENSOR
+out-port = lhsensor_sim:lhsensor:FORCE_SENSOR
+out-port = gsensor_sim:gsensor:ACCELERATION_SENSOR2
+out-port = gyrometer_sim:gyrometer:RATE_GYRO_SENSOR2
+connection = rfsensor_sim:RobotHardware_choreonoid0:rfsensor_sim
+connection = lfsensor_sim:RobotHardware_choreonoid0:lfsensor_sim
+connection = rhsensor_sim:RobotHardware_choreonoid0:rhsensor_sim
+connection = lhsensor_sim:RobotHardware_choreonoid0:lhsensor_sim
+connection = gsensor_sim:RobotHardware_choreonoid0:gsensor_sim
+connection = gyrometer_sim:RobotHardware_choreonoid0:gyrometer_sim

hrpsys_choreonoid/config/SampleRobotCustomizer.yaml

@@ -0,0 +1,5 @@
+bush:
+  springT: 3.3e5
+  dampingT: 3.3e2
+  springR: 2.5e3
+  dampingR: 2.5