add features (#6)

hk1d_bringup_master_follower/CMakeLists.txt

@@ -0,0 +1,16 @@
+cmake_minimum_required(VERSION 3.5)
+project(hk1d_bringup_master_follower)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+find_package(ament_cmake REQUIRED)
+
+install(
+  DIRECTORY config ros2_control launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

hk1d_bringup_master_follower/config/force_slave_config.yaml

@@ -0,0 +1,10 @@
+# Configuration file for EasyCAT based ForceSensor Slave
+vendor_id: 0x0000079a
+product_id: 0xdeadbeef
+tpdo:  # TxPDO
+  - index: 0x1a00
+    channels:
+      - {index: 0x0006, sub_index: 1, type: int16, state_interface: force.0, factor: 0.0028, offset: -0.924}  # mNm on the joint #factor:   fh   = fh / KN * L; // tics to N.m projected on the robot joint in SI, where KN:  K100g*10/9.8  //tics per Newton (10 to go from 100g -> 1kg, 9.8: Kg->N), and K100g=158 // for l=7.25cm ->0.00203,-0.67
+      - {index: 0x0006, sub_index: 2, type: int16, state_interface: force.1}
+sm:  # Sync Manager
+  - {index: 0, type: input, pdo: tpdo, watchdog: disable}

hk1d_bringup_master_follower/config/hk1d.config.xacro

@@ -0,0 +1,52 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hk1d">
+
+    <!-- Enable setting arguments from the launch file -->
+    <xacro:arg name="use_sim" default="false" />
+    <xacro:arg name="use_fake_hardware" default="true" />
+    <xacro:arg name="namespace" default="/" />
+    <xacro:arg name="command_interface" default="force" />
+
+    <xacro:arg name="runtime_config_package" default="hk1d_bringup" />
+    <xacro:arg name="controllers_file" default="hk1d_controllers.yaml" />
+
+    <xacro:property name="description_package" value="hk1d_description"/>
+
+    <!-- Declare world frame -->
+    <link name="world"/>
+    <gazebo reference="world">
+        <static>true</static>
+    </gazebo>
+
+    <!-- Import hk1d urdf file -->
+    <xacro:include filename="$(find ${description_package})/urdf/hk1d.urdf.xacro"/>
+
+    <xacro:hk1d_urdf parent="world" prefix="master_">
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+    </xacro:hk1d_urdf>
+
+    <xacro:hk1d_urdf parent="world" prefix="follower_">
+        <origin xyz="0 -0.15 0" rpy="0 0 0"/>
+    </xacro:hk1d_urdf>
+
+    <!-- Add ros2_control tags -->
+    <xacro:include filename="$(find hk1d_bringup_master_follower)/ros2_control/hk1d.r2c_hardware.xacro" />
+    <xacro:kk1d_r2c_hardware
+        use_sim="$(arg use_sim)"
+        use_fake_hardware="$(arg use_fake_hardware)"
+        command_interface="$(arg command_interface)"
+        control_freq="200"
+        runtime_config_package="$(arg runtime_config_package)"
+    />
+
+    <!-- Add gazebo tags -->
+    <xacro:if value="$(arg use_sim)">
+        <xacro:include filename="$(find ${description_package})/gazebo/hk1d.gazebo.xacro"/>
+        <xacro:iiwa_gazebo
+            prefix="$(arg prefix)"
+            namespace="$(arg namespace)"
+            runtime_config_package="$(arg runtime_config_package)"
+            controllers_file="$(arg controllers_file)"
+        />
+    </xacro:if>
+</robot>

hk1d_bringup_master_follower/config/hk1d_controllers.yaml

@@ -0,0 +1,53 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 500 # 250 Hz  #you should also change maxon_epos3_config.yaml:   - {index: 0x60C2, sub_index: 1, type: int8, value: 1} # Set interpolation time for cyclic modes, this should be equal to Ts in ms = 1000/update_rate
+
+    control_freq: 200
+
+    # Broadcasters
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    master_force_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    follower_force_sensor_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    # Controllers
+    forward_effort_controller:
+      type: forward_command_controller/ForwardCommandController
+
+    # Mock components
+    mock_follower_force_sensor:
+      type: forward_command_controller/ForwardCommandController
+
+# Broadcasters
+master_force_sensor_broadcaster:
+  ros__parameters:
+    frame_id: master_ft_sensor
+    interface_names:
+      force:
+        x: master_ft_sensor/force.0
+
+follower_force_sensor_broadcaster:
+  ros__parameters:
+    frame_id: follower_ft_sensor
+    interface_names:
+      force:
+        x: follower_ft_sensor/force.0
+
+# Controllers
+forward_position_controller:
+  ros__parameters:
+    joints:
+      - master_joint_1
+      - follower_joint_1
+    interface_name: effort
+
+# Mock components
+mock_follower_force_sensor:
+  ros__parameters:
+    joints:
+      - master_force_sensor
+    interface_name: force.0

hk1d_bringup_master_follower/config/maxon_epos3_config.yaml

@@ -0,0 +1,35 @@
+# Configuration file for Maxon EPOS3 drive
+vendor_id: 0x000000fb
+product_id: 0x64400000
+assign_activate: 0x0300  # DC Synch register
+auto_fault_reset: false  # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
+sdo:  # sdo data to be transferred at drive startup
+  - {index: 0x60C2, sub_index: 1, type: int8, value: 2} # Set interpolation time for cyclic modes to 10 ms, this should be equal to Ts in ms
+  - {index: 0x60C2, sub_index: 2, type: int8, value: -3} # Set base 10-3s
+rpdo:  # RxPDO = receive PDO Mapping
+  - index: 0x1603
+    channels:
+      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}  # Control word
+      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan } #position, default: .nan}  # Target position
+      - {index: 0x60ff, sub_index: 0, type: int32, default: 0}  # Target velocity
+      - {index: 0x6071, sub_index: 0, type: int16, command_interface: effort, default: 0, factor: 901.2}  # Target torque, converting from mNm on hk1d joint to motor torque command
+      - {index: 0x60b0, sub_index: 0, type: int32, default: 0}  # Offset position
+      - {index: 0x60b1, sub_index: 0, type: int32, default: 0}  # Offset velocity
+      - {index: 0x60b2, sub_index: 0, type: int16, default: 0}  # Offset torque
+      - {index: 0x6060, sub_index: 0, type: int8, default: 10} #8 # Mode of operation
+      - {index: 0x2078, sub_index: 1, type: uint16, default: 0}  # Digital Output Functionalities
+      - {index: 0x60b8, sub_index: 0, type: uint16, default: 0}  # Touch Probe Function
+      # - {index: 0x607d, sub_index: 1, type: int32, default: -50000}  # Software min Position limit in cyclic torque control
+      # - {index: 0x607d, sub_index: 2, type: int32, default: 50000}  # Software max Position limit in cyclic torque control
+tpdo:  # TxPDO = transmit PDO Mapping
+  - index: 0x1a03
+    channels:
+      - {index: 0x6041, sub_index: 0, type: uint16}  # Status word
+      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 0.00015708}  # Position actual value, (Rad) # Position actual value, factor: x *2*pi/tics_per_rev/capston_ratio; // from encoder tics to joint in SI (rad)
+      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 0.01047}  # Velocity actual value,(Rad/s) factor: v * 66.7 *2*pi/tics_per_rev/capston_ratio;// from motor to joint in SI (rad/s)
+      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort  , factor: 0.0011096}  # Torque actual value, factor: (Nm) // mN.m to N.m    // from motor to joint in SI
+      - {index: 0x6061, sub_index: 0, type: int8}  # Mode of operation display
+      - {index: 0x2071, sub_index: 1, type: int16}  # Digital Input Functionalities State
+      - {index: 0x60b9, sub_index: 0, type: int16}  # Touch Probe Status
+      - {index: 0x60ba, sub_index: 0, type: int32}  # Touch Probe Position 1 Positive Value
+      - {index: 0x60bb, sub_index: 0, type: int32}  # Touch Probe Position 1 Negative Value

