# controller_manager provides the necessary infrastructure to manage multiple controllers efficiently and robustly.
controller_manager:
  ros__parameters:
    update_rate: 10 # update_rate specifies how often (in Hz) the controllers should be updated.

    # The JointTrajectoryController allows you to send joint trajectory commands to a group 
    # of joints on a robot. These commands specify the desired positions for each joint.     
    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    grip_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    # Responsible for publishing the current state of the robot's joints to the /joint_states 
    # ROS 2 topic
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

# Define the parameters for each controller
arm_controller:
  ros__parameters:
    joints:
      - link1_to_link2
      - link2_to_link3
      - link3_to_link4
      - link4_to_link5
      - link5_to_link6
      - link6_to_link6flange

    # The controller will expect position commands as input for each of these joints.
    command_interfaces:
      - position
    
    # Tells the controller that it should expect to receive position data as the state 
    # feedback from the hardware interface,
    state_interfaces:
      - position

    # If true, When set to true, the controller will not use any feedback from the system 
    # (e.g., joint positions, velocities, efforts) to compute the control commands. 
    open_loop_control: true

    # When set to true, it allows the controller to integrate the trajectory goals it receives. 
    # This means that if the goal trajectory only specifies positions, the controller will 
    # numerically integrate the positions to compute the velocities and accelerations required 
    # to follow the trajectory.
    allow_integration_in_goal_trajectories: true

grip_controller:
  ros__parameters:
    joints:
      - gripper_controller
      
    command_interfaces:
      - position

    state_interfaces:
      - position

    open_loop_control: true
    allow_integration_in_goal_trajectories: true

<?xml version="1.0" ?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="mycobot_280">
    <gazebo>
        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
            <parameters>$(find mycobot_gazebo)/config/mycobot_280_controllers.yaml</parameters>
        </plugin>
    </gazebo>
</robot>

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

    <ros2_control name="RobotSystem" type="system">
        <hardware>
            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
        </hardware>

        <joint name="link1_to_link2">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link2_to_link3">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link3_to_link4">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link4_to_link5">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link5_to_link6">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link6_to_link6flange">
            <command_interface name="position">
                <param name="min">-3.05</param>
                <param name="max">3.05</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_controller">
            <command_interface name="position">
                <param name="min">-0.7</param>
                <param name="max">0.15</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_base_to_gripper_left2">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">1.0</param>
            <command_interface name="position">
                <param name="min">-0.8</param>
                <param name="max">0.5</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_left3_to_gripper_left1">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">-1.0</param>
            <command_interface name="position">
                <param name="min">-0.5</param>
                <param name="max">0.5</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_base_to_gripper_right3">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">-1.0</param>
            <command_interface name="position">
                <param name="min">-0.15</param>
                <param name="max">0.7</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_base_to_gripper_right2">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">-1.0</param>
            <command_interface name="position">
                <param name="min">-0.5</param>
                <param name="max">0.8</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_right3_to_gripper_right1">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">1.0</param>
            <command_interface name="position">
                <param name="min">-0.5</param>
                <param name="max">0.5</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

    </ros2_control>    
    
</robot>

# Author: Addison Sears-Collins
# Date: April 29, 2024
# Description: Launch a robotic arm in Gazebo 
import os
from launch import LaunchDescription
from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.actions import RegisterEventHandler
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro

