ROS2 URDF doosan robot arm

전에 실습했던 rb10-1300이 Gazebo에서 제대로 작동하지 않았기때문에 기존에 Gazebo에서 돌아가도록 만들어진

doosan robot arm을 이용해서 어떤 차이가 있는지 확인하였다.

https://github.com/dvalenciar/robotic_arm_environment?tab=readme-ov-file

GitHub - dvalenciar/robotic_arm_environment: Doosan robotic arm, simulation, control, visualization in Gazebo and ROS2 for Reinf

 

먼저 빌드할 공간을 만들어주었다.

source /opt/ros/foxy/setup.bash
mkdir -p ros2_ws/src
cd ..
colcon build

 

몇몇 의존성 및 필요한 패키지 설치

sudo apt install python3-vcstool

sudo apt install ros-foxy-test-msgs

sudo apt install ros-foxy-control-toolbox

sudo apt install ros-foxy-gazebo-ros-pkgs

sudo apt install ros-foxy-xacro

sudo apt install ros-foxy-joint-state-publisher-gui

 

그 다음 깃허브에서 gazebo_ros2_control을 다운받아줬다.

cd ros2_ws/src

git clone -b foxy https://github.com/ros-simulation/gazebo_ros2_control.git

cd ..

colcon build​​

 

또 두산 로봇 암을 다운받았다.

cd ros2_ws/src

git clone https://github.com/dvalenciar/robotic_arm_environment.git

cd ..

colcon build

 

그리고 실행시켜주었다.

cd ros2_ws

. install/setup.bash

ros2 launch my_doosan_pkg my_doosan_gazebo.launch.py

 

또 다른 터미널에서 아래 내용을 실행시켜준다.

cd ros2_ws

. install/setup.bash

ros2 launch my_doosan_pkg my_doosan_rviz.launch.py

 

그럼 아래와 같은 결과가 나온다.

하지만 아직 Gazebo에서는 움직이지 않는다.

 

움직이게 하기 위해서 xacro파일에 damping 계수와 friction계수를 넣었다.

알고보니 레인보우 로보틱스 회사만 그런 것이 아니라 대부분의 회사들도 마찰, 감쇠 계수까지 제공하지는 않았다.

그래서 이 패키지를 만든 사람도 마찰, 감쇠 계수들을 임의로 추가하였던 것이다.

 

 

이 계수들이 없으면 Gazebo에서 실행시 이렇게 분리되어 덜덜거린다.

 

그리고 gazebo_ros2_control 파일을 추가하였다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
	<xacro:macro name="config_ctr_gazebo">

		<!-- I need this for ROS2 control -->
		<!-- I create this file, it was NOT available in the original Doosan repo -->

		<ros2_control name="GazeboSystem" type="system">
			<hardware>
				<plugin>gazebo_ros2_control/GazeboSystem</plugin>
	    	</hardware>

	    	<joint name="joint1">
	    		<command_interface name="position">
	    			<param name="min">-6.2832</param>
	    			<param name="max">6.2832</param>
	    		</command_interface>
	    		<state_interface name="position"/>
	    		<state_interface name="velocity"/>
	    		<state_interface name="effort"/>
	    	</joint>

	    	<joint name="joint2">
	    		<command_interface name="position">
	    			<param name="min">-6.2832</param>
	    			<param name="max">6.2832</param>
	    		</command_interface>
	    		<state_interface name="position"/>
	    		<state_interface name="velocity"/>
	    		<state_interface name="effort"/>
	    	</joint>

	    	<joint name="joint3">
	    		<command_interface name="position">
	    			<param name="min">-2.7925</param>
	    			<param name="max">2.7925</param>
	    		</command_interface>
	    		<state_interface name="position"/>
	    		<state_interface name="velocity"/>
	    		<state_interface name="effort"/>
	    	</joint>

	    	<joint name="joint4">
	    		<command_interface name="position">
	    			<param name="min">-6.2832</param>
	    			<param name="max">6.2832</param>
	    		</command_interface>
	    		<state_interface name="position"/>
	    		<state_interface name="velocity"/>
	    		<state_interface name="effort"/>
	    	</joint>


	    	<joint name="joint5">
	    		<command_interface name="position">
	    			<param name="min">-6.2832</param>
	    			<param name="max">6.2832</param>
	    		</command_interface>
	    		<state_interface name="position"/>
	    		<state_interface name="velocity"/>
	    		<state_interface name="effort"/>
	    	</joint>


	    	<joint name="joint6">
	    		<command_interface name="position">
	    			<param name="min">-6.2832</param>
	    			<param name="max">6.2832</param>
	    		</command_interface>
	    		<state_interface name="position"/>
	    		<state_interface name="velocity"/>
	    		<state_interface name="effort"/>
	    	</joint>
	  	</ros2_control>

		<gazebo>
			<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
        		<!--<robot_sim_type>gazebo_ros2_control/DefaultRobotHWSim</robot_sim_type> -->
				<robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
				<parameters>$(find urdf_tutorial)/config/simple_controller.yaml</parameters>
		    </plugin>
		</gazebo>

	</xacro:macro>
