I am trying to command the Panda robot from the Isaac Sim examples using MoveIt and local ROS Humble installation. I am running Isaac Sim on a workstation and I connect to it using the client.
To start Isaac Sim I use the following command:
docker run --name isaac-sim --entrypoint bash -it --runtime=nvidia --gpus all \
-e "ACCEPT_EULA=Y" \
--rm --network=host \
-e "PRIVACY_CONSENT=Y" \
-e "RMW_IMPLEMENTATION=rmw_fastrtps_cpp" \
-e "LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/isaac-sim/exts/omni.isaac.ros2_bridge/humble/lib" \
-e "AMENT_PREFIX_PATH=/opt/ros/humble" \
-v ~/docker/isaac-sim/cache/kit:/isaac-sim/kit/cache:rw \
-v ~/docker/isaac-sim/cache/ov:/root/.cache/ov:rw \
-v ~/docker/isaac-sim/cache/pip:/root/.cache/pip:rw \
-v ~/docker/isaac-sim/cache/glcache:/root/.cache/nvidia/GLCache:rw \
-v ~/docker/isaac-sim/cache/computecache:/root/.nv/ComputeCache:rw \
-v ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw \
-v ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw \
-v ~/docker/isaac-sim/documents:/root/Documents:rw \
nvcr.io/nvidia/isaac-sim:2023.1.1 \
-c "./runheadless.native.sh -v"
Then I open the Franka simulation using the interface and initialize MoveIt using the demo launch file provided:
docker compose up demo_isaac
With this setup I can control the robot via RViz and the Motion Planner. However, I am having problems controlling the robot using an external node.
This is the node I’ve written following the MoveIt tutorial:
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/pose.hpp>
int main(int argc, char * argv[])
{
// Initialize ROS and create the Node
rclcpp::init(argc, argv);
auto const node = std::make_shared<rclcpp::Node>(
"panda_commander",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
);
// Create a ROS logger
auto const logger = rclcpp::get_logger("panda_commander");
// Create the MoveIt MoveGroup Interface
using moveit::planning_interface::MoveGroupInterface;
auto move_group_interface = MoveGroupInterface(node, "panda_arm");
// Set the target pose using a named target
move_group_interface.setNamedTarget("extended");
// Create a plan to that target pose
auto const [success, plan] = [&move_group_interface, &logger]{
moveit::planning_interface::MoveGroupInterface::Plan msg;
auto const ok = static_cast<bool>(move_group_interface.plan(msg));
return std::make_pair(ok, msg);
}();
// Execute the plan
if(success) {
move_group_interface.execute(plan);
} else {
RCLCPP_ERROR(logger, "Planning failed!");
}
// Shutdown ROS
rclcpp::shutdown();
return 0;
}
To launch this node and pass the necessary parameters, I use this launch file:
import launch
import os
import sys
import yaml
from ament_index_python.packages import get_package_share_directory
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch.substitutions import PathJoinSubstitution, Command, FindExecutable, LaunchConfiguration
from launch_ros.substitutions import FindPackageShare
def get_robot_description():
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name='xacro')]),
' ',
PathJoinSubstitution(
[FindPackageShare("moveit_resources_panda_moveit_config"), 'config', "panda.urdf.xacro"]
),
" ",
"ros2_control_hardware_type:=",
"isaac",
]
)
robot_description = {'robot_description': ParameterValue(robot_description_content, value_type=str)}
return robot_description
def get_robot_description_semantic():
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare("moveit_resources_panda_moveit_config"), "config", "panda.srdf"]),
]
)
robot_description_semantic = {"robot_description_semantic": ParameterValue(robot_description_semantic_content, value_type=str)}
return robot_description_semantic
def get_kinematics_yaml():
kinematics_yaml_path = os.path.join(
get_package_share_directory("moveit_resources_panda_moveit_config"),
"config",
"kinematics.yaml"
)
with open(kinematics_yaml_path, 'r') as file:
kinematics_content = yaml.safe_load(file)
return {'robot_description_kinematics': kinematics_content}
def generate_launch_description():
robot_description = get_robot_description()
robot_description_semantic = get_robot_description_semantic()
robot_description_kinematics = get_kinematics_yaml()
panda_commander = Node(
package="panda_commander",
executable="panda_commander",
name="panda_commander",
output="both",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics
],
)
return launch.LaunchDescription([panda_commander])
Before launching the node I export the following variables:
export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
export FASTRTPS_DEFAULT_PROFILES_FILE=~/.ros/fastdds.xml
where ~/.ros/fastdds.xml:
<?xml version="1.0" encoding="UTF-8" ?>
<license>Copyright (c) 2022-2024, NVIDIA CORPORATION. All rights reserved.
NVIDIA CORPORATION and its licensors retain all intellectual property
and proprietary rights in and to this software, related documentation
and any modifications thereto. Any use, reproduction, disclosure or
distribution of this software and related documentation without an express
license agreement from NVIDIA CORPORATION is strictly prohibited.</license>
<profiles xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles" >
<transport_descriptors>
<transport_descriptor>
<transport_id>UdpTransport</transport_id>
<type>UDPv4</type>
</transport_descriptor>
</transport_descriptors>
<participant profile_name="udp_transport_profile" is_default_profile="true">
<rtps>
<userTransports>
<transport_id>UdpTransport</transport_id>
</userTransports>
<useBuiltinTransports>false</useBuiltinTransports>
</rtps>
</participant>
</profiles>
When launching panda_commander.launch.py I get this output in the console:
[panda_commander-1] [INFO] [1728368089.314232235] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0268301 seconds
[panda_commander-1] [INFO] [1728368089.314314107] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
[panda_commander-1] [INFO] [1728368089.371960527] [move_group_interface]: Ready to take commands for planning group panda_arm.
[panda_commander-1] [INFO] [1728368089.372054262] [move_group_interface]: MoveGroup action client/server ready
[panda_commander-1] [INFO] [1728368089.435675674] [move_group_interface]: Planning request accepted
demo_isaac-1 | [move_group-5] [INFO] [1728368089.372338673] [move_group.moveit.moveit.ros.move_group.move_action]: MoveGroupMoveAction: Received request
demo_isaac-1 | [move_group-5] [INFO] [1728368089.372615209] [move_group.moveit.moveit.ros.move_group.move_action]: executing..
demo_isaac-1 | [move_group-5] [INFO] [1728368089.380284373] [move_group.moveit.moveit.ros.move_group.move_action]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
demo_isaac-1 | [move_group-5] [INFO] [1728368089.380382145] [move_group]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
demo_isaac-1 | [move_group-5] [INFO] [1728368089.380399999] [move_group]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
demo_isaac-1 | [move_group-5] [WARN] [1728368089.380404567] [move_group.moveit.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
demo_isaac-1 | [move_group-5] [INFO] [1728368089.380412442] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
demo_isaac-1 | [move_group-5] [INFO] [1728368089.380426568] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
demo_isaac-1 | [move_group-5] [INFO] [1728368089.380619528] [move_group.moveit.moveit.planners.ompl.model_based_planning_context]: Planner configuration 'panda_arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
demo_isaac-1 | [move_group-5] [INFO] [1728368089.380662719] [move_group]: Calling Planner 'OMPL'
demo_isaac-1 | [move_group-5] [INFO] [1728368089.392256698] [move_group]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
demo_isaac-1 | [move_group-5] [INFO] [1728368089.396686555] [move_group]: Calling PlanningResponseAdapter 'ValidateSolution'
demo_isaac-1 | [move_group-5] [INFO] [1728368089.397418001] [move_group]: Calling PlanningResponseAdapter 'DisplayMotionPath'
demo_isaac-1 | [move_group-5] [WARN] [1728368089.397510143] [move_group.moveit.moveit.ros.planning_pipeline]: The planner plugin did not fill out the 'planner_id' field of the MotionPlanResponse. Setting it to the planner ID name of the MotionPlanRequest assuming that the planner plugin does warn you if it does not use the requested planner.
demo_isaac-1 | [move_group-5] [INFO] [1728368089.397559765] [move_group.moveit.moveit.ros.move_group.move_action]: Motion plan was computed successfully.
Although the Move Group receives the planning command and computes it (it is even shown in RViz), it looks like the response never reaches the commander, as the execution gets stuck in move_group_interface.plan(msg).
What could be wrong? Could it be the ROS middleware configuration?