MoveIt!을 이용한 2-axis Prismatic joint Gazebo에서 구현하기

이번에는 다음 사진처럼 2방향으로 로봇이 linear하게 움직이도록 만들어보겠다.

---

world부터 로봇의 시작 링크 link0 사이의 urdf 내용은 다음과 같다.

<?xml version="1.0" ?>
<robot name="indy7" xmlns:xacro="http://ros.org/wiki/xacro">
  	<xacro:include filename="$(find indy7_description)/urdf/indy7.xacro"/>

	<link name="world"/>

    <joint name="joint_y" type="prismatic">
		<parent link="world"/>
		<child link="base_link"/>
		<origin xyz="0 0 0.05" rpy="0 0 0"/>
		<axis xyz="0 1 0"/>
		<limit effort="1000" velocity="1" lower="-2" upper="2"/>
	</joint>

	<link name="base_link">
		<inertial>
			<origin xyz="0.0 0.0 0.005" rpy="0.0 0.0 0.0"/>
			<mass value="0.01"/>
			<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
		</inertial>
	</link>

    <joint name="joint_z" type="prismatic">
		<parent link="base_link"/>
		<child link="link0"/>
		<origin xyz="0 0 0.05" rpy="0 0 0"/>
		<axis xyz="0 0 1"/>
		<limit effort="1000" velocity="1" lower="-2" upper="2"/>
	</joint>

prismatic joint에서 lower과  upper는 움직이는 최소 최대 범위를 설정해주는 것이다.

 

한 축으로만 움직이게 할 때에는 world와 link0사이에 prismatic joint를 넣어주기만 하면 되는데

 

축이 두 개이상됐을 때에는 그 사이에 link를 world처럼 가상의 link를 넣으면 안 되고 가제보 물리 모델에 적용 가능하도록 inertia 정보를 넣은 link를 추가해줘야 한다. (visual, collision은 안 넣어도 된다. 필수 X. visual을 안 넣어주면 보이지 않아서 없는 것처럼 보이는 효과를 얻을 수 있다.)

크기는 움직이는 범위와 상관없이 있기만 하면 된다. 아주 작아도 상관없다.

 

이렇게 하지 않고 base_link의 정보를

	<link name="base_link"/>

로만 하면 rviz는 실행이 되지만 gazebo와 연동했을 때

[ERROR] [1659676175.867081106]: This robot has a joint named "joint_y" which is not in the gazebo model.
[FATAL] [1659676175.867116828]: Could not initialize robot simulation interface
[ERROR] [1659676196.876837598, 17.019000000]: Action client not connected: arm_controller/follow_joint_trajectory
[ERROR] [1659676213.929547156, 34.049000000]: Action client not connected: base_controller/follow_joint_trajectory
[ERROR] [1659676243.047495897, 63.121000000]: Unable to identify any set of controllers that can actuate the specified joints: [ joint0 joint1 joint2 joint3 joint4 joint5 ]
[ERROR] [1659676243.047510797, 63.121000000]: Known controllers and their joints:
[ERROR] [1659676243.047531921, 63.121000000]: Apparently trajectory initialization failed

다음과 같은 오류들이 뜬다.

 

그래서 prismatic joint 사이에 inertia 정보가 포함이 된 링크를 넣어줘야 한다.

---

매니퓰레이터를 제어할 때 밑에 2개의 prismatic_joint가 움직이면 안 되기 때문에

매니퓰레이터의 joint와 base의 joint를 group으로 나눠야 한다.

나누는 건 moveit_setup_assistant에서 나누면 된다.

 

그리고 launch/ros_controllers.launch에서

  <!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="spawn joint_state_controller"/>
  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="spawn arm_controller"/>
</launch>

여기에

  <!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="spawn joint_state_controller"/>
  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="spawn arm_controller"/>
  <node name="base_controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="spawn base_controller"/>  
</launch>

base group을 제어하기 위해 필요한 노드를 추가해준다. group이름을 어떻게 정했는지에 따라 node_name과 args 내용을 맞게 작성한다.

 

이 외에 link와 joint가 생겼기 때문에 urdf 파일 안의 tranmission, gazebo reference 내용을 추가 작성해주고,

config/ros_controllers.yaml 파일에서 joint 내용을 추가해준다.

---

moveit을 실행해서 base group의 joint를 제어하려면 

MotionPlanning/Planning/Query/Planning Group에서 Group을 바꿀 수 있다.

 

그리고 prismatic joint를 제어하려면 MotionPlanning에서 Planning을 Joints로 바꾸면 joint를 바꿀 수 있는 gui가 나온다.