search

Kinovarobotics / ros2_kortex

ROS2 driver for the Gen3 Kinova robot arm
Other
49 stars 47 forks source link

Gen3_lite has problems in gazebo simulation. #221

Closed htcb0249 closed 5 months ago

htcb0249 commented 5 months ago

I tried the branch pr_gen3-lite-launch-file, but it shows sim_isaac invalid parameter message. The gen3 model also has a problem in Rviz window This sim_isaac parameter is in the description document. 

ros2 launch kortex_bringup kortex_sim_control.launch.py   dof:=6   robot_type:=gen3_lite   use_sim_time:=true  
[INFO] [launch]: All log files can be found below /home/ros2/.ros/log/2024-05-24-08-52-31-362799-ros2-virtual-kinova-2884
[INFO] [launch]: Default logging verbosity is set to INFO
[ERROR] [launch]: Caught exception in launch (see debug for traceback): executed command failed. Command: /opt/ros/humble/bin/xacro /opt/ros/humble/share/kortex_description/robots/kinova.urdf.xacro robot_ip:=xxx.yyy.zzz.www name:=gen3 arm:=gen3_lite dof:=6 vision:=false prefix:="" sim_gazebo:=false sim_ignition:=true simulation_controllers:=/opt/ros/humble/share/kortex_description/arms/gen3/6dof/config/ros2_controllers.yaml gripper:=robotiq_2f_85 
Captured stderr output: error: Invalid parameter "sim_isaac"
when instantiating macro: load_arm (/opt/ros/humble/share/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro)
instantiated from: load_robot (/opt/ros/humble/share/kortex_description/robots/kortex_robot.xacro)
in file: /opt/ros/humble/share/kortex_description/robots/kinova.urdf.xacro

While using command

ros2 launch kortex_bringup kortex_sim_control.launch.py \
  dof:=6 \
  robot_type:=gen3_lite \
  use_sim_time:=true \
  launch_rviz:=false \
  sim_gazebo:=true

image

smoya23 commented 5 months ago

Hi @htcb0249 , This is because there is some dependencies missing in your workspace, could you redo the commands inside our building from source instructions (it was recently updated). Once you have built your workspace, don't forget to source it before running the command again.

cd <your-ros2-rowkspace>
source install/setup.bash

Cheers, Santiago

htcb0249 commented 5 months ago

No, you are misunderstanding what I mean. I've sourced my setup.bash (I'm omitting cd and source commands), and it shows these messages.

Does that updated "main" branch conflict with some packages in ros2 tutorials? I can't build the workspace. When I tried to build them with colcon, it always shows messages like: 

--- stderr: moveit_task_constructor_core    
/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp: In constructor ‘moveit::task_constructor::solvers::PipelinePlanner::PipelinePlanner(const SharedPtr&, const string&)’:
/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:128:104: error: ‘DISPLAY_PATH_TOPIC’ is not a member of ‘planning_pipeline::PlanningPipeline’
  128 | solutions on topic " + planning_pipeline::PlanningPipeline::DISPLAY_PATH_TOPIC);
      |                                                             ^~~~~~~~~~~~~~~~~~

/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:131:66: error: ‘MOTION_PLAN_REQUEST_TOPIC’ is not a member of ‘planning_pipeline::PlanningPipeline’
  131 |                        planning_pipeline::PlanningPipeline::MOTION_PLAN_REQUEST_TOPIC);
      |                                                             ^~~~~~~~~~~~~~~~~~~~~~~~~

/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp: In member function ‘virtual void moveit::task_constructor::solvers::PipelinePlanner::init(const RobotModelConstPtr&)’:
/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:151:45: warning: ‘void planning_pipeline::PlanningPipeline::displayComputedMotionPlans(bool)’ is deprecated: Use generatePlan or ROS parameter API instead. [-Wdeprecated-declarations]
  151 |         planner_->displayComputedMotionPlans(properties().get<bool>("display_motion_plans"));
      |         ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:42:
/home/ros2/ws_moveit/install/moveit_ros_planning/include/moveit_ros_planning/moveit/planning_pipeline/planning_pipeline.h:134:73: note: declared here
  134 | ed("Use generatePlan or ROS parameter API instead.")]] void displayComputedMotionPlans(bool /*flag*/){};
      |                                                             ^~~~~~~~~~~~~~~~~~~~~~~~~~

/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:152:42: warning: ‘void planning_pipeline::PlanningPipeline::publishReceivedRequests(bool)’ is deprecated: Use generatePlan or ROS parameter API instead. [-Wdeprecated-declarations]
  152 |         planner_->publishReceivedRequests(properties().get<bool>("publish_planning_requests"));
      |         ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:42:
