[hrpsys_choreonoid][hrpsys_choreonoid_tutorials] add SampleRobot chor…

…eonoid

hrpsys_choreonoid/iob/iob.cpp

@@ -14,6 +14,8 @@
 #include <rtm/DataInPort.h>
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
+#include <hrpModel/ModelLoaderUtil.h>
+#include <hrpModel/Sensor.h>
 
 #include "RobotHardware_choreonoid.h"
 
@@ -82,6 +84,11 @@ static InPort<TimedDoubleSeq> *ip_lhsensor_sim;
 static InPort<TimedAcceleration3D> *ip_gsensor_sim;
 static InPort<TimedAngularVelocity3D> *ip_gyrometer_sim;
 
+static int rfsensor_id;
+static int lfsensor_id;
+static int rhsensor_id;
+static int lhsensor_id;
+
 static void readGainFile();
 
 static double dt;
@@ -622,6 +629,44 @@ int open_iob(void)
       servo[i] = 1;
     }
 
+    // get sensorId of force sensors
+    {
+      RTC::Manager& rtcManager = RTC::Manager::instance();
+      std::string nameServer = rtcManager.getConfig()["corba.nameservers"];
+      int comPos = nameServer.find(",");
+      if (comPos < 0){
+        comPos = nameServer.length();
+      }
+      nameServer = nameServer.substr(0, comPos);
+      RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str());
+      RTC::Properties& prop = self_ptr->getProperties();
+      hrp::BodyPtr m_robot = hrp::BodyPtr(new hrp::Body());
+      if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), 
+                                   CosNaming::NamingContext::_duplicate(naming.getRootContext())
+                                   )){
+        std::cerr << "failed to load model[" << prop["model"] << "]" << std::endl;
+        return RTC::RTC_ERROR;
+      }
+
+      if (m_robot->sensor<hrp::ForceSensor>("rfsensor") && m_robot->sensor<hrp::ForceSensor>("lfsensor") && m_robot->sensor<hrp::ForceSensor>("rhsensor") && m_robot->sensor<hrp::ForceSensor>("lhsensor") ){
+        rfsensor_id = m_robot->sensor<hrp::ForceSensor>("rfsensor")->id;
+        lfsensor_id = m_robot->sensor<hrp::ForceSensor>("lfsensor")->id;
+        rhsensor_id = m_robot->sensor<hrp::ForceSensor>("rhsensor")->id;
+        lhsensor_id = m_robot->sensor<hrp::ForceSensor>("lhsensor")->id;
+      } else if (m_robot->sensor<hrp::ForceSensor>("rfsensor") && m_robot->sensor<hrp::ForceSensor>("lfsensor") ){
+        rfsensor_id = m_robot->sensor<hrp::ForceSensor>("rfsensor")->id;
+        lfsensor_id = m_robot->sensor<hrp::ForceSensor>("lfsensor")->id;
+        rhsensor_id = 2;
+        lhsensor_id = 3;
+      } else {
+        std::cerr << "could not find rfsensor/lfsensor/rhsensor/lhsensor. use default sensorId" << std::endl;
+        rfsensor_id = 0;
+        lfsensor_id = 1;
+        rhsensor_id = 2;
+        lhsensor_id = 3;
+      }
+    }
+
     std::cerr << "choreonoid IOB is opened" << std::endl;
     return TRUE;
 }
@@ -667,7 +712,7 @@ void iob_update(void)
       if(number_of_force_sensors() >= 1) {
         for(int i = 0; i < 6; i++) {
           //forces[0][i] = m_rfsensor_sim.data[i];
-          force_queue[force_counter][0][i] = m_rfsensor_sim.data[i];
+          force_queue[force_counter][rfsensor_id][i] = m_rfsensor_sim.data[i];
         }
       }
     }
