INIT: working robo-arm
This commit is contained in:
Marcin M
@@ -0,0 +1,3 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
@@ -0,0 +1,37 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(arm_simulation)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclpy REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(trajectory_msgs REQUIRED)
|
||||
|
||||
install(
|
||||
DIRECTORY launch urdf config scripts
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# Install Python scripts directly to the lib folder so 'ros2 run' can find them
|
||||
install(PROGRAMS
|
||||
scripts/move_arm.py
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,24 @@
|
||||
controller_manager:
|
||||
ros__parameters:
|
||||
update_rate: 100 # Hz
|
||||
use_sim_time: true
|
||||
|
||||
joint_state_broadcaster:
|
||||
type: joint_state_broadcaster/JointStateBroadcaster
|
||||
|
||||
arm_controller:
|
||||
type: joint_trajectory_controller/JointTrajectoryController
|
||||
|
||||
arm_controller:
|
||||
ros__parameters:
|
||||
joints:
|
||||
- joint_1
|
||||
- joint_2
|
||||
- joint_3
|
||||
- joint_4
|
||||
command_interfaces:
|
||||
- position
|
||||
state_interfaces:
|
||||
- position
|
||||
- velocity
|
||||
use_sim_time: true
|
||||
@@ -0,0 +1,107 @@
|
||||
import os
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
|
||||
from launch.event_handlers import OnProcessExit
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
import xacro
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# 1. Define package name and paths
|
||||
package_name = "arm_simulation"
|
||||
pkg_share = get_package_share_directory(package_name)
|
||||
|
||||
# Path to your XACRO file
|
||||
xacro_file = os.path.join(pkg_share, "urdf", "arm.urdf.xacro")
|
||||
|
||||
# Process XACRO to raw URDF string
|
||||
robot_description_config = xacro.process_file(xacro_file)
|
||||
robot_description = {"robot_description": robot_description_config.toxml()}
|
||||
|
||||
# 2. Robot State Publisher Node
|
||||
# This broadcasts the robot's coordinate frames and transforms (TF)
|
||||
node_robot_state_publisher = Node(
|
||||
package="robot_state_publisher",
|
||||
executable="robot_state_publisher",
|
||||
output="screen",
|
||||
parameters=[robot_description],
|
||||
)
|
||||
|
||||
# 3. Gazebo Harmonic Launch
|
||||
# Includes the official modern Gazebo launch script.
|
||||
# '-r' tells Gazebo to unpause the physics engine automatically on startup.
|
||||
gazebo = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(
|
||||
get_package_share_directory("ros_gz_sim"),
|
||||
"launch",
|
||||
"gz_sim.launch.py",
|
||||
)
|
||||
),
|
||||
launch_arguments={"gz_args": "-r empty.sdf"}.items(),
|
||||
)
|
||||
|
||||
# 4. Spawn Robot Node (Modern Gazebo Creator)
|
||||
# Replaces the old 'spawn_entity.py'. It grabs the URDF via the /robot_description topic.
|
||||
spawn_entity = Node(
|
||||
package="ros_gz_sim",
|
||||
executable="create",
|
||||
arguments=["-topic", "robot_description", "-name", "simple_arm"],
|
||||
output="screen",
|
||||
)
|
||||
|
||||
# 5. Joint State Broadcaster Spawner
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"joint_state_broadcaster",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# 6. Arm Controller Spawner
|
||||
# Loads the trajectory controller managing your arm's joint configuration.
|
||||
arm_controller_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=[
|
||||
"arm_controller",
|
||||
"--controller-manager",
|
||||
"/controller_manager",
|
||||
],
|
||||
)
|
||||
|
||||
# --- Event Handlers (Timing Control) ---
|
||||
# We must delay activating controllers until the Gazebo simulator is fully running
|
||||
# and has registered the 'ros2_control' plugin manager inside the robot entity.
|
||||
|
||||
# Delay loading joint_state_broadcaster until AFTER the robot is physically spawned
|
||||
delay_joint_state_broadcaster = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=spawn_entity,
|
||||
on_exit=[joint_state_broadcaster_spawner],
|
||||
)
|
||||
)
|
||||
|
||||
# Delay loading arm_controller until AFTER the joint_state_broadcaster is fully active
|
||||
delay_arm_controller = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=joint_state_broadcaster_spawner,
|
||||
on_exit=[arm_controller_spawner],
|
||||
)
|
||||
)
|
||||
|
||||
# Return the launch description with all processes grouped
|
||||
return LaunchDescription(
|
||||
[
|
||||
node_robot_state_publisher,
|
||||
gazebo,
|
||||
spawn_entity,
|
||||
delay_joint_state_broadcaster,
|
||||
delay_arm_controller,
|
||||
]
|
||||
)
|
||||
@@ -0,0 +1,24 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>arm_simulation</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="marcin.matczak@student.put.poznan.pl">marcin</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>trajectory_msgs</depend>
|
||||
<depend>ros_gz_sim</depend>
|
||||
<depend>gz_ros2_control</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
Executable
+59
@@ -0,0 +1,59 @@
|
||||
#!/usr/bin/env python3
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
from builtin_interfaces.msg import Duration
|
||||
|
||||
class ArmTrajectoryPublisher(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('arm_trajectory_publisher')
|
||||
|
||||
# 1. Create a publisher targeting the trajectory controller topic
|
||||
self.publisher_ = self.create_publisher(
|
||||
JointTrajectory,
|
||||
'/arm_controller/joint_trajectory',
|
||||
10
|
||||
)
|
||||
|
||||
# 2. Wait 2 seconds to make sure connections are secure, then execute
|
||||
self.timer = self.create_timer(2.0, self.move_robot_arm)
|
||||
self.get_logger().info('Arm controller node initialized. Waiting to send command...')
|
||||
|
||||
def move_robot_arm(self):
|
||||
# Cancel the timer so this command only sends once
|
||||
self.timer.cancel()
|
||||
|
||||
# 3. Initialize the trajectory message structure
|
||||
msg = JointTrajectory()
|
||||
|
||||
# These names MUST match the joint names defined in your URDF and YAML files exactly
|
||||
msg.joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4']
|
||||
|
||||
# 4. Create a waypoint goal
|
||||
point = JointTrajectoryPoint()
|
||||
|
||||
# Target positions in Radians (joint_1 = 90 degrees, joint_2 = 45 degrees)
|
||||
# Example position: joint_1=0.0, joint_2=0.5, joint_3=0.5, joint_4=-0.5
|
||||
point.positions = [0.0, 0.5, 0.5, -0.5]
|
||||
|
||||
# Tell the physics engine to take exactly 4.0 seconds to execute this transition smoothly
|
||||
point.time_from_start = Duration(sec=4, nanosec=0)
|
||||
|
||||
# Append the waypoint to the trajectory list
|
||||
msg.points.append(point)
|
||||
|
||||
# 5. Publish the message to Gazebo
|
||||
self.get_logger().info('Sending smooth trajectory command to joint_1 and joint_2!')
|
||||
self.publisher_.publish(msg)
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = ArmTrajectoryPublisher()
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -0,0 +1,174 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="four_axis_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<material name="blue"><color rgba="0.0 0.0 0.8 1.0"/></material>
|
||||
<material name="grey"><color rgba="0.5 0.5 0.5 1.0"/></material>
|
||||
<material name="black"><color rgba="0.1 0.1 0.1 1.0"/></material>
|
||||
<material name="red"><color rgba="0.8 0.0 0.0 1.0"/></material>
|
||||
|
||||
<link name="world"/>
|
||||
|
||||
<joint name="virtual_joint" type="fixed">
|
||||
<parent link="world"/>
|
||||
<child link="base_link"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry><cylinder radius="0.2" length="0.1"/></geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry><cylinder radius="0.2" length="0.1"/></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="5.0"/>
|
||||
<inertia ixx="0.05" ixy="0.0" ixz="0.0" iyy="0.05" iyz="0.0" izz="0.1"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint_1" type="revolute">
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.14" upper="3.14" effort="100.0" velocity="2.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="link_1">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.2" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.08" length="0.4"/></geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.2" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.08" length="0.4"/></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.2"/>
|
||||
<mass value="2.0"/>
|
||||
<inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.03" iyz="0.0" izz="0.006"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint_2" type="revolute">
|
||||
<parent link="link_1"/>
|
||||
<child link="link_2"/>
|
||||
<origin xyz="0 0 0.4" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit lower="-1.57" upper="1.57" effort="100.0" velocity="2.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="link_2">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.2" rpy="0 0 0"/>
|
||||
<geometry><box size="0.06 0.06 0.4"/></geometry>
|
||||
<material name="grey"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.2" rpy="0 0 0"/>
|
||||
<geometry><box size="0.06 0.06 0.4"/></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.2"/>
|
||||
<mass value="1.5"/>
|
||||
<inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.001"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint_3" type="revolute">
|
||||
<parent link="link_2"/>
|
||||
<child link="link_3"/>
|
||||
<origin xyz="0 0 0.4" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit lower="-2.0" upper="2.0" effort="100.0" velocity="2.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="link_3">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.15" rpy="0 0 0"/>
|
||||
<geometry><box size="0.05 0.05 0.3"/></geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.15" rpy="0 0 0"/>
|
||||
<geometry><box size="0.05 0.05 0.3"/></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.15"/>
|
||||
<mass value="1.0"/>
|
||||
<inertia ixx="0.008" ixy="0.0" ixz="0.0" iyy="0.008" iyz="0.0" izz="0.0004"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="joint_4" type="revolute">
|
||||
<parent link="link_3"/>
|
||||
<child link="link_4"/>
|
||||
<origin xyz="0 0 0.3" rpy="0 0 0"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
<limit lower="-1.57" upper="1.57" effort="50.0" velocity="3.0"/>
|
||||
</joint>
|
||||
|
||||
<link name="link_4">
|
||||
<visual>
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.04" length="0.1"/></geometry>
|
||||
<material name="red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0.05" rpy="0 0 0"/>
|
||||
<geometry><cylinder radius="0.04" length="0.1"/></geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<origin xyz="0 0 0.05"/>
|
||||
<mass value="0.5"/>
|
||||
<inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.0004"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
|
||||
<ros2_control name="GazeboSimSystem" type="system">
|
||||
<hardware>
|
||||
<plugin>gz_ros2_control/GazeboSimSystem</plugin>
|
||||
</hardware>
|
||||
|
||||
<joint name="joint_1">
|
||||
<command_interface name="position"><param name="min">-3.14</param><param name="max">3.14</param></command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="joint_2">
|
||||
<command_interface name="position"><param name="min">-1.57</param><param name="max">1.57</param></command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="joint_3">
|
||||
<command_interface name="position"><param name="min">-2.0</param><param name="max">2.0</param></command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
|
||||
<joint name="joint_4">
|
||||
<command_interface name="position"><param name="min">-1.57</param><param name="max">1.57</param></command_interface>
|
||||
<state_interface name="position"><param name="initial_value">0.0</param></state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
</ros2_control>
|
||||
|
||||
<gazebo>
|
||||
<plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system.so">
|
||||
<parameters>$(find arm_simulation)/config/ros2_controllers.yaml</parameters>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="base_link"><visual><material><ambient>0.1 0.1 0.1 1.0</ambient><diffuse>0.1 0.1 0.1 1.0</diffuse></material></visual></gazebo>
|
||||
<gazebo reference="link_1"><visual><material><ambient>0.0 0.0 0.8 1.0</ambient><diffuse>0.0 0.0 0.8 1.0</diffuse></material></visual></gazebo>
|
||||
<gazebo reference="link_2"><visual><material><ambient>0.5 0.5 0.5 1.0</ambient><diffuse>0.5 0.5 0.5 1.0</diffuse></material></visual></gazebo>
|
||||
<gazebo reference="link_3"><visual><material><ambient>0.0 0.0 0.8 1.0</ambient><diffuse>0.0 0.0 0.8 1.0</diffuse></material></visual></gazebo>
|
||||
<gazebo reference="link_4"><visual><material><ambient>0.8 0.0 0.0 1.0</ambient><diffuse>0.8 0.0 0.0 1.0</diffuse></material></visual></gazebo>
|
||||
|
||||
</robot>
|
||||
Reference in New Issue
Block a user