Poked it with joint jog

bonk

src/ar3_config/config/ar3_controllers.yaml

@@ -2,7 +2,6 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll
 moveit_simple_controller_manager:
   controller_names:
     - joint_trajectory_controller
-
   joint_trajectory_controller:
     action_ns: follow_joint_trajectory
     type: FollowJointTrajectory

src/ar3_config/config/ar3_joy.yaml

@@ -0,0 +1,69 @@
+###############################################
+# Modify all parameters related to servoing here
+###############################################
+use_gazebo: false # Whether the robot is started in a Gazebo simulation environment
+
+## Properties of incoming commands
+command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
+scale:
+  # Scale parameters are only used if command_in_type=="unitless"
+  linear:  0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
+  rotational: 0.8 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
+  # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
+  joint: 0.5
+
+## Properties of outgoing commands
+publish_period: 0.034  # 1/Nominal publish rate [seconds]
+low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)
+
+# What type of topic does your robot driver expect?
+# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
+command_out_type: trajectory_msgs/JointTrajectory
+
+# What to publish? Can save some bandwidth as most robots only require positions or velocities
+publish_joint_positions: true
+publish_joint_velocities: false
+publish_joint_accelerations: false
+
+## Incoming Joint State properties
+low_pass_filter_coeff: 2.0  # Larger --> trust the filtered data more, trust the measurements less.
+
+## MoveIt properties
+move_group_name:  ar3_arm  # Often 'manipulator' or 'arm'
+planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
+
+## Other frames
+ee_frame_name: link_6  # The name of the end effector link, used to return the EE pose
+robot_link_command_frame: link_6  # commands must be given in the frame of a robot link. Usually either the base or end effector
+
+## Stopping behaviour
+incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a new command
+# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
+# Important because ROS may drop some messages and we need the robot to halt reliably.
+num_outgoing_halt_msgs_to_publish: 4
+
+## Configure handling of singularities and joint limits
+lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity)
+hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
+joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
+
+## Topic names
+cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
+joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
+joint_topic: /joint_states
+status_topic: ~/status # Publish status to this topic
+command_out_topic: /joint_trajectory_controller/joint_trajectory # Publish outgoing commands here
+
+## Collision checking for the entire robot body
+check_collisions: true # Check collisions?
+collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
+# Two collision check algorithms are available:
+# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
+# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
+collision_check_type: threshold_distance
+# Parameters for "threshold_distance"-type collision checking
+self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
+scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
+# Parameters for "stop_distance"-type collision checking
+collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
+min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

src/ar3_config/config/ompl_planning.yaml

@@ -153,3 +153,4 @@ move_group:
       - LazyPRMstarkConfigDefault
       - SPARSkConfigDefault
       - SPARStwokConfigDefault
+

src/ar3_config/launch/dev.launch.py

@@ -1,7 +1,8 @@
 from os.path import join
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch_ros.actions import Node
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 from launch.actions import ExecuteProcess
 import yaml
 
@@ -30,21 +31,58 @@ def generate_launch_description():
             executable='rviz2',
             output='log',
             arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
-            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]),
+            parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
+        ),
 
         # Attatch robot to map
         Node(
             package='tf2_ros',
             executable='static_transform_publisher',
             output='screen',
-            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']),
+            arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
+        ),
+
+        Node(
+            package='joystickmove',
+            executable='move',
+            output='screen',
+        ),
+
+        # Create joystick controller
+        ComposableNodeContainer(
+            name="joystick_container",
+            namespace="/",
+            package="rclcpp_components",
+            executable="component_container",
+            composable_node_descriptions=[
+                ComposableNode(
+                    package="moveit_servo",
+                    plugin="moveit_servo::ServoServer",
+                    name="servo_server",
+                    parameters=[
+                        {'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
+                        robot_description,
+                        robot_description_semantic,
+                    ],
+                    extra_arguments=[{"use_intra_process_comms": True}],
+                ),
+                ComposableNode(
+                    package="joy",
+                    plugin="joy::Joy",
+                    name="joy_node",
+                    extra_arguments=[{"use_intra_process_comms": True}],
+                )
+            ],
+            output="screen",
+        ),
 
         # Controller
         Node(
             package='controller_manager',
             executable='ros2_control_node',
             output='screen',
-            parameters=[robot_description, join(config_root, 'controller_manager.yaml')]),
+            parameters=[robot_description, join(config_root, 'controller_manager.yaml')]
+        ),
 
         # Start controllers
         ExecuteProcess(
@@ -85,12 +123,4 @@ def generate_launch_description():
             executable='robot_state_publisher',
             output='screen',
             parameters=[robot_description]),