@@ -676,7 +721,7 @@ void iob_update(void)
       if(number_of_force_sensors() >= 2) {
         for(int i = 0; i < 6; i++) {
           //forces[1][i] = m_lfsensor_sim.data[i];
-          force_queue[force_counter][1][i] = m_lfsensor_sim.data[i];
+          force_queue[force_counter][lfsensor_id][i] = m_lfsensor_sim.data[i];
         }
       }
     }
@@ -685,7 +730,7 @@ void iob_update(void)
       if(number_of_force_sensors() >= 3) {
         for(int i = 0; i < 6; i++) {
           //forces[2][i] = m_rhsensor_sim.data[i];
-          force_queue[force_counter][2][i] = m_rhsensor_sim.data[i];
+          force_queue[force_counter][rhsensor_id][i] = m_rhsensor_sim.data[i];
         }
       }
     }
@@ -694,7 +739,7 @@ void iob_update(void)
       if(number_of_force_sensors() >= 4) {
         for(int i = 0; i < 6; i++) {
           //forces[3][i] = m_lhsensor_sim.data[i];
-          force_queue[force_counter][3][i] = m_lhsensor_sim.data[i];
+          force_queue[force_counter][lhsensor_id][i] = m_lhsensor_sim.data[i];
         }
       }
     }

hrpsys_choreonoid_tutorials/CMakeLists.txt

