search

moveit / moveit2_tutorials

A sphinx-based centralized documentation repo for MoveIt 2
https://moveit.picknik.ai
BSD 3-Clause "New" or "Revised" License
150 stars 194 forks source link

Got the wrong trajectory from moveit::planning_interface::MoveGroupInterface::computeCartesianPath #975

Open ey39 opened 3 days ago

ey39 commented 3 days ago

Description

Got the wrong trajectory from moveit::planning_interface::MoveGroupInterface::computeCartesianPath

Your environment

Steps to reproduce

I used the moveit2-master branch because I wanted to use moveit2's Python-API, Similarly, my moveit2_tutorials also used the master branch of my repo (but changed all rolling branch related branches of the moveit2_tutorials.repo to humble). 

repositories:
  moveit_task_constructor:
    type: git
    url: https://github.com/moveit/moveit_task_constructor.git
    version: ros2
  moveit_visual_tools:
    type: git
    url: https://github.com/moveit/moveit_visual_tools
    version: ros2
  # Remove ros2_kortex when rolling binaries are available.
  # Need to use this fork since ros2_kortex depends on Gazebo classic packages:
  # https://github.com/Kinovarobotics/ros2_kortex/issues/217
  ros2_kortex:
    type: git
    url: https://github.com/sea-bass/ros2_kortex.git
    version: remove-gazebo-classic
  # Remove ros2_robotiq_gripper when rolling binaries are available.
  ros2_robotiq_gripper:
    type: git
    url: https://github.com/PickNikRobotics/ros2_robotiq_gripper.git
    version: humble
  # Serial is a dependency of ros2_robotiq_gripper. Remove when ros2_robotiq_gripper binaries are available.
  serial:
    type: git
    url: https://github.com/tylerjw/serial.git
    version: ros2
  rviz_visual_tools:
    type: git
    url: https://github.com/PickNikRobotics/rviz_visual_tools.git
    version: ros2
  moveit2:
    type: git
    url: https://github.com/moveit/moveit2
    version: main
  moveit_resources:
    type: git
    url: https://github.com/moveit/moveit_resources
    version: ros2
  moveit_msgs:
    type: git
    url: https://github.com/moveit/moveit_msgs
    version: ros2
  pick_ik:
    type: git
    url: https://github.com/PickNikRobotics/pick_ik
    version: main
  # Doesn't seem to be available from binaries yet
  gz_ros2_control:
    type: git
    url: https://github.com/ros-controls/gz_ros2_control.git
    version: humble

I firstly called move_group.launch.py to reveal the rviz interface and display the panda arm. 

ros2 launch moveit2_tutorials move_group.launch.py
ros2 launch moveit2_tutorials move_group_interface_tutorial.launch.py

Then I call move_group_interface_tutorial.launch.py, found move_group.computeCartesianPath function calculated trajectory always return to the starting point instead of running continuously along the waypoints.

Expected behaviour

the trajectory after computeCartesianPath should running continuously along the waypoints.

Backtrace or Console output

