moveit servo : Link 'panda_link8' not found in model 'ur10' about moveit2

[Yao-Xinchen]

I have encountered the same problem. Have you solved it? @viprob-ai

I am using a custom robot and trying to control it with moveit servo. The servo seems to be trying to find panda_link8, but no file configures this param.

Here is my launch file:

# modified from https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py

import os
import yaml
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition, UnlessCondition
from launch.substitutions import LaunchConfiguration
from launch_param_builder import ParameterBuilder
from moveit_configs_utils import MoveItConfigsBuilder

def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None

def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("scara_description", package_name="scara_moveit_config").to_moveit_configs()

    # launch_as_standalone_node = LaunchConfiguration(
    #     "launch_as_standalone_node", default="true"
    # )

    acceleration_filter_update_period = {"update_period": 0.01}
    planning_group_name = {"planning_group_name": "scara_arm"}

    # servo_params = {
    #     "scara_servo": ParameterBuilder("scara_servo")
    #     .yaml("config/servo_config.yaml")
    #     .to_dict()
    # }
    servo_yaml = load_yaml("scara_servo", "config/servo_config.yaml")
    servo_params = {"moveit_servo": servo_yaml}

    # Launch a standalone Servo node.
    # As opposed to a node component, this may be necessary (for example) if Servo is running on a different PC
    scara_servo = launch_ros.actions.Node(
        package="scara_servo",
        executable="scara_servo",
        name="scara_servo",
        parameters=[
            servo_params,
            acceleration_filter_update_period,
            planning_group_name,
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,
        ],
        output="screen",
        # condition=IfCondition(launch_as_standalone_node),
    )

    return launch.LaunchDescription([
        scara_servo,
    ])

And here is the config for servo.

###############################################
# Modify all parameters related to servoing here
###############################################

# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]

## Properties of outgoing commands
publish_period: 0.01  # 1/Nominal publish rate [seconds]
max_expected_latency: 0.1  # delay between sending a command and the robot executing it [seconds]

command_in_type: "speed_units"  # "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. Unit is [m/s]. Only used for Cartesian commands.
  rotational:  0.8  # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
  # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
  joint: 0.5

# 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: true
publish_joint_accelerations: false

## Plugins for smoothing outgoing commands
use_smoothing: true
smoothing_filter_plugin: "online_signal_smoothing::ButterworthFilterPlugin"

# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
# which other nodes can use as a source for information about the planning environment.
# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
# then is_primary_planning_scene_monitor needs to be set to false.
is_primary_planning_scene_monitor: true
check_octomap_collisions: false  # Check collision against the octomap (if a 3D sensor plugin is available)

## MoveIt properties
move_group_name: scara_arm  # Often 'manipulator' or 'arm'

## 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_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]  # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger.
leaving_singularity_threshold_multiplier: 2.0  # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620)

## 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  # Get joint states from this tpoic
status_topic: ~/status  # Publish status to this topic
command_out_topic: /scara_arm_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.
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]

For more details, please check my repo.

[Yao-Xinchen]

I have found the reason and successfully solved the problem.

The variable auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(this->shared_from_this()); has several members whose default values are related to panda.

These variables can only be initialized with ROS params.

I specified move_group_name, planning_frame, ee_frame_name and robot_link_command_frame in servo_config.yaml, according my own robot. This could successfully start the servo.

[fzoric8]

Hi @Yao-Xinchen ,

I'm wondering how do you use moveit_servo with specified command topic.

I'm getting commands on that topic, but my arm remains stationary, it may have something to do with MoveitControllerManager.
Do you have any advices, thanks :)

[Yao-Xinchen]

Hi, @fzoric8

Do you mean what commands I am sending to the servo? If so, I specify the topics in the servo's parameters.

## Topic names
cartesian_command_in_topic: scara/delta_twist_cmds  # Topic for incoming Cartesian twist commands
joint_command_in_topic: scara/delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: /joint_states
status_topic: scara/status # Publish status to this topic
command_out_topic: /scara_arm_controller/joint_trajectory # Publish outgoing commands here

I think the servo would use these topics. The node I am using to contact with the servo sends topics with the same names.

For now, I have managed to let the servo react to my commands. However, the arm still cannot move. There are outputs indicating my arm is near singularity and limits so it is halted, but it shouldn't be. I'am still trying to solve this. I am not sure whether we are facing the same problem.

I hope this can help you.

[fzoric8]

@Yao-Xinchen ,

Thank you for your fast reply. I've managed to get it going.

I think you're right.

You should check if you're controller is initialized and if it reacts on commands. If that's the case than it is another issue, especially if you're mentioning singularity - I had a similar problem when working with moveit_servo for ROS 1 and MoveIt1 and when working with 6DOF manipulator.

There was an issue when I would want to command pose (x,y,z,roll,pitch,yaw of EE) , almost instantly (or really fast) I would end up in singularity. I've managed to solve the problem to only command (x, z, roll, pitch, yaw) and I concluded that it is nearly impossible to control 6DOF by servoing with a 6DOF, and I'm quite sure it is related to the pseuodo-inverse of the Jacobian losing rank quickly due to physical characteristics of the arm (inability to reach some commanded points).

So if you're using SCARA arm, you can't control it with joystick because it commands linear and angular velocities, which means that you most likely command 6DOF for 4DOF manipulator [in most cases].

But there is this drift dimensions parameter you can configure (in Moveit1 as I did here.

Message still exists as it can be seen here.

And there's also defined service here.

So you can try and set change drift true to all dimensions except X, Y and see if your arm reacts [or those that make sense, for SCARA you would like to command X,Y].

I hope it helps!