1
$\begingroup$

Versions:
Ros2 humble
Moveit2
Gazebo Classic 11
Ubuntu Jammy

Context:
I am working with ROS2 humble, Moveit2, and Gazebo Classic for the simulation of the Panda robot arm. Currently, I am able to use the MotionPlanning tool of Moveit2 in Rviz2 to plan and execute trajectories in Gazebo using different planners from the OMPL library. I am also able to plan movements for a simple pick and place task.

Problem:
Only the planning for pick and place is functional, and executing the planning in Gazebo is not working. No error messages are displayed.

Please feel free to ask for additional information, and thank you.

panda_arm_mtc.cpp (where the task is planned following this tutorial):

static const rclcpp::Logger LOGGER = rclcpp::get_logger("panda_arm_mtc");
namespace mtc = moveit::task_constructor;

class MTCTaskNode
{
public:
  MTCTaskNode(const rclcpp::NodeOptions& options);

  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();

  void doTask();

  void setupPlanningScene();

private:
  // Compose an MTC task from a series of stages.
  mtc::Task createTask();
  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;
};

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
}

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}

void MTCTaskNode::setupPlanningScene()
{
  ...
}

void MTCTaskNode::doTask()
{
  task_ = createTask();

  try
  {
    task_.init();
  }
  catch (mtc::InitStageException& e)
  {
    RCLCPP_ERROR_STREAM(LOGGER, e);
    return;
  }

  if (!task_.plan(5 /* max_solutions */))
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
    return;
  }
  task_.introspection().publishSolution(*task_.solutions().front());

  auto result = task_.execute(*task_.solutions().front());
  if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");
    return;
  }

  return;
}

mtc::Task MTCTaskNode::createTask()
{
  ...
  return task;
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);

  rclcpp::NodeOptions options;
  options.automatically_declare_parameters_from_overrides(true);

  auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
  rclcpp::executors::MultiThreadedExecutor executor;

  auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() {
    executor.add_node(mtc_task_node->getNodeBaseInterface());
    executor.spin();
    executor.remove_node(mtc_task_node->getNodeBaseInterface());
  });

  mtc_task_node->setupPlanningScene();
  mtc_task_node->doTask();

  spin_thread->join();
  rclcpp::shutdown();
  return 0;
}

move_group.launch.py (launch file for the move group):

from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_move_group_launch


def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("panda", package_name="panda_arm_moveit_config").planning_pipelines(pipelines=["ompl"]).to_moveit_configs()
    moveit_config.move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"}
    return generate_move_group_launch(moveit_config)

pick_and_place.launch.py (launch file for the MTC):

from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("panda", package_name="panda_arm_moveit_config").to_moveit_configs()

    pick_and_place = Node(package="panda_arm_mtc", 
                          executable="panda_arm_mtc",
                          output="screen",
                          parameters=[moveit_config.to_dict(),],)
    
    return LaunchDescription([pick_and_place])

Simulation execution log:

