Open mitsav01 opened 2 months ago
It would be helpful to see the console log of both the move_group node and the MTC node. Probably, there you can find a hint to the underlying problem. I guess, that you are missing the task execution capability, which needs to be configured in the move_group node.
These are console logs for move_group node and cartesian. I have already added task execution capabilities in move_group node, but it is not working on real robot. Also, it is not taking current position of robot into account
This log is incomplete. Please don't send images, but paste text console output.
Also, it is not taking current position of robot into account
What exactly do you mean by this?
Since initialization, the actual position of the robot is different then one given in the planning.
Here is console output:
ros2 launch franka_tc demo.launch.py robot_ip:=192.168.101.116 load_gripper:=true [INFO] [launch]: All log files can be found below /home/mitesh/.ros/log/2024-08-19-11-21-15-970291-mitesh-IAI-42926 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [rviz2-1]: process started with pid [42929] [INFO] [cartesian-2]: process started with pid [42930] [INFO] [robot_state_publisher-3]: process started with pid [42931] [INFO] [move_group-4]: process started with pid [42932] [INFO] [ros2_control_node-5]: process started with pid [42933] [INFO] [joint_state_publisher-6]: process started with pid [42934] [INFO] [franka_gripper_node-7]: process started with pid [42935] [INFO] [ros2 run controller_manager spawner panda_arm_controller-8]: process started with pid [42936] [INFO] [ros2 run controller_manager spawner joint_state_broadcaster-9]: process started with pid [42937] [robot_state_publisher-3] [INFO] [1724059276.379871172] [robot_state_publisher]: Robot initialized [franka_gripper_node-7] [INFO] [1724059276.437889866] [panda_gripper]: Trying to establish a connection with the gripper [franka_gripper_node-7] [INFO] [1724059276.444175165] [panda_gripper]: Connected to gripper [move_group-4] [INFO] [1724059276.477479574] [move_group.moveit.moveit.ros.rdf_loader]: Loaded robot model in 0.00745191 seconds [move_group-4] [INFO] [1724059276.477555569] [move_group.moveit.moveit.core.robot_model]: Loading robot model 'panda'... [move_group-4] [WARN] [1724059276.477663824] [move_group.moveit.moveit.core.robot_model]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [move_group-4] [WARN] [1724059276.477684633] [move_group.moveit.moveit.core.robot_model]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [move_group-4] [INFO] [1724059276.495982026] [move_group.moveit.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1 [ros2_control_node-5] [INFO] [1724059276.498435837] [controller_manager]: Subscribing to '/robot_description' topic for robot description. [ros2_control_node-5] [INFO] [1724059276.498505734] [controller_manager]: update rate is 200 Hz [ros2_control_node-5] [INFO] [1724059276.502331480] [controller_manager]: Received robot description from topic. [ros2_control_node-5] [INFO] [1724059276.510686228] [controller_manager.resource_manager]: Loading hardware 'FrankaHardwareInterface' [ros2_control_node-5] [INFO] [1724059276.530796734] [controller_manager.resource_manager]: Loaded hardware 'FrankaHardwareInterface' from plugin 'franka_hardware/FrankaHardwareInterface' [ros2_control_node-5] [INFO] [1724059276.530846141] [controller_manager.resource_manager]: Initialize hardware 'FrankaHardwareInterface' [ros2_control_node-5] [INFO] [1724059276.530875746] [FrankaHardwareInterface]: Connecting to robot at "192.168.101.116" ... [ros2_control_node-5] [INFO] [1724059276.553090993] [FrankaHardwareInterface]: Successfully connected to robot [move_group-4] [INFO] [1724059276.555724603] [move_group.moveit.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' [move_group-4] [INFO] [1724059276.555875984] [move_group.moveit.moveit.ros.moveit_cpp]: Listening to 'joint_states' for joint states [move_group-4] [INFO] [1724059276.559186211] [move_group.moveit.moveit.ros.current_state_monitor]: Listening to joint states on topic 'joint_states' [move_group-4] [INFO] [1724059276.560836120] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects [move_group-4] [INFO] [1724059276.560862203] [move_group.moveit.moveit.ros.planning_scene_monitor]: Stopping existing planning scene publisher. [move_group-4] [INFO] [1724059276.561701620] [move_group.moveit.moveit.ros.planning_scene_monitor]: Stopped publishing maintained planning scene. [move_group-4] [INFO] [1724059276.569656907] [move_group.moveit.moveit.ros.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene' [move_group-4] [INFO] [1724059276.569974635] [move_group.moveit.moveit.ros.planning_scene_monitor]: Starting planning scene monitor [move_group-4] [INFO] [1724059276.575738289] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to '/planning_scene' [move_group-4] [INFO] [1724059276.575765150] [move_group.moveit.moveit.ros.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. [move_group-4] [INFO] [1724059276.579414704] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to 'collision_object' [move_group-4] [INFO] [1724059276.581646104] [move_group.moveit.moveit.ros.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene world geometry [ros2_control_node-5] [INFO] [1724059276.584527605] [error_recovery_service_server]: Error recovery service started [move_group-4] [WARN] [1724059276.587624168] [move_group.moveit.moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead [move_group-4] [ERROR] [1724059276.587657058] [move_group.moveit.moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates [ros2_control_node-5] [INFO] [1724059276.608000151] [param_service_server]: Param service started [cartesian-2] [WARN] [1724059276.612492502] [rcl.logging_rosout]: Publisher already registered for node name: 'mtc_tutorial'. If this is due to multiple nodes with the same name then all logs for the logger named 'mtc_tutorial' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. [move_group-4] [INFO] [1724059276.640428494] [move_group.moveit.moveit.ros.planning_pipeline]: Successfully loaded planner 'OMPL' [move_group-4] [INFO] [1724059276.644393439] [move_group]: Try loading adapter 'default_planning_request_adapters/ResolveConstraintFrames' [move_group-4] [INFO] [1724059276.646326976] [move_group]: Loaded adapter 'default_planning_request_adapters/ResolveConstraintFrames' [move_group-4] [INFO] [1724059276.646356228] [move_group]: Try loading adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' [move_group-4] [INFO] [1724059276.647738611] [move_group]: Loaded adapter 'default_planning_request_adapters/ValidateWorkspaceBounds' [move_group-4] [INFO] [1724059276.647760330] [move_group]: Try loading adapter 'default_planning_request_adapters/CheckStartStateBounds' [move_group-4] [INFO] [1724059276.647823611] [move_group]: Loaded adapter 'default_planning_request_adapters/CheckStartStateBounds' [move_group-4] [INFO] [1724059276.647832972] [move_group]: Try loading adapter 'default_planning_request_adapters/CheckStartStateCollision' [move_group-4] [INFO] [1724059276.647850327] [move_group]: Loaded adapter 'default_planning_request_adapters/CheckStartStateCollision' [move_group-4] [INFO] [1724059276.657265714] [move_group]: Try loading adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' [cartesian-2] [WARN] [1724059276.661618949] [rcl.logging_rosout]: Publisher already registered for node name: 'mtc_tutorial'. If this is due to multiple nodes with the same name then all logs for the logger named 'mtc_tutorial' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. [move_group-4] [INFO] [1724059276.668426062] [move_group]: Loaded adapter 'default_planning_response_adapters/AddTimeOptimalParameterization' [move_group-4] [INFO] [1724059276.668451647] [move_group]: Try loading adapter 'default_planning_response_adapters/ValidateSolution' [move_group-4] [INFO] [1724059276.673267507] [move_group]: Loaded adapter 'default_planning_response_adapters/ValidateSolution' [move_group-4] [INFO] [1724059276.673291049] [move_group]: Try loading adapter 'default_planning_response_adapters/DisplayMotionPath' [move_group-4] [INFO] [1724059276.676271798] [move_group]: Loaded adapter 'default_planning_response_adapters/DisplayMotionPath' [cartesian-2] [INFO] [1724059276.695401943] [mtc_tutorial.moveit.ros.rdf_loader]: Loaded robot model in 0.0146848 seconds [cartesian-2] [INFO] [1724059276.695467678] [mtc_tutorial.moveit.core.robot_model]: Loading robot model 'panda'... [cartesian-2] [WARN] [1724059276.695557872] [mtc_tutorial.moveit.core.robot_model]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [cartesian-2] [WARN] [1724059276.695575872] [mtc_tutorial.moveit.core.robot_model]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [ros2_control_node-5] [INFO] [1724059276.709966609] [controller_manager.resource_manager]: Successful initialization of hardware 'FrankaHardwareInterface' [ros2_control_node-5] [INFO] [1724059276.710150169] [resource_manager]: 'configure' hardware 'FrankaHardwareInterface'
[ros2_control_node-5] [INFO] [1724059276.710172448] [resource_manager]: 'activate' hardware 'FrankaHardwareInterface'
[ros2_control_node-5] [INFO] [1724059276.710276789] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... [ros2_control_node-5] Initializing continuous reading [cartesian-2] [INFO] [1724059276.730142962] [mtc_tutorial.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1 [cartesian-2] [WARN] [1724059276.738679807] [Properties]: Unregistered property type: std::map<std::cxx11::basic_string<char, std::char_traits
, std::allocator cxx11::basic_string<char, std::char_traits>, double, std::less<std:: , std::allocator > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits , std::allocator > const, double> > > [ros2_control_node-5] [INFO] [1724059276.751804732] [controller_manager]: Received robot description from topic. [ros2_control_node-5] [WARN] [1724059276.751833569] [controller_manager]: ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description. [move_group-4] [INFO] [1724059276.826192152] [move_group.moveit.moveit.plugins.simple_controller_manager]: Added FollowJointTrajectory controller for panda_arm_controller [move_group-4] [INFO] [1724059276.826263987] [move_group.moveit.moveit.plugins.simple_controller_manager]: Max effort set to 0.0 [move_group-4] [INFO] [1724059276.831901427] [move_group.moveit.moveit.plugins.simple_controller_manager]: Added GripperCommand controller for panda_hand_controller [move_group-4] [INFO] [1724059276.832100366] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1724059276.832141884] [move_group.moveit.moveit.plugins.simple_controller_manager]: Returned 2 controllers in list [move_group-4] [INFO] [1724059276.833567836] [move_group.moveit.moveit.ros.trajectory_execution_manager]: Trajectory execution is managing controllers [move_group-4] [INFO] [1724059276.833592130] [move_group]: MoveGroup debug mode is ON [move_group-4] [INFO] [1724059276.911270826] [move_group.moveit.moveit.ros.move_group.executable]: [move_group-4] [move_group-4] **** [move_group-4] MoveGroup using: [move_group-4] - apply_planning_scene_service [move_group-4] - clear_octomap_service [move_group-4] - ExecuteTaskSolution [move_group-4] - get_group_urdf [move_group-4] - CartesianPathService [move_group-4] - execute_trajectory_action [move_group-4] - get_planning_scene_service [move_group-4] - kinematics_service [move_group-4] - move_action [move_group-4] - motion_plan_service [move_group-4] - query_planners_service [move_group-4] * - state_validation_service [move_group-4] **** [move_group-4] [move_group-4] [INFO] [1724059276.911326526] [move_group.moveit.moveit.ros.move_group.context]: MoveGroup context using pipeline ompl [move_group-4] [INFO] [1724059276.911341298] [move_group.moveit.moveit.ros.move_group.context]: MoveGroup context initialization complete [move_group-4] Loading 'move_group/ApplyPlanningSceneService'... [move_group-4] Loading 'move_group/ClearOctomapService'... [move_group-4] Loading 'move_group/ExecuteTaskSolutionCapability'... [move_group-4] Loading 'move_group/GetUrdfService'... [move_group-4] Loading 'move_group/MoveGroupCartesianPathService'... [move_group-4] Loading 'move_group/MoveGroupExecuteTrajectoryAction'... [move_group-4] Loading 'move_group/MoveGroupGetPlanningSceneService'... [move_group-4] Loading 'move_group/MoveGroupKinematicsService'... [move_group-4] Loading 'move_group/MoveGroupMoveAction'... [move_group-4] Loading 'move_group/MoveGroupPlanService'... [move_group-4] Loading 'move_group/MoveGroupQueryPlannersService'... [move_group-4] Loading 'move_group/MoveGroupStateValidationService'... [move_group-4] [move_group-4] You can start planning now! [move_group-4] [rviz2-1] [INFO] [1724059277.019497187] [rviz2]: Stereo is NOT SUPPORTED [rviz2-1] [INFO] [1724059277.019595973] [rviz2]: OpenGl version: 4.6 (GLSL 4.6) [rviz2-1] [INFO] [1724059277.045930191] [rviz2]: Stereo is NOT SUPPORTED [ros2_control_node-5] [INFO] [1724059277.262811781] [controller_manager]: Loading controller 'joint_state_broadcaster' [joint_state_publisher-6] [INFO] [1724059277.272678792] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic... [rviz2-1] [WARN] [1724059277.279785939] [rcl.logging_rosout]: Publisher already registered for node name: 'rviz2'. If this is due to multiple nodes with the same name then all logs for the logger named 'rviz2' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. [rviz2-1] [INFO] [1724059277.310115711] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.00440811 seconds [rviz2-1] [INFO] [1724059277.310198620] [rviz2.moveit.core.robot_model]: Loading robot model 'panda'... [rviz2-1] [WARN] [1724059277.310394291] [rviz2.moveit.core.robot_model]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [rviz2-1] [WARN] [1724059277.310416348] [rviz2.moveit.core.robot_model]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [ros2 run controller_manager spawner joint_state_broadcaster-9] [INFO] [1724059277.329796120] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster [ros2_control_node-5] [INFO] [1724059277.332183201] [controller_manager]: Configuring controller 'joint_state_broadcaster' [ros2_control_node-5] [INFO] [1724059277.332260521] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published [ros2 run controller_manager spawner joint_state_broadcaster-9] [INFO] [1724059277.365345766] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster [ros2_control_node-5] [INFO] [1724059277.486080023] [controller_manager]: Loading controller 'panda_arm_controller' [ros2 run controller_manager spawner panda_arm_controller-8] [INFO] [1724059277.531402240] [spawner_panda_arm_controller]: Loaded panda_arm_controller [ros2_control_node-5] [INFO] [1724059277.543672893] [controller_manager]: Configuring controller 'panda_arm_controller' [ros2_control_node-5] [INFO] [1724059277.544295629] [panda_arm_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. [ros2_control_node-5] [INFO] [1724059277.544346042] [panda_arm_controller]: Command interfaces are [effort] and state interfaces are [position velocity]. [ros2_control_node-5] [INFO] [1724059277.544375306] [panda_arm_controller]: Using 'splines' interpolation method. [ros2_control_node-5] [INFO] [1724059277.546999714] [panda_arm_controller]: Action status changes will be monitored at 20.00 Hz. [INFO] [ros2 run controller_manager spawner joint_state_broadcaster-9]: process has finished cleanly [pid 42937] [rviz2-1] [WARN] [1724059277.631175452] [rcl.logging_rosout]: Publisher already registered for node name: 'rviz2'. If this is due to multiple nodes with the same name then all logs for the logger named 'rviz2' will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic. [ros2_control_node-5] [INFO] [1724059277.661005398] [FrankaHardwareInterface]: Preparing command mode switch [ros2_control_node-5] [INFO] [1724059277.661021760] [FrankaHardwareInterface]: ++ panda_joint1/effort [ros2_control_node-5] [INFO] [1724059277.661024431] [FrankaHardwareInterface]: ++ panda_joint2/effort [ros2_control_node-5] [INFO] [1724059277.661026601] [FrankaHardwareInterface]: ++ panda_joint3/effort [ros2_control_node-5] [INFO] [1724059277.661028510] [FrankaHardwareInterface]: ++ panda_joint4/effort [ros2_control_node-5] [INFO] [1724059277.661030418] [FrankaHardwareInterface]: ++ panda_joint5/effort [ros2_control_node-5] [INFO] [1724059277.661032304] [FrankaHardwareInterface]: ++ panda_joint6/effort [ros2_control_node-5] [INFO] [1724059277.661034208] [FrankaHardwareInterface]: ++ panda_joint7/effort [rviz2-1] [INFO] [1724059277.662916203] [rviz2.moveit.ros.rdf_loader]: Loaded robot model in 0.00199655 seconds [rviz2-1] [INFO] [1724059277.662975183] [rviz2.moveit.core.robot_model]: Loading robot model 'panda'... [rviz2-1] [WARN] [1724059277.663027071] [rviz2.moveit.core.robot_model]: Link panda_leftfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [rviz2-1] [WARN] [1724059277.663045733] [rviz2.moveit.core.robot_model]: Link panda_rightfinger has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [ros2_control_node-5] [INFO] [1724059277.663784015] [FrankaHardwareInterface]: Performing command mode switch [ros2_control_node-5] Current mode: joint_torque [ros2_control_node-5] Stopping [ros2_control_node-5] Initializing joint torque control [ros2 run controller_manager spawner panda_arm_controller-8] [INFO] [1724059277.673411355] [spawner_panda_arm_controller]: Configured and activated panda_arm_controller [rviz2-1] [INFO] [1724059277.682111675] [rviz2.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1 [rviz2-1] [INFO] [1724059277.726946137] [rviz2.moveit.ros.planning_scene_monitor]: Starting planning scene monitor [rviz2-1] [INFO] [1724059277.729691387] [rviz2.moveit.ros.planning_scene_monitor]: Listening to '/monitored_planning_scene' [INFO] [ros2 run controller_manager spawner panda_arm_controller-8]: process has finished cleanly [pid 42936]
The log still looks incomplete. There is no logging about MTC planning or execution.
As given in my first comment: In my launch file,I had already integrated MTC script to perform some cartesian movements.
In a separate launch file for Cartesian task gives me following log and launch file looks like given below:
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
moveit_config = (
MoveItConfigsBuilder("moveit_resources_panda")
.robot_description(file_path="config/panda.urdf.xacro")
.to_moveit_configs()
)
node = Node(
package="moveit_task_constructor_demo",
executable="cartesian.py",
output="screen",
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
moveit_config.joint_limits,
moveit_config.planning_pipelines,
],
)
return LaunchDescription([node])
> [INFO] [launch]: All log files can be found below /home/mitesh/.ros/log/2024-08-20-22-44-46-467721-mitesh-IdeaPad-5-14ITL05-369841
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [cartesian.py-1]: process started with pid [369844]
[cartesian.py-1] [INFO] [1724186687.416703174] [moveit_583632045.moveit.ros.rdf_loader]: Loaded robot model in 0.00599962 seconds
[cartesian.py-1] [INFO] [1724186687.416790135] [moveit_583632045.moveit.core.robot_model]: Loading robot model 'panda'...
[cartesian.py-1] [WARN] [1724186687.460944361] [kdl_parser]: The root link panda_link0 has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[cartesian.py-1] [INFO] [1724186687.461099471] [moveit_583632045.moveit.kinematics.kdl_kinematics_plugin]: Joint weights for group 'panda_arm': 1 1 1 1 1 1 1
[cartesian.py-1] [WARN] [1724186687.647100196] [Properties]: Unregistered property type: std::map<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, double, std::less<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > >, std::allocator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, double> > >
The new log is unrelated to the previous one. But the previous one is incomplete!
I configured my real Franka Emika arm for MTC to perform some simple tasks in ROS 2. However, solution is generated in Motion Planning Tasks window in Rviz but it is not moving my robotic arm. Could any one help me with that?
and given is the snippet from launch file:
What changes I should do to move my arm?