Use wrench in seq (new)

idl/ImpedanceControllerService.idl

@@ -30,12 +30,8 @@ module OpenHRP
       double D_r;
       /// Rotation spring [N/rad]
       double K_r;
-      /// Reference Force [N] (x,y,z).
-      DblSequence3 ref_force;
       /// Force gain (x,y,z).
       DblSequence3 force_gain;
-      /// Reference Moment [Nm] (x,y,z).
-      DblSequence3 ref_moment;
       /// Moment gain (x,y,z).
       DblSequence3 moment_gain;
       /// SR-inverse gain for inverse kinematics.

python/hrpsys_config.py

@@ -319,9 +319,9 @@ def connectComps(self):
                                                   self.fk.port("baseRpyRef")])
         connectPorts(self.sh.port("qOut"), self.seq.port("qInit"))
         connectPorts(self.sh.port("zmpOut"), self.seq.port("zmpRefInit"))
-        for sen in filter(lambda x: x.type == "Force", self.sensors):
-            connectPorts(self.seq.port(sen.name + "Ref"),
-                         self.sh.port(sen.name + "In"))
+        for sen in self.getForceSensorNames():
+            connectPorts(self.seq.port(sen + "Ref"),
+                         self.sh.port(sen + "In"))
 
         # connection for st
         if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort(
@@ -340,14 +340,18 @@ def connectComps(self):
             connectPorts(self.abc.port("contactStates"), self.st.port("contactStates"))
             connectPorts(self.abc.port("controlSwingSupportTime"), self.st.port("controlSwingSupportTime"))
             connectPorts(self.rh.port("q"), self.st.port("qCurrent"))
-            for sen in filter(lambda x: x.type == "Force", self.sensors):
-                connectPorts(self.sh.port(sen.name + "Out"),
-                             self.st.port(sen.name + "Ref"))
 
-        if self.ic and self.abc:
-            for sen in filter(lambda x: x.type == "Force", self.sensors):
-                connectPorts(self.ic.port("ref_" + sen.name),
-                             self.abc.port("ref_" + sen.name))
+        # ref force moment connection
+        for sen in self.getForceSensorNames():
+            if self.st:
+                connectPorts(self.sh.port(sen + "Out"),
+                             self.st.port(sen + "Ref"))
+            if self.ic:
+                connectPorts(self.sh.port(sen+"Out"),
+                             self.ic.port("ref_" + sen+"In"))
+            if self.abc:
+                connectPorts(self.sh.port(sen+"Out"),
+                             self.abc.port("ref_" + sen))
 
         #  actual force sensors
         if self.rmfo:
@@ -363,6 +367,8 @@ def connectComps(self):
         # connection for ic
         if self.ic:
             connectPorts(self.rh.port("q"), self.ic.port("qCurrent"))
+            connectPorts(self.sh.port("basePosOut"), self.ic.port("basePosIn"))
+            connectPorts(self.sh.port("baseRpyOut"), self.ic.port("baseRpyIn"))
         # connection for tf
         if self.tf:
             # connection for actual torques
@@ -608,6 +614,16 @@ def getSensors(self, url):
                            filter(lambda x: len(x.sensors) > 0,
                                   self.getBodyInfo(url)._get_links())), [])  # sum is for list flatten
 