@@ -170,6 +170,14 @@ install(CODE
 set(JVRC_RTC_DIRECTORY ${hrpsys_choreonoid_SOURCE_PREFIX})
 set(JVRC_CONF_DIRECTORY ${PROJECT_SOURCE_DIR}/config)
 
+###
+#SampleRobot conid
+###
+if (${hrp2_models_FOUND})
+configure_file(${PROJECT_SOURCE_DIR}/config/SampleRobot_LOAD_OBJ.cnoid.in ${PROJECT_SOURCE_DIR}/config/SampleRobot_LOAD_OBJ.cnoid @ONLY)
+endif()
+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
+
 ###
 #JAXON_RED conid
 ###

hrpsys_choreonoid_tutorials/config/BodyRTC_SampleRobot.RH.conf

@@ -0,0 +1,92 @@
+##
+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
+# out-port = headq:motor_joint:JOINT_VALUE
+####
+# 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
+####
+# vision
+####
+# out-port = HEAD_RANGE:HEAD_RANGE:RANGE_SENSOR
+# out-port = HEAD_LEFT_DEPTH:HEAD_LEFT_CAMERA:CAMERA_RANGE
+# out-port = HEAD_LEFT_CAMERA:HEAD_LEFT_CAMERA:CAMERA_IMAGE
+# out-port = HEAD_RIGHT_CAMERA:HEAD_RIGHT_CAMERA:CAMERA_IMAGE
+#out-port = CHEST_CAMERA:CHEST_CAMERA:CAMERA_IMAGE
+#out-port = LARM_CAMERA:LARM_CAMERA:CAMERA_IMAGE
+#out-port = RARM_CAMERA:RARM_CAMERA:CAMERA_IMAGE
+#out-port = LARM_CAMERA_N:LARM_CAMERA_N:CAMERA_IMAGE
+#out-port = RARM_CAMERA_N:RARM_CAMERA_N:CAMERA_IMAGE
+
+####
+# constraint
+####
+# out-port = F_BODY:WAIST:CONSTRAINT_FORCE
+# out-port = F_CHEST_LINK0:CHEST_JOINT0:CONSTRAINT_FORCE
+# out-port = F_CHEST_LINK1:CHEST_JOINT1:CONSTRAINT_FORCE
+# out-port = F_CHEST_LINK2:CHEST_JOINT2:CONSTRAINT_FORCE
+# out-port = F_HEAD_LINK0:HEAD_JOINT0:CONSTRAINT_FORCE
+# out-port = F_HEAD_LINK1:HEAD_JOINT1:CONSTRAINT_FORCE
+# out-port = F_LARM_LINK0:LARM_JOINT0:CONSTRAINT_FORCE
+# out-port = F_LARM_LINK1:LARM_JOINT1:CONSTRAINT_FORCE
+# out-port = F_LARM_LINK2:LARM_JOINT2:CONSTRAINT_FORCE
+# out-port = F_LARM_LINK3:LARM_JOINT3:CONSTRAINT_FORCE
+# out-port = F_LARM_LINK4:LARM_JOINT4:CONSTRAINT_FORCE
+# out-port = F_LARM_LINK5:LARM_JOINT5:CONSTRAINT_FORCE
+# out-port = F_LARM_LINK6:LARM_JOINT6:CONSTRAINT_FORCE
+# out-port = F_LARM_LINK7:LARM_JOINT7:CONSTRAINT_FORCE
+# out-port = F_LARM_FINGER0:LARM_F_JOINT0:CONSTRAINT_FORCE
+# out-port = F_LARM_FINGER1:LARM_F_JOINT1:CONSTRAINT_FORCE
+# out-port = F_RARM_LINK0:RARM_JOINT0:CONSTRAINT_FORCE
+# out-port = F_RARM_LINK1:RARM_JOINT1:CONSTRAINT_FORCE
+# out-port = F_RARM_LINK2:RARM_JOINT2:CONSTRAINT_FORCE
+# out-port = F_RARM_LINK3:RARM_JOINT3:CONSTRAINT_FORCE
+# out-port = F_RARM_LINK4:RARM_JOINT4:CONSTRAINT_FORCE
+# out-port = F_RARM_LINK5:RARM_JOINT5:CONSTRAINT_FORCE
+# out-port = F_RARM_LINK6:RARM_JOINT6:CONSTRAINT_FORCE
+# out-port = F_RARM_LINK7:RARM_JOINT7:CONSTRAINT_FORCE
+# out-port = F_RARM_FINGER0:RARM_F_JOINT0:CONSTRAINT_FORCE
+# out-port = F_RARM_FINGER1:RARM_F_JOINT1:CONSTRAINT_FORCE
+# out-port = F_LLEG_LINK0:LLEG_JOINT0:CONSTRAINT_FORCE
+# out-port = F_LLEG_LINK1:LLEG_JOINT1:CONSTRAINT_FORCE
+# out-port = F_LLEG_LINK2:LLEG_JOINT2:CONSTRAINT_FORCE
+# out-port = F_LLEG_LINK3:LLEG_JOINT3:CONSTRAINT_FORCE
+# out-port = F_LLEG_LINK4:LLEG_JOINT4:CONSTRAINT_FORCE
+# out-port = F_LLEG_LINK5_UPPER:LLEG_JOINT5:CONSTRAINT_FORCE
+# out-port = F_LLEG_LINK5_LOWER:LLEG_BUSH_PITCH:CONSTRAINT_FORCE
+# out-port = F_RLEG_LINK0:RLEG_JOINT0:CONSTRAINT_FORCE
+# out-port = F_RLEG_LINK1:RLEG_JOINT1:CONSTRAINT_FORCE
+# out-port = F_RLEG_LINK2:RLEG_JOINT2:CONSTRAINT_FORCE
+# out-port = F_RLEG_LINK3:RLEG_JOINT3:CONSTRAINT_FORCE
+# out-port = F_RLEG_LINK4:RLEG_JOINT4:CONSTRAINT_FORCE
+# out-port = F_RLEG_LINK5_UPPER:RLEG_JOINT5:CONSTRAINT_FORCE
+# out-port = F_RLEG_LINK5_LOWER:RLEG_BUSH_PITCH:CONSTRAINT_FORCE

hrpsys_choreonoid_tutorials/config/SampleRobotCustomizer.yaml

@@ -0,0 +1,11 @@
+bush:
+  springT: 3.3e5
+  dampingT: 3.3e2
+  springR: 2.5e3
+  dampingR: 2.5
+
+tilt_laser:
+  TILT_UPPER_BOUND: 1.35
+  TILT_POSITIVE_SPEED: 1.0
+  TILT_LOWER_BOUND: -0.7
+  TILT_NEGATIVE_SPEED: -1.0