URDF Model
<?xml version="1.0"?>
<robot name="6dof_arm" xmlns:xacro="http://ros.org/wiki/xacro">
<!-- Base Link -->
<link name="base_link">
<inertial>
<mass value="1.0"/>
<origin xyz="0 0 0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
<!-- Joint 1 (Base Rotation) -->
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="link1"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-3.14" upper="3.14" effort="100" velocity="1"/>
</joint>
<link name="link1">
<inertial>
<mass value="0.5"/>
<origin xyz="0 0 0.1"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
</inertial>
</link>
<!-- Repeat for Joints 2-6 with DH parameters -->
<!-- Transmission for ros_control -->
<transmission name="joint1_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="joint1_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<!-- Repeat transmissions for all joints -->
</robot>
ROS Controller Configuration
# PID Gains for Joints
joint1_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 10.0, d: 20.0, i_clamp_max: 100.0}
joint2_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 10.0, d: 20.0, i_clamp_max: 100.0}
joint3_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 10.0, d: 20.0, i_clamp_max: 100.0}
joint4_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 10.0, d: 20.0, i_clamp_max: 100.0}
joint5_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 10.0, d: 20.0, i_clamp_max: 100.0}
joint6_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 100.0, i: 10.0, d: 20.0, i_clamp_max: 100.0}
# Controller Manager
controller_manager:
ros__parameters:
update_rate: 100 # Hz
Launch File
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
def generate_launch_description():
return LaunchDescription([
# Robot State Publisher
Node(
package='robot_state_publisher',
executable='robot_state_publisher',
name='robot_state_publisher',
output='screen',
parameters=[{
'robot_description': open(os.path.join(
get_package_share_directory('your_pkg'), 'urdf', 'robot_arm.urdf.xacro'
)).read()
}]
),
# ROS Controllers
Node(
package='controller_manager',
executable='ros2_control_node',
parameters=[os.path.join(
get_package_share_directory('your_pkg'), 'config', 'controllers.yaml'
)],
output='screen'
),
# Spawn Controllers
Node(
package='controller_manager',
executable='spawner',
arguments=['joint1_controller', 'joint2_controller', '--controller-manager',
'/controller_manager'],
output='screen'
),
# MoveIt (Optional)
Node(
package='moveit_ros_move_group',
executable='move_group',
name='move_group',
output='screen'
)
])
Trajectory Execution Node
#!/usr/bin/env python3
import rclpy
from rclpy.action import ActionClient
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
class ArmController:
def __init__(self):
self.node = rclpy.create_node('arm_controller')
self.client = ActionClient(
self.node,
FollowJointTrajectory,
'/joint_trajectory_controller/follow_joint_trajectory'
)
self.joint_names = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6']
def send_goal(self, positions, time=5.0):
goal_msg = FollowJointTrajectory.Goal()
trajectory = JointTrajectory()
trajectory.joint_names = self.joint_names
point = JointTrajectoryPoint()
point.positions = positions
point.time_from_start = self.node.get_clock().now().to_msg() +
rclpy.time.Duration(seconds=time).to_msg()
trajectory.points.append(point)
goal_msg.trajectory = trajectory
self.client.wait_for_server()
return self.client.send_goal_async(goal_msg)
def main(args=None):
rclpy.init(args=args)
controller = ArmController()
controller.send_goal([0.0, 0.5, -1.0, 0.0, 0.0, 0.0]) # Example target
rclpy.spin(controller.node)
rclpy.shutdown()
if __name__ == '__main__':
main()
Hardware Interface
#include <ros/ros.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>
class ArmHardware : public hardware_interface::RobotHW {
public:
ArmHardware() {
// Initialize joint interfaces
hardware_interface::JointStateHandle state_handle("joint1", &pos[0], &vel[0], &eff[0]);
jnt_state_interface.registerHandle(state_handle);
registerInterface(&jnt_state_interface);
hardware_interface::JointHandle eff_handle(jnt_state_interface.getHandle("joint1"),
&cmd[0]);
jnt_eff_interface.registerHandle(eff_handle);
registerInterface(&jnt_eff_interface);
}
void read() {
// Read encoder values from hardware
}
void write() {
// Send commands (e.g., PWM) to motors
}
private:
double cmd[6] = {0};
double pos[6] = {0};
double vel[6] = {0};
double eff[6] = {0};
hardware_interface::JointStateInterface jnt_state_interface;
hardware_interface::EffortJointInterface jnt_eff_interface;
};