</robot>

 

그리고 각 링크들의 pid계수들과 마찰, 최대 최소 속력 계수들도 설정해주었다.

<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
	<xacro:macro name="dsr_config_coeff">

    <!-- I need this for a better simulation in Gazebo-->
    <!-- I modified this file a bit, it was available in the original Doosan repo as macro.gazebo.xacro -->
    <!-- This include the Contact Coefficients and other parameters, still no sure if this is correct --> 
    <!-- More info here http://gazebosim.org/tutorials?tut=ros_urdf -->


    <!-- Base -->
    <gazebo reference="base_0">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>

    <!-- Link1 -->
    <gazebo reference="link1">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>
    
    <!-- Link2 -->
    <gazebo reference="link2">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>
    
    <!-- Link3 -->
    <gazebo reference="link3">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>
    
    <!-- Link4 -->
    <gazebo reference="link4">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>
    
    <!-- Link5 -->
    <gazebo reference="link5">    
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>
    
    <!-- Link6 -->
    <gazebo reference="link6">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>

	</xacro:macro>
</robot>

 

또 실제 로봇과 연동시 필요한 정보들도 넣어주었다(우리는 실제 로봇이랑 연동하는 것이 아니기 때문에 필요 없음)

<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
	<xacro:macro name="dsr_transmission">
		<!-- Transmission 1 -->
		<transmission name="tran1">
			<type>transmission_interface/SimpleTransmission</type>
			<joint name="joint1">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
			</joint>
			<actuator name="motor1">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
				<mechanicalReduction>1</mechanicalReduction>
			</actuator>
		</transmission>
		<!-- Transmission 2 -->
		<transmission name="tran2">
			<type>transmission_interface/SimpleTransmission</type>
			<joint name="joint2">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
			</joint>
			<actuator name="motor2">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
				<mechanicalReduction>1</mechanicalReduction>
			</actuator>
		</transmission>
		<!-- Transmission 3 -->
		<transmission name="tran3">
			<type>transmission_interface/SimpleTransmission</type>
			<joint name="joint3">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
			</joint>
			<actuator name="motor3">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
				<mechanicalReduction>1</mechanicalReduction>
			</actuator>
		</transmission>
		<!-- Transmission 4 -->
		<transmission name="tran4">
			<type>transmission_interface/SimpleTransmission</type>
			<joint name="joint4">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
			</joint>
			<actuator name="motor4">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
				<mechanicalReduction>1</mechanicalReduction>
			</actuator>
		</transmission>
		<!-- Transmission 5 -->
		<transmission name="tran5">
			<type>transmission_interface/SimpleTransmission</type>
			<joint name="joint5">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
			</joint>
			<actuator name="motor5">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
				<mechanicalReduction>1</mechanicalReduction>
			</actuator>
		</transmission>
		<!-- Transmission 6 -->
		<transmission name="tran6">
			<type>transmission_interface/SimpleTransmission</type>
			<joint name="joint6">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
			</joint>
			<actuator name="motor6">
				<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
				<mechanicalReduction>1</mechanicalReduction>
			</actuator>
		</transmission>
	</xacro:macro>
</robot>

 

이제 이 파일들을 따로 저장하고 원래 로봇팔에서 불러와야 한다.

 

원래 로봇팔 xacro코드에 아래와 같은 방식으로 추가해주면 위에서 만든 xacro파일들을 불러올 수 있다.

<xacro:include filename="$(find my_doosan_pkg)/description/xacro/macro.gazebo_config_control.xacro" />
<xacro:include filename="$(find my_doosan_pkg)/description/xacro/macro.transmission.xacro" />
<xacro:include filename="$(find my_doosan_pkg)/description/xacro/macro.internal_config.xacro" />

    .
    .
    .
    .
    .
