Control HC10DTP using an Xbox 360 joystick

[tall1]

Hi,
I want to use a joystick to teleoperate the robot (real-time operation).

I wrote my own node (with the help of ChatGPT) that subscribes to the /joy topic and moves the pose in x and y when something is published to joy:

joystick_teleop.py

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import Joy
from moveit_commander import RobotCommander, MoveGroupCommander
from geometry_msgs.msg import Pose


class JoystickTeleop:
    def __init__(self):
        rospy.init_node('joystick_teleop')
        rospy.logwarn("started teleop node")

        # Subscribe to the joystick topic
        rospy.Subscriber("joy", Joy, self.joy_callback)

        # Create a RobotCommander for the robot
        self.robot = RobotCommander()

        # Create a MoveGroupCommander for the desired planning group
        self.move_group = MoveGroupCommander("arm")  # Replace with your move group name

        # Initialize the target pose
        self.target_pose = Pose()

    def joy_callback(self, data):
        rospy.loginfo("callback executed")
        # Map joystick input to robot motion commands
        # Example: Control the end effector position of a robot arm
        if data.axes[0] != 0 or data.axes[1] != 0:
            print(data)
            # Update the target pose based on the joystick input
            self.target_pose.position.x += data.axes[0] * 0.1
            self.target_pose.position.y += data.axes[1] * 0.1

            # Set the target pose for the move group
            self.move_group.set_pose_target(self.target_pose)
            self.move_group.set_planning_time(5)  # Increase the planning time to 5 seconds
            # Plan and execute the motion
            self.move_group.go(wait=True)


if __name__ == '__main__':
    try:
        joystick_teleop = JoystickTeleop()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

For some reason the robot does not react to the node. I'm getting the following errors:

[ERROR] [1686832400.868134914] [/move_group]: arm/arm: Unable to sample any valid states for goal tree
[ INFO] [1686832400.868209984] [/move_group]: arm/arm: Created 1 states (1 start + 0 goal)
[ INFO] [1686832400.868454296] [/move_group]: No solution found after 5.003464 seconds
[ WARN] [1686832400.868512755] [/move_group]: Timed out
[ INFO] [1686832400.884523583] [/move_group]: Unable to solve the planning problem
[ INFO] [1686832402.538878936] [/move_group]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1686832402.538982584] [/move_group]: Planning attempt 1 of at most 1

This is from the info from the joystick_teleop.py:
[ INFO] [1686832400.885087394] [/moveit_python_wrappers_1686832320279744835]: ABORTED: TIMED_OUT

[marip8]

A couple of comments:

• The Motoman ROS1 driver is not set up by default to support on-the-fly streaming control. There are a couple of PRs ( point-streaming: further development based on PR #88  #215, merge branch boneil_point_streaming manually #88, On-the-fly point streaming and Cartesian jog #37) that implement that functionality to some degree, but you would need to build from that branch to utilize that functionality
• The particular error that you posted is not an error with the robot driver; it's an error with the MoveIt motion planner (presumably OMPL). Apparently the target destination is not reachable, hence the 0 goal states created
• The approach of setting a target destination with a joystick, invoking a motion plan, then executing that motion plan in a tight loop will probably not work fast enough to give you the true teleoperation results that you're presumably hoping for. You should look into something like MoveIt Servo instead for teleop capability. I think it can utilize a twist input command that could be generated from a joystick

[gavanderhoorn]

You should look into something like MoveIt Servo instead for teleop capability. I think it can utilize a twist input command that could be generated from a joystick

it can, but keep in mind that even with servo, motoman_driver is not out-of-the-box usable with it.

[tall1]

it can, but keep in mind that even with servo, motoman_driver is not out-of-the-box usable with it.

So if I understand correctly - in order to support MoveIt Servo I will need to customize the motoman_driver?

[gavanderhoorn]

Correct.

One of the PRs @marip8 mentioned could work.

[jim-rees]

I retired in 2016 and now live a life of leisure in Yucatán. One of my biggest (work-related) regrets is that I didn't have time to follow up on that PR. As I recall there was some fairly important piece of it that I couldn't merge because of IP reasons but I don't remember the details. Good luck and sorry I can't be more help.

[tall1]

Update on our progress -

After using #point-streaming: further development based on PR #88 #215 that @marip8 recommended, we managed to add Point Streaming support on our MoveIt! package.

We also launched the moveit servo and published a geometry_msgs/TwistStamped message to the /servo_server/delta_twist_cmds:

rostopic pub -s /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto
twist:
  linear:
    x: 0.0
    y: 0.01
    z: -0.01
  angular:
    x: 0.0
    y: 0.0
    z: 0.0"

an example of a message in /joint_command topic:

header: 
  seq: 53598
  stamp: 
    secs: 0
    nsecs:         0
  frame_id: "base_link"
joint_names: 
  - joint_1_s
  - joint_2_l
  - joint_3_u
  - joint_4_r
  - joint_5_b
  - joint_6_t
points: 
  - 
    positions: [6.533622581628151e-06, 5.226898065302521e-05, -2.2305332095129415e-05, 1.0779324838949833e-05, -1.0779324838949833e-05, 3.23379754263442e-05]
    velocities: []
    accelerations: []
    effort: []
    time_from_start: 
      secs: 53598
      nsecs:         0

In the roscore console we get the following error:

[ERROR] [1687949105.598324960] [/motion_streaming_interface]: Point streaming queue has reached max allowed elements
[ERROR] [1687949105.619271964] [/motion_streaming_interface]: Aborting point stream operation.  Failed to send point (#0): Invalid message (3) : Insufficient trajectory data.  Must supply valid time, pos, and velocity fields. (3015)

[gavanderhoorn]

I don't believe I know of anyone who's used the point streaming with MoveIt Servo. That doesn't mean you're the first, it just means we don't have any resources (ie: previous issues/PRs/discussions) to point you to here on ros-industrial/motoman.

From the error message I'd suggest reducing the rate at which Servo tries to send new points. Start with a low rate, then gradually increase until you see the error again.

Please be aware of the inherent limitations of this approach, such as what's discussed in #219.

Also note: somewhat pedantic perhaps, but:

After using #point-streaming: further development based on PR #88 #215 that @marip8 recommended

those PRs were not recommended per se. They are just examples showing how something like what you asked for could be added to motoman_driver.

[tall1]

We decreased the publishing rate to 1:10 seconds and it still gives me this error.
I'm trying to understand through the code what is the meaning and how to solve this.