issue: Dual-IIWA7 Arm Setup using ROS2 FRI Stack #189
Assignees
Labels
Comments
|
thanks for raising this issue @rysabh. Including two This should definitely be addressed properly. For your current issue, this is where You'll likely have to adapt the joint names further down the line, here: Then change which file to include: However, I can see that this is quite the hacky and inconvenient solution. |
|
Hi @mhubii, thank you for your reply. As you could see in the following Xacro file which we have created and referenced at all the required places. Our Xacro file takes {robot_name} as an argument to rename joints and links. However, we have already tried this to no avail. <xacro:macro name="xyz_lbr_system_interface"
params="robot_name sim joint_limits system_parameters_path">
.
.
.
.
<xacro:joint_interface name="${robot_name}_A1"
min_position="${joint_limits[robot_name + '_A1']['lower'] * PI / 180}"
max_position="${joint_limits[robot_name + '_A1']['upper'] * PI / 180}"
max_velocity="${joint_limits[robot_name + '_A1']['velocity'] * PI / 180}"
max_torque="${joint_limits[robot_name + '_A1']['effort']}"
sim="${sim}" />
<xacro:joint_interface name="${robot_name}_A2"
min_position="${joint_limits[robot_name + '_A2']['lower'] * PI / 180}"
max_position="${joint_limits[robot_name + '_A2']['upper'] * PI / 180}"
max_velocity="${joint_limits[robot_name + '_A2']['velocity'] * PI / 180}"
max_torque="${joint_limits[robot_name + '_A2']['effort']}"
sim="${sim}" />
<xacro:joint_interface name="${robot_name}_A3"
min_position="${joint_limits[robot_name + '_A3']['lower'] * PI / 180}"
max_position="${joint_limits[robot_name + '_A3']['upper'] * PI / 180}"
max_velocity="${joint_limits[robot_name + '_A3']['velocity'] * PI / 180}"
max_torque="${joint_limits[robot_name + '_A3']['effort']}"
sim="${sim}" />
<xacro:joint_interface name="${robot_name}_A4"
min_position="${joint_limits[robot_name + '_A4']['lower'] * PI / 180}"
max_position="${joint_limits[robot_name + '_A4']['upper'] * PI / 180}"
max_velocity="${joint_limits[robot_name + '_A4']['velocity'] * PI / 180}"
max_torque="${joint_limits[robot_name + '_A4']['effort']}"
sim="${sim}" />
<xacro:joint_interface name="${robot_name}_A5"
min_position="${joint_limits[robot_name + '_A5']['lower'] * PI / 180}"
max_position="${joint_limits[robot_name + '_A5']['upper'] * PI / 180}"
max_velocity="${joint_limits[robot_name + '_A5']['velocity'] * PI / 180}"
max_torque="${joint_limits[robot_name + '_A5']['effort']}"
sim="${sim}" />
<xacro:joint_interface name="${robot_name}_A6"
min_position="${joint_limits[robot_name + '_A6']['lower'] * PI / 180}"
max_position="${joint_limits[robot_name + '_A6']['upper'] * PI / 180}"
max_velocity="${joint_limits[robot_name + '_A6']['velocity'] * PI / 180}"
max_torque="${joint_limits[robot_name + '_A6']['effort']}"
sim="${sim}" />
<xacro:joint_interface name="${robot_name}_A7"
min_position="${joint_limits[robot_name + '_A7']['lower'] * PI / 180}"
max_position="${joint_limits[robot_name + '_A7']['upper'] * PI / 180}"
max_velocity="${joint_limits[robot_name + '_A7']['velocity'] * PI / 180}"
max_torque="${joint_limits[robot_name + '_A7']['effort']}"
sim="${sim}" /> |
|
okay I see, will have to have a look. Thank you for sharing this detailed repository |
|
hi @rysabh, some more sources for joint name inconsistency:
I am currently unsure what the best approach here would be. |
|
Hi @mhubii, Thank you for your response. We implemented the suggested changes, but we are now encountering the following issue: which eventually leads to Despite the ROS 2 service list showing the presence of the controller manager, the bringup file fails to access it. As a result, the socket to the robot doesn't open, and we cannot establish a connection. Could there be another file we should check to resolve this issue? |
|
this might be some namespace issue. Which nodes are shown when you do ros2 node listOut of curiosity, has your Will try to clean the urdf / launch files so dual setup in single |
|
Hi @mhubii! To clarify, I'm working with @rysabh on this project.
Here's the debug information from the command: The following are the corresponding And the reason for calling it Below are additional details that might be helpful: This is what the MoveIt visualization looks like: Topic ListAction Listros2 action list
----
/kuka_green/execute_trajectory
/kuka_green/kuka_green/joint_trajectory_controller/follow_joint_trajectory
/kuka_green/move_actionService List ros2 service list
----
/clear_octomap
/interactive_marker_display_109294906279536/describe_parameters
/interactive_marker_display_109294906279536/get_parameter_types
/interactive_marker_display_109294906279536/get_parameters
/interactive_marker_display_109294906279536/list_parameters
/interactive_marker_display_109294906279536/set_parameters
/interactive_marker_display_109294906279536/set_parameters_atomically
/kuka_green/apply_planning_scene
/kuka_green/check_state_validity
/kuka_green/clear_octomap
/kuka_green/compute_cartesian_path
/kuka_green/compute_fk
/kuka_green/compute_ik
/kuka_green/controller_manager/describe_parameters
/kuka_green/controller_manager/get_parameter_types
/kuka_green/controller_manager/get_parameters
/kuka_green/controller_manager/list_parameters
/kuka_green/controller_manager/set_parameters
/kuka_green/controller_manager/set_parameters_atomically
/kuka_green/get_planner_params
/kuka_green/get_planning_scene
/kuka_green/load_map
/kuka_green/move_group/describe_parameters
/kuka_green/move_group/get_parameter_types
/kuka_green/move_group/get_parameters
/kuka_green/move_group/list_parameters
/kuka_green/move_group/set_parameters
/kuka_green/move_group/set_parameters_atomically
/kuka_green/move_group_private_98117349710368/describe_parameters
/kuka_green/move_group_private_98117349710368/get_parameter_types
/kuka_green/move_group_private_98117349710368/get_parameters
/kuka_green/move_group_private_98117349710368/list_parameters
/kuka_green/move_group_private_98117349710368/set_parameters
/kuka_green/move_group_private_98117349710368/set_parameters_atomically
/kuka_green/moveit_simple_controller_manager/describe_parameters
/kuka_green/moveit_simple_controller_manager/get_parameter_types
/kuka_green/moveit_simple_controller_manager/get_parameters
/kuka_green/moveit_simple_controller_manager/list_parameters
/kuka_green/moveit_simple_controller_manager/set_parameters
/kuka_green/moveit_simple_controller_manager/set_parameters_atomically
/kuka_green/plan_kinematic_path
/kuka_green/query_planner_interface
/kuka_green/robot_state_publisher/describe_parameters
/kuka_green/robot_state_publisher/get_parameter_types
/kuka_green/robot_state_publisher/get_parameters
/kuka_green/robot_state_publisher/list_parameters
/kuka_green/robot_state_publisher/set_parameters
/kuka_green/robot_state_publisher/set_parameters_atomically
/kuka_green/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/get_interactive_markers
/kuka_green/save_map
/kuka_green/set_planner_params
/rviz2/describe_parameters
/rviz2/get_parameter_types
/rviz2/get_parameters
/rviz2/list_parameters
/rviz2/set_parameters
/rviz2/set_parameters_atomically
/rviz2_private_130512447020800/describe_parameters
/rviz2_private_130512447020800/get_parameter_types
/rviz2_private_130512447020800/get_parameters
/rviz2_private_130512447020800/list_parameters
/rviz2_private_130512447020800/set_parameters
/rviz2_private_130512447020800/set_parameters_atomically
/static_transform_publisher_yvNOfvOz1Fr6OppA/describe_parameters
/static_transform_publisher_yvNOfvOz1Fr6OppA/get_parameter_types
/static_transform_publisher_yvNOfvOz1Fr6OppA/get_parameters
/static_transform_publisher_yvNOfvOz1Fr6OppA/list_parameters
/static_transform_publisher_yvNOfvOz1Fr6OppA/set_parameters
/static_transform_publisher_yvNOfvOz1Fr6OppA/set_parameters_atomicallyXacro FileNote: We have hard coded the link and joint names just to ensure there aren't any mistakes on our end! Thanks for your support and prompt responses, @mhubii!. I hope this helps clarify the setup. Let me know if there are specific issues or additional information needed! |
|
sorry been little sick these days. Maybe the easiest approach here would be to try and run These are the relevant errors: [ros2_control_node-2] [ERROR] [1718344151.320701252] [lbr_fri_ros2::FTEstimator]: Failed to construct kdl chain from robot description.
[ros2_control_node-2] [ERROR] [1718344151.320811103] [kuka_green.controller_manager]: The published robot description file (urdf) seems not to be genuine. The following error was caught:Failed to construct kdl chain from robot description.consequentially, the [spawner-8] [INFO] [1718344157.665407521] [kuka_green.spawner_joint_trajectory_controller]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-5] [INFO] [1718344157.686992234] [kuka_green.spawner_joint_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-7] [INFO] [1718344157.703793730] [kuka_green.spawner_lbr_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-6] [INFO] [1718344157.708657323] [kuka_green.spawner_force_torque_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-8] [INFO] [1718344159.686318051] [kuka_green.spawner_joint_trajectory_controller]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-5] [INFO] [1718344159.708807391] [kuka_green.spawner_joint_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-7] [INFO] [1718344159.725092356] [kuka_green.spawner_lbr_state_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-6] [INFO] [1718344159.730374814] [kuka_green.spawner_force_torque_broadcaster]: Waiting for '/kuka_green/controller_manager' services to be available
[spawner-8] [ERROR] [1718344161.706777150] [kuka_green.spawner_joint_trajectory_controller]: Controller manager not available
[spawner-5] [ERROR] [1718344161.729480917] [kuka_green.spawner_joint_state_broadcaster]: Controller manager not available
[spawner-7] [ERROR] [1718344161.745774654] [kuka_green.spawner_lbr_state_broadcaster]: Controller manager not available
[spawner-6] [ERROR] [1718344161.751428356] [kuka_green.spawner_force_torque_broadcaster]: Controller manager not availablejust to better understand. In your multi-robot setup, ideally you would have a single <!-- include the lbr iiwa macro -->
<xacro:include filename="$(find lbr_description)/urdf/iiwa7/iiwa7_description.xacro" />
...
<!-- iiwa green -->
<xacro:iiwa7
robot_name="$(arg robot_name)"
sim="$(arg sim)"
system_parameters_path="$(arg system_parameters_path)" />
<!-- iiwa blue -->
<xacro:iiwa7
robot_name="$(arg robot_name)"
sim="$(arg sim)"
system_parameters_path="$(arg system_parameters_path)" />this fails because of name duplicates |
|
Hello @mhubii, I hope that you are feeling well. <robot name="dual_iiwa7" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- include the lbr iiwa macro -->
<xacro:include filename="$(find lbr_description)/urdf/dual_iiwa7/iiwa7_description.xacro" />
<!-- Arguments -->
<xacro:arg name="sim" default="true" />
<xacro:arg name="system_parameters_path" default="$(find lbr_ros2_control)/config/lbr_system_parameters.yaml" />
<!-- Base link for the world -->
<link name="world"><origin xyz="0 0 0" rpy="0 0 0"/></link>
<xacro:arg name="robot_name_1" default="kuka_blue" />
<xacro:arg name="robot_name_2" default="kuka_green" />
<xacro:iiwa7
robot_name="$(arg robot_name_1)"
parent = "world"
sim="$(arg sim)"
system_parameters_path="$(arg system_parameters_path)">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:iiwa7>
<xacro:iiwa7
robot_name="$(arg robot_name_2)"
parent = "world"
sim="$(arg sim)"
system_parameters_path="$(arg system_parameters_path)">
<origin xyz="1 1 0" rpy="0 0 0"/>
</xacro:iiwa7>
</robot>
Also, we have changed the Now we can perform dual arm manipulation using our custom MoveIt package by launching three programs as follows: (1) the custom dual arm MoveIt package to handle motion planning, (2) the driver for robot_1 with moveit:=false, (3) the driver for robot_2 with moveit:=false. We achieve this by providing goals to the follow_joint_trajectory action of both robot drivers. However, we're encountering a specific issue. Our custom MoveIt package, successfully plans trajectories; however, execution fails when pressing the "execute" button in Rviz. Interestingly, if I modify the <!-- macros for simulation and real hardware -->
<!-- <xacro:dual_iiwa7_gazebo robot_name="${robot_name}" />
<xacro:dual_iiwa7_system_interface
robot_name="${robot_name}"
sim="${sim}"
joint_limits="${joint_limits}"
system_parameters_path="${system_parameters_path}" />
-->However, if we want to visualize the live robot position in our custom MoveIt package, we need these lines enabled. If we disable these control elements and use follow_joint_trajectory action, we can plan and execute trajectories. However, no live view of real robots during execution in MoveIt. Therefore, could you please help us in visualizing the live robot position during real robot execution? (which we believe could be possible using those control elements enabled in |
|
thank you for coming back to this interesting topic! Can I execute your modifications here: https://github.com/rysabh/dual_kuka_fri ? |
|
I understand that when using 2 robots, we have to name the joint differently. If so, do I just need to change the name of the joint_state macro of joint_limit.yaml, lbr_system_interface? lbr_fri_ros2_stack/lbr_ros2_control/include/lbr_ros2_control/lbr_state_broadcaster.hpp |
|
yes so the issue is exactly that joints / links need to be named uniquely |
mhubii
added a commit
that referenced
this issue
Oct 19, 2024
5 tasks
mhubii
added a commit
that referenced
this issue
Oct 19, 2024
mhubii
added a commit
that referenced
this issue
Oct 21, 2024
mhubii
added a commit
that referenced
this issue
Oct 23, 2024
mhubii
added a commit
that referenced
this issue
Oct 23, 2024
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
issue: dual_kuka_fri
I want to create a dual-iiwa7 arm setup in ROS2 using the FRI stack. I have already created a custom Xacro file that uses the robot namespace to spawn two robots. However, for simplifying the issue, here I will only spawn one robot in mycustom moveit package named "green_moveit_config", which uses a custom {robot_name}. Our public Github repo is here.
In the Xacro file, I have also renamed link and joint names with a prefix to manage both arms in the same environment.
After creating the custom URDF, I used the MoveIt Setup Assistant to generate the MoveIt configurations.
I followed the instructions here in the documentation to modify the controller and other related files.
Additionally, I have remapped certain topics as follows to connect topics from our MoveIt package to topics from the robot driver (bringup.launch.py):
{robot_name}_display_planned_path->display_planned_path{robot_name}_joint_states->joint_states{robot_name}_monitored_planning_scene->monitored_planning_scene{robot_name}_planning_scene->planning_scene{robot_name}_planning_scene_world->planning_scene_world{robot_name}_robot_description->robot_description{robot_name}_robot_description_semantic->robot_description_semanticHowever, I am encountering several issues:
Steps to Reproduce the Error:
bringup.launch.pywithmodel:=iiwa7andrviz:=falseandmoveit:=falseandsim:=false.ros2 launch green_moveit_config demo.launch.Error:
I can visualize the current robot state and also planning to an new state works. However, the execution on the real robot fails.
There is a mismatch in joint names. We observe this when doing a ROS2 service call
/kuka_green/controller_manager/list_controllers controller_manager_msgs/srv/ListControllersand get the following response:We observe that joints are not renamed here.
Expected Behavior:
The system should correctly recognize and manage the custom link and joint names without any mismatches, and the controller manager should operate as expected based on the modified
lbr_controller.yamlfile.Actual Behavior:
Mismatch in link and joint names leading to errors in the system's operation. The controller manager does not reflect the changes expected from the modified configuration.
Importantly, we noticed that, when the link and joint names are not changed in Xacro, and the launch file is launched with default
robot_name:=lbr, it works. However, having same link names (ex. link0 for robot1 and robot2) is not a possibiiltiy for dual arm setup.Therefore, we are currently stuck and desperately need your help!
The text was updated successfully, but these errors were encountered: