증상

시뮬레이션으로 카메라 영상으로 물체 위치를 파악하고 보드를 잡아 옮기는 작업을 반복하였을 때 ArUco 마커가 있는 위치까지의 도달은 문제가 없었다. 하지만 물체를 집어서 옮기고 내려 놓는 작업을 반복할 때 처음에는 그리퍼가 대상물을 잘 잡아서 옮기는 것으로 보였지만 반복해서 실행시키다 보면 그리퍼가 물체를 잡지 못하였는데 완료로 판단하거나 물체를 잡을 때 그리퍼가 매우 느리게 움직이거나 하는 이상한 동작이 발생하였다.

그리퍼 동작 테스트

처음에는 물리 엔진에서 내가 모르는 마찰이나 충돌이 발생하는 것으로 생각했었다. 그래서, 테스트를 위해서 보드를 잡기 위해 하강하기 직전 위치의 허공에서 그리퍼를 열었다 닫는 작업을 수행시켰다.
예상 했던 것과는 달리 공중에서도 그리퍼를 다 열거다 닫지 않았는데 작업이 완료되거나, 지정한 값까지만 그리퍼를 닫아야 하는데 다 닫혀버리거나, 아예 움직이지 않는 상황 등 예기치 못했던 상황들이 발생하였다.
즉, 다른 물체와의 충돌이나 마찰이 없어도 발생한다는 것이었다.

Gripper URDF 인자 변경

arm_gripper.urdf.xacro 파일에서 effort, velocity, lower 값을 바꿔가면서 테스트를 해 보았다.

그리퍼가 움직이지 못하고 멈추는 것을 보고 effort를 높여 보았다. 그랬더니 그리퍼의 손가락이 서로 충돌해서 튕겨 나가고 끝에서 부딛혀 다시 되돌아 오는 증상이 발생하였다.

너무 빠르게 움직여서 문제가 생기는 것으로 예상하고 velocity를 낮춰 보았다. 이제는 손가락이 튕겨서 열렸다 닫히는 현상은 사라졌지만 손가락이 움직이지 못해서 그리퍼가 닫히지 않는 현상이 발생하였다.

단순히 인자를 변경하는 것으로는 해결되지 않을 것으로 판단되었다.

URDF에서 SDF로의 변환 확인

런치 파일 arm_study_bringup.launch.py는 xacro로 만든 URDF를 그대로 Gazebo에 넘기고, Gazebo가 이것을 SDF로 변환해서 읽는다. 동일한 변환을 직접 실행하여 mimic joint 부분을 확인하였다.

xacro src/arm_description/urdf/ur5e.urdf.xacro ur_type:=ur5e \
  controllers_yaml:=install/arm_bringup/share/arm_bringup/config/arm_controllers.yaml \
  > /tmp/arm.urdf
gz sdf -p /tmp/arm.urdf > /tmp/arm.sdf

변환된 SDF의 finger2_joint에는 <axis> 아래에 <mimic>이 들어 있었다.

<axis>
  <xyz>0 1 0</xyz>
  <mimic joint='finger1_joint'>
    <multiplier>1</multiplier>
    <offset>0</offset>
    <reference>0</reference>
  </mimic>

Gazebo 물리 엔진 테스트

원래 bullet-featherstone을 사용한 이유는 Gazebo의 기본 물리 엔진이 mimic joint를 지원하지 않는다고 하여서 방법을 찾던 중에 https://github.com/ros-controls/gz_ros2_control/issues/340 링크의 글을 읽고 적용한 것이다.

이 물리 엔진은 launch 파일에서 직접 인자로 넘겨 주는데 이 엔진을 지우면 어떻게 될까하는 생각에 인자로 넘겨주던 엔진을 삭제하고 시뮬레이션을 실행해 보았다.
그랬더니, 여전히 그리퍼가 비슷하게 동작을 한다!!!?
가제보 로그를 살펴 보니 다음과 같은 로그가 출력되고 있었다.

[gazebo-10] [INFO] [1791162200.186989363] [gz_ros_control]: Joint 'finger2_joint'is mimicking joint 'finger1_joint' with multiplier: 1 and offset: 0
[gazebo-10] [Err] [Physics.cc:1808] Attempting to create a mimic constraint for joint [finger2_joint] but the chosen physics engine does not support mimic constraints, so no constraint will be created.

즉, gz_ros_control이 <mimic> 태그를 읽어서 뭔가를 하고 있다는 말이다. 소스 코드에 해당 부분을 찾아보니 다음과 같은 부분이 있다.
gz_system.cpp

double position_error =
  position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier;

double velocity_sp = (-1.0) * position_error * this->dataPtr->update_rate;

위치 오류에 비례해서 속도 명령을 계산하고 있다.

결론적으로 mimic joint에 bullet-featherstone의 물리 제약과 gz_ros2_control의 속도 명령이 동시에 실행되고 있었던 것이다.

ros2_control의 개입을 막음

둘 중 하나를 없애야 하는데 Gazebo의 로그에서 보듯이 기본 물리 엔진인 dartsim이 mimic 제약을 지원하지 않는다는 내용이 있다. 실제 시뮬레이션 화면에서도 그리퍼 동작은 어색하다. 앞서 언급했던 github issue 내용이 맞다는 뜻이므로 물리 제약을 남기고 ros2_control 쪽이 개입하지 않도록 해야 한다.

ros2_control 문서 에 방법이 있다.

If someone wants to deactivate the mimic joint behavior for whatever reason without changing
the URDF, it can be done by setting the attribute mimic=false of the joint tag in the
<ros2_control> section of the XML.

gripper_joint_control.xacro 파일의 해당 항목을 아래와 같이 수정하였다.

<joint name="${tf_prefix}finger2_joint" mimic="false">

수정 사항을 적용하고 앞서 증상을 잡으려고 바꿨던 값들은 모두 원래대로 되돌렸다. 이후 테스트에서는 그리퍼가 보드를 정상적으로 잡아서 이동시켰고 반복 테스트에도 문제가 발생하지 않았다.