MoveIt with gripper - Githubissues

Hello

I am looking into using the stack to control my kuka iiwa robot. I'm also thinking that I could use MoveIt to help me move the robot, and also in aiding in grasping of objects, is this something that you've experience in, or what methods have you seen work using this stack?

Best regards.

hi @TobiasAxelsen , once you finished setting things up (see for example https://lbr-fri-ros2-stack-doc.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_fri_ros2_stack/doc/robot_setup.html), you can launch moveit to control the robot via

ros2 launch lbr_bringup bringup.launch.py \
    model:=iiwa7 # [iiwa7, iiwa14, med7, med14] \
    sim:=true # [true, false] \
    rviz:=true # [true, false] \
    moveit:=true # [true, false]

personally, I have no experience with adding a gripper, so if you could share your insights that would be great.

Hello @mhubii Mhubii, appreciate the answer!

Which topic is it that moveit subscribes to, to gain the information of which positions to go to.

Best regards.

I believe moveit implements an action server but they also have a Python api. Is that what you meant? Can try to write a demo

That is exactly what I meant actually :)

Hello again @mhubii .

I dug a bit more around in your documentation, and saw that you've implemented ros-control, but only for joint positions. Maybe I am just unable to find it, but have you guys also made ros-control by the use of cartesian coordinates, such that you can just give the robot a pose that you wish to move to?

Best regards.

hi @TobiasAxelsen , so ros2_control doesn't provide this functionality (as far as I am aware of). The joint trajectory controller in ros2_control just provides a smooth interpolation for going from q0 to q1, where qi is a vector representing joint states. It is the user's responsibility to compute the target q1. This can e.g. be done using path planning, that is what moveit does, or simply using inverse kinematics.

The moveit python bindings are available here: https://github.com/ros-planning/moveit2/tree/main/moveit_py, also see here https://moveit.picknik.ai/main/doc/examples/jupyter_notebook_prototyping/jupyter_notebook_prototyping_tutorial.html#notebook-setup

Alternatively, you can interface moveit through the action server:

Lets assume you launch this:

ros2 launch lbr_bringup bringup.launch.py model:=iiwa7 sim:=true moveit:=true

Then call

ros2 action list

You should see

/lbr/execute_trajectory                                                     # <-- moveit2 action server
/lbr/move_action                                                              # <-- moveit2 action server
/lbr/position_trajectory_controller/follow_joint_trajectory # <-- ros2 control follow joint trajectory action server

In order to move the robot via sending poses, you can create a client and send requests to the action server under /lbr/move_action.

Please find how to do this in the answer in the next comment!

There is a repository https://github.com/AndrejOrsula/pymoveit2 that help users do that. We will add support for the lbr_fri_ros2_stack.

I hope this clarifies some of your questions and will help you move forward in your project. Please let me know @TobiasAxelsen !

A custom example for interfacting the moveit action server is this. Create a file move_robot.py, copy paste this:

#!/usr/bin/python3
from typing import List

import rclpy
from geometry_msgs.msg import Point, Pose, Quaternion
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
    BoundingVolume,
    Constraints,
    OrientationConstraint,
    PositionConstraint,
)
from rclpy.action import ActionClient
from rclpy.node import Node
from shape_msgs.msg import SolidPrimitive
from std_msgs.msg import Header

class MoveGroupActionClientNode(Node):
    def __init__(self, node_name: str) -> None:
        super().__init__(node_name)

        self.action_server = "/lbr/move_action"
        self.move_group_name = "arm"
        self.base = "link_0"
        self.end_effector = "link_ee"

        self.move_group_action_client = ActionClient(
            self, MoveGroup, self.action_server
        )

        self.get_logger().info(f"Waiting for action server {self.action_server}...")
        if not self.move_group_action_client.wait_for_server(timeout_sec=1):
            raise RuntimeError(
                f"Couldn't connect to action server {self.action_server}."
            )
        self.get_logger().info(f"Done.")

    def send_goal_async(self, target: Pose):
        goal = MoveGroup.Goal()
        goal.request.allowed_planning_time = 1.0
        goal.request.goal_constraints.append(
            Constraints(
                position_constraints=[
                    PositionConstraint(
                        header=Header(frame_id=self.base),
                        link_name=self.end_effector,
                        constraint_region=BoundingVolume(
                            primitives=[SolidPrimitive(type=2, dimensions=[0.0001])],
                            primitive_poses=[Pose(position=target.position)],
                        ),
                        weight=1.0,
                    )
                ],
                orientation_constraints=[
                    OrientationConstraint(
                        header=Header(frame_id=self.base),
                        link_name=self.end_effector,
                        orientation=target.orientation,
                        absolute_x_axis_tolerance=0.001,
                        absolute_y_axis_tolerance=0.001,
                        absolute_z_axis_tolerance=0.001,
                        weight=1.0,
                    )
                ],
            )
        )
        goal.request.group_name = self.move_group_name
        goal.request.max_acceleration_scaling_factor = 0.1
        goal.request.max_velocity_scaling_factor = 0.01
        goal.request.num_planning_attempts = 1

        return self.move_group_action_client.send_goal_async(goal)

def main(args: List = None) -> None:
    rclpy.init(args=args)
    move_group_action_client_node = MoveGroupActionClientNode(
        "move_group_action_client_node"
    )

    pose = Pose(
        position=Point(x=0.0, y=0.0, z=1.0),
        orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
    )
    future = move_group_action_client_node.send_goal_async(pose)
    rclpy.spin_until_future_complete(
        move_group_action_client_node, future
    )  # gets stuck for invalid goals

    rclpy.shutdown()

if __name__ == "__main__":
    main()

Launch

ros2 launch lbr_bringup bringup.launch.py model:=iiwa7 sim:=true moveit:=true base_frame:=world

Run

python3 move_robot.py

Action servers can be a little difficult to deal with in the beginning, but you can read a bit about them here https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html

Hello @mhubii.

I created the copy pasted the code you gave me, and put it in a file called move_robot.py

I then put this file under ~/ros_ws/src

When I open the simulation, and then run the move_robot.py file I am told that

tobias@tobias-Yoga-Slim-7-14ARE05:~/ros_ws/src$ python3 move_robot.py 
[INFO] [1699862721.227189770] [move_group_action_client_node]: Waiting for action server /lbr/move_action...
[INFO] [1699862721.227765550] [move_group_action_client_node]: Done.

I'd assume that this would have moved the simulation, but when I look in the simulation, nothing has moved.

At other times, I got the following:

tobias@tobias-Yoga-Slim-7-14ARE05:~/ros_ws$ colcon build
tobias@tobias-Yoga-Slim-7-14ARE05:~/ros_ws$ source install/setup.bash 
tobias@tobias-Yoga-Slim-7-14ARE05:~/ros_ws$ cd src/
tobias@tobias-Yoga-Slim-7-14ARE05:~/ros_ws/src$ python3 move_robot.py 
[INFO] [1699862875.812407778] [move_group_action_client_node]: Waiting for action server /lbr/move_action...
Traceback (most recent call last):
  File "/home/tobias/ros_ws/src/move_robot.py", line 95, in <module>
    main()
  File "/home/tobias/ros_ws/src/move_robot.py", line 78, in main
    move_group_action_client_node = MoveGroupActionClientNode(
  File "/home/tobias/ros_ws/src/move_robot.py", line 34, in __init__
    raise RuntimeError(
RuntimeError: Couldn't connect to action server /lbr/move_action.

Alright I did some digging, and I will post my current status below.

I start off by running

ros2 launch lbr_bringup bringup.launch.py model:=iiwa7 sim:=true moveit:=true base_frame:=world

I've also checked that the ros2 action list is as you assumed it would be, and that it was.

I've put the move_robot.py file under ~/lbr-stack, and I execute it from there using

tobias@tobias-Yoga-Slim-7-14ARE05:~/lbr-stack$ python3 move_robot.py

Doing this, I look at my terminal that I brang up the simulation in and I get the following output:

[move_group-9] [INFO] [1699866221.476593380] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-9] [INFO] [1699866221.476801506] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[move_group-9] [INFO] [1699866221.476999994] [moveit_move_group_default_capabilities.move_action_capability]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[move_group-9] [WARN] [1699866221.477038127] [moveit_move_group_capabilities_base.move_group_capability]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as start state in the motion planning request
[move_group-9] [WARN] [1699866221.477058171] [moveit_move_group_capabilities_base.move_group_capability]: Execution of motions should always start at the robot's current state. Ignoring the state supplied as difference in the planning scene diff
[move_group-9] [INFO] [1699866221.477077587] [moveit_ros.plan_execution]: Planning attempt 1 of at most 1
[move_group-9] [INFO] [1699866221.477778720] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-9] [ERROR] [1699866222.485828873] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - arm/arm: Unable to sample any valid states for goal tree
[move_group-9] [WARN] [1699866222.485881882] [moveit.ompl_planning.model_based_planning_context]: Timed out
[move_group-9] [INFO] [1699866222.512127175] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-9] [INFO] [1699866222.512191568] [moveit_move_group_default_capabilities.move_action_capability]: Timeout reached

While this happens, I get the following on the python3 move_robot.py terminal

tobias@tobias-Yoga-Slim-7-14ARE05:~/lbr-stack$ python3 move_robot.py 
[INFO] [1699866221.474760477] [move_group_action_client_node]: Waiting for action server /lbr/move_action...
[INFO] [1699866221.475212626] [move_group_action_client_node]: Done.

Through some more testing (and now cd to lbr stack, woopsies) i managed to have the goal accepted once(?) and then tried relaunching sim and getting new errors... Will update if I find some more information...

I think the position for the EE might just have been outside of the workspace... I am a moron admittedly...

I am unable to go to my RobotLAB at this time, are you aware if executing this code in the sim like above will also cause the physical robot to move, @mhubii ?

Edit*

It works on the Real Kuka Iiwa 7 :)

Hi @TobiasAxelsen , yes, you can execute the above code in simulation. You can launch

ros2 launch lbr_bringup bringup.launch.py \
    model:=iiwa7 \
    sim:=true \
    base_frame:=world \
    moveit:=true

then run the above script. That should work for simulation and the real system unless there is a bug somewhere

tobias@tobias-Yoga-Slim-7-14ARE05:~/lbr-stack$ python3 move_robot.py [INFO] [1699866221.474760477] [move_group_action_client_node]: Waiting for action server /lbr/move_action...

