0편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ros2, gazebo][0] – ros2, gazebo 설치 및 예제 실행
1편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ros2, gazebo][1] – ros2 publisher subscriber c++, python 구현 및 실행
2편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ros2, gazebo][2] – ros2 와 아두이노 통신 (micro ros arduino), OpenCR, robotis, dynamixel 모터 개발 환경 구성 및 예제
3편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ros2, gazebo][3] – ros2 와 아두이노(OpenCR)로 여러개의 dynamixel 모터 제어 및 엔코더 값 받기, robotis
2편, 3편 생략 가능
4편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ROS2, gazebo][4] – ROS2 u2d2 를 활용한 모터 제어, dynamixel SDK tutorial
5편 – [robot arm, manipulator 프로젝트][ubuntu 20.04, ROS2, gazebo][5]- ROS2 u2d2 를 활용하여 여러개의 dynamixel 모터 제어, 3DOF manipulator
참고- Complete Guide to Building a Robot in ROS2: URDF (.xacro) – Tutorial from Start to Finish for Both Rviz2 and Gazebo
1. make complex robot and simple control in rviz2
You need the folder structure and files to match the layout shown in the image above.
r2d2.rviz
PPanels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /TF1
- /RobotModel1
- /RobotModel1/Description Topic1
Splitter Ratio: 0.5
Tree Height: 617
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
axis:
Value: true
body:
Value: true
box:
Value: true
head:
Value: true
leg1:
Value: true
leg2:
Value: true
odom:
Value: true
rod:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: false
Tree:
odom:
axis:
body:
head:
rod:
box:
{}
leg1:
{}
leg2:
{}
Update Interval: 0
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
axis:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
body:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
box:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
leg1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
leg2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
rod:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: odom
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 10
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.459797203540802
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.618575572967529
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002cb000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1340
X: 72
Y: 60
demo.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
urdf_file_name = 'r2d2.urdf.xml'
urdf = os.path.join(
get_package_share_directory('urdf_tutorial_r2d2'),
urdf_file_name)
with open(urdf, 'r') as infp:
robot_desc = infp.read()
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
arguments=[urdf]),
Node(
package='urdf_tutorial_r2d2',
executable='state_publisher',
name='state_publisher',
output='screen'),
])r2d2.urdf.xml
<robot name="r2d2">
<link name="axis">
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.01" length=".5" />
</geometry>
<material name="gray">
<color rgba=".2 .2 .2 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.01" length=".5" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<link name="leg1">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.3" />
<geometry>
<box size=".20 .10 .8" />
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -.3" />
<geometry>
<box size=".20 .10 .8" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="leg1connect" type="fixed">
<origin xyz="0 .30 0" />
<parent link="axis"/>
<child link="leg1"/>
</joint>
<link name="leg2">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.3" />
<geometry>
<box size=".20 .10 .8" />
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -.3" />
<geometry>
<box size=".20 .10 .8" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="leg2connect" type="fixed">
<origin xyz="0 -.30 0" />
<parent link="axis"/>
<child link="leg2"/>
</joint>
<link name="body">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -0.2" />
<geometry>
<cylinder radius=".20" length=".6"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<origin xyz="0 0 0.2" />
<geometry>
<cylinder radius=".20" length=".6"/>
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="tilt" type="revolute">
<parent link="axis"/>
<child link="body"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit upper="0" lower="-.5" effort="10" velocity="10" />
</joint>
<link name="head">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<geometry>
<sphere radius=".4" />
</geometry>
<material name="white" />
</visual>
<collision>
<origin/>
<geometry>
<sphere radius=".4" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="swivel" type="continuous">
<origin xyz="0 0 0.1" />
<axis xyz="0 0 1" />
<parent link="body"/>
<child link="head"/>
</joint>
<link name="rod">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.1" />
<geometry>
<cylinder radius=".02" length=".2" />
</geometry>
<material name="gray" />
</visual>
<collision>
<origin/>
<geometry>
<cylinder radius=".02" length=".2" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="periscope" type="prismatic">
<origin xyz=".12 0 .15" />
<axis xyz="0 0 1" />
<limit upper="0" lower="-.5" effort="10" velocity="10" />
<parent link="head"/>
<child link="rod"/>
</joint>
<link name="box">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<geometry>
<box size=".05 .05 .05" />
</geometry>
<material name="blue" >
<color rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin/>
<geometry>
<box size=".05 .05 .05" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="boxconnect" type="fixed">
<origin xyz="0 0 0" />
<parent link="rod"/>
<child link="box"/>
</joint>
</robot>
state_publisher.py
from math import sin, cos, pi
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped
class StatePublisher(Node):
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
degree = pi / 180.0
loop_rate = self.create_rate(30)
# robot state
tilt = 0.
tinc = degree
swivel = 0.
angle = 0.
height = 0.
hinc = 0.005
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'axis'
joint_state = JointState()
try:
while rclpy.ok():
rclpy.spin_once(self)
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = ['swivel', 'tilt', 'periscope']
joint_state.position = [swivel, tilt, height]
# update transform
# (moving in a circle with radius=2)
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = cos(angle)*2
odom_trans.transform.translation.y = sin(angle)*2
odom_trans.transform.translation.z = 0.7
odom_trans.transform.rotation = \
euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
# send the joint state and transform
self.joint_pub.publish(joint_state)
self.broadcaster.sendTransform(odom_trans)
# Create new robot state
tilt += tinc
if tilt < -0.5 or tilt > 0.0:
tinc *= -1
height += hinc
if height > 0.2 or height < 0.0:
hinc *= -1
swivel += degree
angle += degree/4
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
def euler_to_quaternion(roll, pitch, yaw):
qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
return Quaternion(x=qx, y=qy, z=qz, w=qw)
def main():
node = StatePublisher()
if __name__ == '__main__':
main()Lastly, run colcon build to compile the workspace, and then source the setup file
ros2 launch urdf_tutorial_r2d2 demo.launch.py
rviz2Execute the following steps in two separate terminals and load the configuration in RViz2
In RViz2, click on File -> Open Config from the top menu.
or
rviz2 -d ~/ros2_ws/src/urdf_tutorial_r2d2/config/r2d2.rvizresult
2. 3dof manipulator
state_publisher.py code
from math import sin, cos, pi
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
from geometry_msgs.msg import Quaternion
from sensor_msgs.msg import JointState
from tf2_ros import TransformBroadcaster, TransformStamped
class StatePublisher(Node):
def __init__(self):
rclpy.init()
super().__init__('state_publisher')
qos_profile = QoSProfile(depth=10)
self.joint_pub = self.create_publisher(JointState, 'joint_states', qos_profile)
self.broadcaster = TransformBroadcaster(self, qos=qos_profile)
self.nodeName = self.get_name()
self.get_logger().info("{0} started".format(self.nodeName))
degree = pi / 180.0
loop_rate = self.create_rate(30)
# robot state
tilt = 0.
tinc = degree
tinc2 = degree
tinc3 = degree
swivel = 0.
swivel2 =0.
angle = 0.
height = 0.
hinc = 0.005
# message declarations
odom_trans = TransformStamped()
odom_trans.header.frame_id = 'odom'
odom_trans.child_frame_id = 'axis'
joint_state = JointState()
try:
while rclpy.ok():
rclpy.spin_once(self)
# update joint_state
now = self.get_clock().now()
joint_state.header.stamp = now.to_msg()
joint_state.name = ['swivel','swivel2', 'tilt', 'periscope']
joint_state.position = [swivel, swivel2, tilt, height]
# update transform
# (moving in a circle with radius=2)
odom_trans.header.stamp = now.to_msg()
odom_trans.transform.translation.x = 0.0#cos(angle)*2
odom_trans.transform.translation.y = 0.0#sin(angle)*2
odom_trans.transform.translation.z = 0.7
odom_trans.transform.rotation = euler_to_quaternion(0, 0, 0) # roll,pitch,yaw
# euler_to_quaternion(0, 0, angle + pi/2) # roll,pitch,yaw
# send the joint state and transform
self.joint_pub.publish(joint_state)
self.broadcaster.sendTransform(odom_trans)
# Create new robot state
tilt += tinc
if tilt < -0.5 or tilt > 0.0:
tinc *= -1
height += hinc
if height > 0.2 or height < 0.0:
hinc *= -1
# swivel += degree
swivel += tinc2
if swivel < -0.6 or swivel > 0.0:
tinc2 *= -1
swivel2 += tinc3
if swivel2 < -0.6 or swivel2 > 0.0:
tinc3 *= -1
angle += degree/4
# This will adjust as needed per iteration
loop_rate.sleep()
except KeyboardInterrupt:
pass
def euler_to_quaternion(roll, pitch, yaw):
qx = sin(roll/2) * cos(pitch/2) * cos(yaw/2) - cos(roll/2) * sin(pitch/2) * sin(yaw/2)
qy = cos(roll/2) * sin(pitch/2) * cos(yaw/2) + sin(roll/2) * cos(pitch/2) * sin(yaw/2)
qz = cos(roll/2) * cos(pitch/2) * sin(yaw/2) - sin(roll/2) * sin(pitch/2) * cos(yaw/2)
qw = cos(roll/2) * cos(pitch/2) * cos(yaw/2) + sin(roll/2) * sin(pitch/2) * sin(yaw/2)
return Quaternion(x=qx, y=qy, z=qz, w=qw)
def main():
node = StatePublisher()
if __name__ == '__main__':
main()manipulator3dof.urdf.xml
<robot name="r2d2">
<link name="axis">
<visual>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.01" length=".5" />
</geometry>
<material name="gray">
<color rgba=".2 .2 .2 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="1.57 0 0" />
<geometry>
<cylinder radius="0.01" length=".5" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<link name="leg1">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.3" />
<geometry>
<box size=".20 .10 .8" />
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -.3" />
<geometry>
<box size=".20 .10 .8" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="leg1connect" type="fixed">
<origin xyz="0 .30 0" />
<parent link="axis"/>
<child link="leg1"/>
</joint>
<link name="leg2">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.3" />
<geometry>
<box size=".20 .10 .8" />
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -.3" />
<geometry>
<box size=".20 .10 .8" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="leg2connect" type="fixed">
<origin xyz="0 -.30 0" />
<parent link="axis"/>
<child link="leg2"/>
</joint>
<link name="body">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -0.2" />
<geometry>
<box size="0.4 0.4 0.4" />
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 -0.2" />
<geometry>
<box size="0.4 0.4 0.4" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="tilt" type="revolute">
<parent link="axis"/>
<child link="body"/>
<origin xyz="0 0 0" rpy="0 0 0" />
<axis xyz="0 0 1" />
<limit upper="0" lower="-.5" effort="10" velocity="10" />
</joint>
<link name="head">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0.2 0 0.1" />
<geometry>
<box size="0.2 0.2 0.2" />
</geometry>
<material name="white" />
</visual>
<collision>
<origin xyz="0.2 0 0.1" />
<geometry>
<box size="0.2 0.2 0.2" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="swivel" type="revolute">
<origin xyz="0 0 0.1" rpy="0 0 0" />
<axis xyz="1 0 0" />
<parent link="body"/>
<child link="head"/>
<limit upper="0" lower="-.5" effort="10" velocity="10" />
</joint>
<link name="head2">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0.2 0 0.4" />
<geometry>
<box size="0.2 0.2 1.0" />
</geometry>
<material name="white" />
</visual>
<collision>
<origin xyz="0.2 0 0.4" />
<geometry>
<box size="0.2 0.2 1.0" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="swivel2" type="revolute">
<origin xyz="0.2 0 0.1" rpy="0 0 0" />
<axis xyz="0 1 0" />
<parent link="head"/>
<child link="head2"/>
<limit upper="0" lower="-.5" effort="10" velocity="10" />
</joint>
<link name="rod">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<origin xyz="0 0 -.1" />
<geometry>
<cylinder radius=".02" length=".2" />
</geometry>
<material name="gray" />
</visual>
<collision>
<origin/>
<geometry>
<cylinder radius=".02" length=".2" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="periscope" type="prismatic">
<origin xyz=".12 0 .15" />
<axis xyz="0 0 1" />
<limit upper="0" lower="-.5" effort="10" velocity="10" />
<parent link="head"/>
<child link="rod"/>
</joint>
<link name="box">
<inertial>
<mass value="1"/>
<inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
<origin/>
</inertial>
<visual>
<geometry>
<box size=".05 .05 .05" />
</geometry>
<material name="blue" >
<color rgba="0 0 1 1" />
</material>
</visual>
<collision>
<origin/>
<geometry>
<box size=".05 .05 .05" />
</geometry>
<contact_coefficients mu="0" kp="1000.0" kd="1.0"/>
</collision>
</link>
<joint name="boxconnect" type="fixed">
<origin xyz="0 0 0" />
<parent link="rod"/>
<child link="box"/>
</joint>
</robot>
demo2.launch.py
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time', default='false')
urdf_file_name = 'manipulator3dof.urdf.xml'
urdf = os.path.join(
get_package_share_directory('urdf_tutorial_r2d2'),
urdf_file_name)
with open(urdf, 'r') as infp:
robot_desc = infp.read()
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use simulation (Gazebo) clock if true'),
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}],
arguments=[urdf]),
Node(
package='urdf_tutorial_r2d2',
executable='state_publisher',
name='state_publisher',
output='screen'),
])in terminal
4. final result
each terminal
rviz2 -d ~/ros2_ws/src/py_pubsub/config/r2d2.rviz
ros2 launch py_pubsub demo.launch.py
ros2 run test_dynamixel test_move_pos3 /dev/ttyUSB0
ros2 run py_pubsub manipulator_test
ros2 run py_pubsub plot_angle
ros2 run py_pubsub state_publisher
0 Comments