search

pal-robotics / tiago_simulation

http://wiki.ros.org/Robots/TIAGo
Apache License 2.0
46 stars 34 forks source link

MoveIt 2 Move Group C++ Interface #43

Open kavikode opened 2 months ago

kavikode commented 2 months ago

Motion planning to control Tiago MoveIt 2 Move Group C++ Interface for ROS 2 Humble

Terminal 1: ros2 launch tiago_gazebo tiago_gazebo.launch.py moveit:=True world_name:=pick is_public_sim:=True

Terminal 2: ros2 launch tiago_moveit_config moveit_rviz.launch.py

Then I used the following C++ file pose_goal.cep from the repo https://github.com/ut-ims-robotics/pool-thesis-2023-moveit2-examples and I modified it for the planning group arm:

include

include <rclcpp/rclcpp.hpp>

include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char * argv[]) { // Initialize ROS and create the Node rclcpp::init(argc, argv); auto const node = std::make_shared("pose_goal");

// Create a ROS logger auto const logger = rclcpp::get_logger("pose_goal");

// Create the MoveIt Move Group Interface for panda arm using moveit::planning_interface::MoveGroupInterface; auto move_group_interface = MoveGroupInterface(node, "arm");

// Create a target Pose for the end-effector geometry_msgs::msg::Pose target_pose; target_pose.orientation.w = 1.0; target_pose.position.x = 0.28; target_pose.position.y = -0.2; target_pose.position.z = 0.5;

// Set the target pose move_group_interface.setPoseTarget(target_pose);

// Create a plan to that target pose and check if that plan is successful moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success = (move_group_interface.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

// If the plan is successful, execute the plan if(success) { move_group_interface.execute(my_plan); } else { RCLCPP_ERROR(logger, "Planing failed!"); }

// Shutdown rclcpp::shutdown(); return 0; }

Terminal 3: ros2 run cpp_examples pose_goal

Results:

[INFO] [1725589103.149830117] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 2.06175 seconds [INFO] [1725589103.150369063] [moveit_robot_model.robot_model]: Loading robot model 'tiago'... [INFO] [1725589103.150455974] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint [WARN] [1725589103.153558698] [moveit_robot_model.robot_model]: Link base_laser_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.153734240] [moveit_robot_model.robot_model]: Link base_sonar_01_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.153859441] [moveit_robot_model.robot_model]: Link base_sonar_02_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.153933842] [moveit_robot_model.robot_model]: Link base_sonar_03_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.154007663] [moveit_robot_model.robot_model]: Link caster_back_left_1_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.154122424] [moveit_robot_model.robot_model]: Link caster_back_right_1_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.154208935] [moveit_robot_model.robot_model]: Link caster_front_left_1_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.154289036] [moveit_robot_model.robot_model]: Link caster_front_right_1_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.172293933] [moveit_robot_model.robot_model]: Link base_cover_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725589103.213727746] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml! [INFO] [1725589103.273724043] [move_group_interface]: Ready to take commands for planning group arm. [INFO] [1725589103.273980655] [move_group_interface]: MoveGroup action client/server ready [INFO] [1725589103.276105519] [move_group_interface]: Planning request accepted [INFO] [1725589108.368755300] [move_group_interface]: Planning request aborted [ERROR] [1725589108.369412777] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached [ERROR] [1725589108.369453317] [pose_goal]: Planing failed!

How can I fix this error because I did use the correct planning group named arm? Please advise.

Thank you.

saikishor commented 1 month ago

Hello @kavikode !

Can you try without launching the moveIt config again in a different terminal?

Thank you

kavikode commented 1 month ago

Same issue @saikishor thanks for your reply. Please provide additional suggestions

Terminal 1: ros2 launch tiago_gazebo tiago_gazebo.launch.py moveit:=True world_name:=pick is_public_sim:=True

Terminal 2: ros2 run cpp_examples pose_goal

