Move it

[josefinemonnet]

Could you maybe provide a little demo script using move it to control the robot? Would be really helpful for me.

Thank you in advance.

[mhubii]

Hi @josefinemonnet , yes will definitely work on that. A little busy this week though.

[Nicolai-98]

I am trying to find the move group of the arm right now using a little script but I am having some trouble. The Plan is to later use move_it to control the arm more precisely. For example making sure movements are deterministic even on 7DOF using additional parameters like status and turn (KUKA style).

• First I start the launch script hoping it starts the move group. ros2 launch lbr_bringup bringup.launch.py sim:=true moveit:=true
  • Note I am seeing some output that could be related to the problem:
    • [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?
    • [rviz2-9] [INFO] [1721117634.530722284] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?
• Then I launch my c++ node:

#include <memory> 
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char *argv[])
{
  // Initialize ROS and create the Node
  rclcpp::init(argc, argv);
  auto const node = std::make_shared<rclcpp::Node>(
      "hello_moveit",
      rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
  auto const logger = rclcpp::get_logger("hello_moveit");
  using moveit::planning_interface::MoveGroupInterface;
  auto move_group_interface = MoveGroupInterface(node, "arm");
  ...

Output:

[ERROR] [1721118585.081668920] [hello_moveit]: Could not find parameter robot_description and did not receive robot_description via std_msgs::msg::String subscription within 10.000000 seconds.
Error:   Error document empty.
         at line 100 in ./urdf_parser/src/model.cpp
Failed to parse robot description using: urdf_xml_parser/URDFXMLParser
[INFO] [1721118595.089338332] [moveit_rdf_loader.rdf_loader]: Unable to parse URDF
[FATAL] [1721118595.089669561] [move_group_interface]: Unable to construct robot model. Please make sure all needed information is on the parameter server.
terminate called after throwing an instance of 'std::runtime_error'
  what():  Unable to construct robot model. Please make sure all needed information is on the parameter server.
[ros2run]: Aborted

Things I considered:

• The parameters I set for the MoveGroupInterface might be wrong. I tried several combinations. But the namespace should be lbr and the planning_group should be arm.
• I checked the ros2 param list and I see the robot_description being used by multiple nodes. I am a bit lost though on what exactly I need and where it should be published so I can use it with the MoveGroupInterface.
• Could it be that the robot_description is not properly being loaded into the ROS parameter server? Unfortunately I do not quite understand how the launch file works. I found this in the move_group_launch.py:

   # RViz
    rviz = RVizMixin.node_rviz(
        rviz_config_pkg=f"{model}_moveit_config",
        rviz_config="config/moveit.rviz",
        parameters=LBRMoveGroupMixin.params_rviz(
            moveit_configs=moveit_configs_builder.to_moveit_configs()
        )
        + [{"use_sim_time": LaunchConfiguration("sim")}],
        remappings=[
            ("display_planned_path", robot_name + "/display_planned_path"),
            ("joint_states", robot_name + "/joint_states"),
            ("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
            ("robot_description", robot_name + "/robot_description"),
            ("robot_description_semantic", robot_name + "/robot_description_semantic"),
        ],
    )

Maybe this also needs to be published somewhere else? I would appreciate any guidance on this.

[mhubii]

could you try to run your node in namespace lbr. maybe that helps. You can do this via

ros2 run my_package my_node --ros-args -r __ns:=/lbr

If you run your node from a launch file, there is a namespace argument to every node

node = Node(
    ...
   namespace="lbr",
)

[Nicolai-98]

Thank you for the input. This helped!
It now finds the robot:

run lbr_moveit lbr_moveit --ros-args -r __ns:=/lbr
[INFO] [1721214923.569955746] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 1.00808 seconds
[INFO] [1721214923.570008807] [moveit_robot_model.robot_model]: Loading robot model 'iiwa7'...
[INFO] [1721214923.570018007] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[WARN] [1721214923.651692203] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!

However there still seems to be a problem with the kinematics plugin.

[mhubii]

no worries, maybe you'll have to parse this file

https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_moveit_config/iiwa7_moveit_config/config/kinematics.yaml

Try

ros2 run lbr_moveit lbr_moveit --ros-args \
    -r __ns:=/lbr \
    --params-file `ros2 pkg prefix iiwa7_moveit_config`/share/iiwa7_moveit_config/config/kinematics.yaml

Note you can also parse parameters from a yaml file via launch files. E.g.

import os

from ament_index_python import get_package_share_directory
from launch_ros.actions import Node

config_path = os.path.join(
    get_package_share_directory("iiwa7_moveit_config"), "config", "kinematics.yaml"
)

my_node = Node(
    ...
    parameters=[config_path],
)

Please note that MoveIt might require other configs, too.

[Nicolai-98]

Thank you. I tried

ros2 run lbr_moveit lbr_moveit --ros-args -r __ns:=/lbr --params-file /home/ros2_ws/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/kinematics.yaml`

The path to the file is correct but I get:

[ERROR] [1721721951.561096407] [rcl]: Failed to parse global arguments
terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidROSArgsError'
 what():  failed to initialize rcl: Couldn't parse params file: '--params-file /home/ros2_ws/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/kinematics.yaml'. Error: Cannot have a value before ros__parameters at line 2, at ./src/parse.c:793, at ./src/rcl/arguments.c:406

I also tried the approach using a launch file:

import os
from launch import LaunchDescription
from ament_index_python import get_package_share_directory
from launch_ros.actions import Node

def generate_launch_description():

    config_path = os.path.join(
        get_package_share_directory("iiwa7_moveit_config"), "config", "kinematics.yaml"
    )

    return LaunchDescription([
        Node(
            package='lbr_moveit',  
            executable='lbr_moveit',  
            name='lbr_moveit_node',  
            namespace="lbr",
            parameters=[config_path]
        ),
    ])

The results are identical:

[lbr_moveit-1] [ERROR] [1721723234.073546384] [rcl]: Failed to parse global arguments
[lbr_moveit-1] terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidROSArgsError'
[lbr_moveit-1]   what():  failed to initialize rcl: Couldn't parse params file: '--params-file /home/ros2_ws/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/kinematics.yaml'. Error: Cannot have a value before ros__parameters at line 2, at ./src/parse.c:793, at ./src/rcl/arguments.c:406
[ERROR] [lbr_moveit-1]: process has died [pid 2973, exit code -6, cmd '/home/ros2_ws/install/lbr_moveit/lib/lbr_moveit/lbr_moveit --ros-args -r __node:=lbr_moveit_node --params-file /home/ros2_ws/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/kinematics.yaml'].

Since the error suggested that there cannot be a value before ros_parameters. I added ros__parameters to the kinematics.yaml.
Then the error changed to: Error: There are no node names before ros__parameters at line 1, at ./src/parse.c:629, at ./src/rcl/arguments.c:406
This is probably not the right approach but might give insight into the problem.

[mhubii]

arg you are right, how annoying. This file is from MoveIt and not configured to be used as parameters file.

You'd have to add your node name, however, this isn't a good solution.

E.g.

lbr_moveit:
  ros__parameters:
    arm:
      kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
      kinematics_solver_search_resolution: 0.0050000000000000001
      kinematics_solver_timeout: 0.0050000000000000001

A slightly better approach would be to read the configs as opposed to parsing the path.

May I ask, do you have a fork of this repo that we could collaborate on?

Just wondering if there is a better way to use the MoveIt C++ API (haven't got experience either)

[ahnwoohyeon]

I had the same problem, so I followed the part where the name space was added as /lbr. However, the problem is that when I checked the action, the server was declared as /lbr/move_action, but when I run c++ code, it was declared as /move_action client, so it doesn't seem to have much response. How do I solve this?

[mhubii]

Putting this for reference:

https://github.com/moveit/moveit2_tutorials/blob/20be39797b17e460fda48975a0a8ee25f8ad87ac/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py#L10C5-L10C18

(configs are parsed to the node via moveit config builder)

[Nicolai-98]

arg you are right, how annoying. This file is from MoveIt and not configured to be used as parameters file.

You'd have to add your node name, however, this isn't a good solution.

E.g.

lbr_moveit:
  ros__parameters:
    arm:
      kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
      kinematics_solver_search_resolution: 0.0050000000000000001
      kinematics_solver_timeout: 0.0050000000000000001

A slightly better approach would be to read the configs as opposed to parsing the path.

May I ask, do you have a fork of this repo that we could collaborate on?

Just wondering if there is a better way to use the MoveIt C++ API (haven't got experience either)

Ah seems I was close with the ros__parameters key. But I agree it's not an optimal solution.
Regarding the fork: Currently all I have done is create a c++ ros2 package called lbr_moveit that contains node with

• the hello world moveit content following the docs: https://moveit.picknik.ai/main/doc/tutorials/your_first_project/your_first_project.html
• and the launch file for that node (posted above)
  in order to test out moveit with this stack.
  I am also looking for an optimal way to achieve this.
  I can create a fork with these changes for easier collaboration tomorrow morning.