This output is expected. It just informs that the action client established a connection to the action server.

This repository clearly misses documentation on how to use it through the python moveit API

[move_group-9] [ERROR] [1699866222.485828873] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - arm/arm: Unable to sample any valid states for goal tree

This error suggests that the ompl path planner had some error, maybe that is what causes the fail?

A custom example for interfacting the moveit action server is this. Create a file move_robot.py, copy paste this:

#!/usr/bin/python3
from typing import List

import rclpy
from geometry_msgs.msg import Point, Pose, Quaternion
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
    BoundingVolume,
    Constraints,
    OrientationConstraint,
    PositionConstraint,
)
from rclpy.action import ActionClient
from rclpy.node import Node
from shape_msgs.msg import SolidPrimitive
from std_msgs.msg import Header

class MoveGroupActionClientNode(Node):
    def __init__(self, node_name: str) -> None:
        super().__init__(node_name)

        self.action_server = "/lbr/move_action"
        self.move_group_name = "arm"
        self.base = "link_0"
        self.end_effector = "link_ee"

        self.move_group_action_client = ActionClient(
            self, MoveGroup, self.action_server
        )

        self.get_logger().info(f"Waiting for action server {self.action_server}...")
        if not self.move_group_action_client.wait_for_server(timeout_sec=1):
            raise RuntimeError(
                f"Couldn't connect to action server {self.action_server}."
            )
        self.get_logger().info(f"Done.")

    def send_goal_async(self, target: Pose):
        goal = MoveGroup.Goal()
        goal.request.allowed_planning_time = 1.0
        goal.request.goal_constraints.append(
            Constraints(
                position_constraints=[
                    PositionConstraint(
                        header=Header(frame_id=self.base),
                        link_name=self.end_effector,
                        constraint_region=BoundingVolume(
                            primitives=[SolidPrimitive(type=2, dimensions=[0.0001])],
                            primitive_poses=[Pose(position=target.position)],
                        ),
                        weight=1.0,
                    )
                ],
                orientation_constraints=[
                    OrientationConstraint(
                        header=Header(frame_id=self.base),
                        link_name=self.end_effector,
                        orientation=target.orientation,
                        absolute_x_axis_tolerance=0.001,
                        absolute_y_axis_tolerance=0.001,
                        absolute_z_axis_tolerance=0.001,
                        weight=1.0,
                    )
                ],
            )
        )
        goal.request.group_name = self.move_group_name
        goal.request.max_acceleration_scaling_factor = 0.1
        goal.request.max_velocity_scaling_factor = 0.01
        goal.request.num_planning_attempts = 1

        return self.move_group_action_client.send_goal_async(goal)

def main(args: List = None) -> None:
    rclpy.init(args=args)
    move_group_action_client_node = MoveGroupActionClientNode(
        "move_group_action_client_node"
    )

    pose = Pose(
        position=Point(x=0.0, y=0.0, z=1.0),
        orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0),
    )
    future = move_group_action_client_node.send_goal_async(pose)
    rclpy.spin_until_future_complete(
        move_group_action_client_node, future
    )  # gets stuck for invalid goals

    rclpy.shutdown()

if __name__ == "__main__":
    main()

Launch

ros2 launch lbr_bringup bringup.launch.py model:=iiwa7 sim:=true moveit:=true base_frame:=world

Run

python3 move_robot.py

Hello @mhubii

By the use of this code, is it possible to have the robot move in cartesian space, instead of joint space? I am looking to move in cartesian space, as we find that this often creates a bit more predictable movement, and thus allows for less space for the robot to be used.

Hi @TobiasAxelsen , fair question. So moveit has a variable called "Use Cartesian Path". Surely that is accessible somewhere through the action server, but I didn't check

It might be simple to start looking into the moveit python API

Hi @mhubii. When using the MoveIt GUI, and checking the variable "Use Cartesian Path", and then planning & executing a trajectory, Rviz will immediately crash, and I am provided with the following output in my terminal, from the launch command to Rviz dying on me.

