Open minku1219 opened 1 week ago
rhaschke
commented
1 week ago Looks like your robot's move group doesn't have a unique end-effector frame. Thus, you need to specify the frame explicitly by setting the ik_frame property of the Cartesian x +0.2 stage.
minku1219
commented
1 week ago Could you please elaborate where and how to add this setIkFrame? Thanks in advance!
rhaschke
commented
1 week ago Just augment the stage definition:
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian_interpolation);
stage->setGroup(group);with a line defining the ik frame:
stage->setIKFrame("your robot's eef frame");Thanks @rhaschke for the solution. The error is gone but the robot is unable to move. Could you please provide with some snippet to make it work with UR5
rhaschke
commented
1 week ago How can I provide help if you don't specify the exact error you are observing?
minku1219
commented
1 week ago Actually it is not showing any error, instead the robot i am working with is not moving.
minku1219
commented
1 week ago I have'nt found any example for working with UR5 or any UR manipulator, so i am trying this on my own
rhaschke
commented
1 week ago Actually it is not showing any error, instead the robot i am working with is not moving.
The original code of cartesian.cpp is not attempting to execute the solution:
https://github.com/moveit/moveit_task_constructor/blob/5a4480814628db4207d9734d63dd907619ced93b/demo/src/cartesian.cpp#L142-L143
You need to execute it for the robot to move:
task.execute(*task.solutions().front());
minku1219
commented
1 week ago Added this and encountered an error!
[INFO] [launch]: All log files can be found below /home/tcs/.ros/log/2024-09-10-14-35-32-732590-tcs-Precision-7710-150197
[INFO] [launch]: Default logging verbosity is set to INFO
WARNING:root:Cannot infer URDF from /opt/ros/humble/share/ur_moveit_config. -- using config/ur.urdf
WARNING:root:Cannot infer SRDF from /opt/ros/humble/share/ur_moveit_config. -- using config/ur.srdf
[INFO] [cartesian-1]: process started with pid [150198]
[cartesian-1] [INFO] [1725959133.689321885] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0616049 seconds
[cartesian-1] [INFO] [1725959133.689485639] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[cartesian-1] [INFO] [1725959133.689501686] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[cartesian-1] Plan!
[cartesian-1] PN6moveit16task_constructor11TaskPrivateE
[cartesian-1] Init!
[cartesian-1] 1
[cartesian-1] [ERROR] [1725959134.262591040] [moveit_task_constructor_executor_140722771479248]: Failed to connect to the 'execute_task_solution' action server
minku1219
commented
1 week ago Using this launch file
import os
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ur_moveit_config.launch_common import load_yaml
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.conditions import IfCondition
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from ament_index_python.packages import get_package_share_directory
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
ur_type = LaunchConfiguration("ur_type")
use_fake_hardware = LaunchConfiguration("use_fake_hardware")
safety_limits = LaunchConfiguration("safety_limits")
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
safety_k_position = LaunchConfiguration("safety_k_position")
# General arguments
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
_publish_robot_description_semantic = LaunchConfiguration("publish_robot_description_semantic")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_joint_limits_file = LaunchConfiguration("moveit_joint_limits_file")
moveit_config_file = LaunchConfiguration("moveit_config_file")
warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
prefix = LaunchConfiguration("prefix")
use_sim_time = LaunchConfiguration("use_sim_time")
launch_rviz = LaunchConfiguration("launch_rviz")
launch_servo = LaunchConfiguration("launch_servo")
exe = LaunchConfiguration("exe")
joint_limit_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "joint_limits.yaml"]
)
kinematics_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "default_kinematics.yaml"]
)
physical_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "physical_parameters.yaml"]
)
visual_params = PathJoinSubstitution(
[FindPackageShare(description_package), "config", ur_type, "visual_parameters.yaml"]
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
" ",
"robot_ip:=xxx.yyy.zzz.www",
" ",
"joint_limit_params:=",
joint_limit_params,
" ",
"kinematics_params:=",
kinematics_params,
" ",
"physical_params:=",
physical_params,
" ",
"visual_params:=",
visual_params,
" ",
"safety_limits:=",
safety_limits,
" ",
"safety_pos_margin:=",
safety_pos_margin,
" ",
"safety_k_position:=",
safety_k_position,
" ",
"name:=",
"ur",
" ",
"ur_type:=",
ur_type,
" ",
"script_filename:=ros_control.urscript",
" ",
"input_recipe_filename:=rtde_input_recipe.txt",
" ",
"output_recipe_filename:=rtde_output_recipe.txt",
" ",
"prefix:=",
prefix,
" ",
]
)
robot_description = {"robot_description": robot_description_content}
# MoveIt Configuration
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "srdf", moveit_config_file]
),
" ",
"name:=",
# Also ur_type parameter could be used but then the planning group names in yaml
# configs has to be updated!
"ur",
" ",
"prefix:=",
prefix,
" ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
publish_robot_description_semantic = {
"publish_robot_description_semantic": _publish_robot_description_semantic
}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
)
robot_description_planning = {
"robot_description_planning": load_yaml(
str(moveit_config_package.perform(context)),
os.path.join("config", str(moveit_joint_limits_file.perform(context))),
)
}
# Planning Configuration
ompl_planning_pipeline_config = {
"move_group": {
"planning_plugin": "ompl_interface/OMPLPlanner",
"request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"start_state_max_bounds_error": 0.1,
}
}
ompl_planning_yaml = load_yaml("ur_moveit_config", "config/ompl_planning.yaml")
ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml)
# Trajectory Execution Configuration
controllers_yaml = load_yaml("ur_moveit_config", "config/controllers.yaml")
# the scaled_joint_trajectory_controller does not work on fake hardware
change_controllers = context.perform_substitution(use_fake_hardware)
if change_controllers == "true":
controllers_yaml["scaled_joint_trajectory_controller"]["default"] = False
controllers_yaml["joint_trajectory_controller"]["default"] = True
moveit_controllers = {
"moveit_simple_controller_manager": controllers_yaml,
"moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager",
}
trajectory_execution = {
"moveit_manage_controllers": False,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
warehouse_ros_config = {
"warehouse_plugin": "warehouse_ros_sqlite::DatabaseConnection",
"warehouse_host": warehouse_sqlite_path,
}
# Start the actual move_group node/action server
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
publish_robot_description_semantic,
robot_description_kinematics,
robot_description_planning,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
{"use_sim_time": use_sim_time},
warehouse_ros_config,
],
)
# rviz with moveit configuration
rviz_config_file = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "rviz", "view_robot.rviz"]
)
rviz_node = Node(
package="rviz2",
condition=IfCondition(launch_rviz),
executable="rviz2",
name="rviz2_moveit",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
ompl_planning_pipeline_config,
robot_description_kinematics,
robot_description_planning,
warehouse_ros_config,
],
)
# Servo node for realtime control
servo_yaml = load_yaml("ur_moveit_config", "config/ur_servo.yaml")
servo_params = {"moveit_servo": servo_yaml}
servo_node = Node(
package="moveit_servo",
condition=IfCondition(launch_servo),
executable="servo_node_main",
parameters=[
servo_params,
robot_description,
robot_description_semantic,
],
output="screen",
)
package = "moveit_task_constructor_demo"
package_shared_path = get_package_share_directory(package)
mtc_node = Node(
package=package,
executable=LaunchConfiguration("exe"),
output="screen",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
],
)
nodes_to_start = [mtc_node]
return nodes_to_start
def generate_launch_description():
declared_arguments = []
# UR specific arguments
declared_arguments.append(
DeclareLaunchArgument(
"ur_type",
description="Type/series of used UR robot.",
choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e", "ur20", "ur30"],
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_fake_hardware",
default_value="false",
description="Indicate whether robot is running with fake hardware mirroring command to its states.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_limits",
default_value="true",
description="Enables the safety limits controller if true.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_pos_margin",
default_value="0.15",
description="The margin to lower and upper limits in the safety controller.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"safety_k_position",
default_value="20",
description="k-position factor in the safety controller.",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="ur_description",
description="Description package with robot URDF/XACRO files. Usually the argument "
"is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="ur.urdf.xacro",
description="URDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"publish_robot_description_semantic",
default_value="True",
description="Whether to publish the SRDF description on topic /robot_description_semantic.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
default_value="ur_moveit_config",
description="MoveIt config package with robot SRDF/XACRO files. Usually the argument "
"is not set, it enables use of a custom moveit config.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
default_value="ur.srdf.xacro",
description="MoveIt SRDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_joint_limits_file",
default_value="joint_limits.yaml",
description="MoveIt joint limits that augment or override the values from the URDF robot_description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"warehouse_sqlite_path",
default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
description="Path where the warehouse database should be stored",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for "
"multi-robot setup. If changed than also joint names in the controllers' configuration "
"have to be updated.",
)
)
declared_arguments.append(
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_servo", default_value="true", description="Launch Servo?")
)
declared_arguments.append(
DeclareLaunchArgument("exe", default_value="cartesian", description="Launch executable?")
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])Cartesian.cpp code:
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/connect.h>
#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
using namespace moveit::task_constructor;
Task createTask(const rclcpp::Node::SharedPtr& node) {
Task t;
t.stages()->setName("Cartesian Path");
const std::string group = "ur_manipulator";
const std::string eef = "tool0";
// create Cartesian interpolation "planner" to be used in various stages
auto cartesian_interpolation = std::make_shared<solvers::CartesianPath>();
// create a joint-space interpolation "planner" to be used in various stages
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
// start from a fixed robot state
t.loadRobotModel(node);
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
{
auto& state = scene->getCurrentStateNonConst();
state.setToDefaultValues(state.getJointModelGroup(group), "ready");
auto fixed = std::make_unique<stages::FixedState>("initial state");
fixed->setState(scene);
t.add(std::move(fixed));
}
{
auto stage = std::make_unique<stages::MoveRelative>("x +0.01", cartesian_interpolation);
stage->setGroup(group);
stage->setIKFrame("tool0");
geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world";
direction.vector.x = 0.01;
stage->setDirection(direction);
t.add(std::move(stage));
}
// {
// auto stage = std::make_unique<stages::MoveRelative>("y -0.01", cartesian_interpolation);
// stage->setGroup(group);
// stage->setIKFrame("ur5_tool0");
// geometry_msgs::msg::Vector3Stamped direction;
// direction.header.frame_id = "world";
// direction.vector.y = -0.01;
// stage->setDirection(direction);
// t.add(std::move(stage));
// }
// { // rotate about TCP
// auto stage = std::make_unique<stages::MoveRelative>("rz +45°", cartesian_interpolation);
// stage->setGroup(group);
// stage->setIKFrame("ur5_tool0");
// geometry_msgs::msg::TwistStamped twist;
// twist.header.frame_id = "world";
// twist.twist.angular.z = M_PI / 4.;
// stage->setDirection(twist);
// t.add(std::move(stage));
// }
// { // perform a Cartesian motion, defined as a relative offset in joint space
// auto stage = std::make_unique<stages::MoveRelative>("joint offset", cartesian_interpolation);
// stage->setGroup(group);
// stage->setIKFrame("ur5_tool0");
// std::map<std::string, double> offsets = { { "ur5_wrist_3_joint", M_PI / 6. }, { "ur5_wrist_1_joint", -M_PI / 6 } };
// stage->setDirection(offsets);
// t.add(std::move(stage));
// }
// { // move from reached state back to the original state, using joint interpolation
// stages::Connect::GroupPlannerVector planners = { { group, joint_interpolation } };
// auto connect = std::make_unique<stages::Connect>("connect", planners);
// t.add(std::move(connect));
// }
// { // final state is original state again
// auto fixed = std::make_unique<stages::FixedState>("final state");
// fixed->setState(scene);
// t.add(std::move(fixed));
// }
return t;
}
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("mtc_tutorial");
std::thread spinning_thread([node] { rclcpp::spin(node); });
auto task = createTask(node);
try {
if (task.plan()){
task.introspection().publishSolution(*task.solutions().front());
task.execute(*task.solutions().front());
}
} catch (const InitStageException& ex) {
std::cerr << "planning failed with exception" << std::endl << ex << task;
}
// keep alive for interactive inspection in rviz
spinning_thread.join();
return 0;
}To allow execution, you also need to enable the task execution capability in your move_group launch file:
minku1219
commented
1 week ago Thanks @rhaschke . I'll just give it a try.
minku1219
commented
1 week ago On adding this to the capabilities:-
moveit_config.move_group_capabilities = { "capabilities": """pilz_industrial_motion_planner/MoveGroupSequenceAction \ pilz_industrial_motion_planner/MoveGroupSequenceService \ move_group/ExecuteTaskSolutionCapability""" }
move_group_node = Node( package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
moveit_config.joint_limits,
planning_pipelines_param,
moveit_config.pilz_cartesian_limits,
moveit_config.move_group_capabilities,
moveit_config.trajectory_execution,
moveit_config.planning_scene_monitor,
{"use_sim_time": use_sim_time},
warehouse_ros_config,
],
remappings=remappings,
)Same error encountered:-
[INFO] [cartesian-1]: process started with pid [163881] [cartesian-1] [INFO] [1725961230.361890400] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0540685 seconds [cartesian-1] [INFO] [1725961230.361966498] [moveit_robot_model.robot_model]: Loading robot model 'ur5'... [cartesian-1] [INFO] [1725961230.361975480] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint [cartesian-1] Plan! [cartesian-1] PN6moveit16task_constructor11TaskPrivateE [cartesian-1] Init! [cartesian-1] 1 [cartesian-1] [ERROR] [1725961230.932017126] [moveit_task_constructor_executor_140722062763376]: Failed to connect to the 'execute_task_solution' action server
minku1219
commented
1 week ago Hope this helps!
rhaschke
commented
1 week ago Did you restarted your move_group node?
It should state Loading 'move_group/ExecuteTaskSolutionCapability'... in its startup log.
Please correctly quote code using triple backticks!
minku1219
commented
1 week ago Yes! i started the move_group node.
Complete log is:-
[INFO] [launch]: All log files can be found below /home/tcs/.ros/log/2024-09-10-15-38-09-813656-tcs-Precision-7710-171263
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [dashboard_client-2]: process started with pid [171276]
[INFO] [controller_stopper_node-3]: process started with pid [171278]
[INFO] [ur_ros2_control_node-1]: process started with pid [171274]
[INFO] [urscript_interface-4]: process started with pid [171280]
[INFO] [robot_state_publisher-5]: process started with pid [171282]
[INFO] [spawner-6]: process started with pid [171284]
[INFO] [spawner-7]: process started with pid [171286]
[INFO] [spawner-8]: process started with pid [171288]
[INFO] [spawner-9]: process started with pid [171291]
[INFO] [spawner-10]: process started with pid [171293]
[INFO] [spawner-11]: process started with pid [171296]
[controller_stopper_node-3] [INFO] [1725962890.713165343] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller
[dashboard_client-2] [INFO] [1725962890.717344348] [UR_Client_Library:]: Connected: Universal Robots Dashboard Server
[dashboard_client-2]
[ur_ros2_control_node-1] [WARN] [1725962890.814710877] [ur5.controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ur_ros2_control_node-1] [INFO] [1725962890.815450780] [resource_manager]: Loading hardware 'ur5'
[ur_ros2_control_node-1] [INFO] [1725962890.818591969] [resource_manager]: Initialize hardware 'ur5'
[ur_ros2_control_node-1] [INFO] [1725962890.818695611] [resource_manager]: Successful initialization of hardware 'ur5'
[ur_ros2_control_node-1] [INFO] [1725962890.818991597] [resource_manager]: 'configure' hardware 'ur5'
[ur_ros2_control_node-1] [INFO] [1725962890.819006971] [resource_manager]: Successful 'configure' of hardware 'ur5'
[ur_ros2_control_node-1] [INFO] [1725962890.819055130] [resource_manager]: 'activate' hardware 'ur5'
[ur_ros2_control_node-1] [INFO] [1725962890.819073033] [URPositionHardwareInterface]: Starting ...please wait...
[ur_ros2_control_node-1] [INFO] [1725962890.819092053] [URPositionHardwareInterface]: Initializing driver...
[ur_ros2_control_node-1] [WARN] [1725962890.823825220] [UR_Client_Library:ur5_]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[robot_state_publisher-5] [INFO] [1725962890.831832775] [ur5.robot_state_publisher]: got segment ur5_base
[robot_state_publisher-5] [INFO] [1725962890.832904654] [ur5.robot_state_publisher]: got segment ur5_base_link
[robot_state_publisher-5] [INFO] [1725962890.833714417] [ur5.robot_state_publisher]: got segment ur5_base_link_inertia
[robot_state_publisher-5] [INFO] [1725962890.833744543] [ur5.robot_state_publisher]: got segment ur5_flange
[robot_state_publisher-5] [INFO] [1725962890.833762145] [ur5.robot_state_publisher]: got segment ur5_forearm_link
[robot_state_publisher-5] [INFO] [1725962890.833779817] [ur5.robot_state_publisher]: got segment ur5_ft_frame
[robot_state_publisher-5] [INFO] [1725962890.833795862] [ur5.robot_state_publisher]: got segment ur5_shoulder_link
[robot_state_publisher-5] [INFO] [1725962890.833817815] [ur5.robot_state_publisher]: got segment ur5_tool0
[robot_state_publisher-5] [INFO] [1725962890.833843922] [ur5.robot_state_publisher]: got segment ur5_tool_2f85
[robot_state_publisher-5] [INFO] [1725962890.833860395] [ur5.robot_state_publisher]: got segment ur5_tool_2f85_tactile
[robot_state_publisher-5] [INFO] [1725962890.833875854] [ur5.robot_state_publisher]: got segment ur5_tool_calib
[robot_state_publisher-5] [INFO] [1725962890.833891794] [ur5.robot_state_publisher]: got segment ur5_tool_epick
[robot_state_publisher-5] [INFO] [1725962890.833910550] [ur5.robot_state_publisher]: got segment ur5_upper_arm_link
[robot_state_publisher-5] [INFO] [1725962890.833935394] [ur5.robot_state_publisher]: got segment ur5_wrist_1_link
[robot_state_publisher-5] [INFO] [1725962890.833948184] [ur5.robot_state_publisher]: got segment ur5_wrist_2_link
[robot_state_publisher-5] [INFO] [1725962890.833961460] [ur5.robot_state_publisher]: got segment ur5_wrist_3_link
[robot_state_publisher-5] [INFO] [1725962890.833975286] [ur5.robot_state_publisher]: got segment world
[ur_ros2_control_node-1] [INFO] [1725962891.157190397] [UR_Client_Library:ur5_]: Negotiated RTDE protocol version to 2.
[ur_ros2_control_node-1] [INFO] [1725962891.158036672] [UR_Client_Library:ur5_]: Setting up RTDE communication with frequency 125.000000
[ur_ros2_control_node-1] [WARN] [1725962892.229232770] [UR_Client_Library:ur5_]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead use the RobotReceiveTimeout, to set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-1] [WARN] [1725962892.229276883] [UR_Client_Library:ur5_]: DEPRECATION NOTICE: Setting the keepalive count has been deprecated. Instead you should set the timeout directly in the write commands. Please change your code to set the read timeout in the write commands directly. This keepalive count will overwrite the timeout passed to the write functions.
[ur_ros2_control_node-1] [INFO] [1725962892.229283851] [URPositionHardwareInterface]: Calibration checksum: 'calib_209549117540498681'.
[ur_ros2_control_node-1] [ERROR] [1725962893.268202351] [URPositionHardwareInterface]: The calibration parameters of the connected robot don't match the ones from the given kinematics config file. Please be aware that this can lead to critical inaccuracies of tcp positions. Use the ur_calibration tool to extract the correct calibration from the robot and pass that into the description. See [https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_calibration/README.md] for details.
[ur_ros2_control_node-1] [INFO] [1725962893.268331117] [URPositionHardwareInterface]: System successfully started!
[ur_ros2_control_node-1] [INFO] [1725962893.268347974] [resource_manager]: Successful 'activate' of hardware 'ur5'
[controller_stopper_node-3] [INFO] [1725962893.273142367] [Controller stopper]: Service available
[controller_stopper_node-3] [INFO] [1725962893.273185336] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers
[controller_stopper_node-3] [INFO] [1725962893.273244144] [Controller stopper]: Service available
[ur_ros2_control_node-1] [WARN] [1725962893.275497255] [ur5.controller_manager]: Could not enable FIFO RT scheduling policy
[ur_ros2_control_node-1] [WARN] [1725962893.275722250] [UR_Client_Library:ur5_]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details.
[ur_ros2_control_node-1] [INFO] [1725962893.313235567] [ur5.controller_manager]: Loading controller 'force_torque_sensor_broadcaster'
[ur_ros2_control_node-1] [INFO] [1725962893.329900932] [ur5.controller_manager]: Loading controller 'joint_state_broadcaster'
[spawner-10] [INFO] [1725962893.330395071] [ur5.spawner_force_torque_sensor_broadcaster]: Loaded force_torque_sensor_broadcaster
[ur_ros2_control_node-1] [INFO] [1725962893.345622308] [ur5.controller_manager]: Loading controller 'forward_position_controller'
[spawner-7] [INFO] [1725962893.346379410] [ur5.spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ur_ros2_control_node-1] [INFO] [1725962893.361801500] [ur5.controller_manager]: Configuring controller 'force_torque_sensor_broadcaster'
[spawner-11] [INFO] [1725962893.362597371] [ur5.spawner_forward_position_controller]: Loaded forward_position_controller
[ur_ros2_control_node-1] [INFO] [1725962893.370351286] [ur5.controller_manager]: Configuring controller 'joint_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1725962893.370920490] [ur5.joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[ur_ros2_control_node-1] [INFO] [1725962893.377875210] [ur5.controller_manager]: Configuring controller 'forward_position_controller'
[ur_ros2_control_node-1] [INFO] [1725962893.378578775] [ur5.forward_position_controller]: configure successful
[spawner-10] [INFO] [1725962893.402572370] [ur5.spawner_force_torque_sensor_broadcaster]: Configured and activated force_torque_sensor_broadcaster
[spawner-7] [INFO] [1725962893.418422944] [ur5.spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[ur_ros2_control_node-1] [INFO] [1725962893.437612539] [ur5.controller_manager]: Loading controller 'io_and_status_controller'
[spawner-8] [INFO] [1725962893.450347137] [ur5.spawner_io_and_status_controller]: Loaded io_and_status_controller
[ur_ros2_control_node-1] [INFO] [1725962893.453643389] [ur5.controller_manager]: Loading controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-1] [WARN] [1725962893.466049875] [ur5.scaled_joint_trajectory_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[spawner-6] [INFO] [1725962893.474447668] [ur5.spawner_scaled_joint_trajectory_controller]: Loaded scaled_joint_trajectory_controller
[ur_ros2_control_node-1] [INFO] [1725962893.475258046] [ur5.controller_manager]: Configuring controller 'io_and_status_controller'
[ur_ros2_control_node-1] [INFO] [1725962893.482337164] [ur5.controller_manager]: Configuring controller 'scaled_joint_trajectory_controller'
[ur_ros2_control_node-1] [INFO] [1725962893.482600358] [ur5.scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ur_ros2_control_node-1] [INFO] [1725962893.482646793] [ur5.scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity].
[ur_ros2_control_node-1] [INFO] [1725962893.482672625] [ur5.scaled_joint_trajectory_controller]: Using 'splines' interpolation method.
[ur_ros2_control_node-1] [INFO] [1725962893.483497226] [ur5.scaled_joint_trajectory_controller]: Controller state will be published at 100.00 Hz.
[ur_ros2_control_node-1] [INFO] [1725962893.486966568] [ur5.scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
[spawner-8] [INFO] [1725962893.514472684] [ur5.spawner_io_and_status_controller]: Configured and activated io_and_status_controller
[ur_ros2_control_node-1] [INFO] [1725962893.515043549] [ur5.controller_manager]: Loading controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1725962893.521800134] [ur5.speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: ur5_
[spawner-9] [INFO] [1725962893.531656981] [ur5.spawner_speed_scaling_state_broadcaster]: Loaded speed_scaling_state_broadcaster
[ur_ros2_control_node-1] [INFO] [1725962893.546758157] [ur5.controller_manager]: Configuring controller 'speed_scaling_state_broadcaster'
[ur_ros2_control_node-1] [INFO] [1725962893.546847898] [ur5.speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz
[spawner-6] [INFO] [1725962893.546846605] [ur5.spawner_scaled_joint_trajectory_controller]: Configured and activated scaled_joint_trajectory_controller
[INFO] [spawner-11]: process has finished cleanly [pid 171296]
[spawner-9] [INFO] [1725962893.585889028] [ur5.spawner_speed_scaling_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster
[INFO] [spawner-10]: process has finished cleanly [pid 171293]
[INFO] [spawner-7]: process has finished cleanly [pid 171286]
WARNING:root:Cannot infer URDF from `/opt/ros/humble/share/ur_moveit_config`. -- using config/ur.urdf
WARNING:root:Cannot infer SRDF from `/opt/ros/humble/share/ur_moveit_config`. -- using config/ur.srdf
[INFO] [spawner-6]: process has finished cleanly [pid 171284]
[INFO] [spawner-8]: process has finished cleanly [pid 171288]
[INFO] [spawner-9]: process has finished cleanly [pid 171291]
[INFO] [move_group-12]: process started with pid [171461]
[INFO] [rviz2-13]: process started with pid [171463]
[INFO] [servo_node_main-14]: process started with pid [171465]
[servo_node_main-14] [WARN] [1725962894.346589768] [moveit_servo.servo_node]: Intra-process communication is disabled, consider enabling it by adding:
[servo_node_main-14] extra_arguments=[{'use_intra_process_comms' : True}]
[servo_node_main-14] to the Servo composable node in the launch file
[move_group-12] [WARN] [1725962894.351178763] [move_group.move_group]: Falling back to using the the move_group node namespace (deprecated behavior).
[servo_node_main-14] [INFO] [1725962894.411957245] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.063054 seconds
[servo_node_main-14] [INFO] [1725962894.412028818] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[servo_node_main-14] [INFO] [1725962894.412037766] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[move_group-12] [INFO] [1725962894.418232194] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0663944 seconds
[move_group-12] [INFO] [1725962894.418256988] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[move_group-12] [INFO] [1725962894.418279679] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[servo_node_main-14] [INFO] [1725962894.479002947] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'ur5/joint_states'
[servo_node_main-14] [INFO] [1725962894.483241508] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[servo_node_main-14] [INFO] [1725962894.483268827] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[servo_node_main-14] [INFO] [1725962894.485628189] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-12] [INFO] [1725962894.487416609] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-12] [INFO] [1725962894.487609535] [moveit.ros_planning_interface.moveit_cpp]: Listening to '/ur5/joint_states' for joint states
[servo_node_main-14] [INFO] [1725962894.487751485] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on '/servo_node/publish_planning_scene'
[move_group-12] [INFO] [1725962894.488678663] [moveit_ros.current_state_monitor]: Listening to joint states on topic '/ur5/joint_states'
[move_group-12] [INFO] [1725962894.489620686] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[move_group-12] [INFO] [1725962894.489642957] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-12] [INFO] [1725962894.491240266] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[move_group-12] [INFO] [1725962894.491264560] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-12] [INFO] [1725962894.492045604] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-12] [INFO] [1725962894.493480300] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry
[move_group-12] [WARN] [1725962894.495509759] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-12] [ERROR] [1725962894.495552099] [moveit.ros.occupancy_map_monitor.middleware_handle]: No 3D sensor plugin(s) defined for octomap updates
[move_group-12] [INFO] [1725962894.499008044] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'move_group'
[move_group-12] [INFO] [1725962894.513765712] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-12] [INFO] [1725962894.520307042] [moveit.pilz_industrial_motion_planner]: Available plugins: pilz_industrial_motion_planner/PlanningContextLoaderCIRC pilz_industrial_motion_planner/PlanningContextLoaderLIN pilz_industrial_motion_planner/PlanningContextLoaderPTP
[move_group-12] [INFO] [1725962894.520328990] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderCIRC
[move_group-12] [INFO] [1725962894.522974936] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [CIRC]
[move_group-12] [INFO] [1725962894.523002379] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderLIN
[move_group-12] [INFO] [1725962894.524744156] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [LIN]
[move_group-12] [INFO] [1725962894.524769688] [moveit.pilz_industrial_motion_planner]: About to load: pilz_industrial_motion_planner/PlanningContextLoaderPTP
[move_group-12] [INFO] [1725962894.526653695] [moveit.pilz_industrial_motion_planner]: Registered Algorithm [PTP]
[move_group-12] [INFO] [1725962894.526680275] [moveit.ros_planning.planning_pipeline]: Using planning interface 'Pilz Industrial Motion Planner'
[move_group-12] [INFO] [1725962894.562104038] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for /ur5/scaled_joint_trajectory_controller
[move_group-12] [INFO] [1725962894.564515865] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for /ur5/joint_trajectory_controller
[move_group-12] [INFO] [1725962894.564708294] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-12] [INFO] [1725962894.564745228] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-12] [INFO] [1725962894.566090475] [moveit_ros.trajectory_execution_manager]: Trajectory execution is managing controllers
[move_group-12] [INFO] [1725962894.566119264] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-12] [ERROR] [1725962894.576845430] [move_group.move_group]: Exception while loading move_group capability 'move_group/ExecuteTaskSolutionCapability': According to the loaded plugin descriptions the class move_group/ExecuteTaskSolutionCapability with base class type move_group::MoveGroupCapability does not exist. Declared types are move_group/ApplyPlanningSceneService move_group/ClearOctomapService move_group/MoveGroupCartesianPathService move_group/MoveGroupExecuteService move_group/MoveGroupExecuteTrajectoryAction move_group/MoveGroupGetPlanningSceneService move_group/MoveGroupKinematicsService move_group/MoveGroupMoveAction move_group/MoveGroupPlanService move_group/MoveGroupQueryPlannersService move_group/MoveGroupStateValidationService move_group/TfPublisher pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService
[move_group-12] [INFO] [1725962894.603066962] [moveit.pilz_industrial_motion_planner.move_group_sequence_action]: initialize move group sequence action
[move_group-12] [INFO] [1725962894.609238550] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-12] [WARN] [1725962894.609378427] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.609515480] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.609568612] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.609635603] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.609679226] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.609738489] [move_group]: Failed loading deceleration limits
[move_group-12] [INFO] [1725962894.609850638] [moveit.pilz_industrial_motion_planner.joint_limits_aggregator]: Reading limits from namespace robot_description_planning
[move_group-12] [WARN] [1725962894.609907575] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.609956319] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.610003688] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.610050187] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.610091740] [move_group]: Failed loading deceleration limits
[move_group-12] [WARN] [1725962894.610136882] [move_group]: Failed loading deceleration limits
[move_group-12] [INFO] [1725962894.613251766] [move_group.move_group]:
[move_group-12]
[move_group-12] ********************************************************
[move_group-12] * MoveGroup using:
[move_group-12] * - ApplyPlanningSceneService
[move_group-12] * - ClearOctomapService
[move_group-12] * - CartesianPathService
[move_group-12] * - ExecuteTrajectoryAction
[move_group-12] * - GetPlanningSceneService
[move_group-12] * - KinematicsService
[move_group-12] * - MoveAction
[move_group-12] * - MotionPlanService
[move_group-12] * - QueryPlannersService
[move_group-12] * - StateValidationService
[move_group-12] * - SequenceAction
[move_group-12] * - SequenceService
[move_group-12] ********************************************************
[move_group-12]
[move_group-12] [INFO] [1725962894.613306311] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin pilz_industrial_motion_planner/CommandPlanner
[move_group-12] [INFO] [1725962894.613322518] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-12] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-12] Loading 'move_group/ClearOctomapService'...
[move_group-12] Loading 'move_group/ExecuteTaskSolutionCapability'...
[move_group-12] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-12] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-12] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-12] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-12] Loading 'move_group/MoveGroupMoveAction'...
[move_group-12] Loading 'move_group/MoveGroupPlanService'...
[move_group-12] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-12] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-12] Loading 'pilz_industrial_motion_planner/MoveGroupSequenceAction'...
[move_group-12] Loading 'pilz_industrial_motion_planner/MoveGroupSequenceService'...
[move_group-12]
[move_group-12] You can start planning now!
[move_group-12]
[rviz2-13] [INFO] [1725962895.214873028] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-13] [INFO] [1725962895.214948578] [rviz2]: OpenGl version: 4.6 (GLSL 4.6)
[rviz2-13] [INFO] [1725962895.248468133] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-13] 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-13] at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[rviz2-13] [ERROR] [1725962898.459572632] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-13] [INFO] [1725962898.475399969] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-13] [INFO] [1725962898.604162137] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0501723 seconds
[rviz2-13] [INFO] [1725962898.604245554] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[rviz2-13] [INFO] [1725962898.604265880] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-13] [INFO] [1725962898.689412557] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-13] [INFO] [1725962898.691541818] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-13] [INFO] [1725962898.928957037] [interactive_marker_display_102174492830784]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-13] [INFO] [1725962898.936663561] [moveit_ros_visualization.motion_planning_frame]: group ur5_manipulator
[rviz2-13] [INFO] [1725962898.936687868] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'ur5_manipulator' in namespace ''
[rviz2-13] [INFO] [1725962898.947253660] [interactive_marker_display_102174492830784]: Sending request for interactive markers
[rviz2-13] [INFO] [1725962898.954637116] [move_group_interface]: Ready to take commands for planning group ur5_manipulator.
[rviz2-13] [INFO] [1725962898.981410285] [interactive_marker_display_102174492830784]: Service response received for initialization
WARNING:root:Cannot infer URDF from `/opt/ros/humble/share/ur_moveit_config`. -- using config/ur.urdf
WARNING:root:Cannot infer SRDF from `/opt/ros/humble/share/ur_moveit_config`. -- using config/ur.srdf
[INFO] [robot_control-15]: process started with pid [171544]
[robot_control-15] [INFO] [1725962901.082688493] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[robot_control-15] [INFO] [1725962901.082741438] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[robot_control-15] [INFO] [1725962901.082751023] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[robot_control-15] [INFO] [1725962901.119212286] [move_group_interface]: Ready to take commands for planning group ur5_manipulator.
[robot_control-15] [INFO] [1725962901.119919361] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[robot_control-15] [INFO] [1725962901.125733270] [xrm_robot_node]: Reference frame: world
[robot_control-15] [INFO] [1725962901.125854049] [xrm_robot_node]: End effector link: ur5_tool0The log states that the capability is correctly loaded.
minku1219
commented
1 week ago I think you should notice this in the log
[move_group-12] [ERROR] [1725963244.479810848] [move_group.move_group]: Exception while loading move_group capability 'move_group/ExecuteTaskSolutionCapability': According to the loaded plugin descriptions the class move_group/ExecuteTaskSolutionCapability with base class type move_group::MoveGroupCapability does not exist. Declared types are move_group/ApplyPlanningSceneService move_group/ClearOctomapService move_group/MoveGroupCartesianPathService move_group/MoveGroupExecuteService move_group/MoveGroupExecuteTrajectoryAction move_group/MoveGroupGetPlanningSceneService move_group/MoveGroupKinematicsService move_group/MoveGroupMoveAction move_group/MoveGroupPlanService move_group/MoveGroupQueryPlannersService move_group/MoveGroupStateValidationService move_group/TfPublisher pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceServiceIn this case, you probably didn't sourced your MTC workspace correctly.
minku1219
commented
1 week ago Ah! Now i got the issue. thanks for your valuable and prompt response!
minku1219
commented
1 week ago Upon calling the execution, I had an error in Robot node side:-
[move_group-12] [WARN] [1725963972.080310394] [moveit_task_constructor_visualization.execute_task_solution]: The trajectory of stage '3' from task '' does not have any controllers specified for trajectory execution. This might lead to unexpected controller selection.
[move_group-12] [INFO] [1725963972.080372726] [moveit_task_constructor_visualization.execute_task_solution]: Executing TaskSolution
[move_group-12] [INFO] [1725963972.080424543] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-12] [INFO] [1725963972.080456608] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-12] [INFO] [1725963972.080560695] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-12] [INFO] [1725963972.084257908] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-12] [INFO] [1725963972.084367379] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-12] [INFO] [1725963972.084410606] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-12] [INFO] [1725963972.084949653] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-12] [INFO] [1725963972.084991267] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-12] [INFO] [1725963972.085282172] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to /ur5/scaled_joint_trajectory_controller
[ur_ros2_control_node-1] [INFO] [1725963972.085649461] [ur5.scaled_joint_trajectory_controller]: Received new action goal
[ur_ros2_control_node-1] [INFO] [1725963972.085702989] [ur5.scaled_joint_trajectory_controller]: Accepted new action goal
[move_group-12] [INFO] [1725963972.085841741] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: /ur5/scaled_joint_trajectory_controller started execution
[move_group-12] [INFO] [1725963972.085899744] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[ur_ros2_control_node-1] [WARN] [1725963972.091499784] [ur5.scaled_joint_trajectory_controller]: Aborted due to state tolerance violation
[move_group-12] [WARN] [1725963972.136140538] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller '/ur5/scaled_joint_trajectory_controller' failed with error PATH_TOLERANCE_VIOLATED:
[move_group-12] [WARN] [1725963972.136223351] [moveit_ros.trajectory_execution_manager]: Controller handle /ur5/scaled_joint_trajectory_controller reports status ABORTED
[move_group-12] [INFO] [1725963972.136258886] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status ABORTED ...Log in cartesian executable side:-
[cartesian-1] [INFO] [1725963972.010288576] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.055288 seconds
[cartesian-1] [INFO] [1725963972.010385900] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[cartesian-1] [INFO] [1725963972.010397927] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[cartesian-1] Plan!
[cartesian-1] PN6moveit16task_constructor11TaskPrivateE
[cartesian-1] Init!
[cartesian-1] 1
[cartesian-1] [ERROR] [1725963972.144757040] [moveit_task_constructor_executor_140733679541664]: Goal was aborted or canceled
This is obviously a controller issue. Maybe googling the error message helps, e.g. https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/issues/1072
minku1219
commented
1 week ago Do you have any script which we can use to run any example of MTC with UR. Since i am modifying the cartesian.cpp, i think there must be an script which can directly be used with UR
rhaschke
commented
1 week ago I don't have a UR available and thus cannot provide a script that is tested on a UR.
MoveIt and MTC are essentially robot-agnostic. They run on any robot as long as the robot controller satisfies the required API. Check the example_move.py script suggested in the above link.
minku1219
commented
1 week ago Sure! Thanks.
I just wanted to know how we can set the initial state as the current state in the scripts?
// start from a fixed robot state
t.loadRobotModel(node);
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
{
auto& state = scene->getCurrentState();
// state.setToDefaultValues(state.getJointModelGroup(group), "ready");
auto fixed = std::make_unique<stages::FixedState>("initial state");
fixed->setState(scene);
t.add(std::move(fixed));
}I want my robot to start execution from current state instead of the fixed state. On debugging i came to know that it is starting from a fixed robot state which is causing the path tolerance issue
minku1219
commented
1 week ago As in moveit API we have a direct function i.e. setStartStateToCurrentState().
rhaschke
commented
1 week ago See the docs or have a look at another example: https://github.com/moveit/moveit_task_constructor/blob/5a4480814628db4207d9734d63dd907619ced93b/demo/src/modular.cpp#L101
minku1219
commented
6 days ago Hi, Nothing is available in the docs. I tried this but it did'nt worked for me.
minku1219
commented
6 days ago Error Log after using it.
[modular-1] [INFO] [1726036561.404413082] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0772726 seconds
[modular-1] [INFO] [1726036561.404535399] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[modular-1] [INFO] [1726036561.404546615] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[modular-1] Plan!
[modular-1] PN6moveit16task_constructor11TaskPrivateE
[modular-1] Init!
[modular-1] [WARN] [1726036564.470796678] [CurrentState]: failed to acquire current PlanningScene
[modular-1] 0 - ← 0 → - 0 / Reusable Containers
[modular-1] 0 - ← 0 → - 0 / current
[modular-1] - 0 → 0 → - 0 / Cartesian Path
[modular-1] - 0 → 0 → - 0 / x +0.2
[modular-1] Failing stage(s):
[modular-1] -1
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[modular-1] [INFO] [1726036572.663836270] [rclcpp]: signal_handler(signum=2)
[INFO] [modular-1]: process has finished cleanly [pid 30914]Another Log with cartesian example:-
[cartesian-1] [INFO] [1726036800.764109858] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0543219 seconds
[cartesian-1] [INFO] [1726036800.764200250] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[cartesian-1] [INFO] [1726036800.764226708] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[cartesian-1] Plan!
[cartesian-1] PN6moveit16task_constructor11TaskPrivateE
[cartesian-1] Init!
[cartesian-1] planning failed with exception
[cartesian-1] Error initializing stage:
[cartesian-1] Cartesian Path: cannot connect end interface of 'current' (→) to start interface of 'initial state' (←)
[cartesian-1] 0 - ← 0 → - 0 / Cartesian Path
[cartesian-1] 0 - ← 0 → - - / current
[cartesian-1] - - ← 0 → - 0 / initial state
[cartesian-1] - 0 → 0 → - 0 / x +0.2Cartesian Example code :-
Task createTask(const rclcpp::Node::SharedPtr& node) {
Task t;
t.stages()->setName("Cartesian Path");
const std::string group = "ur5_manipulator";
const std::string eef = "ur5_tool0";
// create Cartesian interpolation "planner" to be used in various stages
auto cartesian_interpolation = std::make_shared<solvers::CartesianPath>();
// create a joint-space interpolation "planner" to be used in various stages
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
// start from a fixed robot state
t.loadRobotModel(node);
t.add(std::make_unique<stages::CurrentState>("current"));
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
{
auto& state = scene->getCurrentStateNonConst();
state.setToDefaultValues(state.getJointModelGroup(group), "ready");
auto fixed = std::make_unique<stages::FixedState>("initial state");
fixed->setState(scene);
t.add(std::move(fixed));
}
{
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian_interpolation);
stage->setGroup(group);
stage->setIKFrame("ur5_tool0");
geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world";
direction.vector.x = 0.02;
stage->setDirection(direction);
t.add(std::move(stage));
}
return t;
}
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("mtc_tutorial");
std::thread spinning_thread([node] { rclcpp::spin(node); });
auto task = createTask(node);
try {
if (task.plan()){
task.introspection().publishSolution(*task.solutions().front());
task.execute(*task.solutions().front());
}
} catch (const InitStageException& ex) {
std::cerr << "planning failed with exception" << std::endl << ex << task;
}
// keep alive for interactive inspection in rviz
spinning_thread.join();
return 0;
}Modular.cpp code :-
std::unique_ptr<SerialContainer> createModule(const std::string& group) {
auto c = std::make_unique<SerialContainer>("Cartesian Path");
c->setProperty("group", group);
// create Cartesian interpolation "planner" to be used in stages
auto cartesian = std::make_shared<solvers::CartesianPath>();
// create joint interpolation "planner"
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();
{
auto stage = std::make_unique<stages::MoveRelative>("x +0.2", cartesian);
stage->properties().configureInitFrom(Stage::PARENT, { "group" });
stage->setIKFrame("ur5_tool0");
geometry_msgs::msg::Vector3Stamped direction;
direction.header.frame_id = "world";
direction.vector.x = 0.2;
stage->setDirection(direction);
c->insert(std::move(stage));
}
return c;
}
Task createTask(const rclcpp::Node::SharedPtr& node) {
Task t;
t.loadRobotModel(node);
t.stages()->setName("Reusable Containers");
t.add(std::make_unique<stages::CurrentState>("current"));
const std::string group = "ur5_manipulator";
t.add(createModule(group));
return t;
}
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("mtc_tutorial");
std::thread spinning_thread([node] { rclcpp::spin(node); });
auto task = createTask(node);
try {
if (task.plan()){
task.introspection().publishSolution(*task.solutions().front());
task.execute(*task.solutions().front());
}
} catch (const InitStageException& ex) {
std::cerr << "planning failed with exception" << std::endl << ex << task;
}
// keep alive for interactive inspection in rviz
spinning_thread.join();
return 0;
}Hi @rhaschke,
Thank you for your support. I managed to get this to work with my UR5. I will be now working more precisely with Pick Place demo.
minku1219
commented
5 days ago HI @rhaschke ,
While going through the demo examples, i encountered with an issue.
Code Snippet:-
{ // move back to ready pose
auto stage = std::make_unique<stages::MoveTo>("moveTo ready", joint_interpolation);
stage->setIKFrame("ur5_tool0");
stage->properties().configureInitFrom(Stage::PARENT);
stage->setGoal("ready");
c->insert(std::move(stage));
}Error log:-
[ur_test-1] [INFO] [1726119849.361650285] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0595334 seconds
[ur_test-1] [INFO] [1726119849.361747493] [moveit_robot_model.robot_model]: Loading robot model 'ur5'...
[ur_test-1] [INFO] [1726119849.361767808] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ur_test-1] Plan!
[ur_test-1] PN6moveit16task_constructor11TaskPrivateE
[ur_test-1] Init!
[ur_test-1] planning failed with exception
[ur_test-1] Error initializing stage:
[ur_test-1] moveTo ready: Unknown joint pose: readyIt seems that the joint goal with name "ready" is not available in the API. Could you please help me to how to assign these values in the code?
minku1219
commented
17 hours ago Thanks @rhaschke for the response. Sorry for the delay in response from my side, I was occupied with something else.
Hi, I am working upon MTC for using it with UR5. I tried using cartesian executable wihth UR5 but it did'nt worked and showed me an error.
Error:-
[cartesian-1] Failing stage(s): [cartesian-1] x +0.2 (0/1): missing ik_frame
Full log message :-
[INFO] [launch]: All log files can be found below /home/tcs/.ros/log/2024-09-10-12-00-02-734367-tcs-Precision-7710-68104 [INFO] [launch]: Default logging verbosity is set to INFO WARNING:root:Cannot infer URDF from
/opt/ros/humble/share/ur_moveit_config. -- using config/ur.urdf WARNING:root:Cannot infer SRDF from/opt/ros/humble/share/ur_moveit_config. -- using config/ur.srdf [INFO] [cartesian-1]: process started with pid [68105] [cartesian-1] [INFO] [1725949803.501420331] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0461322 seconds [cartesian-1] [INFO] [1725949803.501527067] [moveit_robot_model.robot_model]: Loading robot model 'ur5'... [cartesian-1] [INFO] [1725949803.501541152] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint [cartesian-1] Plan! [cartesian-1] PN6moveit16task_constructor11TaskPrivateE [cartesian-1] Init! [cartesian-1] 0 - ← 0 → - 0 / Cartesian Path [cartesian-1] 1 - ← 1 → - 0 / initial state [cartesian-1] - 0 → 0 → - 0 / x +0.2 [cartesian-1] Failing stage(s): [cartesian-1] x +0.2 (0/1): missing ik_frame [cartesian-1] -1 ^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)[INFO] [cartesian-1]: process has finished cleanly [pid 68105]