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