Results: [INFO] [1725754186.961776205] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.20369 seconds [INFO] [1725754186.961980359] [moveit_robot_model.robot_model]: Loading robot model 'tiago'... [INFO] [1725754186.962056810] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint [WARN] [1725754186.964275623] [moveit_robot_model.robot_model]: Link base_laser_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754186.964374345] [moveit_robot_model.robot_model]: Link base_sonar_01_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754186.964445406] [moveit_robot_model.robot_model]: Link base_sonar_02_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754186.964512778] [moveit_robot_model.robot_model]: Link base_sonar_03_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754186.964580239] [moveit_robot_model.robot_model]: Link caster_back_left_1_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754186.964652870] [moveit_robot_model.robot_model]: Link caster_back_right_1_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754186.964761753] [moveit_robot_model.robot_model]: Link caster_front_left_1_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754186.964844874] [moveit_robot_model.robot_model]: Link caster_front_right_1_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754186.994311546] [moveit_robot_model.robot_model]: Link base_cover_link has visual geometry but no collision geometry. Collision geometry will be left empty. Fix your URDF file by explicitly specifying collision geometry. [WARN] [1725754187.053520434] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml! [INFO] [1725754187.091982490] [move_group_interface]: Ready to take commands for planning group arm. [INFO] [1725754187.092218894] [move_group_interface]: MoveGroup action client/server ready [INFO] [1725754187.094711543] [move_group_interface]: Planning request accepted [INFO] [1725754192.218530470] [move_group_interface]: Planning request aborted [ERROR] [1725754192.219021770] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached [ERROR] [1725754192.219077391] [pose_goal]: Planing failed!

kavikode commented 1 month ago

Similarly, I also tried to modify the code https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/motion_planning_api/src/motion_planning_api_tutorial.cpp to adapt to Tiago


#include <pluginlib/class_loader.hpp>

// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/planning_scene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/move_group_interface/move_group_interface.h>