[INFO] [launch]: All log files can be found below /home/ey/.ros/log/2024-09-26-23-46-11-075732-ey-virtual-machine-5617 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [move_group_interface_tutorial-1]: process started with pid [5618] [move_group_interface_tutorial-1] [WARN] [1727365571.251595272] [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. [move_group_interface_tutorial-1] [INFO] [1727365571.263280680] [move_group_interface_tutorial.moveit.ros.rdf_loader]: Loaded robot model in 0.00294252 seconds [move_group_interface_tutorial-1] [INFO] [1727365571.263327994] [move_group_interface_tutorial.moveit.core.robot_model]: Loading robot model 'panda'... [move_group_interface_tutorial-1] [INFO] [1727365571.277731914] [move_group_interface_tutorial.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1 [move_group_interface_tutorial-1] [INFO] [1727365571.292837594] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Ready to take commands for planning group panda_arm. [move_group_interface_tutorial-1] [INFO] [1727365571.313169618] [move_group_interface_tutorial.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [move_group_interface_tutorial-1] [WARN] [1727365571.320529175] [move_group_interface_tutorial.moveit.ros.current_state_monitor]: Unable to update multi-DOF joint 'virtual_joint':Failure to lookup transform between 'world'and 'panda_link0' with TF exception: "world" passed to lookupTransform argument target_frame does not exist. [move_group_interface_tutorial-1] [INFO] [1727365571.320837877] [move_group_interface_tutorial.remote_control]: RemoteControl Ready. [move_group_interface_tutorial-1] [INFO] [1727365571.322565584] [move_group_demo]: Planning frame: world [move_group_interface_tutorial-1] [INFO] [1727365571.322578248] [move_group_demo]: End effector link: panda_link8 [move_group_interface_tutorial-1] [INFO] [1727365571.322582815] [move_group_demo]: Available Planning Groups: [move_group_interface_tutorial-1] [INFO] [1727365571.322611730] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to start the demo [move_group_interface_tutorial-1] [INFO] [1727365573.358570356] [move_group_interface_tutorial.remote_control]: ... continuing [move_group_interface_tutorial-1] [INFO] [1727365573.358724720] [move_group_interface_tutorial.moveit.ros.move_group_interface]: MoveGroup action client/server ready [move_group_interface_tutorial-1] [INFO] [1727365573.359358864] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request accepted [move_group_interface_tutorial-1] [INFO] [1727365573.390824402] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request complete! [move_group_interface_tutorial-1] [INFO] [1727365573.391028347] [move_group_interface_tutorial.moveit.ros.move_group_interface]: time taken to generate plan: 0.022278 seconds [move_group_interface_tutorial-1] [INFO] [1727365573.391063295] [move_group_demo]: Visualizing plan 1 (pose goal) [move_group_interface_tutorial-1] [INFO] [1727365573.391070895] [move_group_demo]: Visualizing plan 1 as trajectory line [move_group_interface_tutorial-1] [INFO] [1727365573.392054062] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo [move_group_interface_tutorial-1] [INFO] [1727365577.498908222] [move_group_interface_tutorial.remote_control]: ... continuing [move_group_interface_tutorial-1] [INFO] [1727365577.500637893] [move_group_interface_tutorial.moveit.ros.move_group_interface]: MoveGroup action client/server ready [move_group_interface_tutorial-1] [INFO] [1727365577.501171969] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request accepted [move_group_interface_tutorial-1] [INFO] [1727365577.537738275] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request complete! [move_group_interface_tutorial-1] [INFO] [1727365577.537784339] [move_group_interface_tutorial.moveit.ros.move_group_interface]: time taken to generate plan: 0.0220382 seconds [move_group_interface_tutorial-1] [INFO] [1727365577.537800582] [move_group_demo]: Visualizing plan 2 (joint space goal) [move_group_interface_tutorial-1] [INFO] [1727365577.538228251] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo [move_group_interface_tutorial-1] [INFO] [1727365581.359280294] [move_group_interface_tutorial.remote_control]: ... continuing [move_group_interface_tutorial-1] [INFO] [1727365581.360855254] [move_group_interface_tutorial.moveit.ros.move_group_interface]: MoveGroup action client/server ready [move_group_interface_tutorial-1] [INFO] [1727365581.361546700] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request accepted [move_group_interface_tutorial-1] [INFO] [1727365591.413440856] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request aborted [move_group_interface_tutorial-1] [ERROR] [1727365591.414126732] [move_group_interface_tutorial.moveit.ros.move_group_interface]: MoveGroupInterface::plan() failed or timeout reached [move_group_interface_tutorial-1] [INFO] [1727365591.414156683] [move_group_demo]: Visualizing plan 3 (constraints) FAILED [move_group_interface_tutorial-1] [INFO] [1727365591.415300019] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo [move_group_interface_tutorial-1] [INFO] [1727365595.347770588] [move_group_interface_tutorial.remote_control]: ... continuing [move_group_interface_tutorial-1] [INFO] [1727365595.414977666] [move_group_demo]: Visualizing plan 4 (Cartesian path) (100.00% achieved) [move_group_interface_tutorial-1] [INFO] [1727365595.415084964] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo [move_group_interface_tutorial-1] [INFO] [1727365609.281129128] [move_group_interface_tutorial.remote_control]: ... continuing [move_group_interface_tutorial-1] [INFO] [1727365609.290553763] [move_group_interface_tutorial.moveit.ros.move_group_interface]: MoveGroup action client/server ready [move_group_interface_tutorial-1] [INFO] [1727365609.291292672] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request accepted [move_group_interface_tutorial-1] [INFO] [1727365609.336568813] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request complete! [move_group_interface_tutorial-1] [INFO] [1727365609.337005864] [move_group_interface_tutorial.moveit.ros.move_group_interface]: time taken to generate plan: 0.0225531 seconds [move_group_interface_tutorial-1] [INFO] [1727365609.337027181] [move_group_demo]: Visualizing plan 5 (with no obstacles) [move_group_interface_tutorial-1] [INFO] [1727365609.338053928] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to continue the demo [move_group_interface_tutorial-1] [INFO] [1727365615.783958602] [move_group_interface_tutorial.remote_control]: ... continuing [move_group_interface_tutorial-1] [INFO] [1727365615.784015681] [move_group_demo]: Add an object into the world [move_group_interface_tutorial-1] [INFO] [1727365615.784181188] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz [move_group_interface_tutorial-1] [INFO] [1727365620.359408501] [move_group_interface_tutorial.remote_control]: ... continuing [move_group_interface_tutorial-1] [INFO] [1727365620.359563859] [move_group_interface_tutorial.moveit.ros.move_group_interface]: MoveGroup action client/server ready [move_group_interface_tutorial-1] [INFO] [1727365620.360672791] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request accepted [move_group_interface_tutorial-1] [INFO] [1727365620.394557040] [move_group_interface_tutorial.moveit.ros.move_group_interface]: Planning request complete! [move_group_interface_tutorial-1] [INFO] [1727365620.394957547] [move_group_interface_tutorial.moveit.ros.move_group_interface]: time taken to generate plan: 0.011747 seconds [move_group_interface_tutorial-1] [INFO] [1727365620.394992222] [move_group_demo]: Visualizing plan 6 (pose goal move around cuboid) [move_group_interface_tutorial-1] [INFO] [1727365620.395684188] [move_group_interface_tutorial.remote_control]: Waiting to continue: Press 'next' in the RvizVisualToolsGui window once the plan is complete

https://github.com/user-attachments/assets/4eb9cbc7-7ba9-4e7f-a6d3-6808f6fda080