def generate_launch_description():

  # Constants for paths to different files and folders
  package_name_description = 'mycobot_description'
  package_name_gazebo = 'mycobot_gazebo'

  default_robot_name = 'mycobot_280'
  gazebo_launch_file_path = 'launch'
  gazebo_models_path = 'models'
  rviz_config_file_path = 'rviz/mycobot_280_arduino_view_description.rviz'
  urdf_file_path = 'urdf/ros2_control/gazebo/mycobot_280.urdf.xacro'
  world_file_path = 'worlds/empty.world' # e.g. 'world/empty.world', 'world/house.world'

  # Set the path to different files and folders.  
  pkg_ros_gz_sim = FindPackageShare(package='ros_gz_sim').find('ros_gz_sim')  
  pkg_share_description = FindPackageShare(package=package_name_description).find(package_name_description)
  pkg_share_gazebo = FindPackageShare(package=package_name_gazebo).find(package_name_gazebo)

  default_rviz_config_path = os.path.join(pkg_share_description, rviz_config_file_path)  
  default_urdf_model_path = os.path.join(pkg_share_gazebo, urdf_file_path)
  gazebo_launch_file_path = os.path.join(pkg_share_gazebo, gazebo_launch_file_path)   
  gazebo_models_path = os.path.join(pkg_share_gazebo, gazebo_models_path)
  world_path = os.path.join(pkg_share_gazebo, world_file_path)
  
  # Launch configuration variables specific to simulation
  robot_name = LaunchConfiguration('robot_name')
  rviz_config_file = LaunchConfiguration('rviz_config_file')
  use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
  use_rviz = LaunchConfiguration('use_rviz')
  use_sim_time = LaunchConfiguration('use_sim_time')
  world = LaunchConfiguration('world')
  
  # Set the default pose
  x = LaunchConfiguration('x')
  y = LaunchConfiguration('y')
  z = LaunchConfiguration('z')
  roll = LaunchConfiguration('roll')
  pitch = LaunchConfiguration('pitch')
  yaw = LaunchConfiguration('yaw')
  
  # Declare the launch arguments  
  declare_robot_name_cmd = DeclareLaunchArgument(
    name='robot_name',
    default_value=default_robot_name,
    description='The name for the robot')

  declare_rviz_config_file_cmd = DeclareLaunchArgument(
    name='rviz_config_file',
    default_value=default_rviz_config_path,
    description='Full path to the RVIZ config file to use')

  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
    name='use_robot_state_pub',
    default_value='True',
    description='Whether to start the robot state publisher')

  declare_use_rviz_cmd = DeclareLaunchArgument(
    name='use_rviz',
    default_value='True',
    description='Whether to start RVIZ')
    
  declare_use_sim_time_cmd = DeclareLaunchArgument(
    name='use_sim_time',
    default_value='true',
    description='Use simulation (Gazebo) clock if true')

  declare_world_cmd = DeclareLaunchArgument(
    name='world',
    default_value=world_path,
    description='Full path to the world model file to load')

  declare_x_cmd = DeclareLaunchArgument(
    name='x',
    default_value='0.0',
    description='x component of initial position, meters')

  declare_y_cmd = DeclareLaunchArgument(
    name='y',
    default_value='0.0',
    description='y component of initial position, meters')
    
  declare_z_cmd = DeclareLaunchArgument(
    name='z',
    default_value='0.05',
    description='z component of initial position, meters')
    
  declare_roll_cmd = DeclareLaunchArgument(
    name='roll',
    default_value='0.0',
    description='roll angle of initial orientation, radians')

  declare_pitch_cmd = DeclareLaunchArgument(
    name='pitch',
    default_value='0.0',
    description='pitch angle of initial orientation, radians')

  declare_yaw_cmd = DeclareLaunchArgument(
    name='yaw',
    default_value='0.0',
    description='yaw angle of initial orientation, radians')
    
  # Specify the actions

  set_env_vars_resources = AppendEnvironmentVariable(
    'GZ_SIM_RESOURCE_PATH',
    gazebo_models_path)

  # Start arm controller
  start_arm_controller_cmd = ExecuteProcess(
    cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
        'arm_controller'],
        output='screen')

  # Start Gazebo environment
  start_gazebo_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
      os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
    launch_arguments=[('gz_args', [' -r -v 4 ', world])])

  # Start gripper controller
  start_gripper_controller_cmd =  ExecuteProcess(
    cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
        'grip_controller'],
        output='screen')
  
  # Launch joint state broadcaster
  start_joint_state_broadcaster_cmd = ExecuteProcess(
    cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
        'joint_state_broadcaster'],
        output='screen')
    
  # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
  doc = xacro.parse(open(default_urdf_model_path))
  xacro.process_doc(doc)
  start_robot_state_publisher_cmd = Node(
    condition=IfCondition(use_robot_state_pub),
    package='robot_state_publisher',
    executable='robot_state_publisher',
    name='robot_state_publisher',
    output='screen',
    parameters=[{
      'use_sim_time': use_sim_time, 
      'robot_description': doc.toxml()}])

  # Launch RViz
  start_rviz_cmd = Node(
    condition=IfCondition(use_rviz),
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', rviz_config_file])  
    
  # Spawn the robot
  start_gazebo_ros_spawner_cmd = Node(
    package='ros_gz_sim',
    executable='create',
    arguments=[
      '-string', doc.toxml(),
      '-name', robot_name,
      '-allow_renaming', 'true',
      '-x', x,
      '-y', y,
      '-z', z,
      '-R', roll,
      '-P', pitch,
      '-Y', yaw
      ],
    output='screen')

  # Launch the joint state broadcaster after spawning the robot
  load_joint_state_broadcaster_cmd = RegisterEventHandler(
     event_handler=OnProcessExit(
     target_action=start_gazebo_ros_spawner_cmd ,
     on_exit=[start_joint_state_broadcaster_cmd],))

  # Launch the arm controller after launching the joint state broadcaster
  load_arm_controller_cmd = RegisterEventHandler(
    event_handler=OnProcessExit(
    target_action=start_joint_state_broadcaster_cmd,
    on_exit=[start_arm_controller_cmd],))

  # Launch the gripper controller after launching the arm controller
  load_gripper_controller_cmd = RegisterEventHandler(
    event_handler=OnProcessExit(
    target_action=start_arm_controller_cmd,
    on_exit=[start_gripper_controller_cmd],))

  # Create the launch description and populate
  ld = LaunchDescription()

  # Declare the launch options
  ld.add_action(declare_robot_name_cmd)
  ld.add_action(declare_rviz_config_file_cmd)
  ld.add_action(declare_use_robot_state_pub_cmd)  
  ld.add_action(declare_use_rviz_cmd) 
  ld.add_action(declare_use_sim_time_cmd)
  ld.add_action(declare_world_cmd)

  ld.add_action(declare_x_cmd)
  ld.add_action(declare_y_cmd)
  ld.add_action(declare_z_cmd)
  ld.add_action(declare_roll_cmd)
  ld.add_action(declare_pitch_cmd)
  ld.add_action(declare_yaw_cmd)  

  # Add any actions
  ld.add_action(set_env_vars_resources)
  ld.add_action(start_gazebo_cmd)
  ld.add_action(start_robot_state_publisher_cmd)
  ld.add_action(start_rviz_cmd)
  
  ld.add_action(start_gazebo_ros_spawner_cmd)

  ld.add_action(load_joint_state_broadcaster_cmd)
  ld.add_action(load_arm_controller_cmd) 
  ld.add_action(load_gripper_controller_cmd) 

  return ld

#! /usr/bin/env python3

"""
Description:
  ROS 2: Executes a sample trajectory for a robotic arm in Gazebo
-------
Publishing Topics (ROS 2):
  Desired goal pose of the robotic arm
    /arm_controller/joint_trajectory - trajectory_msgs/JointTrajectory
  
  Desired goal pose of the gripper
    /grip_controller/joint_trajectory - trajectory_msgs/JointTrajectory
-------
Author: Addison Sears-Collins
Date: April 29, 2024
"""

import rclpy # Python client library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration
from std_msgs.msg import Header  # Import the Header message

# Define constants
arm_joints = [ 'link1_to_link2', 
               'link2_to_link3', 
               'link3_to_link4', 
               'link4_to_link5', 
               'link5_to_link6', 
               'link6_to_link6flange']

gripper_joints = ['gripper_controller']

class ExampleJointTrajectoryPublisherPy(Node):
    """This class executes a sample trajectory for a robotic arm
    
    """      
    def __init__(self):
        """ Constructor.
      
        """
        # Initialize the class using the constructor
        super().__init__('example_joint_trajectory_publisher_py')    
 
        # Create the publisher of the desired arm and gripper goal poses
        self.arm_pose_publisher = self.create_publisher(JointTrajectory, '/arm_controller/joint_trajectory', 1)
        self.gripper_pose_publisher = self.create_publisher(JointTrajectory, '/grip_controller/joint_trajectory', 1)

        self.timer_period = 5.0  # seconds 5.0
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

        self.frame_id = "base_link"
       
        # Desired time from the trajectory start to arrive at the trajectory point.
        # Needs to be less than or equal to the self.timer_period above to allow
        # the robotic arm to smoothly transition between points.
        self.duration_sec = 2 
        self.duration_nanosec = 0.5 * 1e9 # (seconds * 1e9)

        # Set the desired goal poses for the robotic arm.
        self.arm_positions = []
        self.arm_positions.append([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # Home location
        self.arm_positions.append([-1.345, -1.23, 0.264, -0.296, 0.389, -1.5]) # Goal location
        self.arm_positions.append([-1.345, -1.23, 0.264, -0.296, 0.389, -1.5]) 
        self.arm_positions.append([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # Home location

        self.gripper_positions = []
        self.gripper_positions.append([0.0]) # Open gripper
        self.gripper_positions.append([0.0])
        self.gripper_positions.append([-0.70]) # Close gripper
        self.gripper_positions.append([-0.70]) 

        # Keep track of the current trajectory we are executing
        self.index = 0

    def timer_callback(self):
        """Set the goal pose for the robotic arm.
    
        """
        # Create new JointTrajectory messages
        msg_arm = JointTrajectory()
        msg_arm.header = Header()  
        msg_arm.header.frame_id = self.frame_id  
        msg_arm.joint_names = arm_joints

        msg_gripper = JointTrajectory()
        msg_gripper.header = Header()  
        msg_gripper.header.frame_id = self.frame_id  
        msg_gripper.joint_names = gripper_joints

        # Create JointTrajectoryPoints
        point_arm = JointTrajectoryPoint()
        point_arm.positions = self.arm_positions[self.index]
        point_arm.time_from_start = Duration(sec=int(self.duration_sec), nanosec=int(self.duration_nanosec))  # Time to next position
        msg_arm.points.append(point_arm)
        self.arm_pose_publisher.publish(msg_arm)

        point_gripper = JointTrajectoryPoint()
        point_gripper.positions = self.gripper_positions[self.index]
        point_gripper.time_from_start = Duration(sec=int(self.duration_sec), nanosec=int(self.duration_nanosec))  
        msg_gripper.points.append(point_gripper)
        self.gripper_pose_publisher.publish(msg_gripper)

        # Reset the index
        if self.index == len(self.arm_positions) - 1:
            self.index = 0
        else:
            self.index = self.index + 1
    
def main(args=None):
  
    # Initialize the rclpy library
    rclpy.init(args=args)
  
    # Create the node
    example_joint_trajectory_publisher_py = ExampleJointTrajectoryPublisherPy()
  
    # Spin the node so the callback function is called.
    rclpy.spin(example_joint_trajectory_publisher_py)
    
    # Destroy the node
    example_joint_trajectory_publisher_py.destroy_node()
  
    # Shutdown the ROS client library for Python
    rclpy.shutdown()
  
if __name__ == '__main__':
  main()

/**
 * @file example_joint_trajectory_publisher.cpp
 * @brief ROS 2: Executes a sample trajectory for a robotic arm in Gazebo
 * 
 * Publishing Topics (ROS 2):
 *   Desired goal pose of the robotic arm
 *     /arm_controller/joint_trajectory - trajectory_msgs::msg::JointTrajectory
 *   
 *   Desired goal pose of the gripper
 *     /grip_controller/joint_trajectory - trajectory_msgs::msg::JointTrajectory
 * 
 * @author Addison Sears-Collins
 * @date April 29, 2024
 */

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "std_msgs/msg/header.hpp"

using namespace std::chrono_literals;

// Define constants
const std::vector<std::string> arm_joints = {
    "link1_to_link2",
    "link2_to_link3",
    "link3_to_link4",
    "link4_to_link5",
    "link5_to_link6",
    "link6_to_link6flange"
};

const std::vector<std::string> gripper_joints = {
    "gripper_controller"
};

class ExampleJointTrajectoryPublisherCpp : public rclcpp::Node
{
public:
    ExampleJointTrajectoryPublisherCpp()
        : Node("example_joint_trajectory_publisher_cpp")
    {
        // Create the publisher of the desired arm and gripper goal poses
        arm_pose_publisher_ = create_publisher<trajectory_msgs::msg::JointTrajectory>("/arm_controller/joint_trajectory", 1);
        gripper_pose_publisher_ = create_publisher<trajectory_msgs::msg::JointTrajectory>("/grip_controller/joint_trajectory", 1);

        // Create a timer to periodically call the timerCallback function
        timer_ = create_wall_timer(5s, std::bind(&ExampleJointTrajectoryPublisherCpp::timerCallback, this));

        frame_id_ = "base_link";

        // Desired time from the trajectory start to arrive at the trajectory point.
        // Needs to be less than or equal to the timer period above to allow
        // the robotic arm to smoothly transition between points.
        duration_sec_ = 2;
        duration_nanosec_ = 0.5 * 1e9;  // (seconds * 1e9)

        // Set the desired goal poses for the robotic arm.
        arm_positions_ = {
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0},  // Home location
            {-1.345, -1.23, 0.264, -0.296, 0.389, -1.5},  // Goal location
            {-1.345, -1.23, 0.264, -0.296, 0.389, -1.5},
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}  // Home location
        };

        gripper_positions_ = {
            {0.0},  // Open gripper
            {0.0},
            {-0.70},  // Close gripper
            {-0.70}
        };

        // Keep track of the current trajectory we are executing
        index_ = 0;
    }

private:
    void timerCallback()
    {
        // Create new JointTrajectory messages for arm and gripper
        auto msg_arm = trajectory_msgs::msg::JointTrajectory();
        msg_arm.header.frame_id = frame_id_;
        msg_arm.joint_names = arm_joints;

        auto msg_gripper = trajectory_msgs::msg::JointTrajectory();
        msg_gripper.header.frame_id = frame_id_;
        msg_gripper.joint_names = gripper_joints;

        // Create JointTrajectoryPoints for arm and gripper
        auto point_arm = trajectory_msgs::msg::JointTrajectoryPoint();
        point_arm.positions = arm_positions_[index_];
        point_arm.time_from_start = rclcpp::Duration(duration_sec_, duration_nanosec_);
        msg_arm.points.push_back(point_arm);
        arm_pose_publisher_->publish(msg_arm);

        auto point_gripper = trajectory_msgs::msg::JointTrajectoryPoint();
        point_gripper.positions = gripper_positions_[index_];
        point_gripper.time_from_start = rclcpp::Duration(duration_sec_, duration_nanosec_);
        msg_gripper.points.push_back(point_gripper);
        gripper_pose_publisher_->publish(msg_gripper);

        // Reset the index 
        if (index_ == arm_positions_.size() - 1) {
            index_ = 0;
        } else {
            index_++;
        }
    }

    // Publishers for arm and gripper joint trajectories
    rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr arm_pose_publisher_;
    rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr gripper_pose_publisher_;

    // Timer for periodic callback
    rclcpp::TimerBase::SharedPtr timer_;

    // Frame ID for the joint trajectories
    std::string frame_id_;

    // Duration for each trajectory point
    int duration_sec_;
    int duration_nanosec_;

    // Desired goal poses for the robotic arm and gripper
    std::vector<std::vector<double>> arm_positions_;
    std::vector<std::vector<double>> gripper_positions_;

    // Index to keep track of the current trajectory point
    size_t index_;
};

int main(int argc, char * argv[])
{
    // Initialize the ROS 2 client library
    rclcpp::init(argc, argv);

    // Create an instance of the ExampleJointTrajectoryPublisherCpp node
    auto node = std::make_shared<ExampleJointTrajectoryPublisherCpp>();

    // Spin the node to execute the callbacks
    rclcpp::spin(node);

    // Shutdown the ROS 2 client library
    rclcpp::shutdown();

    return 0;
}