-
-        # Fake robot state publisher
-        # Node(
-        #     package='ar3_description',
-        #     executable='state_publisher',
-        #     name='state_publisher',
-        #     emulate_tty=True,
-        #     output='screen'),
     ])

src/ar3_hardware/include/ar3_hardware/ar3_system.hpp

@@ -60,6 +60,20 @@ private:
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j4_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j5_pub_;
   rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr j6_pub_;
+
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j1;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j2;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j3;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j4;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j5;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr s_j6;
+
+  void update_j1_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j2_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j3_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j4_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j5_cb(std_msgs::msg::Float32::SharedPtr msg);
+  void update_j6_cb(std_msgs::msg::Float32::SharedPtr msg);
 };
 
 }  // namespace ar3_hardware

src/ar3_hardware/src/ar3_system.cpp

@@ -125,6 +125,13 @@ hardware_interface::return_type Ar3SystemHardware::start()
   j5_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j5", 10);
   j6_pub_ = nh_->create_publisher<std_msgs::msg::Float32>("/move/j6", 10);
 
+  s_j1 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j1", 10, std::bind(&Ar3SystemHardware::update_j1_cb, this, std::placeholders::_1));
+  s_j2 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j2", 10, std::bind(&Ar3SystemHardware::update_j2_cb, this, std::placeholders::_1));
+  s_j3 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j3", 10, std::bind(&Ar3SystemHardware::update_j3_cb, this, std::placeholders::_1));
+  s_j4 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j4", 10, std::bind(&Ar3SystemHardware::update_j4_cb, this, std::placeholders::_1));
+  s_j5 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j5", 10, std::bind(&Ar3SystemHardware::update_j5_cb, this, std::placeholders::_1));
+  s_j6 = nh_->create_subscription<std_msgs::msg::Float32>("/update/j6", 10, std::bind(&Ar3SystemHardware::update_j6_cb, this, std::placeholders::_1));
+
   status_ = hardware_interface::status::STARTED;
 
   RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "System Successfully started!");
@@ -143,10 +150,35 @@ hardware_interface::return_type Ar3SystemHardware::stop()
   return hardware_interface::return_type::OK;
 }
 
