이번 튜토리얼은 관절(joint)에 작용하는 forcre/torque sensor의 사용법을 설명한다. 이 센서는 force, torque 읽은값을 topic으로 publish 한다.

force_torque_tutorial.world 이름으로 world를 저장한다.

<?xml version="1.0"?>
<sdf version="1.6">
  <world name="default">
    <physics name="default_physics" default="0" type="ode">
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
      <ode>
        <solver>
          <type>quick</type>
          <iters>50</iters>
          <sor>1.0</sor> <!-- Important, see issue #2209 -->
          <use_dynamic_moi_rescaling>false</use_dynamic_moi_rescaling>
        </solver>
      </ode>
    </physics>
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://sun</uri>
    </include>
    <model name="model_1">
      <link name="link_1">
        <pose>0 0 2.0 0 0 0</pose>
        <inertial>
          <inertia>
            <ixx>0.100000</ixx>
            <ixy>0.000000</ixy>
            <ixz>0.000000</ixz>
            <iyy>0.100000</iyy>
            <iyz>0.000000</iyz>
            <izz>0.100000</izz>
          </inertia>
          <mass>10.000000</mass>
        </inertial>
        <visual name="visual_sphere">
          <geometry>
            <sphere>
              <radius>0.100000</radius>
            </sphere>
          </geometry>
        </visual>
        <visual name="visual_cylinder">
          <pose>0 0 -0.75 0 0 0</pose>
          <geometry>
            <cylinder>
              <radius>0.0100000</radius>
              <length>1.5</length>
            </cylinder>
          </geometry>
        </visual>
        <collision name="collision_sphere">
          <max_contacts>250</max_contacts>
          <geometry>
            <sphere>
              <radius>0.100000</radius>
            </sphere>
          </geometry>
        </collision>
      </link>
      <joint name="joint_01" type="revolute">
        <parent>world</parent>
        <child>link_1</child>
        <pose>0 0 -1.5 0 0 0</pose>
        <axis>
          <limit>
            <lower>-1.57079</lower>
            <upper>1.57079</upper>
          </limit>
          <dynamics>
            <damping>0.000000</damping>
            <friction>0.000000</friction>
          </dynamics>
          <xyz>1.000000 0.000000 0.000000</xyz>
        </axis>
        <sensor name="force_torque" type="force_torque">
          <update_rate>30</update_rate>
        </sensor>
      </joint>
    </model>
  </world>
</sdf>

terminal을 열고, 이 코드를 실행한다. (--verbose)

gazebo --verbose force_torque_tutorial.world

새로운 terminal에서 다음과 같은 명령어와 함께 topic viewer를 연다:

gz topic --view /gazebo/default/model_1/joint_01/force_torque/wrench

Y방향으로 500N의 크기로 link_1에 Apply a force. topic viewer (ctrl-t)로 결과값을 보자.

이 world는 하나의 링크와 하나의 관절을 갖는다. 링크는 관절로부터 1.5m offser을 갖는 10kg, 구 모양이다. 관절은 링크(child)와 world(parent)로 연결되어 있고, x방향으로 회전이 가능하다. 시뮬레이션이 처음 시작할때는 링크는 관절 위에서 평형을 이루고 있다. 이때 torque는 없다. 관절에 작용하는 힘은 중력(-z 방향)으로부터 온다.

forceJointZ = mass * g
            = 10 kg * -9.8 m/s^2
            = -98 N

link_1에 외력을 넣어주면, 링크를 넘어지고, 관절 한계인 90도에서 멈추게 된다. 관절 한계는 링크가 ground plane 위에서 멈출 수 있도록 유지해주는 역할이다. 관절의 +Y방향은 ground plane로 들어가는 방향이다. 링크에 작용하는 중력은 x방향으로 토크를 만든다.

질량은 동일하고, 힘의 크기도 동일. 힘의 방향은 Y방향으로 +98N되도록 변화한다. 구는 Joint의 z방향으로 1.5m 떨어져있기에, x방향의 토크는:

torqueJoint01_x = r X R
                = ||r|| * ||F|| * sin(theta)
                = distanceZ * (massLink * g) * sin(theta)
                = 1.5 m * (10 kg * 9.8 m/s^2) * sin(-90)
                = -147 Nm

Note: 관절 limit 근처 구간의 측정값은 물리 엔진의 파라미터에 따라 튈 수 있다. issue #2209를 보아라

모든 센서는 SDF sensor schema안에서 공통된 파라미터 set을 갖는다.

만약 true라면 센서는 항상 force와 torque를 측정한다. 만약 false라면 센서는 subscriber가 sensor's topic에 연결되어 있을때만 업데이트가 된다. 이러한 설정은 code를 통해 센서를 접근할때 매우 중요하다. 만약 subscriber가 없다면 ForceTorqueSensor::Torque() or ForceTorqueSensor::Force()를 부르는 것은 안정화된 데이터를 반환할 것이다. 이러한 문제는 IsActive()가 false를 반환하는지를 확인하는것으로 발견할 수 있다. 코드는 SetActive(true)를 부름으로써, 센서가 subscriber가 없는 상황에서 업데이트 할 수 있도록 한다.

이것은 센서가 스스로 데이터를 업데이트 하는 rate (Hz)를 말한다. 이것은 simulated second 마다 센서에 의해 published되는 메세지의 수일 것이다.

만약 ture라면, gazebo client는 관절에 작용하는 force와 torque를 visualization하는것을 보여줄 것이다.

force/torque 센서는 현재 이 파라미터를 지원하지 핞는다.

force/torque 센서는 현재 이 파라미터를 지원하지 핞는다.

공간상에서 'x y z roll pitch yaw (position + orientation)'의 순서의 Floating point number 의미한다. 이것은 parent joint로부터 센서 frame을 묘사한다.

force/torque sensor는 <sensor> tag with the attribute type set to force_torque를 추가함으로써 생성할 수 있다. 아래에는 위 방법 외 설정하는 방법이 2가지 더 제시되어있다.

<sensor name="my_cool_sensor" type="force_torque">
  <force_torque>
    <frame>child</frame>
    <measure_direction>child_to_parent</measure_direction>
  </force_torque>
</sensor>

The value of this element may be one of: child, parent, or sensor. It is the frame in which the forces and torques should be expressed. The values parent and child refer to the parent or child links of the joint. The value sensor means the measurement is rotated by the rotation component of the <pose> of this sensor. The translation component of the pose has no effect on the measurement.

Regardless of this setting, the torque component is always expressed about the origin of the joint frame.

This is the direction of the measurement. Try changing the example above to parent_to_child. After being toppled the sensor reports a force of -98 N on the Y axis and a torque of +147 Nm about the X axis. This is the same magnitude as before but opposite direction.

While the SDF schema allows a <sensor> tag to be placed on either a link or a joint, the force/torque sensor only works on joints. If the sensor is added to a link, running gazebo with --verbose shows the following error:

[Err] [Link.cc:114] A link cannot load a [force_torque] sensor.

The above example places a force/torque sensor on a revolute joint. However, real force/torque sensors are typically rigidly mounted to another rigid body. A real sensor could not measure the force and torque exactly at the revolute joint origin. Modeling this way is reasonable if the real sensor is close enough to the joint that the error from the offset is negligible.

If this error is not negligible, the rigid body can be split into two links with a fixed joint at the location of the real sensor.