...
[move_group-5] [INFO] [1689592870.698780604] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[move_group-5] [INFO] [1689592870.698889903] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[move_group-5] [INFO] [1689592870.698906653] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[move_group-5] [INFO] [1689592871.321979902] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-5] [INFO] [1689592871.322168305] [moveit.ros_planning_interface.moveit_cpp]: Listening to 'joint_states' for joint states
[move_group-5] [INFO] [1689592871.323616415] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[move_group-5] [INFO] [1689592871.324728862] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-5] [INFO] [1689592871.324759438] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-5] [INFO] [1689592871.327312082] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-5] [INFO] [1689592871.327346465] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-5] [INFO] [1689592871.328489616] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-5] [INFO] [1689592871.329895672] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-5] [INFO] [1689592871.972277168] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'ompl'
[move_group-5] [INFO] [1689592872.060370556] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-5] [INFO] [1689592872.072131288] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[move_group-5] [INFO] [1689592872.072167060] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[move_group-5] [INFO] [1689592872.072176098] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[move_group-5] [INFO] [1689592872.072212512] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-5] [INFO] [1689592872.072236911] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[move_group-5] [INFO] [1689592872.072245944] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-5] [INFO] [1689592872.072265491] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-5] [INFO] [1689592872.072273758] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was not set. Using default value: 0.020000
[move_group-5] [INFO] [1689592872.072315281] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[move_group-5] [INFO] [1689592872.072337264] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-5] [INFO] [1689592872.072344838] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[move_group-5] [INFO] [1689592872.072350946] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-5] [INFO] [1689592872.072356905] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-5] [INFO] [1689592872.072362866] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-5] [INFO] [1689592872.072368982] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[rviz2-6] 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-6] at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[move_group-5] [INFO] [1689592872.381651891] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for panda_arm_controller
[move_group-5] [INFO] [1689592872.400456015] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for panda_gripper_controller
[move_group-5] [INFO] [1689592872.400690433] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-5] [INFO] [1689592872.400731725] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-5] [INFO] [1689592872.404956648] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-5] [INFO] [1689592872.408444748] [move_group.move_group]: MoveGroup debug mode is ON
[rviz2-6] [INFO] [1689592872.470332790] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[rviz2-6] [INFO] [1689592872.470432012] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[rviz2-6] [INFO] [1689592872.470455095] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[move_group-5] [INFO] [1689592872.526920180] [move_group.move_group]: 
[move_group-5] 
[move_group-5] ********************************************************
[move_group-5] * MoveGroup using: 
[move_group-5] *     - ApplyPlanningSceneService
[move_group-5] *     - ClearOctomapService
[move_group-5] *     - CartesianPathService
[move_group-5] *     - ExecuteTrajectoryAction
[move_group-5] *     - GetPlanningSceneService
[move_group-5] *     - KinematicsService
[move_group-5] *     - MoveAction
[move_group-5] *     - MotionPlanService
[move_group-5] *     - QueryPlannersService
[move_group-5] *     - StateValidationService
[move_group-5] ********************************************************
[move_group-5] 
[move_group-5] [INFO] [1689592872.529149528] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-5] [INFO] [1689592872.529897860] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-5] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-5] Loading 'move_group/ClearOctomapService'...
[move_group-5] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-5] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-5] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-5] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-5] Loading 'move_group/MoveGroupMoveAction'...
[move_group-5] Loading 'move_group/MoveGroupPlanService'...
[move_group-5] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-5] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-5] 
[move_group-5] You can start planning now!
[move_group-5] 
[gzserver-7] [INFO] [1689592873.595553714] [gazebo_ros2_control]: connected to service!! robot_state_publisher
[gzserver-7] [INFO] [1689592873.597039275] [gazebo_ros2_control]: Recieved urdf from param server, parsing...
...
[gzserver-7] [INFO] [1689592873.615566584] [resource_manager]: Successful initialization of hardware 'GazeboSystem'
[gzserver-7] [INFO] [1689592873.615909827] [resource_manager]: 'configure' hardware 'GazeboSystem' 
[gzserver-7] [INFO] [1689592873.615928050] [resource_manager]: Successful 'configure' of hardware 'GazeboSystem'
[gzserver-7] [INFO] [1689592873.615953032] [resource_manager]: 'activate' hardware 'GazeboSystem' 
[gzserver-7] [INFO] [1689592873.615961489] [resource_manager]: Successful 'activate' of hardware 'GazeboSystem'
[gzserver-7] [INFO] [1689592873.616086818] [gazebo_ros2_control]: Loading controller_manager
[INFO] [spawn_entity.py-9]: process has finished cleanly [pid 20076]
[gzserver-7] [INFO] [1689592873.659593547] [gazebo_ros2_control]: Loaded gazebo_ros2_control.
[gzserver-7] [INFO] [1689592873.710507103] [controller_manager]: Loading controller 'panda_arm_controller'
[gzserver-7] [INFO] [1689592873.775209412] [controller_manager]: Setting use_sim_time=True for panda_arm_controller to match controller manager (see ros2_control#325 for details)
[spawner-2] [INFO] [1689592873.777258947] [spawner_panda_arm_controller]: Loaded panda_arm_controller
[gzserver-7] [INFO] [1689592873.784426930] [controller_manager]: Configuring controller 'panda_arm_controller'
[gzserver-7] [INFO] [1689592873.785638189] [panda_arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[gzserver-7] [INFO] [1689592873.785780902] [panda_arm_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[gzserver-7] [INFO] [1689592873.785873827] [panda_arm_controller]: Using 'splines' interpolation method.
[gzserver-7] [INFO] [1689592873.797013712] [panda_arm_controller]: Controller state will be published at 50.00 Hz.
[gzserver-7] [INFO] [1689592873.823526027] [panda_arm_controller]: Action status changes will be monitored at 20.00 Hz.
[gzserver-7] [INFO] [1689592873.844353379] [controller_manager]: Loading controller 'panda_gripper_controller'
[gzserver-7] [INFO] [1689592873.871139541] [controller_manager]: Setting use_sim_time=True for panda_gripper_controller to match controller manager (see ros2_control#325 for details)
[spawner-3] [INFO] [1689592873.876687212] [spawner_panda_gripper_controller]: Loaded panda_gripper_controller
[gzserver-7] [INFO] [1689592873.879093894] [controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-2] [INFO] [1689592873.880226398] [spawner_panda_arm_controller]: Configured and activated panda_arm_controller
[gzserver-7] [INFO] [1689592873.917727500] [controller_manager]: Setting use_sim_time=True for joint_state_broadcaster to match controller manager (see ros2_control#325 for details)
[gzserver-7] [INFO] [1689592873.920916507] [controller_manager]: Configuring controller 'panda_gripper_controller'
[gzserver-7] [INFO] [1689592873.921866808] [panda_gripper_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[spawner-4] [INFO] [1689592873.922025659] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[gzserver-7] [INFO] [1689592873.924092070] [panda_gripper_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[gzserver-7] [INFO] [1689592873.925641307] [panda_gripper_controller]: Using 'splines' interpolation method.
[gzserver-7] [INFO] [1689592873.928013226] [panda_gripper_controller]: Controller state will be published at 50.00 Hz.
[gzserver-7] [INFO] [1689592873.933774157] [panda_gripper_controller]: Action status changes will be monitored at 20.00 Hz.
[gzserver-7] [INFO] [1689592873.949026522] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[gzserver-7] [INFO] [1689592873.949678887] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-3] [INFO] [1689592873.968321461] [spawner_panda_gripper_controller]: Configured and activated panda_gripper_controller
[spawner-4] [INFO] [1689592873.981441838] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[INFO] [spawner-2]: process has finished cleanly [pid 20050]
[INFO] [spawner-4]: process has finished cleanly [pid 20054]
[INFO] [spawner-3]: process has finished cleanly [pid 20052]
[loader-10] [INFO] [1689592875.562597472] [gazebo_moveit_collision_loader]: Object object detected!
[rviz2-6] [ERROR] [1689592876.320043846] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-6] [INFO] [1689592876.462809531] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-6] [INFO] [1689592877.076167413] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.175 seconds
[rviz2-6] [INFO] [1689592877.079250692] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[rviz2-6] [INFO] [1689592877.081959923] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-6] [INFO] [1689592877.732489786] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-6] [INFO] [1689592877.734887195] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-6] [INFO] [1689592877.789659069] [interactive_marker_display_94207754922048]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-6] [INFO] [1689592877.838975551] [interactive_marker_display_94207754922048]: Sending request for interactive markers
[rviz2-6] [INFO] [1689592877.874175552] [interactive_marker_display_94207754922048]: Service response received for initialization
[rviz2-6] [INFO] [1689592877.903002833] [moveit_ros_visualization.motion_planning_frame]: group panda_arm
[rviz2-6] [INFO] [1689592877.903067394] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'panda_arm' in namespace ''
[rviz2-6] [INFO] [1689592878.043439714] [move_group_interface]: Ready to take commands for planning group panda_arm.