tobias@tobias-Yoga-Slim-7-14ARE05:~/lbr-stack$ ros2 launch lbr_bringup bringup.launch.py moveit:=true sim:=true
[INFO] [launch]: All log files can be found below /home/tobias/.ros/log/2023-11-21-09-49-51-134306-tobias-Yoga-Slim-7-14ARE05-6199
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [gzserver-1]: process started with pid [6619]
[INFO] [gzclient-2]: process started with pid [6621]
[INFO] [spawn_entity.py-3]: process started with pid [6624]
[INFO] [spawner-4]: process started with pid [6626]
[INFO] [robot_state_publisher-5]: process started with pid [6628]
[INFO] [spawner-6]: process started with pid [6630]
[INFO] [static_transform_publisher-7]: process started with pid [6632]
[INFO] [static_transform_publisher-8]: process started with pid [6634]
[INFO] [move_group-9]: process started with pid [6637]
[static_transform_publisher-8] [INFO] [1700556592.178000872] [static_transform_publisher_NG8X75i22ZLKdZrS]: Spinning until stopped - publishing transform
[static_transform_publisher-8] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-8] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-8] from 'base_frame' to 'lbr/base_frame'
[static_transform_publisher-7] [INFO] [1700556592.178243568] [static_transform_publisher_W247E5SDT6XqZRHE]: Spinning until stopped - publishing transform
[static_transform_publisher-7] translation: ('0.000000', '0.000000', '0.000000')
[static_transform_publisher-7] rotation: ('0.000000', '0.000000', '0.000000', '1.000000')
[static_transform_publisher-7] from 'world' to 'base_frame'
[robot_state_publisher-5] [INFO] [1700556592.179903399] [lbr.robot_state_publisher]: got segment base_frame
[robot_state_publisher-5] [INFO] [1700556592.180085753] [lbr.robot_state_publisher]: got segment link_0
[robot_state_publisher-5] [INFO] [1700556592.180102864] [lbr.robot_state_publisher]: got segment link_1
[robot_state_publisher-5] [INFO] [1700556592.180114318] [lbr.robot_state_publisher]: got segment link_2
[robot_state_publisher-5] [INFO] [1700556592.180127658] [lbr.robot_state_publisher]: got segment link_3
[robot_state_publisher-5] [INFO] [1700556592.180139042] [lbr.robot_state_publisher]: got segment link_4
[robot_state_publisher-5] [INFO] [1700556592.180150146] [lbr.robot_state_publisher]: got segment link_5
[robot_state_publisher-5] [INFO] [1700556592.180162438] [lbr.robot_state_publisher]: got segment link_6
[robot_state_publisher-5] [INFO] [1700556592.180174870] [lbr.robot_state_publisher]: got segment link_7
[robot_state_publisher-5] [INFO] [1700556592.180187371] [lbr.robot_state_publisher]: got segment link_ee
[move_group-9] [INFO] [1700556592.211893320] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[move_group-9] [INFO] [1700556592.211992354] [moveit_robot_model.robot_model]: Loading robot model 'iiwa7'...
[move_group-9] [INFO] [1700556592.212004646] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[spawn_entity.py-3] [INFO] [1700556592.750761817] [lbr.spawn_entity]: Spawn Entity started
[spawn_entity.py-3] [INFO] [1700556592.751120448] [lbr.spawn_entity]: Loading entity published on topic robot_description
[spawn_entity.py-3] /opt/ros/humble/local/lib/python3.10/dist-packages/rclpy/qos.py:307: UserWarning: DurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL is deprecated. Use DurabilityPolicy.TRANSIENT_LOCAL instead.
[spawn_entity.py-3]   warnings.warn(
[spawn_entity.py-3] [INFO] [1700556592.753504317] [lbr.spawn_entity]: Waiting for entity xml on robot_description
[spawn_entity.py-3] [INFO] [1700556592.764899716] [lbr.spawn_entity]: Waiting for service /spawn_entity, timeout = 30
[spawn_entity.py-3] [INFO] [1700556592.765218817] [lbr.spawn_entity]: Waiting for service /spawn_entity
[move_group-9] [INFO] [1700556592.838651441] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-9] [INFO] [1700556592.838841408] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-9] [INFO] [1700556592.839570753] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-9] [INFO] [1700556592.840029186] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/lbr/attached_collision_object' for attached collision objects
[move_group-9] [INFO] [1700556592.840055866] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-9] [INFO] [1700556592.840407233] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/lbr/planning_scene'
[move_group-9] [INFO] [1700556592.840426719] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-9] [INFO] [1700556592.840796525] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-9] [INFO] [1700556592.841192381] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-9] [WARN] [1700556592.841517629] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-9] [ERROR] [1700556592.841531807] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[move_group-9] [INFO] [1700556593.010983430] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'ompl'
[move_group-9] [INFO] [1700556593.020770750] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-9] [INFO] [1700556593.022991402] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[move_group-9] [INFO] [1700556593.023006068] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[move_group-9] [INFO] [1700556593.023012214] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[move_group-9] [INFO] [1700556593.023030931] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-9] [INFO] [1700556593.023046576] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[move_group-9] [INFO] [1700556593.023052442] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-9] [INFO] [1700556593.023064315] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-9] [INFO] [1700556593.023070950] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was set to 0.050000
[move_group-9] [INFO] [1700556593.023078074] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[move_group-9] [INFO] [1700556593.023089667] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-9] [INFO] [1700556593.023095255] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-9] [INFO] [1700556593.023101470] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-9] [INFO] [1700556593.023107197] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-9] [INFO] [1700556593.023112924] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-9] [INFO] [1700556593.023118302] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-9] [INFO] [1700556593.055451981] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for position_trajectory_controller
[move_group-9] [INFO] [1700556593.055600392] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-9] [INFO] [1700556593.055619319] [moveit.plugins.moveit_simple_controller_manager]: Returned 1 controllers in list
[move_group-9] [INFO] [1700556593.056068953] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-9] [INFO] [1700556593.056091860] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-9] [INFO] [1700556593.073471482] [move_group.move_group]: 
[move_group-9] 
[move_group-9] ********************************************************
[move_group-9] * MoveGroup using: 
[move_group-9] *     - ApplyPlanningSceneService
[move_group-9] *     - ClearOctomapService
[move_group-9] *     - CartesianPathService
[move_group-9] *     - ExecuteTrajectoryAction
[move_group-9] *     - GetPlanningSceneService
[move_group-9] *     - KinematicsService
[move_group-9] *     - MoveAction
[move_group-9] *     - MotionPlanService
[move_group-9] *     - QueryPlannersService
[move_group-9] *     - StateValidationService
[move_group-9] ********************************************************
[move_group-9] 
[move_group-9] [INFO] [1700556593.073535875] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-9] [INFO] [1700556593.073546910] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-9] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-9] Loading 'move_group/ClearOctomapService'...
[move_group-9] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-9] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-9] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-9] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-9] Loading 'move_group/MoveGroupMoveAction'...
[move_group-9] Loading 'move_group/MoveGroupPlanService'...
[move_group-9] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-9] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-9] 
[move_group-9] You can start planning now!
[move_group-9] 
[spawn_entity.py-3] [INFO] [1700556593.770251633] [lbr.spawn_entity]: Calling service /spawn_entity
[spawn_entity.py-3] [INFO] [1700556593.951623315] [lbr.spawn_entity]: Spawn status: SpawnEntity: Successfully spawned entity [lbr]
[gzserver-1] [INFO] [1700556594.101994585] [gazebo_ros2_control]: Loading gazebo_ros2_control plugin
[gzserver-1] [INFO] [1700556594.104924887] [lbr.gazebo_ros2_control]: Starting gazebo_ros2_control plugin in namespace: /lbr
[gzserver-1] [INFO] [1700556594.105169748] [lbr.gazebo_ros2_control]: Starting gazebo_ros2_control plugin in ros 2 node: gazebo_ros2_control
[gzserver-1] [INFO] [1700556594.105225062] [lbr.gazebo_ros2_control]: Loading parameter files /home/tobias/lbr-stack/install/lbr_ros2_control/share/lbr_ros2_control/config/lbr_controllers.yaml
[INFO] [spawn_entity.py-3]: process has finished cleanly [pid 6624]
[INFO] [rviz2-10]: process started with pid [7879]
[rviz2-10] Warning: Ignoring XDG_SESSION_TYPE=wayland on Gnome. Use QT_QPA_PLATFORM=wayland to run on Wayland anyway.
[rviz2-10] [INFO] [1700556594.435092467] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-10] [INFO] [1700556594.435310789] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-10] [INFO] [1700556594.450803459] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-10] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-10]          at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[gzserver-1] [ERROR] [1700556594.607549923] [lbr.gazebo_ros2_control]: robot_state_publisher service not available, waiting again...
[spawner-4] [INFO] [1700556594.660917707] [lbr.spawner_joint_state_broadcaster]: Waiting for '/lbr/controller_manager' node to exist
[spawner-6] [INFO] [1700556594.663061185] [lbr.spawner_position_trajectory_controller]: Waiting for '/lbr/controller_manager' node to exist
[gzserver-1] [INFO] [1700556595.010532402] [lbr.gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-1] [INFO] [1700556595.011725350] [lbr.gazebo_ros2_control]: Received urdf from param server, parsing...
[gzserver-1] [INFO] [1700556595.023193945] [lbr.gazebo_ros2_control]: Loading joint: A1
[gzserver-1] [INFO] [1700556595.023289277] [lbr.gazebo_ros2_control]:   State:
[gzserver-1] [INFO] [1700556595.023305969] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023321264] [lbr.gazebo_ros2_control]:        velocity
[gzserver-1] [INFO] [1700556595.023335093] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023348363] [lbr.gazebo_ros2_control]:   Command:
[gzserver-1] [INFO] [1700556595.023361353] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023430425] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023446209] [lbr.gazebo_ros2_control]: Loading joint: A2
[gzserver-1] [INFO] [1700556595.023459060] [lbr.gazebo_ros2_control]:   State:
[gzserver-1] [INFO] [1700556595.023471911] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023484901] [lbr.gazebo_ros2_control]:        velocity
[gzserver-1] [INFO] [1700556595.023499009] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023511859] [lbr.gazebo_ros2_control]:   Command:
[gzserver-1] [INFO] [1700556595.023524640] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023547408] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023561656] [lbr.gazebo_ros2_control]: Loading joint: A3
[gzserver-1] [INFO] [1700556595.023574576] [lbr.gazebo_ros2_control]:   State:
[gzserver-1] [INFO] [1700556595.023587497] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023600278] [lbr.gazebo_ros2_control]:        velocity
[gzserver-1] [INFO] [1700556595.023612779] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023627027] [lbr.gazebo_ros2_control]:   Command:
[gzserver-1] [INFO] [1700556595.023638480] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023658525] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023672423] [lbr.gazebo_ros2_control]: Loading joint: A4
[gzserver-1] [INFO] [1700556595.023685623] [lbr.gazebo_ros2_control]:   State:
[gzserver-1] [INFO] [1700556595.023699032] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023712023] [lbr.gazebo_ros2_control]:        velocity
[gzserver-1] [INFO] [1700556595.023726200] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023737794] [lbr.gazebo_ros2_control]:   Command:
[gzserver-1] [INFO] [1700556595.023750924] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023770898] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023784517] [lbr.gazebo_ros2_control]: Loading joint: A5
[gzserver-1] [INFO] [1700556595.023797787] [lbr.gazebo_ros2_control]:   State:
[gzserver-1] [INFO] [1700556595.023810777] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023823558] [lbr.gazebo_ros2_control]:        velocity
[gzserver-1] [INFO] [1700556595.023836548] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023849818] [lbr.gazebo_ros2_control]:   Command:
[gzserver-1] [INFO] [1700556595.023862669] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023881805] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023895145] [lbr.gazebo_ros2_control]: Loading joint: A6
[gzserver-1] [INFO] [1700556595.023908973] [lbr.gazebo_ros2_control]:   State:
[gzserver-1] [INFO] [1700556595.023920357] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023932859] [lbr.gazebo_ros2_control]:        velocity
[gzserver-1] [INFO] [1700556595.023947106] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.023959049] [lbr.gazebo_ros2_control]:   Command:
[gzserver-1] [INFO] [1700556595.023970503] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.023986706] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.024143428] [lbr.gazebo_ros2_control]: Loading joint: A7
[gzserver-1] [INFO] [1700556595.024164939] [lbr.gazebo_ros2_control]:   State:
[gzserver-1] [INFO] [1700556595.024177720] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.024192177] [lbr.gazebo_ros2_control]:        velocity
[gzserver-1] [INFO] [1700556595.024204958] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.024218926] [lbr.gazebo_ros2_control]:   Command:
[gzserver-1] [INFO] [1700556595.024234430] [lbr.gazebo_ros2_control]:        position
[gzserver-1] [INFO] [1700556595.024256570] [lbr.gazebo_ros2_control]:        effort
[gzserver-1] [INFO] [1700556595.024395832] [resource_manager]: Initialize hardware 'lbr_system_interface' 
[gzserver-1] [INFO] [1700556595.024602420] [resource_manager]: Successful initialization of hardware 'lbr_system_interface'
[gzserver-1] [INFO] [1700556595.024710184] [resource_manager]: 'configure' hardware 'lbr_system_interface' 
[gzserver-1] [INFO] [1700556595.024722546] [resource_manager]: Successful 'configure' of hardware 'lbr_system_interface'
[gzserver-1] [INFO] [1700556595.024734559] [resource_manager]: 'activate' hardware 'lbr_system_interface' 
[gzserver-1] [INFO] [1700556595.024743079] [resource_manager]: Successful 'activate' of hardware 'lbr_system_interface'
[gzserver-1] [INFO] [1700556595.024840507] [lbr.gazebo_ros2_control]: Loading controller_manager
[gzserver-1] [WARN] [1700556595.053259557] [lbr.gazebo_ros2_control]:  Desired controller update period (0.005 s) is slower than the gazebo simulation period (0.001 s).
[gzserver-1] [INFO] [1700556595.053631528] [lbr.gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gzserver-1] [INFO] [1700556595.075080974] [lbr.controller_manager]: Loading controller 'position_trajectory_controller'
[gzserver-1] [WARN] [1700556595.104608184] [lbr.position_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[gzserver-1] [INFO] [1700556595.104776849] [lbr.controller_manager]: Setting use_sim_time=True for position_trajectory_controller to match controller manager (see ros2_control#325 for details)
[spawner-6] [INFO] [1700556595.110550781] [lbr.spawner_position_trajectory_controller]: Loaded position_trajectory_controller
[gzserver-1] [INFO] [1700556595.113455731] [lbr.controller_manager]: Loading controller 'joint_state_broadcaster'
[gzserver-1] [INFO] [1700556595.128155363] [lbr.controller_manager]: Setting use_sim_time=True for joint_state_broadcaster to match controller manager (see ros2_control#325 for details)
[gzserver-1] [INFO] [1700556595.130207210] [lbr.controller_manager]: Configuring controller 'position_trajectory_controller'
[gzserver-1] [INFO] [1700556595.130996897] [lbr.position_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[gzserver-1] [INFO] [1700556595.131088877] [lbr.position_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[gzserver-1] [INFO] [1700556595.131140420] [lbr.position_trajectory_controller]: Using 'splines' interpolation method.
[spawner-4] [INFO] [1700556595.130995431] [lbr.spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[gzserver-1] [INFO] [1700556595.133598600] [lbr.position_trajectory_controller]: Controller state will be published at 50.00 Hz.
[gzserver-1] [INFO] [1700556595.146165788] [lbr.position_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[gzserver-1] [INFO] [1700556595.156256218] [lbr.controller_manager]: Configuring controller 'joint_state_broadcaster'
[gzserver-1] [INFO] [1700556595.156393595] [lbr.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-6] [INFO] [1700556595.171360925] [lbr.spawner_position_trajectory_controller]: Configured and activated position_trajectory_controller
[spawner-4] [INFO] [1700556595.182011757] [lbr.spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[INFO] [spawner-6]: process has finished cleanly [pid 6630]
[INFO] [spawner-4]: process has finished cleanly [pid 6626]
[rviz2-10] [ERROR] [1700556597.565668396] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-10] [INFO] [1700556597.588608663] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-10] [INFO] [1700556597.594015514] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> lbr. Reloading params.
[rviz2-10] [WARN] [1700556597.681059690] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-10] [INFO] [1700556597.758820162] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0788911 seconds
[rviz2-10] [INFO] [1700556597.758960752] [moveit_robot_model.robot_model]: Loading robot model 'iiwa7'...
[rviz2-10] [INFO] [1700556597.758987431] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-10] [INFO] [1700556598.229921468] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-10] [INFO] [1700556598.230576224] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/lbr/monitored_planning_scene'
[rviz2-10] [INFO] [1700556598.824939753] [interactive_marker_display_94096093134544]: Connected on namespace: lbr/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-10] [INFO] [1700556598.840322360] [interactive_marker_display_94096093134544]: Sending request for interactive markers
[rviz2-10] [INFO] [1700556598.843375862] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopping planning scene monitor
[rviz2-10] [WARN] [1700556598.874198833] [interactive_marker_display_94096093134544]: Server not available during initialization, resetting
[rviz2-10] [INFO] [1700556598.901620915] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0494885 seconds
[rviz2-10] [INFO] [1700556598.901648293] [moveit_robot_model.robot_model]: Loading robot model 'iiwa7'...
[rviz2-10] [INFO] [1700556598.901655975] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-10] [INFO] [1700556599.321662278] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-10] [INFO] [1700556599.322360894] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/lbr/monitored_planning_scene'
[rviz2-10] [INFO] [1700556599.342392234] [moveit_ros_visualization.motion_planning_frame]: group arm
[rviz2-10] [INFO] [1700556599.342427992] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'arm' in namespace 'lbr'
[rviz2-10] [INFO] [1700556599.354328765] [interactive_marker_display_94096093134544]: Sending request for interactive markers
[rviz2-10] [INFO] [1700556599.357089286] [move_group_interface]: Ready to take commands for planning group arm.
[rviz2-10] [INFO] [1700556599.360772960] [moveit_ros_visualization.motion_planning_frame]: group arm
[rviz2-10] [INFO] [1700556599.385761168] [interactive_marker_display_94096093134544]: Service response received for initialization
[move_group-9] [INFO] [1700556611.422021672] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Received request to compute Cartesian path
[move_group-9] [INFO] [1700556611.422147594] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Attempting to follow 1 waypoints for link 'link_ee' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[move_group-9] [INFO] [1700556611.429838660] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Computed Cartesian path with 37 points (followed 100.000000% of requested trajectory)
[rviz2-10] [INFO] [1700556611.430279774] [moveit_ros_visualization.motion_planning_frame_planning]: Achieved 100.000000 % of Cartesian path
[rviz2-10] [INFO] [1700556611.431620506] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[rviz2-10] [INFO] [1700556612.431853194] [moveit_ros.current_state_monitor]: Didn't receive robot state (joint angles) with recent timestamp within 1.000000 seconds. Requested time 1700556611.431668, but latest received state has time 0.000000.
[rviz2-10] Check clock synchronization if your are running ROS across multiple machines!
[rviz2-10] [ERROR] [1700556612.431990920] [move_group_interface]: Failed to fetch current robot state
[ERROR] [rviz2-10]: process has died [pid 7879, exit code -11, cmd '/opt/ros/humble/lib/rviz2/rviz2 -d /home/tobias/lbr-stack/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/moveit.rviz --ros-args -r __node:=rviz2 --params-file /tmp/launch_params_1xwxz3zf --params-file /tmp/launch_params_3m3fvpks -r robot_description:=lbr/robot_description -r robot_description_semantic:=lbr/robot_description_semantic -r display_planned_path:=lbr/display_planned_path -r monitored_planning_scene:=lbr/monitored_planning_scene'].