static const rclcpp::Logger LOGGER = rclcpp::get_logger("motion_planning_api_tutorial");

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  std::shared_ptr<rclcpp::Node> motion_planning_api_tutorial_node =
      rclcpp::Node::make_shared("motion_planning_api_tutorial", node_options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(motion_planning_api_tutorial_node);
  std::thread([&executor]() { executor.spin(); }).detach();

  // BEGIN_TUTORIAL
  // Start
  // ^^^^^
  // Setting up to start using a planner is pretty easy. Planners are
  // setup as plugins in MoveIt and you can use the ROS pluginlib
  // interface to load any planner that you want to use. Before we can
  // load the planner, we need two objects, a RobotModel and a
  // PlanningScene. We will start by instantiating a
  // :moveit_codedir:`RobotModelLoader<moveit_ros/planning/robot_model_loader/include/moveit/robot_model_loader/robot_model_loader.h>`
  // object, which will look up the robot description on the ROS
  // parameter server and construct a
  // :moveit_codedir:`RobotModel<moveit_core/robot_model/include/moveit/robot_model/robot_model.h>`
  // for us to use.
  const std::string PLANNING_GROUP = "arm";

//  const std::string PLANNING_GROUP = "panda_arm";
  robot_model_loader::RobotModelLoader robot_model_loader(motion_planning_api_tutorial_node, "robot_description");
  const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel();
  /* Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group*/
  moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model));
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

  // Using the
  // :moveit_codedir:`RobotModel<moveit_core/robot_model/include/moveit/robot_model/robot_model.h>`,
  // we can construct a
  // :moveit_codedir:`PlanningScene<moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h>`
  // that maintains the state of the world (including the robot).
  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

  // Configure a valid robot state
  planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

  // We will now construct a loader to load a planner, by name.
  // Note that we are using the ROS pluginlib library here.
  std::unique_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
  planning_interface::PlannerManagerPtr planner_instance;
  std::string planner_plugin_name;

  // We will get the name of planning plugin we want to load
  // from the ROS parameter server, and then load the planner
  // making sure to catch all exceptions.
  if (!motion_planning_api_tutorial_node->get_parameter("planning_plugin", planner_plugin_name))
    RCLCPP_FATAL(LOGGER, "Could not find planner plugin name");
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
        "moveit_core", "planning_interface::PlannerManager"));
  }
  catch (pluginlib::PluginlibException& ex)
  {
    RCLCPP_FATAL(LOGGER, "Exception while creating planning plugin loader %s", ex.what());
  }
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model, motion_planning_api_tutorial_node,
                                      motion_planning_api_tutorial_node->get_namespace()))
      RCLCPP_FATAL(LOGGER, "Could not initialize planner instance");
    RCLCPP_INFO(LOGGER, "Using planning interface '%s'", planner_instance->getDescription().c_str());
  }
  catch (pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (const auto& cls : classes)
      ss << cls << " ";
    RCLCPP_ERROR(LOGGER, "Exception while loading planner '%s': %s\nAvailable plugins: %s", planner_plugin_name.c_str(),
                 ex.what(), ss.str().c_str());
  }

  moveit::planning_interface::MoveGroupInterface move_group(motion_planning_api_tutorial_node, PLANNING_GROUP);

  // Visualization
  // ^^^^^^^^^^^^^
  // The package MoveItVisualTools provides many capabilities for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools(motion_planning_api_tutorial_node, "base_footprint",
                                                      "move_group_tutorial", move_group.getRobotModel());
  visual_tools.enableBatchPublishing();
  visual_tools.deleteAllMarkers();  // clear all old markers
  visual_tools.trigger();

  /* Remote control is an introspection tool that allows users to step through a high level script
     via buttons and keyboard shortcuts in RViz */
  visual_tools.loadRemoteControl();

  /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);

  /* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
  visual_tools.trigger();

  /* We can also use visual_tools to wait for user input */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

  // Pose Goal
  // ^^^^^^^^^
  // We will now create a motion plan request for the arm of the Robot
  // specifying the desired pose of the end-effector as input.
  visual_tools.trigger();
  planning_interface::MotionPlanRequest req;
  planning_interface::MotionPlanResponse res;
  geometry_msgs::msg::PoseStamped pose;
  pose.header.frame_id = "base_footprint";
  pose.pose.position.x = 0.3;
  pose.pose.position.y = 0.4;
  pose.pose.position.z = 0.75;
  pose.pose.orientation.w = 1.0;

  // A tolerance of 0.01 m is specified in position
  // and 0.01 radians in orientation
  std::vector<double> tolerance_pose(3, 0.01);
  std::vector<double> tolerance_angle(3, 0.01);

  // We will create the request as a constraint using a helper function available
  // from the
  // :moveit_codedir:`kinematic_constraints<moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h>`
  // package.
  moveit_msgs::msg::Constraints pose_goal =
      kinematic_constraints::constructGoalConstraints("arm_7_joint", pose, tolerance_pose, tolerance_angle);

  req.group_name = PLANNING_GROUP;
  req.goal_constraints.push_back(pose_goal);

  // We now construct a planning context that encapsulate the scene,
  // the request and the response. We call the planner using this
  // planning context
  planning_interface::PlanningContextPtr context =
      planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
    return 0;
  }

  // Visualize the result
  // ^^^^^^^^^^^^^^^^^^^^
  std::shared_ptr<rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>> display_publisher =
      motion_planning_api_tutorial_node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path",
                                                                                               1);
  moveit_msgs::msg::DisplayTrajectory display_trajectory;

  /* Visualize the trajectory */
  moveit_msgs::msg::MotionPlanResponse response;
  res.getMessage(response);

  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);

  /* Set the state in the planning scene to the final state of the last plan */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());

  // Display the goal state
  visual_tools.publishAxisLabeled(pose.pose, "goal_1");
  visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* We can also use visual_tools to wait for user input */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // Joint Space Goals
  // ^^^^^^^^^^^^^^^^^
  // Now, setup a joint space goal
  moveit::core::RobotState goal_state(robot_model);
  std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
  goal_state.setJointGroupPositions(joint_model_group, joint_values);
  moveit_msgs::msg::Constraints joint_goal =
      kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
  req.goal_constraints.clear();
  req.goal_constraints.push_back(joint_goal);

  // Call the planner and visualize the trajectory
  /* Re-construct the planning context */
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  /* Call the Planner */
  context->solve(res);
  /* Check that the planning was successful */
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
    return 0;
  }
  /* Visualize the trajectory */
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);

  /* Now you should see two planned trajectories in series*/
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);

  /* We will add more goals. But first, set the state in the planning
     scene to the final state of the last plan */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());

  // Display the goal state
  visual_tools.publishAxisLabeled(pose.pose, "goal_2");
  visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for user input */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  /* Now, we go back to the first goal to prepare for orientation constrained planning */
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal);
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  res.getMessage(response);

  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);

  /* Set the state in the planning scene to the final state of the last plan */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());

  // Display the goal state
  visual_tools.trigger();

  /* Wait for user input */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  // Adding Path Constraints
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // Let's add a new pose goal again. This time we will also add a path constraint to the motion.
  /* Let's create a new pose goal */

  pose.pose.position.x = 0.32;
  pose.pose.position.y = -0.25;
  pose.pose.position.z = 0.65;
  pose.pose.orientation.w = 1.0;
  moveit_msgs::msg::Constraints pose_goal_2 =
      kinematic_constraints::constructGoalConstraints("arm_7_joint", pose, tolerance_pose, tolerance_angle);

  /* Now, let's try to move to this new pose goal*/
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal_2);

  /* But, let's impose a path constraint on the motion.
     Here, we are asking for the end-effector to stay level*/
  geometry_msgs::msg::QuaternionStamped quaternion;
  quaternion.header.frame_id = "base_footprint";
  req.path_constraints = kinematic_constraints::constructGoalConstraints("arm_7_joint", quaternion);

  // Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
  // (the workspace of the robot)
  // because of this, we need to specify a bound for the allowed planning volume as well;
  // Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
  // but that is not being used in this example).
  // We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done
  // in this volume
  // when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
      req.workspace_parameters.min_corner.z = -2.0;
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
      req.workspace_parameters.max_corner.z = 2.0;

  // Call the planner and visualize all the plans created so far.
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher->publish(display_trajectory);

  /* Set the state in the planning scene to the final state of the last plan */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());

  // Display the goal state
  visual_tools.publishAxisLabeled(pose.pose, "goal_3");
  visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  // END_TUTORIAL
  /* Wait for user input */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo");
  planner_instance.reset();

  rclcpp::shutdown();
  return 0;
}

