이제 arm_control_app 노드가 카메라에서 영상을 받아서 집어야 할 물체의 좌표를 확인하고 물체를 집어서 지정된 위치로 이동시킬 수 있는 모든 준비가 되었다. 회전도 적용되어 있어서 보드가 어느 각도로 놓여 있어도 긴 변의 양쪽을 잡아 목표 지점에 놓게 했다.
그리퍼 위치와 각도 적용
「마커를 로봇의 좌표계로 넘기기」에서 비전 노드가 마커를 marker_0 프레임으로 발행하게 했다. arm_control_app은 TF 버퍼와 리스너를 만들고 base_link에서 marker_0으로 가는 변환을 얻는다.
(Writing a listener (C++)).
tf_stamped = tf_buffer_->lookupTransform(
"base_link", "marker_0", tf2::TimePointZero,
tf2::durationFromSec(1.0));인자는 순서대로 기준 프레임(base_link), 대상 프레임(marker_0), 조회 시각, 변환이 버퍼에 없을 때 기다릴 시간이다. 비전 노드에서는 영상이 찍힌 순간의 변환이 필요하기 때문에 타임스탬프를 넘겨주었지만, 여기서는 물체의 위치로 이동을 시작하는 때의 위치가 필요하므로 가장 최근 변환을 얻기 위해 조회 시간을 tf2::TimePointZero로 설정하였다.
변환 결과의 x, y 좌표값을 잡을 대상의 평면상 위치로 사용한다. 높이는 「마커를 로봇의 좌표계로 넘기기」에서 z를 계산하였을 때 결과 값이 −0.017로 나와 오차가 너무 커서 사용할 수가 없었다. 그래서 이미 알고 있는 보드 높이값을 상수 설정하여 사용하였다. ExecutePickAndPlaceCycle은 집는 위치와 회전, 놓는 위치와 회전을 인자로 받게 바꿨다.
bool ExecutePickAndPlaceCycle(std::shared_ptr<ArmController>& arm_ctrl,
geometry_msgs::msg::Vector3 marker_pos, geometry_msgs::msg::Quaternion marker_rot,
geometry_msgs::msg::Vector3 target_pos, geometry_msgs::msg::Quaternion target_rot)회전 적용의 필요
지금까지 그리퍼 자세는 상수였고 손가락은 항상 월드 y 방향으로 닫혔다. 보드는 60 × 40mm이고, 그리퍼 개방폭은 「ArUco 마커 인식」에서 넓힌 70mm다. 보드가 z 축으로 θ만큼 돌아가 있으면 손가락이 닫히는 방향으로 잰 보드 폭은 이렇다.
폭(θ) = 0.06·|sin θ| + 0.04·|cos θ|보드의 대각선 길이가 가장 길고 그 값은 √(0.06² + 0.04²) = 0.0721m다. 그때 각도는 tan θ = 0.06/0.04 = 1.5에서 θ는 약 56.3°다. 이 각도에서는 계산상으로 보드가 그리퍼의 손가락 사이에 들어가지도 않고, 비스듬하게 집으려고 하기 때문에 정상적으로 집어 올릴 수 없을 것이다.
마커 회전과 그리퍼 자세
marker_0의 z 축은 위를 향한다. 그리퍼는 아래를 봐야 하므로 마커 자세를 자기 x 축 기준으로 180도 뒤집어야 한다.
단위 벡터인 u를 기준으로 θ만큼 도는 회전의 쿼터니언은 아래 수식과 같다(Quaternions and spatial rotation).
단위 벡터:
쿼터니언:
x 축 (1, 0, 0)을 기준으로 180도 회전시킨다면 쿼터니언은 아래와 같다.
q = cos(180°/2) + (1·i + 0·j + 0·k)·sin(180°/2)
= cos 90° + i·sin 90°
= 0 + 1·i + 0·j + 0·k실수부를 제일 끝에 쓰는 ROS 2의 (x, y, z, w) 표기법으로 쓰면 (1, 0, 0, 0)이다.
tf2::Quaternion marker_q;
tf2::fromMsg(marker_rot, marker_q);
tf2::Quaternion align;
align.setRPY(0.0, 0.0, M_PI_2);
tf2::Quaternion tcp_q = marker_q * align * tf2::Quaternion(1, 0, 0, 0);쿼터니언은 곱하는 순서에 따라 결과가 달라진다(Quaternion fundamentals). a b는 a만큼 돌아간 좌표계에서, 그 좌표계의 축을 기준으로 다시 b만큼 더 돈다는 뜻이다. 그래서 tcp_q = marker_q align * tf2::Quaternion(1, 0, 0, 0)은 왼쪽부터 읽는다. 코드에서는 마커의 회전을 기준으로 보드의 긴 변의 양쪽을 그리퍼가 잡게 하기 위해서 z축을 중심으로 90도를 회전시켰다. 그 결과에 다시 x축을 기준으로 180도 회전하도록 하였다.
「마커를 로봇의 좌표계로 넘기기」에서 본 것처럼 OpenCV 4.6의 마커 좌표계에서 x축은 마커 그림의 오른쪽이다. 카메라 TF에서 영상 오른쪽은 base_link의 −y다. 보드가 0°일 때 마커 x축이 −y를 향해 yaw가 −90°가 된 것이다. align으로 z 축 90도를 더해 그리퍼 yaw를 보드 yaw와 맞추면, 손가락이 보드의 짧은 변 방향으로 닫혀 긴 변 양쪽을 잡는다.
파지한 보드를 내려 놓을 때에는 코드에서 정한 보드의 목표 자세에 x축을 기준으로 180도 뒤집기 연산만 하면 된다.
tf2::Quaternion place_q = target_q * tf2::Quaternion(1, 0, 0, 0);보드 각도별 테스트
bringup launch에 board_yaw 인자를 추가해, 보드를 돌린 상태로 스폰할 수 있게 했다. ros_gz_sim의 create에 -Y 인자로 넘긴다(ros_gz_sim).
실행 중에는 Gazebo의 set_pose 서비스로 보드를 돌렸다(UserCommands). 회전은 쿼터니언으로 주며, z 축 ψ 회전의 쿼터니언은 z = sin(ψ/2), w = cos(ψ/2)다. 위치도 함께 적었다.
gz service -s /world/default/set_pose \
--reqtype gz.msgs.Pose --reptype gz.msgs.Boolean --timeout 2000 \
--req 'name: "board", position: {x: 0.4, y: 0.6, z: 0.0025}, orientation: {x: 0, y: 0, z: 0.4719, w: 0.8817}'| 각도 | z | w |
|---|---|---|
| 0° | 0 | 1 |
| 30° | 0.2588 | 0.9659 |
| 45° | 0.3827 | 0.9239 |
| 56.31° | 0.4719 | 0.8817 |
| 90° | 0.7071 | 0.7071 |
다섯 각도 모두에서 보드를 집어 목표 지점에 놓았다.
