MoveIt with gripper

[TobiasAxelsen]

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.

[mhubii]

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.

[TobiasAxelsen]

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.

[mhubii]

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

[TobiasAxelsen]

That is exactly what I meant actually :)

[TobiasAxelsen]

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.

[mhubii]

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 !

[mhubii]

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

[TobiasAxelsen]

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...

[TobiasAxelsen]

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 :)

[mhubii]

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

[mhubii]

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.

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

[mhubii]

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