+    # public method to get sensors list
+    def getForceSensorNames(self):
+        '''!@brief
+        Get list of force sensor names. Returns existence force sensors and virtual force sensors. self.sensors and virtual force sensors are assumed.
+        '''
+        ret = map (lambda x : x.name, filter(lambda x: x.type == "Force", self.sensors))
+        if self.vs != None:
+            ret += filter(lambda x: str.find(x, 'v') >= 0 and str.find(x, 'sensor') >= 0, self.vs.ports.keys())
+        return ret
+
     def connectLoggerPort(self, artc, sen_name, log_name=None):
         '''!@brief
         Connect port to logger

rtc/ImpedanceController/ImpedanceController.h

@@ -106,21 +106,23 @@ class ImpedanceController
   InPort<TimedDoubleSeq> m_qCurrentIn;
   TimedDoubleSeq m_qRef;
   InPort<TimedDoubleSeq> m_qRefIn;
+  TimedPoint3D m_basePos;
+  InPort<TimedPoint3D> m_basePosIn;
+  TimedOrientation3D m_baseRpy;
+  InPort<TimedOrientation3D> m_baseRpyIn;
   std::vector<TimedDoubleSeq> m_force;
   std::vector<InPort<TimedDoubleSeq> *> m_forceIn;
+  std::vector<TimedDoubleSeq> m_ref_force;
+  std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
   TimedOrientation3D m_rpy;
-  TimedOrientation3D m_rpyRef;
   InPort<TimedOrientation3D> m_rpyIn;
-  InPort<TimedOrientation3D> m_rpyRefIn;
 
   // </rtc-template>
 
   // DataOutPort declaration
   // <rtc-template block="outport_declare">
   TimedDoubleSeq m_q;
   OutPort<TimedDoubleSeq> m_qOut;
-  std::vector<TimedDoubleSeq> m_ref_force;
-  std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
 
   // </rtc-template>
 
@@ -173,7 +175,7 @@ class ImpedanceController
 
   std::map<std::string, ImpedanceParam> m_impedance_param;
   std::map<std::string, VirtualForceSensorParam> m_sensors;
-  std::map<std::string, hrp::Vector3> abs_forces, abs_moments;
+  std::map<std::string, hrp::Vector3> abs_forces, abs_moments, abs_ref_forces, abs_ref_moments;
   double m_dt;
   hrp::BodyPtr m_robot;
   coil::Mutex m_mutex;

rtc/ImpedanceController/ImpedanceControllerService_impl.cpp

@@ -19,8 +19,6 @@ CORBA::Boolean ImpedanceControllerService_impl::setImpedanceControllerParam(cons
 CORBA::Boolean ImpedanceControllerService_impl::getImpedanceControllerParam(const char *i_name_, OpenHRP::ImpedanceControllerService::impedanceParam_out i_param_)
 {
   i_param_ = new OpenHRP::ImpedanceControllerService::impedanceParam();
-  i_param_->ref_force.length(3);
-  i_param_->ref_moment.length(3);
   i_param_->force_gain.length(3);
   i_param_->moment_gain.length(3);
   return m_impedance->getImpedanceControllerParam(std::string(i_name_), *i_param_);

rtc/SequencePlayer/SequencePlayer.cpp

@@ -130,15 +130,28 @@ RTC::ReturnCode_t SequencePlayer::onInitialize()
 
     unsigned int dof = m_robot->numJoints();
 
+
+    // Setting for wrench data ports (real + virtual)
+    std::vector<std::string> fsensor_names;
+    //   find names for real force sensors
     int npforce = m_robot->numSensors(hrp::Sensor::FORCE);
-    m_wrenches.resize(npforce);
-    m_wrenchesOut.resize(npforce);
     for (unsigned int i=0; i<npforce; i++){
-      hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i);
-      m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(s->name+"Ref").c_str(), m_wrenches[i]);
+      fsensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name);
+    }
+    //   find names for virtual force sensors
+    coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
+    int nvforce = virtual_force_sensor.size()/10;
+    for (unsigned int i=0; i<nvforce; i++){
+      fsensor_names.push_back(virtual_force_sensor[i*10+0]);
+    }
+    //   add ports for all force sensors
+    int nforce  = npforce + nvforce;
+    m_wrenches.resize(nforce);
+    m_wrenchesOut.resize(nforce);
+    for (unsigned int i=0; i<nforce; i++){
+      m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Ref").c_str(), m_wrenches[i]);
       m_wrenches[i].data.length(6);
-      registerOutPort(std::string(s->name+"Ref").c_str(), *m_wrenchesOut[i]);
-      std::cerr << s->name << std::endl;
+      registerOutPort(std::string(fsensor_names[i]+"Ref").c_str(), *m_wrenchesOut[i]);
     }
 
     if (prop.hasKey("seq_optional_data_dim")) {
@@ -147,7 +160,7 @@ RTC::ReturnCode_t SequencePlayer::onInitialize()
       optional_data_dim = 1;
     }
 
-    m_seq = new seqplay(dof, dt, npforce, optional_data_dim);
+    m_seq = new seqplay(dof, dt, nforce, optional_data_dim);
 
     m_qInit.data.length(dof);
     for (unsigned int i=0; i<dof; i++) m_qInit.data[i] = 0.0;

rtc/StateHolder/StateHolder.cpp

@@ -146,8 +146,9 @@ RTC::ReturnCode_t StateHolder::onInitialize()
   m_baseRpy.data.y = m_basePose.data.orientation.y = rpy[2];
   m_zmp.data.x = m_zmp.data.y = m_zmp.data.z = 0.0;
 
-  // wrench data ports
+  // Setting for wrench data ports (real + virtual)
   std::vector<std::string> fsensor_names;
+  //   find names for real force sensors
   for ( int k = 0; k < lis->length(); k++ ) {
     OpenHRP::SensorInfoSequence& sensors = lis[k].sensors;
     for ( int l = 0; l < sensors.length(); l++ ) {
@@ -156,12 +157,19 @@ RTC::ReturnCode_t StateHolder::onInitialize()
       }
     }
   }
-
   int npforce = fsensor_names.size();
-  m_wrenches.resize(npforce);
-  m_wrenchesIn.resize(npforce);
-  m_wrenchesOut.resize(npforce);
-  for (unsigned int i=0; i<npforce; i++){
+  //   find names for virtual force sensors
+  coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ",");
+  int nvforce = virtual_force_sensor.size()/10;
+  for (unsigned int i=0; i<nvforce; i++){
+    fsensor_names.push_back(virtual_force_sensor[i*10+0]);
+  }
+  //   add ports for all force sensors
+  int nforce  = npforce + nvforce;
+  m_wrenches.resize(nforce);
+  m_wrenchesIn.resize(nforce);
+  m_wrenchesOut.resize(nforce);
+  for (unsigned int i=0; i<nforce; i++){
     m_wrenchesIn[i] = new InPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"In").c_str(), m_wrenches[i]);
     m_wrenchesOut[i] = new OutPort<TimedDoubleSeq>(std::string(fsensor_names[i]+"Out").c_str(), m_wrenches[i]);
     m_wrenches[i].data.length(6);

sample/SampleRobot/CMakeLists.txt

@@ -6,12 +6,19 @@ foreach(_idx 0 1)
   list(GET controller_period_list ${_idx} _tmp_controller_period)
   set(CONTROLLER_TIME ${_tmp_controller_time})
   set(CONTROLLER_PERIOD ${_tmp_controller_period})
+  set(VirtualForceSensorSetting "#virtual_force_sensor") # comment out by default
   configure_file(SampleRobot.RobotHardware.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.RobotHardware.${_tmp_controller_period}.conf)
   configure_file(SampleRobot.xml.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.${_tmp_controller_period}.xml)
   configure_file(SampleRobot.torque.xml.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.${_tmp_controller_period}.torque.xml)
   configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.${_tmp_controller_period}.conf)
 endforeach()
 
+# for virtual force sensor testing
+set(CONTROLLER_TIME 0.002)
+set(CONTROLLER_PERIOD 500)
+set(VirtualForceSensorSetting "virtual_force_sensor")
+configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.vfs.conf)
+
 set(robot_waist_translation "-0.8  0  0" "-4.15  -0.2  0" "-5.83  -0.2  -0.6096")
 set(file_suffix "SlopeUpDown" "StairUp" "StairDown")
 foreach(_idx 0 1 2)
@@ -31,6 +38,7 @@ configure_file(samplerobot_remove_force_offset.py.in ${CMAKE_CURRENT_BINARY_DIR}
 configure_file(samplerobot_stabilizer.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_stabilizer.py)
 configure_file(samplerobot_sequence_player.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_sequence_player.py)
 configure_file(samplerobot_collision_detector.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_collision_detector.py)
+configure_file(samplerobot_virtual_force_sensor.py.in ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_virtual_force_sensor.py)
 
 install(PROGRAMS
   ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_data_logger.py
@@ -42,6 +50,7 @@ install(PROGRAMS
   ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_walk.py
   ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_sequence_player.py
   ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_collision_detector.py
+  ${CMAKE_CURRENT_BINARY_DIR}/samplerobot_virtual_force_sensor.py
   DESTINATION share/hrpsys/samples/SampleRobot)
 
 install(FILES 
@@ -55,6 +64,8 @@ install(FILES
   ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.200.xml
   ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.200.torque.xml
   ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.RobotHardware.200.conf
+  # for virtual force sensor
+  ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.vfs.conf
   SampleRobot.PDgain.dat
   # TerrainFloor
   ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.SlopeUpDown.xml

sample/SampleRobot/SampleRobot.conf.in

@@ -9,3 +9,6 @@ pdgains_sim.file_name: @CMAKE_INSTALL_PREFIX@/share/hrpsys/samples/SampleRobot/S
 
 # CollisionDetector Setting
 collision_pair: RARM_WRIST_P:WAIST LARM_WRIST_P:WAIST RARM_WRIST_P:RLEG_HIP_R LARM_WRIST_P:LLEG_HIP_R RARM_WRIST_R:RLEG_HIP_R LARM_WRIST_R:LLEG_HIP_R
+
+# virtual force sensor
+@VirtualForceSensorSetting@: vlhsensor, CHEST, LARM_WRIST_R, 0,0,0, 0,0,1,0, vrhsensor, CHEST, RARM_WRIST_R, 0,0,0, 0,0,1,0,

sample/SampleRobot/samplerobot_impedance_controller.py.in

@@ -41,16 +41,32 @@ def demo():
     if ret2 and ret1[1].base_name == ret3[1].base_name and ret1[1].target_name == ret3[1].target_name:
         print "setImpedanceControllerParam => OK"
     # 3. set ref force and moment
-    ret1[1].ref_force=[10,10,-10]
+    hcf.seq_svc.setWrenches([0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             10,10,-10,0,0,0,], 2.0);
+    hcf.seq_svc.waitInterpolation();
     hcf.ic_svc.setImpedanceControllerParam("rhsensor", ret1[1])
     time.sleep(2)
-    ret1[1].ref_force=[0,0,0]
+    hcf.seq_svc.setWrenches([0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,], 2.0);
+    hcf.seq_svc.waitInterpolation();
     hcf.ic_svc.setImpedanceControllerParam("rhsensor", ret1[1])
     time.sleep(2)
-    ret1[1].ref_moment=[0.2,0.2,-0.5]
+    hcf.seq_svc.setWrenches([0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0.2,0.2,-0.5], 2.0);
+    hcf.seq_svc.waitInterpolation();
     hcf.ic_svc.setImpedanceControllerParam("rhsensor", ret1[1])
     time.sleep(2)
-    ret1[1].ref_moment=[0,0,0]
+    hcf.seq_svc.setWrenches([0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0], 2.0);
+    hcf.seq_svc.waitInterpolation();
     hcf.ic_svc.setImpedanceControllerParam("rhsensor", ret1[1])
     time.sleep(2)
     # 4. stop

sample/SampleRobot/samplerobot_virtual_force_sensor.py.in

@@ -0,0 +1,78 @@
+#!/usr/bin/env python
+
+try:
+    from hrpsys.hrpsys_config import *
+    import OpenHRP
+except:
+    print "import without hrpsys"
+    import rtm
+    from rtm import *
+    from OpenHRP import *
+    import waitInput
+    from waitInput import *
+    import socket
+    import time
+
+def init ():
+    global hcf
+    hcf = HrpsysConfigurator()
+    hcf.getRTCList = hcf.getRTCListUnstable
+    hcf.init ("SampleRobot(Robot)0", "@OPENHRP_DIR@/share/OpenHRP-3.1/sample/model/sample1.wrl")
+
+def demo ():
+    init()
+    # set initial pose from sample/controller/SampleController/etc/Sample.pos
+    initial_pose = [-7.779e-005,  -0.378613,  -0.000209793,  0.832038,  -0.452564,  0.000244781,  0.31129,  -0.159481,  -0.115399,  -0.636277,  0,  0,  0,  -7.77902e-005,  -0.378613,  -0.000209794,  0.832038,  -0.452564,  0.000244781,  0.31129,  0.159481,  0.115399,  -0.636277,  0,  0,  0,  0,  0,  0]
+
+    # 1. Check wrench data ports existence (real force sensor + virtual force sensor)
+    hcf.seq_svc.setJointAngles(initial_pose, 2.0)
+    hcf.seq_svc.waitInterpolation()
+    fsensor_names = hcf.getForceSensorNames()
+    # vs check
+    port_names = fsensor_names
+    if all(map (lambda x : hcf.vs.port(x), fsensor_names)):
+        print hcf.vs.name(), "ports are OK (", port_names, ")"
+    # seq check
+    port_names = map (lambda x : x+"Ref", fsensor_names)
+    if all(map (lambda x : hcf.seq.port(x), port_names)):
+        print hcf.seq.name(), "ports are OK (", port_names, ")"
+    # sh check
+    port_names = map (lambda x : x+"Out", fsensor_names)
+    if all(map (lambda x : hcf.sh.port(x), port_names)):
+        print hcf.sh.name(), "ports are OK (", port_names, ")"
+    port_names = map (lambda x : x+"In", fsensor_names)
+    if all(map (lambda x : hcf.sh.port(x), port_names)):
+        print hcf.sh.name(), "ports are OK (", port_names, ")"
+    # ic check
+    port_names = map (lambda x : "ref_"+x+"In", fsensor_names)
+    if all(map (lambda x : hcf.ic.port(x), port_names)):
+        print hcf.ic.name(), "ports are OK (", port_names, ")"
+    # abc check
+    port_names = map (lambda x : "ref_"+x+"In", fsensor_names)
+    if all(map (lambda x : hcf.ic.port(x), port_names)):
+        print hcf.ic.name(), "ports are OK (", port_names, ")"
+
+    # 2. Test impedance controller
+    ret1=hcf.ic_svc.getImpedanceControllerParam("vrhsensor")
+    ret1[1].base_name="CHEST"
+    ret1[1].target_name="RARM_WRIST_R"
+    ret1[1].K_r=1.0
+    ret1[1].D_r=2.0
+    ret1[1].M_r=0.2
+    ret2=hcf.ic_svc.setImpedanceControllerParam("vrhsensor", ret1[1])
+    hcf.seq_svc.setWrenches([0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             10,10,-10,0,0,0,], 2.0);
+    hcf.seq_svc.waitInterpolation();
+    hcf.seq_svc.setWrenches([0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,
+                             0,0,0,0,0,0,], 2.0);
+    hcf.seq_svc.waitInterpolation();
+    hcf.ic_svc.deleteImpedanceController("vrhsensor")
+    print "test ImpedanceController for virtual force sensor => OK"