Unable to use MTC with UR5 #612
Comments
|
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 |
|
Could you please elaborate where and how to add this setIkFrame? |
|
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. |
|
How can I provide help if you don't specify the exact error you are observing? |
|
Actually it is not showing any error, instead the robot i am working with is not moving. |
|
I have'nt found any example for working with UR5 or any UR manipulator, so i am trying this on my own |
The original code of moveit_task_constructor/demo/src/cartesian.cpp Lines 142 to 143 in 5a44808 You need to execute it for the robot to move: task.execute(*task.solutions().front());
|
|
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 |
|
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 |
|
Thanks @rhaschke . |
|
On adding this to the capabilities:- moveit_config.move_group_capabilities = { move_group_node = Node( Same error encountered:- [INFO] [cartesian-1]: process started with pid [163881] |
|
Hope this helps! |
|
Did you restarted your |
|
Yes! i started the move_group node. Complete log is:- |
|
The log states that the capability is correctly loaded. |
|
I think you should notice this in the log |
|
In this case, you probably didn't sourced your MTC workspace correctly. |
|
Ah! Now i got the issue. |
|
Upon calling the execution, I had an error in Robot node side:- Log in cartesian executable side:- |
|
This is obviously a controller issue. Maybe googling the error message helps, e.g. |
|
Do you have any script which we can use to run any example of MTC with UR. |
|
I don't have a UR available and thus cannot provide a script that is tested on a UR. |
|
Sure! Thanks. I just wanted to know how we can set the initial state as the current state in the scripts? I want my robot to start execution from current state instead of the fixed state. |
|
As in moveit API we have a direct function i.e. setStartStateToCurrentState(). |
|
See the docs or have a look at another example: moveit_task_constructor/demo/src/modular.cpp Line 101 in 5a44808 |
|
Hi, Nothing is available in the docs. |
|
Error Log after using it. |
|
Another Log with cartesian example:- |
|
Cartesian Example code :- |
|
Modular.cpp code :- |
|
Hi @rhaschke, Thank you for your support. |
|
HI @rhaschke , While going through the demo examples, i encountered with an issue. Code Snippet:- Error log:- It seems that the joint goal with name "ready" is not available in the API. |
|
Named poses, like |
|
Thanks @rhaschke for the response. |
|
Hi @rhaschke code snippet I am using :- This is the snippet I am trying to make use of. Log output for the reference :- Hope to see your response ASAP. |
|
Please state a concrete question. Skimming through the log, I see the error When reading the logs and your code, you should have been able to come to that conclusion yourself. |
|
I apologize for the oversight and will refrain from using GitHub issues for usage support in the future. Thanks again for your help! |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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.urdfWARNING: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)
[cartesian-1] [INFO] [1725949815.137223296] [rclcpp]: signal_handler(signum=2)
[INFO] [cartesian-1]: process has finished cleanly [pid 68105]
The text was updated successfully, but these errors were encountered: