Continuation of #8, which implements an interface for planning and executing Cartesian paths with a single target waypoint (specified as pose goal).

How would I add Force_checker to pymoveit2.py script so that when I have an 3D uneven object on which I want to go from one point to another with -10N force I could do it with just using ex_pose_goal.py?

How do can I change Cartesian speed for Franka Emika with this script? I tried changing "max_cartesian_speed" parameter, but it does not change anything. Can you help?

I have set the joints names in the robots/myrobot.py in the following order:

def joint_names() -> List[str]:
    return [
        'base_joint',
        'elbow_joint',
        'shoulder_joint',
        'wrist_pitch_joint',
        'wrist_roll_joint',
    ]

But the follow_joint_trajectory message received by the controller has different order:

joint_names: 
- base_joint
- shoulder_joint
- elbow_joint
- wrist_pitch_joint
- wrist_roll_joint

So the example code: examples/ex_joint_goal.py follows which ones?

    # Declare parameter for joint positions
    node.declare_parameter(
        "joint_positions",
        [
            0.0,
            0.0,
            -0.7853981633974483,
            0.0,
            1.5707963267948966,
        ],
    )

I tried to add a 3D occupancy map of the robots surroundings using the add_collision_mesh functionality.

I discretized the robots workspace into 5cm cubes which I can toggle.

However the performance is really low in RViz with a framerate of 0/1/2 fps.

With multiple cubes that represent the ground floor: 2fps

A single mesh to represent the ground floor: 31 fps

Performance test with multiple cubes: 0 fps

Is this expected behaviour? Are collision meshes not meant to be uesd for collision detection?

Is this performance due to using a VM?

for x in range(0, world_dimensions_x):
                for y in range(0, world_dimensions_y):
                    z = 4
                    self.add_collision_mesh_cube(x, y, z)
....
....
    def add_collision_mesh_cube(self, x: int, y: int, z: int):
        filepath = path.join(
            path.dirname(path.realpath(__file__)), "5cm_blender_cube.stl"
        )
        quat = Quaternion(x=1.0, y=0.0, z=0.0, w=0.0)

        center_coordinates_x = 17
        center_coordinates_y = 17
        center_coordinates_z = 4

        step_size = 0.05
        offset_z = -0.025

        x_rviz = x * step_size - (center_coordinates_x * step_size)
        y_rviz = y * step_size - (center_coordinates_y * step_size)
        z_rviz = z * step_size - (center_coordinates_z * step_size) + offset_z

        position = Point(x=x_rviz, y=y_rviz, z=z_rviz)
        id = str(x) + "_" + str(y) + "_" + str(z) + "_cube"
        self._moveit2.add_collision_mesh(
            filepath=filepath, id=id, position=position, quat_xyzw=quat
        )

My 5cm cube:
5cm_blender_cube.zip

Hello, first of all, thanks for the cool repo, and for providing the Python binding with Moveit on Humble.

in moveit1 there was the following two function

arm = MoveGroupCommander("arm")
arm.set_named_target("home")
arm.go()

    <group_state name="home" group="arm">
        <joint name="joint1" value="0" />
        <joint name="joint2" value="-1.309" />
        <joint name="joint3" value="-0.261799" />
        <joint name="joint4" value="0" />
        <joint name="joint5" value="1.5708" />
        <joint name="joint6" value="0" />
    </group_state>

by setting the home referring joints configuration inside the .srdf file, it was super easy to move to a pre-defined target pose.

I was searching, a way how to do it with help of your package pymoveit2, and I found only the move_to_configuration() function which allows moving to a joint position, however, I am not able to move directly to get joint angles which I defined inside .srdf, is there any way to get them from moviet config? without hard- coding inside the my application node?

All best

Hi, I followed the instructions but the robot is not executing the planned motion as expected

Here is the error I get in the move_group.launch.py terminal

[ERROR] [1688223063.534889158] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached
[move_group-2] [WARN] [1688223160.048471449] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'tool0' to planning frame'world' (Could not find a connection between 'world' and 'tool0' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-2] [WARN] [1688223160.048533739] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'flange' to planning frame'world' (Could not find a connection between 'world' and 'flange' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-2] [WARN] [1688223160.048550778] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'wrist_3_link' to planning frame'world' (Could not find a connection between 'world' and 'wrist_3_link' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-2] [WARN] [1688223160.048565094] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Unable to transform object from frame 'ft_frame' to planning frame'world' (Could not find a connection between 'world' and 'ft_frame' because they are not part of the same tree.Tf has two or more unconnected trees.)
[move_group-2] [ERROR] [1688223160.048647317] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-2] [ERROR] [1688223160.048668730] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-2] [ERROR] [1688223160.048743758] [moveit_robot_state.conversions]: Found empty JointState message
[move_group-2] [ERROR] [1688223160.048881414] [moveit_robot_state.conversions]: Found empty JointState message

