Overview

In this section we use the sample model "PD_HGtest" distributed with OpenHRP package to describe the simulation procedure, of "free" joint-type root joint in high-gain mode and other joints in torque-control mode. Models used for sample have simple structure that composed of ROOT joint and 2 links. ROOT joint is provided position and posture and other links are provided joint-torque by the controller. Basic procedure is as follows.

  1. Simulation project settings
  2. Making Controller
  3. ControllerBridge settings

Hereafter these procedures are explained in detail.

Simulation project settings

The simulation project settings are described in PD_HGtest.xml.

If nothing is specified, the control mode of each joint in the model is set to torque-control mode in default. High-gain settings are applied only to the ROOT joint as we desired. Please see the following highlighted part in red.

PD_HGtest.xml 
     :
     :
     <item class="com.generalrobotix.ui.item.GrxModelItem" name="testRobot" select="true" url="$(PROJECT_DIR)/../model/PD_HGtest.wrl">
            <property name="isRobot" value="true"/>
            <property name="controller" value="PD_HGtestController"/>
            <property name="controlTime" value="0.002"/>
            <property name="setupDirectory" value="$(PROJECT_DIR)/../controller/PD_HGtest"/>
            <property name="setupCommand" value="PD_HGtest$(BIN_SFX)"/>
            <property name="ROOT.mode" value="HighGain"/>
            <property name="ROOT.translation" value="0.0 0.0 1.0 "/>
     </item>
     :
     :


Making Controller

Controller's source files are sample/controller/PD_HGtest/PD_HGtest.{h, cpp}.

Here we make a simple controller that moves ROOT joint to the direction of X-axis and outputs joint-torque (=0) to the other joints. X-axis vectors (position, velocity, acceleration) are defined as suitable in advance and saved in sample/controller/PD_HGtest/etc/root.dat.

First we set output ports for each joint. Regarding the ROOT joint, we prepare ports to output position, velosity, acceleration and the data length is set to 12,6,6 in the order.

PD_HGtest.h 
     :
     :
  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  TimedDoubleSeq m_torque0;
  OutPort<TimedDoubleSeq> m_torque0Out;
  TimedDoubleSeq m_torque1;
  OutPort<TimedDoubleSeq> m_torque1Out;
  
  TimedDoubleSeq m_root_trans;
  OutPort<TimedDoubleSeq> m_root_transOut;
  TimedDoubleSeq m_root_vel;
  OutPort<TimedDoubleSeq> m_root_velOut;
  TimedDoubleSeq m_root_acc;
  OutPort<TimedDoubleSeq> m_root_accOut;
     :
     :

PD_HGtest.cpp 
     :
     :
PD_HGtest::PD_HGtest(RTC::Manager* manager)
  : RTC::DataFlowComponentBase(manager),
    // <rtc-template block="initializer">
    m_torque0Out("torque0", m_torque0),
    m_torque1Out("torque1", m_torque1),
    m_root_transOut("root_trans", m_root_trans),
    m_root_velOut("root_vel", m_root_vel),
    m_root_accOut("root_acc", m_root_acc)
     :
     :
  // Registration: InPort/OutPort/Service
  // <rtc-template block="registration">
  // Set OutPort buffer
  registerOutPort("torque0", m_torque0Out);
  registerOutPort("torque1", m_torque1Out);
  registerOutPort("root_trans", m_root_transOut);
  registerOutPort("root_vel", m_root_velOut);
  registerOutPort("root_acc", m_root_accOut);
  
  m_torque0.data.length(1);
  m_torque1.data.length(1);
  m_root_trans.data.length(12);
  m_root_vel.data.length(6);
  m_root_acc.data.length(6);
     :
     :

Next we describe the output operation; the process that actually output a value.

First the output values for the the ROOT joint are read from file (1).

Next, the output values are set except for the ROOT joint (2).

Set the position of ROOT joint (3). Data is ordered to position vector and 3x3 posture matrix.

Set the velocity (acceleration) of ROOT joint (4). Data is ordered to velocity (acceleration) vector and angular velocity (angular acceleration).

Data is been output (5).

PD_HGtest.cpp 
     :
     :
RTC::ReturnCode_t PD_HGtest::onExecute(RTC::UniqueId ec_id)
{
     :
     :
  static double root_x_p, root_x_v, root_x_a;           (1)
  if(!waist.eof()){
    waist >> root_x_p;  //skip time
    waist >> root_x_a;
    waist >> root_x_v;
    waist >> root_x_p;
  }

  m_torque0.data[0] = 0.0;                              (2)
  m_torque1.data[0] = 0.0;
  
  for(int i=0; i<12; i++)                               (3)
    m_root_trans.data[i] = 0;
  m_root_trans.data[0] = root_x_p;    //x
  m_root_trans.data[2] = 1.0;          //z
  m_root_trans.data[3] = 1.0;
  m_root_trans.data[7] = 1.0;
  m_root_trans.data[11] = 1.0;
  for(int i=0; i<6; i++)                                (4)
      m_root_vel.data[i] = 0.0;
  m_root_vel.data[0] = root_x_v;
  for(int i=0; i<6; i++)                                    
      m_root_acc.data[i] = 0.0;
  m_root_acc.data[0] = root_x_a;

  m_torque0Out.write();                                 (5)
  m_torque1Out.write();
  m_root_transOut.write();
  m_root_velOut.write();
  m_root_accOut.write();

  return RTC::RTC_OK;
  }
     :
     :


ControllerBridge settings

Next we configure ControllerBridge settings. ControllerBridge settings are described in sample/controller/PD_HGtest/bridge.conf. 

in-port= torque0:J0:JOINT_TORQUE
in-port= torque1:J1:JOINT_TORQUE

in-port= root_trans:ROOT:ABS_TRANSFORM
in-port= root_vel:ROOT:ABS_VELOCITY
in-port= root_acc:ROOT:ABS_ACCELERATION
 
connection= torque0:torque0
connection= torque1:torque1

connection= root_trans:root_trans
connection= root_vel:root_vel
connection= root_acc:root_acc
 
server-name= PD_HGtestController

By now, preparation for the simulation is done. You can now execute the simulation.