/home/ros2/ws_moveit/install/moveit_ros_planning/include/moveit_ros_planning/moveit/planning_pipeline/planning_pipeline.h:135:73: note: declared here
  135 | ed("Use generatePlan or ROS parameter API instead.")]] void publishReceivedRequests(bool /*flag*/){};
      |                                                             ^~~~~~~~~~~~~~~~~~~~~~~

/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp: In member function ‘moveit::task_constructor::solvers::PlannerInterface::Result moveit::task_constructor::solvers::PipelinePlanner::plan(const PlanningSceneConstPtr&, const MotionPlanRequest&, robot_trajectory::RobotTrajectoryPtr&)’:
/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:213:22: error: ‘struct planning_interface::MotionPlanResponse’ has no member named ‘trajectory_’; did you mean ‘trajectory’?
  213 |         result = res.trajectory_;
      |                      ^~~~~~~~~~~
      |                      trajectory
/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:214:67: error: ‘error_code_to_string’ is not a member of ‘moveit::core’; did you mean ‘errorCodeToString’?
  214 |   return { success, success ? std::string() : moveit::core::error_code_to_string(res.error_code_.val) };
      |                                                             ^~~~~~~~~~~~~~~~~~~~
      |                                                             errorCodeToString
/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:214:92: error: ‘struct planning_interface::MotionPlanResponse’ has no member named ‘error_code_’; did you mean ‘error_code’?
  214 | ss ? std::string() : moveit::core::error_code_to_string(res.error_code_.val) };
      |                                                             ^~~~~~~~~~~
      |                                                             error_code
/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:214:109: error: could not convert ‘{success, <expression error>}’ from ‘<brace-enclosed initializer list>’ to ‘moveit::task_constructor::solvers::PlannerInterface::Result’
  214 | ::string() : moveit::core::error_code_to_string(res.error_code_.val) };
      |                                                                      ^
      |                                                                      |
      |                                                                      <brace-enclosed initializer list>

In file included from /usr/include/x86_64-linux-gnu/c++/11/bits/c++allocator.h:33,
                 from /usr/include/c++/11/bits/allocator.h:46,
                 from /usr/include/c++/11/memory:64,
                 from /home/ros2/ws_moveit/install/moveit_core/include/moveit_core/moveit/macros/declare_ptr.h:37,
                 from /home/ros2/ws_moveit/install/moveit_core/include/moveit_core/moveit/macros/class_forward.h:39,
                 from /home/ros2/ws_moveit/src/moveit_task_constructor/core/include/moveit/task_constructor/solvers/planner_interface.h:41,
                 from /home/ros2/ws_moveit/src/moveit_task_constructor/core/include/moveit/task_constructor/solvers/pipeline_planner.h:41,
                 from /home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:39:
/usr/include/c++/11/ext/new_allocator.h: In instantiation of ‘void __gnu_cxx::new_allocator<_Tp>::construct(_Up*, _Args&& ...) [with _Up = planning_pipeline::PlanningPipeline; _Args = {const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const char* const&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&}; _Tp = planning_pipeline::PlanningPipeline]’:
/usr/include/c++/11/bits/alloc_traits.h:516:17:   required from ‘static void std::allocator_traits<std::allocator<_Tp1> >::construct(std::allocator_traits<std::allocator<_Tp1> >::allocator_type&, _Up*, _Args&& ...) [with _Up = planning_pipeline::PlanningPipeline; _Args = {const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const char* const&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&}; _Tp = planning_pipeline::PlanningPipeline; std::allocator_traits<std::allocator<_Tp1> >::allocator_type = std::allocator<planning_pipeline::PlanningPipeline>]’
/usr/include/c++/11/bits/shared_ptr_base.h:519:39:   required from ‘std::_Sp_counted_ptr_inplace<_Tp, _Alloc, _Lp>::_Sp_counted_ptr_inplace(_Alloc, _Args&& ...) [with _Args = {const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const char* const&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&}; _Tp = planning_pipeline::PlanningPipeline; _Alloc = std::allocator<planning_pipeline::PlanningPipeline>; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/11/bits/shared_ptr_base.h:650:16:   required from ‘std::__shared_count<_Lp>::__shared_count(_Tp*&, std::_Sp_alloc_shared_tag<_Alloc>, _Args&& ...) [with _Tp = planning_pipeline::PlanningPipeline; _Alloc = std::allocator<planning_pipeline::PlanningPipeline>; _Args = {const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const char* const&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&}; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/11/bits/shared_ptr_base.h:1342:14:   required from ‘std::__shared_ptr<_Tp, _Lp>::__shared_ptr(std::_Sp_alloc_shared_tag<_Tp>, _Args&& ...) [with _Alloc = std::allocator<planning_pipeline::PlanningPipeline>; _Args = {const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const char* const&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&}; _Tp = planning_pipeline::PlanningPipeline; __gnu_cxx::_Lock_policy _Lp = __gnu_cxx::_S_atomic]’
/usr/include/c++/11/bits/shared_ptr.h:409:59:   required from ‘std::shared_ptr<_Tp>::shared_ptr(std::_Sp_alloc_shared_tag<_Tp>, _Args&& ...) [with _Alloc = std::allocator<planning_pipeline::PlanningPipeline>; _Args = {const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const char* const&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&}; _Tp = planning_pipeline::PlanningPipeline]’
/usr/include/c++/11/bits/shared_ptr.h:862:14:   required from ‘std::shared_ptr<_Tp> std::allocate_shared(const _Alloc&, _Args&& ...) [with _Tp = planning_pipeline::PlanningPipeline; _Alloc = std::allocator<planning_pipeline::PlanningPipeline>; _Args = {const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const char* const&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&}]’
/usr/include/c++/11/bits/shared_ptr.h:878:39:   required from ‘std::shared_ptr<_Tp> std::make_shared(_Args&& ...) [with _Tp = planning_pipeline::PlanningPipeline; _Args = {const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, const char* const&, const std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&}]’
/home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:106:66:   required from here
/usr/include/c++/11/ext/new_allocator.h:162:11: error: no matching function for call to ‘planning_pipeline::PlanningPipeline::PlanningPipeline(const std::shared_ptr<const moveit::core::RobotModel>&, const std::shared_ptr<rclcpp::Node>&, std::__cxx11::basic_string<char>&, const char* const&, const std::__cxx11::basic_string<char>&)’
  162 |         { ::new((void *)__p) _Up(std::forward<_Args>(__args)...); }
      |           ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
In file included from /home/ros2/ws_moveit/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:42:
/home/ros2/ws_moveit/install/moveit_ros_planning/include/moveit_ros_planning/moveit/planning_pipeline/planning_pipeline.h:126:3: note: candidate: ‘planning_pipeline::PlanningPipeline::PlanningPipeline(const RobotModelConstPtr&, const std::shared_ptr<rclcpp::Node>&, const string&, const std::vector<std::__cxx11::basic_string<char> >&, const std::vector<std::__cxx11::basic_string<char> >&, const std::vector<std::__cxx11::basic_string<char> >&)’
  126 |   PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr<rclcpp::Node>& node,
      |   ^~~~~~~~~~~~~~~~
/home/ros2/ws_moveit/install/moveit_ros_planning/include/moveit_ros_planning/moveit/planning_pipeline/planning_pipeline.h:127:92: note:   no known conversion for argument 4 from ‘const char* const’ to ‘const std::vector<std::__cxx11::basic_string<char> >&’
  127 | tring& parameter_namespace, const std::vector<std::string>& planner_plugin_names,
      |                             ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~

/home/ros2/ws_moveit/install/moveit_ros_planning/include/moveit_ros_planning/moveit/planning_pipeline/planning_pipeline.h:113:3: note: candidate: ‘planning_pipeline::PlanningPipeline::PlanningPipeline(const RobotModelConstPtr&, const std::shared_ptr<rclcpp::Node>&, const string&)’
  113 |   PlanningPipeline(const moveit::core::RobotModelConstPtr& model, const std::shared_ptr<rclcpp::Node>& node,
      |   ^~~~~~~~~~~~~~~~
/home/ros2/ws_moveit/install/moveit_ros_planning/include/moveit_ros_planning/moveit/planning_pipeline/planning_pipeline.h:113:3: note:   candidate expects 3 arguments, 5 provided
gmake[2]: *** [src/CMakeFiles/moveit_task_constructor_core.dir/build.make:258: src/CMakeFiles/moveit_task_constructor_core.dir/solvers/pipeline_planner.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:256: src/CMakeFiles/moveit_task_constructor_core.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< moveit_task_constructor_core [12.1s, exited with code 2]
Aborted  <<< kinova_gen3_7dof_robotiq_2f_85_moveit_config [0.92s]

Summary: 59 packages finished [1min 4s]
  1 package failed: moveit_task_constructor_core
  1 package aborted: kinova_gen3_7dof_robotiq_2f_85_moveit_config
  1 package had stderr output: moveit_task_constructor_core
  9 packages not processed

or failed with moveit_task_constructor_capabilities package.

I tried to build only ros2_kortex in a new ubuntu 22.04 virtual machine, It can be built. 

export COLCON_WS=~/workspace/ros2_kortex_ws
mkdir -p $COLCON_WS/src
cd $COLCON_WS
git clone https://github.com/Kinovarobotics/ros2_kortex.git src/ros2_kortex
vcs import src --skip-existing --input src/ros2_kortex/ros2_kortex.$ROS_DISTRO.repos
vcs import src --skip-existing --input src/ros2_kortex/ros2_kortex-not-released.$ROS_DISTRO.repos
rosdep install --ignore-src --from-paths src -y -r
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

After that, I tried the command to start simulation of gen3_lite.

source ~/workspace/ros2_kortex_ws/install/setup.bash

ros2 launch kortex_bringup kortex_sim_control.launch.py robot_type:=gen3_lite dof:=6 gripper:=gen3_lite_2f use_sim_time:=true

image image sim_gazebo:=true image

However, when I changed "gen3_lite" to "gen3", the model displayed normal.

ros2 launch kortex_bringup kortex_sim_control.launch.py   dof:=6   robot_type:=gen3   use_sim_time:=true

image

No matter which command I use, this simulated arm does not respond to commands like:

  ros2 topic pub /joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{
    header: {stamp: now},
    joint_names: ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'],
    points: [
      {
        positions: [0.0, 0.5, 1.0, 0.5, 0.5, 0.2],
        time_from_start: {sec: 5}
      }
    ]
  }" -1

Terminal shows:

[ruby $(which ign) gazebo-7] [INFO] [1716793015.871516302] [controller_manager]: Empty activate and deactivate list, not requesting switch
smoya23 commented 5 months ago

Hi @htcb0249,

Moveit task constructor is not part of our dependencies, so something is wrong in your workspace. These are the dependencies that should be present in your workspace.

image

Note that I have installed moveit2 from source directly inside my workspace. At the end of the build you should see 114 packages built, not 59

image

Also, I don't know which ROS2 distro you're on, but just in case, note that currently our driver is not compatible with ROS 2 Rolling, as Rolling has been ported to Ubuntu 24.04 and all of the simulation packages have not been ported yet. I see from your build commands that you're also missing the command to import the simulation packages 

vcs import src --skip-existing --input src/ros2_kortex/simulation.humble.repos

Which would explain why the Gen3_Lite is appearing on Gazebo but collapsing with gravity (gazebo controllers are part of the simulation packages that I suspect are missing)

htcb0249 commented 5 months ago

In my Ubuntu 22.04 ROS2 humble distro workspace (no move it tutorials), the dependencies are as follows: image Then I execute the commands you provided. image There are only 63 packages, that might be the problem. image

smoya23 commented 5 months ago

@htcb0249 63 is the correct number of packages if you don't have installed moveit, i had 114 packages as I installed moveit from source directly in my workspace. If you don't plan to use Moveit, then your workspace looks fine. Does your build finish without issues ? If you re-run the simulation launch file, are you able to see the Gen3_lite in Gazebo/Ignition ?

Also, please note that our driver does not include the Gen3_lite moveit config, so if you wish to use a Gen3_lite arm with MoveIt, you will need to generate it yourself using the Moveit assistant GUI.

htcb0249 commented 5 months ago

Thank you, I can run the simulation and see Gen3_lite in Gazebo/Ignition now. But I don't know how to control it to move. image

I attempted to build my workspace using ROS2 Kortex and MoveIt 2 to control my simulated arm before transitioning to my real robotic arm (Gen3 Lite), which is currently under maintenance.

Although I noticed that the MoveIt 2 tutorials in the Humble branch use different (main branch is kinova robotic arm) examples (main branch is kinova robotic arm) and API version such as the Quick Start, which might be causing conflicts, I will attempt to replace some files from the main branch to see if that resolves the issue.

Based on your response, I understand that I cannot use MoveIt 2 to design the motion for my Gen3_Lite. Is it possible to use the Gen3 as a substitute?

smoya23 commented 5 months ago

@htcb0249 You can use the Gen3_Lite with MoveIt, it's just that we don't provide the MoveIt config as we do for the Gen3 6/7 dof arms, so you need to generate it yourself if you want to use Moveit to plan movements for your Gen3_Lite using Moveit's Setup Assistant. Regarding the Moveit 2 tutorials, there are indeed some modifications necessary on the main branch to make it work on Humble. Even though I can't confirm any date yet, we are currently in the process of creating some video tutorials where we follow along these tutorials on Humble. These tutorials will be available on our youtube page and/or on Kinova Academy.

htcb0249 commented 5 months ago

Ok, I got it. That means all I need to do is to set up using Moveit's Setup Assistant, and it is the normal way to use the Gen3 Lite with MoveIt. I'll try it. Thank you.