<xacro:config_ctr_gazebo /> 
<xacro:dsr_transmission />
<xacro:dsr_config_coeff />

 

그리고 컨트롤러의 설정을 포함하는 구성파일도 만들어줘야하는데 /config라는 폴더를 만들고 안에 simple_controller.yaml을 만들어준다.

controller_manager:
  ros__parameters:
    update_rate: 100 # Hz


    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster


    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController


joint_trajectory_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6

    write_op_modes:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
    
    interface_name: position

    command_interfaces:
      - position
    
    state_interfaces:
      - position
      - velocity

    state_publish_rate: 50.0 # Defaults to 50
    action_monitor_rate: 20.0 # Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    hardware_state_has_offset: true
    deduce_states_from_derivatives: true
    
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)

 

여기에는 컨트롤러 유형과 컨트롤러가 적용될 조인트가 포함되어 있다. 우리의 경우 Joint Trajectory Controller > Position Controller로 작업할 것이다.

 

이제 만든 이 모든 것들을 포함하는 launch파일을 만들것이다.

필요한 모든 라이브러리를 가져오기 시작한 다음  패키지의 공유 디렉토리에서 xacro파일을 읽는다. 그런 다음 robot_state_publisher를 실행하여 로봇의 모든 상태를 TF에 게시하고 매개변수 서버에 넣는다. 다음 단계에서 gazebo world를 로드하고 로봇을 생성한다. 마지막으로 컨트롤러를 로드하고 시작한다.

 

'''
Author: David Valencia
Date: 26 / 08 /2021

Describer:  
			
			This scrip LOAD and START a basic joint_trajectory_controller
			The info and configuration of the controller can be found in the config folder:
			
			/src/my_doosan_pkg/config/simple_controller.yaml

			Note: I remove rviz here in order to using an other rviz config later on my own environment 
                  Also, gazebo and the empty world are launched in my own environment later 

			--> I will invoke this launch file later in my environment launch file <--

			Update:
			I do not start rviz here
			I do not start gzebo here in order to start later in my own environment.
			Just spawn the robot 
			
			- Robot model m1013 color white
			- Robot model a0912 color blue
'''

import os
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.substitutions import Command
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():


	#robot model to option m1013 or a0912 
	
	robot_model = 'a0912'
	#robot_model = 'm1013'

	xacro_file = get_package_share_directory('my_doosan_pkg') + '/description'+'/xacro/'+ robot_model +'.urdf.xacro'

	
	# Robot State Publisher 
	robot_state_publisher = Node(package    ='robot_state_publisher',
								 executable ='robot_state_publisher',
								 name       ='robot_state_publisher',
								 output     ='both',
								 parameters =[{'robot_description': Command(['xacro', ' ', xacro_file])           
								}])


	# Spawn the robot in Gazebo
	spawn_entity_robot = Node(package     ='gazebo_ros', 
							  executable  ='spawn_entity.py', 
							  arguments   = ['-entity', 'my_doosan_robot', '-topic', 'robot_description'],
							  output      ='screen')

	# Gazebo   
	#world_file_name = 'my_empty_world.world'
	#world = os.path.join(get_package_share_directory('my_doosan_pkg'), 'worlds', world_file_name)
	#gazebo_node = ExecuteProcess(cmd=['gazebo', '--verbose', world,'-s', 'libgazebo_ros_factory.so'], output='screen')


	'''
	# RViz
	rviz_config_file = get_package_share_directory('my_doosan_pkg') + "/rviz/view_config.rviz"
	rviz_node = Node(package='rviz2',
					 executable='rviz2',
					 name='rviz2',
					 output='log',
					 arguments=['-d', rviz_config_file])
	'''
	


	# load and START the controllers in launch file
	
	load_joint_state_broadcaster = ExecuteProcess(
										cmd=['ros2', 'control', 'load_controller', '--set-state', 'start','joint_state_broadcaster'],
										output='screen')

	
	load_joint_trajectory_controller = ExecuteProcess( 
										cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_trajectory_controller'], 
										output='screen')



	return LaunchDescription([robot_state_publisher, spawn_entity_robot, load_joint_state_broadcaster, load_joint_trajectory_controller  ])

 

 

그리고 setup.py를 편집하여 방금 만든 폴더와 파일들을 찾을 수 있도록 한다.