How do I find or define the robot_description, robot_description_semantic, kinematics_yaml, and planning_yaml for Tiago in order to modify https://github.com/moveit/moveit2_tutorials/blob/humble/doc/examples/motion_planning_api/launch/motion_planning_api_tutorial.launch.py?

Thank you

saikishor commented 1 month ago

Hello @kavikode!

Thank you for writing to us. You can find that information in the repo: https://github.com/pal-robotics/tiago_moveit_config. This should give you the information you are looking for.

Regarding the MoveIt integration with TIAGo, we have checked on our end and it works interfacing with MoveIt. You can also test it with the MoveIt Planning Scene plugin in the Rviz2. You can find the information on how to reproduce it here : https://github.com/pal-robotics/tiago_simulation?tab=readme-ov-file#simulation--moveit-2

Thank you!

Best Regards, PAL Robotics Team

kavikode commented 1 month ago

Thanks for your message. I have made the following changes in the results are still negative. How can I best resolve this? After this obstacle is overcome with your support, I can make significant progress and contribute to examples that can be used by others. Please help.


def generate_launch_description():
    robot_description_config = load_file(
        "tiago_description", "robots/tiago.urdf.xacro"  # Assume Tiago's URDF is in xacro format
    )
    robot_description = {"robot_description": robot_description_config}

    robot_description_semantic_config = load_file(
        "tiago_moveit_config", "config/srdf/tiago.srdf.xacro"  # Adjusted for Tiago's SRDF file
    )
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_config
    }

    kinematics_yaml = load_yaml(
        "tiago_moveit_config", "config/kinematics_kdl.yaml"  # Kinematics for Tiago
    )

    planning_yaml = load_yaml(
        "tiago_moveit_config", "config/ompl_planning.yaml"  # Planning configurations for Tiago
    )

    planning_plugin = {"planning_plugin": "ompl_interface/OMPLPlanner"}

    return LaunchDescription([
        Node(
            package="moveit2_tutorials",  
            executable="motion_planning_api_tutorial",  
            name="motion_planning_api_tutorial",
            parameters=[
                robot_description,
                robot_description_semantic,
                kinematics_yaml,
                planning_yaml,
                planning_plugin,
            ],
        )
    ])