Do you have any idea of why this happens?

woah that's strange, can confirm this happens, but don't know why. Would this happen to other robots that support moveit, too? Might be a moveit bug

Will ask around and see if I can have some of my co-students check if this also happens for them... Possibly test on an ur3, will report back if I find anything

Added an issue #ros-planning#moveit2#2545. Hopefully we find a solution in a timely manner

Hey Martin! Thank you for everything so far, I have no idea where we would be without your help! We appreciate it!

Any updates regarding MoveIt? We are considering different approaches to making a cartesian trajectory. I tried to get it working through the action server, but I haven't found any documentation, so it feels like sharpshooting blindfolded. Do you know where to find any documentation for it?

Specifically, I tried writing to referenceTrajectories in the action server, and add a custom path I created by interpolating between two poses. The server denies my request though, as it detects some error with the classes if I recall correctly. I have yet to spot my error, but I don't even know if the action server works this way, so it feels demotivating to sink even more time into this method without knowing for sure. In the end, I am just guessing that giving it a cartesian path in referenceTrajectories will make it move like I want to. I didn't see any easy toggle in the interface for using a cartesian path instead.

We have considered if it is an option to just skip MoveIt, and send a Pose directly to the Kuka and ask it to move there in cartesian space. We have done this with UR robots in the past, but we don't know if this is possible with the Kuka through FRI, and if there is a "simple" way to do this through lbr stack. We would lose out on collision detection, but at this point we just need the robot to move properly.