Here is the error in the ex_pose_goal terminal

$ ros2 run pymoveit2 ex_pose_goal.py --ros-args -p position:="[0.25, 0.0, 1.0]" -p quat_xyzw:="[0.0, 0.0, 0.0, 1.0]" -p cartesian:=False
[INFO] [1688223160.047524623] [ex_pose_goal]: Moving to {position: [0.25, 0.0, 1.0], quat_xyzw: [0.0, 0.0, 0.0, 1.0]}
[WARN] [1688223161.135484471] [ex_pose_goal]: Action server 'joint_trajectory_controller/follow_joint_trajectory' is not yet available. Better luck next time!
[WARN] [1688223161.136446016] [ex_pose_goal]: Cannot wait until motion is executed (no motion is in progress).

You can see the planned motion below but no execution

It tells me that /joint_states is not publishing messages

I found some parts where the frame_id was not added to the header. Is it intentional for some reason? It could give wrong trajectory planning if you are planning with robots in a different frame than the default.
See my diff below to locate the missing parts.

Cheers,

diff --git a/pymoveit2/moveit2.py b/pymoveit2/moveit2.py
index c4a2db4..3987ff8 100644
--- a/pymoveit2/moveit2.py
+++ b/pymoveit2/moveit2.py
@@ -697,6 +697,7 @@ class MoveIt2:

         stamp = self._node.get_clock().now().to_msg()
         self.__compute_fk_req.header.stamp = stamp
+        self.__compute_fk_req.header.frame_id = "base_link"

         if not self.__compute_fk_client.wait_for_service(
             timeout_sec=wait_for_server_timeout_sec
@@ -994,6 +995,7 @@ class MoveIt2:

         stamp = self._node.get_clock().now().to_msg()
         self.__cartesian_path_request.header.stamp = stamp
+        self.__cartesian_path_request.header.frame_id = "base_link"

         self.__cartesian_path_request.path_constraints = (
             self.__move_action_goal.request.path_constraints
diff --git a/pymoveit2/moveit2_servo.py b/pymoveit2/moveit2_servo.py
index 5291c00..31a3f88 100644
--- a/pymoveit2/moveit2_servo.py
+++ b/pymoveit2/moveit2_servo.py
@@ -130,6 +130,7 @@ class MoveIt2Servo:

         twist_msg = deepcopy(self.__twist_msg)
         twist_msg.header.stamp = self._node.get_clock().now().to_msg()
+        twist_msg.header.frame_id = "base_link"
         twist_msg.twist.linear.x *= linear[0]
         twist_msg.twist.linear.y *= linear[1]
         twist_msg.twist.linear.z *= linear[2]

Feature Request

Is it possible to add the functionality to set poses of already existing collision objects?

As seen in the moveit messages docs:
it should be possible to send a MOVE type message.

Expected behaviour:

Set pose of collision object by using the mesh_id.

Hi,
I'm using your library to move a robotic arm. For some reason using the default planner I have problems in the planning. I would use the "pilz_industrilal_motion_planner" as pipeline_id but I found it only inside "moveit2.py" (line 1182) and it is commented.

What would you suggest to change the default planner and set pilz? Is it possible without changing the source?

What's the license for this repo?

wait_until_executed only waits when the plan has been send for execution, but when the planning takes longer, there is no execution yet and the function returns. I think that might be what happens in #28 as well.

Found while working on Jazzy-Harmonic adaptation

(
AndrejOrsula/panda_ign_moveit2#34
AndrejOrsula/ign_moveit2_examples#27
)

Calling compute_ik service can take >1 s of execution time. This is very unexpected, considering that a full kinematic plan can be obtained within milliseconds.

Hello Andrej! This is not really a bug or an error but rather a poor performance problem

As you can see here! The robot struggles to do the circular servo motion in ignition gazebo simulation.

But it seems to work ideally in fake simulation in Rviz.

Am I doing something wrong or is the moveit_servo not doing a good enough job with the ign gazebo simulation?

I tried to replicate these results with the UR5 robot that I have worked on and it seems to be facing a similar issue. Is there a solution to this problem?

Hi, Andrej Orsula,

Many thanks for offering this convenient tool :-)

Could you kindly let me know if there is an easy way to get the ee pose of the robot, so that I can do something like:

curr_pose = moveit2.get_ee_pose()
curr_pose.position.x = curr_pose.position.x + 0.1

moveit2.move_to_pose(position=curr_pose.position, quat_xyzw=curr_pose.quat_xyzw, cartesian=cartesian)
moveit2.wait_until_executed()

Any help is appreciated.

Hi @AndrejOrsula, I was looking for a way to send target poses to a UR3e from python to Moveit2 and came across your work.
I managed to get your bridge working with the ROS2 drivers for the UR by creating a new robot class:

from typing import List

MOVE_GROUP_ARM: str = "ur_manipulator"
MOVE_GROUP_GRIPPER: str = "NA"

def joint_names(prefix: str = "") -> List[str]:
    return [
        prefix + "elbow_join",
        prefix + "shoulder_lift_joint",
        prefix + "shoulder_pan_joint",
        prefix + "wrist_1_joint",
        prefix + "wrist_2_joint",
        prefix + "wrist_3_joint",
    ]


def base_link_name(prefix: str = "") -> str:
    return prefix + "base_link"


def end_effector_name(prefix: str = "") -> str:
    return prefix + "tool0"


def gripper_joint_names(prefix: str = "") -> List[str]:
    return [
        prefix + "NA",
        prefix + "NA",
    ]

However the UR drivers do not include a gripper in their default URDF/SRDF, so I was wondering how you would want the UR class to handle this (so that i can make a PR).

Hi there,

Thanks for the library - found it so much easier than trying to get the MoveIt API working. I came across a bug (?) which seems to happen during initialization. The function __init_move_action_goal creates a request with a cartesian speed parameter. This parameter is missing in my request. Syntax highlighting seems to not find it either (see picture below)

I am using ROS Iron.

I've commented out that line and the code works fine (however I have not tested robot movements - just using this library to attach collision objects).

Currently, sending a new pose will starts the path planning but it is not in the form of a cartesian shortest path between the initial pose and the goal pose.
Is there a way to obtain such behaviour?

#8 added a cartesian flag to move_to_configuration(...). However, setting that flag to True (e.g., by modifying the move_to_configuration call in ex_joint_goal.py) fails with an IndexError. The reason it fails it that the cartesian planner expects position and orientation goal constraints (here and here), whereas move_to_configuration(...) only sets joint goal constraints (here).

The only way to rectify this is to set position and orientation goal constraints before calling move_to_configuration(...). However, at that point, it seems better to either use move_to_pose(...) (if the user only cares about pose goal constraints) or make a custom call to plan(...) (if the user wants to combine pose and joint constraints).

Although MoveIt2's RVIZ interface allows for cartesian planning with a joint configuration as the goal, as far as I can tell it first uses FK to convert the goal configuration into a goal end effector pose, and then computes a cartesian plan with that end effector pose as the goal constraint.

I see three options for addressing this:

1. Remove the cartesian flag from move_to_configuration(...).
2. If move_to_configuration(...) is called with the cartesian flag, first compute FK and then set position/orientation constraints based on the end effector pose (as opposed to setting joint constraints).
3. Keep the code as-is, but add to the docstring of move_to_configuration(...) that the cartesian flag only works if position and orientation goal constraints have been set beforehand.

I'm happy to make the changes, but before diving in I wanted to get others' thoughts on this issue and the best way to address it.

Hi everyone,

I'm trying to bridge some movegroup functions to python I think that is not supported yet.

And after a bit of research I saw this repo, and I was wondering if I can use this functions to move my UR robot (ur3e). I see that you can perfectly move panda robots, but i dont know how to bridge it to UR.

Thanks in advance

Hello,
I am trying to control a 4dof scara robot with this package. Below I have attached the model of my robot.
First, I tried using ex_fk.py to get end effector position by giving joint angles as arguments.
Then, I used the same values with 12 decimal places to get joint angles using ex_ik.py. It works perfectly, it gives joint angles close to [0, 0, 0, 0] but with decimal places.
Then again, I used the same values but rounded-off to 1 decimal. I am getting an error that "IK computation failed! Error code: -31."

I have also attached the screenshot of my robot description terminal and the ex_ik launch terminal.
What could be the reason that it is not calculating IK with less decimal places?

Hi,

I am trying to use pymoveit2 on the UR manipulator, and I wonder if there is anyone who already implemented this on UR.
Thank you in advance!

Hi,

I was wondering if it's possible to split the move into two stages: planning and execution, and fail planning if the pose is not possible before starting the execution phase.

It is the way how MoveIt C++ interface works and is very convenient.

Currenly move_to_pose will start moving straight away and hit itself if the move is not attainable.

Thanks.

Excuse me Sir,
I noticed there is no panda_moveit_config in your package, thus i can't run the examples. Where can I get that?

In your code:

        pose = Pose()
        pose.position = position
        pose.orientation = quat_xyzw
        msg.pose = pose

However, from ROS2, it should be

        ...
        msg.mesh_poses.append(pose)

How can we use a real robot running microRos node (joint_states publisher and follow_trajectory_action server) and connected over serial to microRos Agent with your library? I have generated the moveit config for the robot using the moveit_setup_assistant. Any example or hints would be helpful.