Results:

ros2 launch moveit2_tutorials motion_planning_api_tutorial.launch.py [INFO] [launch]: All log files can be found below /home/ubuntu/.ros/log/2024-09-09-22-39-12-671073-ip-172-31-55-37-5784 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [motion_planning_api_tutorial-1]: process started with pid [5785] [motion_planning_api_tutorial-1] Error: No link elements found in urdf file [motion_planning_api_tutorial-1] at line 206 in ./urdf_parser/src/model.cpp [motion_planning_api_tutorial-1] Failed to parse robot description using: urdf_xml_parser/URDFXMLParser [motion_planning_api_tutorial-1] [INFO] [1725935953.128737107] [moveit_rdf_loader.rdf_loader]: Unable to parse URDF [motion_planning_api_tutorial-1] terminate called after throwing an instance of 'std::invalid_argument' [motion_planning_api_tutorial-1] what(): RobotState cannot be constructed with nullptr RobotModelConstPtr [ERROR] [motion_planning_api_tutorial-1]: process has died [pid 5785, exit code -6, cmd '/home/ubuntu/kai_ws/src/tiago_public_ws/install/moveit2_tutorials/lib/moveit2_tutorials/motion_planning_api_tutorial --ros-args -r __node:=motion_planning_api_tutorial --params-file /tmp/launch_params_k25f935q --params-file /tmp/launch_params_mxrthahq --params-file /tmp/launch_params_40ivx91y --params-file /tmp/launch_params_9a3a81kv --params-file /tmp/launch_paramsv0f3qpc'].

kavikode commented 1 month ago

still trying after many weeks and requesting help again please

I understand using ros2 launch tiago_gazebo tiago_gazebo.launch.py moveit:=True and ros2 launch tiago_moveit_config moveit_rviz.launch.py, however, I am unable to move it using C++ code. It is a huge struggle as there are no examples to move the end effector using IK. Can you please share a small code sample in C++?

saikishor commented 1 month ago

Hello @kavikode!

If you see the above error, it clearly says that it failed to parse the robot_description. Check if the robort_description is being published or not, and better to do some checking around your code.

[motion_planning_api_tutorial-1] Failed to parse robot description using: urdf_xml_parser/URDFXMLParser
[motion_planning_api_tutorial-1] [INFO] [1725935953.128737107] [moveit_rdf_loader.rdf_loader]: Unable to parse URDF
[motion_planning_api_tutorial-1] terminate called after throwing an instance of 'std::invalid_argument'
[motion_planning_api_tutorial-1] what(): RobotState cannot be constructed with nullptr RobotModelConstPtr

As of now, we don't have much to share publicly regarding the MoveIt integration. May be you can get some insights from our public package: play_motions2.

I hope this helps,

Thank you!

Best Regards, PAL Robotics Team