We also tried briefly with the C++ library for MoveIt, without success. We wanted to try the newly developed Python library, but there is basically no documentation, so we have no idea how to use it.

hi @EliasTDam . So if you can skip Moveit, and just want to execute a Cartesian motion, you can do something like: dq = J^# @ dx, where dq is the joint velocity, J^# is the Jacobian pseudo inverse, and dx is the end-effector velocity. There are many ways to achieve that. I can provide you with insructions, np.

In its simplest form, you do:

Launch LBRServer on smartPAD
Command mode: POSITION
Control mode: POSITION
Rate 10ms

Run:

ros2 launch lbr_bringup bringup.launch.py model:=iiwa7 sim:=false ctrl:=lbr_joint_position_command_controller

Create a main.py and execute it (replace iiwa7 everywhere with your robot)

import os

import kinpy
import numpy as np
import rclpy
import xacro
from ament_index_python import get_package_share_directory
from lbr_fri_idl.msg import LBRJointPositionCommand, LBRState
from rcl_interfaces.srv import GetParameters
from rclpy.node import Node

class CartesianMotion(Node):
    def __init__(self, node_name: str) -> None:
        super().__init__(node_name)

        # something to compute robot kinematics
        self.robot_description = xacro.process(
            os.path.join(
                get_package_share_directory("lbr_description"),
                "urdf/iiwa7/iiwa7.urdf.xacro",
            )
        )
        self.kinematic_chain = kinpy.build_serial_chain_from_urdf(
            data=self.robot_description,
            root_link_name="link_0",
            end_link_name="link_ee",
        )

        # publisher / subscriber to command robot
        self.lbr_joint_position_command_ = LBRJointPositionCommand()
        self.lbr_joint_position_command_pub_ = self.create_publisher(
            LBRJointPositionCommand, "/lbr/command/joint_position", 1
        )
        self.lbr_state_sub_ = self.create_subscription(
            LBRState, "/lbr/state", self._on_lbr_state, 1
        )

        # get control rate from controller_manager
        self._dt = None
        self._retrieve_update_rate()

    def _on_lbr_state(self, lbr_state: LBRState) -> None:
        if self._dt is None:
            return
        # get joint states
        q = lbr_state.measured_joint_position

        # compute Jacobian and pseudo inverse
        J = self.kinematic_chain.jacobian(q)
        J_pinv = np.linalg.pinv(J, rcond=0.1)

        # given pose increment, compute joint increment
        dx = np.zeros(6)  # [x,y,z,r,p,y]
        dx[2] = 1  # move up along z-axis

        # compute joint velocity
        dq = J_pinv @ dx

        # compute target position
        q_target = q + dq * self._dt

        # send target position to robot
        self.lbr_joint_position_command_.joint_position = q_target
        self.lbr_joint_position_command_pub_.publish(self.lbr_joint_position_command_)

    def _retrieve_update_rate(self) -> float:
        paramter_client = self.create_client(
            GetParameters, "lbr/controller_manager/get_parameters"
        )
        paramter_name = "update_rate"
        while not paramter_client.wait_for_service(timeout_sec=1.0):
            if not rclpy.ok():
                raise RuntimeError("Interrupted while waiting for service.")
            self.get_logger().info(f"Waiting for {paramter_client.srv_name}...")
        future = paramter_client.call_async(
            GetParameters.Request(names=[paramter_name])
        )
        rclpy.spin_until_future_complete(self, future)
        if future.result() is None:
            raise RuntimeError(f"Failed to get parameter '{paramter_name}'.")
        update_rate = future.result().values[0].integer_value
        self.get_logger().info(f"{paramter_name}: {update_rate} Hz")
        self._dt = 1.0 / float(update_rate)