hk1d_bringup_master_follower/launch/hk1d.launch.py

@@ -0,0 +1,194 @@
+# Copyright 2023 ICube Laboratory, University of Strasbourg
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'runtime_config_package',
+            default_value='hk1d_bringup_master_follower',
+            description='Config package with the various runtime config files (ros2 controllers, gazebo, etc.).',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'prefix',
+            default_value='""',
+            description='Prefix of the joint/link names, useful for multi-robot setup.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'namespace',
+            default_value='/',
+            description='Namespace of launched nodes, useful for multi-robot setup. \
+                         If changed than also the namespace in the controllers \
+                         configuration needs to be updated. Expected format "<ns>/".',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'use_sim',
+            default_value='false',
+            description='Start robot in Gazebo simulation.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'use_fake_hardware',
+            default_value='true',
+            description='Start robot with fake hardware mirroring command to its states.',
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'command_interface',
+            default_value='force',
+            description='Robot command interface [position|velocity|effort].',
+        )
+    )
+
+    # Initialize Arguments
+    runtime_config_package = LaunchConfiguration('runtime_config_package')
+    prefix = LaunchConfiguration('prefix')
+    namespace = LaunchConfiguration('namespace')
+    use_sim = LaunchConfiguration('use_sim')
+    use_fake_hardware = LaunchConfiguration('use_fake_hardware')
+    command_interface = LaunchConfiguration('command_interface')
+    description_package = "hk1d_description"
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name='xacro')]),
+            ' ',
+            PathJoinSubstitution(
+                [FindPackageShare('hk1d_bringup_master_follower'), 'config', 'hk1d.config.xacro']
+            ),
+            ' ',
+            'prefix:=', prefix,
+            ' ',
+            'namespace:=', namespace,
+            ' ',
+            'use_sim:=', use_sim,
+            ' ',
+            'use_fake_hardware:=', use_fake_hardware,
+            ' ',
+            'command_interface:=', command_interface,
+            ' ',
+            'description_package:=', description_package,
+            ' ',
+            'runtime_config_package:=', runtime_config_package,
+        ]
+    )
+
+    robot_description = {'robot_description': robot_description_content}
+
+    # Get SRDF via xacro
+    def get_robot_description_semantic_content():
+        return Command([
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [FindPackageShare(description_package), "srdf", "hk1d.srdf.xacro"]
+            ),
+            " ",
+            "name:=", "iiwa",
+            " ",
+            "prefix:=", prefix,
+            " ",
+            'description_package:=', description_package,
+            ' ',
+            'runtime_config_package:=', runtime_config_package,
+        ])
+
+    # robot_description_semantic = {
+    #     'robot_description_semantic': get_robot_description_semantic_content()
+    # }
+
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare("hk1d_description"), "rviz", "hk1d.rviz"]
+    )
+
+    # logger = launch.substitutions.LaunchConfiguration("log_level");
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare(runtime_config_package),
+            'config',
+            'hk1d_controllers.yaml',
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        # parameters=[]
+        parameters=[robot_description, robot_controllers],
+        output="both",
+        # arguments=['--ros-args', '--log-level', "debug"]
+    )
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+        # arguments=['--ros-args', '--log-level', "debug"]
+    )
+
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config_file],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    master_force_sensor_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["master_force_sensor_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+    follower_force_sensor_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["follower_force_sensor_broadcaster", "--controller-manager", "/controller_manager"],
+    )
+
+    nodes = [
+        control_node,
+        robot_state_pub_node,
+        joint_state_broadcaster_spawner,
+        master_force_sensor_broadcaster_spawner,
+        follower_force_sensor_broadcaster_spawner,
+        rviz_node,
+    ]
+
+    return LaunchDescription(declared_arguments + nodes)

hk1d_bringup_master_follower/package.xml

@@ -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>hk1d_bringup_master_follower</name>
+  <version>0.0.0</version>
+  <description>Package with launch files and run-time configurations for TWO hk1d systems (1 dof haptic kits).</description>
+
+  <maintainer email="[email protected]">Fadi Al-Masalmah</maintainer>
+
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>ros2_control</exec_depend>
+  <exec_depend>ros2_controllers</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>hk1d_description</exec_depend>
+  <exec_depend>rviz2</exec_depend>
+  <exec_depend>xacro</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>