(os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*.*'))),

 

cd ros2_ws

colcon build --packages-up-to my_doosan_pkg

. install/setup.bash

ros2 launch my_doosan_pkg my_doosan_gazebo_controller.launch.py

 

정상적으로 작동한다.

이제 명령을 주어서 움직이도록 해보겠다.

 

Trajectory용 노드 파일을 만들어주어야 한다.

'''
Author: David Valencia
Date: 26 / 08 /2021

Describer:  An action Client to move the robot joint to a specific position

			This script send the position "angles" of each joint under 
			
			the ACTION - SERVICE /joint_trajectory_controller/joint_trajectory
			
			I need to run first the my_doosan_controller in order to load and start the controllers
			Update: I can also run my environment launch file 

			Executable name in the setup file: trajectory_points_act_server		
'''

import rclpy
from rclpy.duration import Duration
from rclpy.action import ActionClient
from rclpy.node import Node
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint


class TrajectoryActionClient(Node):

	def __init__(self):

		super().__init__('points_publisher_node_action_client')
		self.action_client = ActionClient (self, FollowJointTrajectory, '/joint_trajectory_controller/follow_joint_trajectory')

	def send_goal(self):

		points = []

		point1_msg = JointTrajectoryPoint()
		point1_msg.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ]
		point1_msg.time_from_start = Duration(seconds=2.0).to_msg()

		point2_msg = JointTrajectoryPoint()
		point2_msg.positions=[0.0, 0.0, 0.52, 0.0, 0.0, 0.0] 
		point2_msg.time_from_start = Duration(seconds=4, nanoseconds=0).to_msg()

		point3_msg = JointTrajectoryPoint()
		point3_msg.positions=[0.0, 0.0, -0.17, 0.0, 0.0, 0.0] 
		point3_msg.time_from_start = Duration(seconds=6, nanoseconds=0).to_msg()

		point4_msg = JointTrajectoryPoint()
		point4_msg.positions=[0.0, 0.0, -0.52, 0.0, 0.0, 0.0] 
		point4_msg.time_from_start = Duration(seconds=8, nanoseconds=0).to_msg()

		point5_msg = JointTrajectoryPoint()
		point5_msg.positions=[0.0, 0.0, 0.17, 0.0, 0.0, 0.0] 
		point5_msg.time_from_start = Duration(seconds=10, nanoseconds=0).to_msg()

		points.append(point1_msg)
		points.append(point2_msg)
		points.append(point3_msg)
		points.append(point4_msg)
		points.append(point5_msg)

		joint_names = ['joint1','joint2','joint3','joint4','joint5','joint6']
		goal_msg = FollowJointTrajectory.Goal()
		goal_msg.goal_time_tolerance = Duration(seconds=1, nanoseconds=0).to_msg()
		goal_msg.trajectory.joint_names = joint_names
		goal_msg.trajectory.points = points

		self.action_client.wait_for_server()
		self.send_goal_future = self.action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
		self.send_goal_future.add_done_callback(self.goal_response_callback)

	
	def goal_response_callback(self, future):
		
		goal_handle = future.result()

		if not goal_handle.accepted:
			self.get_logger().info('Goal rejected ')
			return

		self.get_logger().info('Goal accepted')

		self.get_result_future= goal_handle.get_result_async()
		self.get_result_future.add_done_callback(self.get_result_callback)

	def get_result_callback (self, future):
		
		result = future.result().result
		self.get_logger().info('Result: '+str(result))
		rclpy.shutdown()

		
	def feedback_callback(self, feedback_msg):
		feedback = feedback_msg.feedback


def main(args=None):

	rclpy.init()
	
	action_client = TrajectoryActionClient()
	future = action_client.send_goal()
	rclpy.spin(action_client)

if __name__ == '__main__':
	main()

 

이 파일을 /my_doosan_pkg 폴더 안에 저장하고 이름을 joint_points_act_service.py라고 정했다.

이 파일도 실행 가능하도록 setup.py에 추가하여야한다.

entry_points={
        'console_scripts': [
                'trajectory_points_act_server = my_doosan_pkg.joint_points_act_service:main',
        ],
    },

 

그리고 다른 터미널 창에서 아래 내용을 실행한다.

. install/setup.bash

ros2 run my_doosan_pkg trajectory_points_act_server

 

그림과 같이 움직이는 것을 볼 수 있다.