def main(args: list = None) -> None:
    rclpy.init(args=args)
    rclpy.spin(CartesianMotion("cartesian_motion"))
    rclpy.shutdown()

if __name__ == "__main__":
    main()

This, however, controls a velocity and not a target position. Also, if your pipeline is more complex, using a script can be quite inconvenient. There are ros2_controllers that essentially implement the above but control a position: https://github.com/fzi-forschungszentrum-informatik/cartesian_controllers/tree/ros2

I can have a look at that later and let you know how to use it.

Thanks Martin! This is exactly the kind of insight we needed. We just tested it, and also gained a better understanding of the /lbr/command/position topic. Perhaps we might just implement our own trajectory planner for the sake of learning, or use the library you sent. Thank you very much!

Okay hope this helps you move forward. Just be aware that the robot will execute what you ask it to execute. There are some velocity checks but be mindful regardless and execute in T1 mode until you are confident.

I'd just like to get back to you here Martin, and tell you that we now have it working in cartesian space as we wanted and interfacing with our own systems as a service. It is custom for our usecase, but I'll attach the code here for future reference for others: https://github.com/andreasHovaldt/pose_commander

very excited to hear about this! Maybe this could be added to the demos?

Theoretically yes, but it is setup as a service with a custom interface. It would probably work if anyone wants to use it, as long as they modify the URDF imported and the links for the kinematic chain in Kinpy (like mentioned earlier in this issue). They also have to import our custom interface.

For us it is a handy tool, as it interpolates straight cartesian paths and executes them rather smoothly. I would love to modify it to make it more universal, so it can easily be used by others, but like many others we are caught up in our semester project.

makes a lot of sense. In case you find time, you can fork this repo, add the pose_commander as is somewhere to demos, and I can review / fork your fork to suggest / add changes etc.

hi @EliasTDam , the Cartesian path through moveit is now fixed, thanks to @josefinemonnet

lbr-stack / lbr_fri_ros2_stack

MoveIt with gripper #133