MTC node execution log:

[INFO] [launch]: All log files can be found below /home/name/.ros/log/2023-07-17-13-22-38-267334-name-Latitude-E6440-20514
[INFO] [launch]: Default logging verbosity is set to INFO
Using load_yaml() directly is deprecated. Use xacro.load_yaml() instead.
[INFO] [panda_arm_mtc-1]: process started with pid [20523]
[panda_arm_mtc-1] [INFO] [1689592961.444558258] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0064205 seconds
[panda_arm_mtc-1] [INFO] [1689592961.444658116] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[panda_arm_mtc-1] [INFO] [1689592961.444668525] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[panda_arm_mtc-1] [INFO] [1689592962.088215649] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[panda_arm_mtc-1] [INFO] [1689592962.105172121] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.path_tolerance' was not set. Using default value: 0.100000
[panda_arm_mtc-1] [INFO] [1689592962.105292227] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.resample_dt' was not set. Using default value: 0.100000
[panda_arm_mtc-1] [INFO] [1689592962.105312547] [moveit_ros.add_time_optimal_parameterization]: Param 'ompl.min_angle_change' was not set. Using default value: 0.001000
[panda_arm_mtc-1] [INFO] [1689592962.105378228] [moveit_ros.fix_workspace_bounds]: Param 'ompl.default_workspace_bounds' was not set. Using default value: 10.000000
[panda_arm_mtc-1] [INFO] [1689592962.105435003] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_bounds_error' was set to 0.100000
[panda_arm_mtc-1] [INFO] [1689592962.105455241] [moveit_ros.fix_start_state_bounds]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[panda_arm_mtc-1] [INFO] [1689592962.105546627] [moveit_ros.fix_start_state_collision]: Param 'ompl.start_state_max_dt' was not set. Using default value: 0.500000
[panda_arm_mtc-1] [INFO] [1689592962.105572513] [moveit_ros.fix_start_state_collision]: Param 'ompl.jiggle_fraction' was not set. Using default value: 0.020000
[panda_arm_mtc-1] [INFO] [1689592962.105582136] [moveit_ros.fix_start_state_collision]: Param 'ompl.max_sampling_attempts' was not set. Using default value: 100
[panda_arm_mtc-1] [INFO] [1689592962.105601862] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[panda_arm_mtc-1] [INFO] [1689592962.105608732] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Resolve constraint frames to robot links'
[panda_arm_mtc-1] [INFO] [1689592962.105625569] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[panda_arm_mtc-1] [INFO] [1689592962.105632106] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[panda_arm_mtc-1] [INFO] [1689592962.105637812] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[panda_arm_mtc-1] [INFO] [1689592962.105642848] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[panda_arm_mtc-1] [WARN] [1689592962.516542765] [moveit_trajectory_processing.time_optimal_trajectory_generation]: Joint acceleration limits are not defined. Using the default 1 rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml.
[panda_arm_mtc-1] [WARN] [1689592962.665459587] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 8. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592962.693353187] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592962.768295490] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 3. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592962.924449999] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592963.004426862] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 9. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592963.120895527] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 7. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592963.137004320] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592963.180938134] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592963.219878363] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 9. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592963.474352271] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [INFO] [1689592963.643502475] [moveit_ros.fix_start_state_bounds]: Starting state is just outside bounds (joint 'panda_joint4'). Assuming within bounds.
[panda_arm_mtc-1] [INFO] [1689592963.645507376] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[panda_arm_mtc-1] [INFO] [1689592963.751207691] [moveit.ros_planning.planning_pipeline]: Planning adapters have added states at index positions: [ 0 ]
[panda_arm_mtc-1] [INFO] [1689592963.754329134] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[panda_arm_mtc-1] [WARN] [1689592963.964187049] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 9. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592964.003367794] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [INFO] [1689592964.036416554] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[panda_arm_mtc-1] [INFO] [1689592964.217425717] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[panda_arm_mtc-1] [WARN] [1689592964.415226851] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592964.433370715] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [INFO] [1689592964.516329507] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[panda_arm_mtc-1] [WARN] [1689592964.760586337] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592964.881385003] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 9. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592964.956140434] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 9. Try a lower max_step.
[panda_arm_mtc-1] [WARN] [1689592965.032973922] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1] [INFO] [1689592965.075008835] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[panda_arm_mtc-1] [WARN] [1689592965.217434346] [moveit_robot_state.cartesian_interpolator]: The computed trajectory is too short to detect jumps in joint-space Need at least 10 steps, only got 2. Try a lower max_step.
[panda_arm_mtc-1]   1  - ←   5 →   -  5 / pick and place
[panda_arm_mtc-1]     1  - ←   1 →   -  0 / current
[panda_arm_mtc-1]     -  0 →   1 →   -  1 / open
[panda_arm_mtc-1]     -  1 →   1 ←   3  - / move to pick
[panda_arm_mtc-1]     3  - ←   3 →   -  3 / pick object
[panda_arm_mtc-1]       3  - ←   3 ←   2  - / approach object
[panda_arm_mtc-1]       2  - ←   9 →   -  1 / grasp pose IK
[panda_arm_mtc-1]        25  - ←  25 →   - 25 / generate grasp pose
[panda_arm_mtc-1]       -  1 →   8 →   -  0 / allow collision (panda_gripper,object)
[panda_arm_mtc-1]       -  0 →   8 →   -  0 / close
[panda_arm_mtc-1]       -  0 →   8 →   -  0 / attach object
[panda_arm_mtc-1]       -  0 →   6 →   -  6 / lift object
[panda_arm_mtc-1]     -  3 →   5 ←  19  - / move to place
[panda_arm_mtc-1]    19  - ←  19 →   -  0 / place object
[panda_arm_mtc-1]      38  - ←  38 →   -  3 / place pose IK
[panda_arm_mtc-1]        80  - ←  80 →   - 80 / generate place pose
[panda_arm_mtc-1]       -  3 →  35 →   -  0 / open
[panda_arm_mtc-1]       -  0 →  35 →   -  0 / forbid collision (hand,object)
[panda_arm_mtc-1]       -  0 →  35 →   -  0 / detach object
[panda_arm_mtc-1]       -  0 →  19 →   - 19 / retreat
[panda_arm_mtc-1]     -  0 →  15 →   - 15 / return home
$\endgroup$

2 Answers 2

1
$\begingroup$

I noticed that you added moveit_config.move_group_capabilities = {"capabilities": "move_group/ExecuteTaskSolutionCapability"} to the auto-generated move_group.launchp.py script.

To make that work I needed to modify moveit_configs_utils/launches.py to use that parameter.

I'm open to suggestions if you solved it another way, but see the following PR for details: https://github.com/ros-planning/moveit2/pull/2587

$\endgroup$
0
$\begingroup$

This is a bug that can be solved by remapping the follow_joint_trajectory topics for your moveit2 controllers. I also remap the /get_planning_scene topic for good measure. For example, if you have controllers arm_controller and gripper_controller, add this argument to your move_group_node:

 remappings = [
        ('/get_planning_scene', 'get_planning_scene'),
        ('/arm_controller/follow_joint_trajectory',
         'arm_controller/follow_joint_trajectory'),
        ('/gripper_controller/follow_joint_trajectory',
         'gripper_controller/follow_joint_trajectory'),
    ]
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.