+void Ar3SystemHardware::update_j1_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[0] = msg->data;
+}
+
+void Ar3SystemHardware::update_j2_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[1] = msg->data;
+}
+
+void Ar3SystemHardware::update_j3_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[2] = msg->data;
+}
+
+void Ar3SystemHardware::update_j4_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[3] = msg->data;
+}
+
+void Ar3SystemHardware::update_j5_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[4] = msg->data;
+}
+
+void Ar3SystemHardware::update_j6_cb(std_msgs::msg::Float32::SharedPtr msg) {
+    hw_positions_[5] = msg->data;
+}
+
 hardware_interface::return_type Ar3SystemHardware::read()
 {
   //RCLCPP_INFO(rclcpp::get_logger("Ar3SystemHardware"), "Reading...");
 
+  /*
   for (uint i = 0; i < hw_commands_.size(); i++)
   {
     hw_positions_[i] = 6.9;
@@ -156,6 +188,7 @@ hardware_interface::return_type Ar3SystemHardware::read()
   //    "Got position state %.5f for '%s'!", hw_positions_[i],
   //    info_.joints[i].name.c_str());
   }
+  */
 
   //RCLCPP_INFO(
   //  rclcpp::get_logger("Ar3SystemHardware"), "Joints successfully read! (%.5f,%.5f,%.5f)",

src/cobot_platform/cobot_platform/move.py

@@ -38,6 +38,14 @@ class CobotMove(Node):
         self.create_subscription(Float32, base_topic+'j5', self._move_j5_cb, rclpy.qos.qos_profile_sensor_data)
         self.create_subscription(Float32, base_topic+'j6', self._move_j6_cb, rclpy.qos.qos_profile_sensor_data)
 
+        publish_base_topic = 'update/'
+        self.s_j1 = self.create_publisher(Float32, publish_base_topic+'j1', rclpy.qos.qos_profile_sensor_data)
+        self.s_j2 = self.create_publisher(Float32, publish_base_topic+'j2', rclpy.qos.qos_profile_sensor_data)
+        self.s_j3 = self.create_publisher(Float32, publish_base_topic+'j3', rclpy.qos.qos_profile_sensor_data)
+        self.s_j4 = self.create_publisher(Float32, publish_base_topic+'j4', rclpy.qos.qos_profile_sensor_data)
+        self.s_j5 = self.create_publisher(Float32, publish_base_topic+'j5', rclpy.qos.qos_profile_sensor_data)
+        self.s_j6 = self.create_publisher(Float32, publish_base_topic+'j6', rclpy.qos.qos_profile_sensor_data)
+
         qos = QoSProfile(depth=10)
         self._joint_pub = self.create_publisher(JointState, 'joint_states', qos)
         self._joint_state = JointState()
@@ -76,8 +84,8 @@ class CobotMove(Node):
         line = self._serial.readline().decode('UTF-8')
 
         blocks = re.findall(r'[ABCDEF]-*\d+', line)
-        a = float(blocks[0][1:]) / 2000. * pi * 2.
-        b = float(blocks[1][1:]) / 3840.64 * pi * 2.
+        a = float(blocks[0][1:]) / 8000. * pi * 2.
+        b = float(blocks[1][1:]) / 10000 * pi * 2.
         c = float(blocks[2][1:]) / 10000. * pi * 2.
         d = float(blocks[3][1:]) / 2000. * pi * 2.
         e = float(blocks[4][1:]) / 2000. * pi * 2.
@@ -89,8 +97,14 @@ class CobotMove(Node):
 
         self._joint_pub.publish(self._joint_state)
 
-#        print(self._joint_state.position)
+        self.s_j1.publish(Float32(data=a))
+        self.s_j2.publish(Float32(data=b))
+        self.s_j3.publish(Float32(data=c))
+        self.s_j4.publish(Float32(data=d))
+        self.s_j5.publish(Float32(data=e))
+        self.s_j6.publish(Float32(data=f))
 
+    #    self._logger.info(f'published {a} {b} {c} {d} {e} {f}')
     # def _push ?
 
 def main(args=None):
@@ -105,3 +119,4 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
+

src/joystickmove/joystickmove/move.py

@@ -1,8 +1,40 @@
 import rclpy
 from rclpy.node import Node
 
+from std_srvs.srv import Trigger
+
 from std_msgs.msg import Float32
 from sensor_msgs.msg import Joy
+from geometry_msgs.msg import TwistStamped
+from moveit_msgs.msg import PlanningScene, PlanningSceneWorld
+from control_msgs.msg import JointJog
+
+from enum import Enum
+
+class Buttons(Enum):
+    CROSS = 0
+    CIRCLE = 1
+    TRIANGLE = 2
+    SQUARE = 3
+    L1 = 4
+    R1 = 5
+    L2 = 6
+    R2 = 7
+    SELECT = 8
+    START = 9
+    CENTER = 10
+    L3 = 11
+    R3 = 12
+
+class Axes(Enum):
+    LEFT_X = 0
+    LEFT_Y = 1
+    L1 = 2
+    RIGHT_X = 3
+    RIGHT_Y = 4
+    R1 = 5
+    DPAD_UPDOWN = 6
+    DPAD_LEFTRIGHT = 7
 
 class JoyMove(Node):
     def __init__(self):
@@ -10,13 +42,73 @@ class JoyMove(Node):
         self._logger = self.get_logger()
         self._logger.info('joystick init')
 
+        JOY_TOPIC = '/joy'
+        TWIST_TOPIC = '/servo_server/delta_twist_cmds'
+        JOINT_TOPIC = '/servo_server/delta_joint_cmds'
+        EE = 'joint_6'
+        BASE = 'base_link'
+
         self.create_subscription(Joy, '/joy', self._joy_cb, rclpy.qos.qos_profile_sensor_data)
 
-        self.base_pub = self.create_publisher(Float32, '/move/j1', 10)
+        self.twist_pub = self.create_publisher(TwistStamped, TWIST_TOPIC, 10);
+        self.joint_pub = self.create_publisher(JointJog, JOINT_TOPIC, 10);
+        self.collision_pub = self.create_publisher(PlanningScene, "/planning_scene", 10);
+
+        self.servo_client = self.create_client(Trigger, '/servo_server/start_servo')
+        # Check if the a service is available
+        while not self.servo_client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('servo_server not available, waiting again...')
+        self._logger.info('servo_server connected.')
+
+        req = Trigger.Request()
+        self.servo_client.call_async(req)
+
+        '''
+        ps = PlanningScene
+        ps.world = PlanningSceneWorld
+        ps.is_diff = True
+        self.collision_pub.publish(ps)
+        '''
 
     def _joy_cb(self, msg):
-        asd = msg.axes[1]
-        self.base_pub.publish(Float32(data=asd))
+        axes = msg.axes
+        buttons = msg.buttons
+
+        if buttons[Buttons.CROSS.value] or buttons[Buttons.CIRCLE.value] or buttons[Buttons.TRIANGLE.value] or buttons[Buttons.SQUARE.value] or axes[Axes.DPAD_UPDOWN.value] or axes[Axes.DPAD_LEFTRIGHT.value]:
+
+            joint = JointJog()
+
+            joint.joint_names = ['joint_1', 'joint_2',
+                                 'joint_6', 'joint_5']
+            joint.velocities = [float(axes[Axes.DPAD_UPDOWN.value]), float(axes[Axes.DPAD_LEFTRIGHT.value]),
+                                float(buttons[Buttons.CIRCLE.value] - buttons[Buttons.CROSS.value]), float(buttons[Buttons.SQUARE.value] - buttons[Buttons.TRIANGLE.value])]
+            joint.duration = 1.0
+
+            joint.header.stamp = self.get_clock().now().to_msg()
+            joint.header.frame_id = "link_3"
+            self.joint_pub.publish(joint)
+            return
+
+        ts = TwistStamped()
+
+        ts.twist.linear.z = axes[Axes.RIGHT_Y.value]
+        ts.twist.linear.y = axes[Axes.RIGHT_X.value]
+
+        lin_x_right = -0.5 * axes[Axes.R1.value] # - AXIS_DEFAULTS.at(R1));
+        lin_x_left = 0.5 * axes[Axes.L1.value] # - AXIS_DEFAULTS.at(L1));
+        ts.twist.linear.x = lin_x_right + lin_x_left
+
+        ts.twist.angular.y = axes[Axes.LEFT_Y.value]
+        ts.twist.angular.x = axes[Axes.LEFT_X.value]
+
+        roll_positive = buttons[Buttons.R2.value]
+        roll_negative = -1 * buttons[Buttons.L2.value]
+        ts.twist.angular.z = float(roll_positive + roll_negative)
+
+        ts.header.stamp = self.get_clock().now().to_msg()
+        ts.header.frame_id = "link_6"
+
+        self.twist_pub.publish(ts)
 
 
 def main(args=None):

src/joystickmove/setup.py

@@ -20,7 +20,7 @@ setup(
     tests_require=['pytest'],
     entry_points={
         'console_scripts': [
-            'joystickmove = joystickmove.move:main'
+            'move = joystickmove.move:main'
         ],
     },
 )