kortex_dual_driver.launch does not work

[psalidoiri]

Description

When we run the following command 'roslaunch kortex_driver kortex_dual_driver.launch left_ip_address:=192.168.1.12 right_ip_address:=192.168.1.10 ' we do not receive any errors but the Kinova robots do not appear in their real position in RVIZ (attached image).

Version

ROS distribution : Melodic

Branch and commit you are using : melodic-devel

[felixmaisonneuve]

HI @psalidoiri,

This is expected behaviour.
By launching kortex_dual_driver.launch, your are using the kortex_dual_robots.xacro description

ros_kortex/kortex_driver/launch/kortex_dual_driver.launch

Line 63 in 7ba75b3

command="$(find xacro)/xacro --inorder $(find kortex_description)/multiple_robots/kortex_dual_robots.xacro

This description loads two Gen3 arms at these positions :

ros_kortex/kortex_description/multiple_robots/kortex_dual_robots.xacro

Lines 21 to 33 in 7ba75b3

	<link name="world" />
	<joint name="world_to_$(arg left_prefix)root" type="fixed">
	<child link="$(arg left_prefix)base_link" />
	<parent link="world" />
	<!-- Here you can change the origin of the left arm-->
	<origin xyz="-1 0 0" rpy="0 0 0" />
	</joint>
	<joint name="world_to_$(arg right_prefix)root" type="fixed">
	<child link="$(arg right_prefix)base_link" />
	<parent link="world" />
	<!-- Here you can change the origin of the right arm -->
	<origin xyz="1 0 0" rpy="0 0 0" />
	</joint>

The dual arm launch file does not launch the kortex_driver node and does not launch the moveit for this dual arm config.

ros_kortex/kortex_driver/launch/kortex_driver.launch

Lines 60 to 97 in 7ba75b3

	<node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->
	<param name="sim" value="false"/>
	<param name="ip_address" value="$(arg ip_address)"/>
	<param name="username" value="$(arg username)"/>
	<param name="password" value="$(arg password)"/>
	<param name="cyclic_data_publish_rate" value="$(arg cyclic_data_publish_rate)"/>
	<param name="api_rpc_timeout_ms" value="$(arg api_rpc_timeout_ms)"/>
	<param name="api_session_inactivity_timeout_ms" value="$(arg api_session_inactivity_timeout_ms)"/>
	<param name="api_connection_inactivity_timeout_ms" value="$(arg api_connection_inactivity_timeout_ms)"/>
	<param name="default_goal_time_tolerance" value="$(arg default_goal_time_tolerance)"/>
	<param name="default_goal_tolerance" value="$(arg default_goal_tolerance)"/>
	<param name="arm" value="$(arg arm)"/>
	<param name="gripper" value="$(arg gripper)"/>
	<param name="dof" value="$(arg dof)"/>
	<param name="use_hard_limits" value="$(arg use_hard_limits)"/>
	<param name="robot_name" value="$(arg robot_name)"/>
	<param name="prefix" value="$(arg prefix)"/>
	<rosparam command="load" file="$(find kortex_description)/arms/$(arg arm)/$(arg dof)dof/config/joint_limits.yaml" subst_value="true"/>
	<!-- If there is a gripper, load the active joint names for it -->
	<rosparam command="load" file="$(find kortex_description)/grippers/$(arg gripper)/config/joint_limits.yaml" unless="$(eval not arg('gripper'))" subst_value="true"/>
	</node>
	<!-- Start MoveIt! main executable -->
	<group if="$(arg start_moveit)">
	<!-- TODO Find cleaner way to do that and that will work with other arms -->
	<!-- Without gripper -->
	<include file="$(find kortex_description)/../kortex_move_it_config/$(arg arm)_move_it_config/launch/move_group.launch" if="$(eval not arg('gripper'))">
	<arg name="dof" value="$(arg dof)"/>
	<arg name="use_hard_limits" value="$(arg use_hard_limits)"/>
	<arg name="prefix" value="$(arg prefix)"/>
	</include>
	<!-- With gripper -->
	<include file="$(find kortex_description)/../kortex_move_it_config/$(arg arm)_$(arg gripper)_move_it_config/launch/move_group.launch" unless="$(eval not arg('gripper'))">
	<arg name="dof" value="$(arg dof)"/>
	<arg name="use_hard_limits" value="$(arg use_hard_limits)"/>
	<arg name="prefix" value="$(arg prefix)"/>
	</include>

The first thing you need to do is create your own moveit config with your two arms. That is specific for everyone, it depends on what arms you are using. It is possible to use the MoveIt SetupAssistant (that was used to create every moveit config initially), but I think there was some modifications on the configs with time and the SetupAssistant is not working anymore.This is something that I hope could be fixed in the future. The other option would be to copy one of the config in kortex_move_it_config and modify each file to match your config, starting with move_group.launch (the entry point used by the kortex_driver.launch)

ros_kortex/kortex_driver/launch/kortex_driver.launch

Line 87 in 7ba75b3

<include file="$(find kortex_description)/../kortex_move_it_config/$(arg arm)_move_it_config/launch/move_group.launch" if="$(eval not arg('gripper'))">

Once this is done, if you want to use the driver (to send commands to the arms), you might need to to modify the update the actual kortex_driver to be able to handle multiple arms at the same time. This is probably A LOT of work, but I never really got the time to look into it. If you want to do something like this, the easiest/fastest way is simply to launch two driver at the same time (unless this causes other conflict issues, I never tried that).

Hope it clear things out for you.
I am open to answer any wuestion you might have on this topic.

Best regards,
Felix

[psalidoiri]

Hi @felixmaisonneuve ,
I think there is a mistake in your answer. The dual arm launch file is launching the kortex_arm_driver and this launch file calls the kortex_driver launch file:

ros_kortex/kortex_driver/launch/kortex_dual_driver.launch

Line 98 in cec7161

<include file="$(find kortex_driver)/launch/kortex_arm_driver.launch">

ros_kortex/kortex_driver/launch/kortex_arm_driver.launch

Line 35 in cec7161

<node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->

Anyway, these days we have been testing code changes to make the dual_kortex launch file work:

1. We modify the kortex_arm_driver.launch:
   

   ros_kortex/kortex_driver/launch/kortex_arm_driver.launch

   Line 35 in cec7161

   <node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen"> <!--launch-prefix="gdb -ex run args"-->

<node name="$(arg robot_name)_driver" pkg="kortex_driver" type="kortex_arm_driver" output="screen" ns="$(arg robot_name)"> launch-prefix="gdb -ex run args"

2. kortex_description/arms/gen3/7dof/config/joint_limits.yaml:
   

   ros_kortex/kortex_description/arms/gen3/7dof/config/joint_limits.yaml

   Lines 1 to 8 in cec7161

   	joint_names:
   	- $(arg prefix)joint_1
   	- $(arg prefix)joint_2
   	- $(arg prefix)joint_3
   	- $(arg prefix)joint_4
   	- $(arg prefix)joint_5
   	- $(arg prefix)joint_6
   	- $(arg prefix)joint_7

 joint_names: 
   - joint_1 
   - joint_2 
   - joint_3 
   - joint_4 
   - joint_5 
   - joint_6 
   - joint_7 

3. The config file of the gripper that we are using kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml:
   

   ros_kortex/kortex_description/grippers/robotiq_2f_85/config/joint_limits.yaml

   Lines 1 to 2 in cec7161

   	gripper_joint_names:
   	- $(arg prefix)finger_joint

 gripper_joint_names: 
   - finger_joint 

4. We modify the kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp file, reversing the changes before this commit (changes in line 655 and 666):
   

   ros_kortex/kortex_driver/src/non-generated/driver/kortex_arm_driver.cpp

   Lines 655 to 666 in cec7161

   	joint_state.name[i] = m_arm_joint_names[i];
   	joint_state.position[i] = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(base_feedback.actuators[i].position));
   	joint_state.velocity[i] = m_math_util.toRad(base_feedback.actuators[i].velocity);
   	joint_state.effort[i] = base_feedback.actuators[i].torque;
   	}
   	if (isGripperPresent())
   	{
   	for (int i = 0; i < base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor.size(); i++)
   	{
   	int joint_state_index = base_feedback.actuators.size() + i;
   	joint_state.name[joint_state_index] = m_gripper_joint_names[i];

     joint_state.name[i] = m_prefix + m_arm_joint_names[i]; 
     joint_state.position[i] = m_math_util.wrapRadiansFromMinusPiToPi(m_math_util.toRad(base_feedback.actuators[i].position)); 
     joint_state.velocity[i] = m_math_util.toRad(base_feedback.actuators[i].velocity); 
     joint_state.effort[i] = base_feedback.actuators[i].torque; 
 } 
  
 if (isGripperPresent()) 
 { 
     for (int i = 0; i < base_feedback.interconnect.oneof_tool_feedback.gripper_feedback[0].motor.size(); i++) 
     { 
         int joint_state_index = base_feedback.actuators.size() + i; 
         joint_state.name[joint_state_index] = m_prefix + m_gripper_joint_names[i];

Now, we can see the Kinova robots in their real position:
roslaunch kortex_driver kortex_dual_driver.launch left_ip_address:=192.168.1.12 left_gripper:=robotiq_2f_85

Best regards,
Pablo S.

[felixmaisonneuve]

You are right, the dual arm launch file is launching the kortex_arm_driver.

This is amazing you made it work. Thank you for sharing your work here.

The commit you mentionned is very interesting. I remember doing this modification because it was causing an error.
I will keep in mind you managed to make the dual arm config work, but I won't revert the changes in the yaml yet.

Do you need more support on this topic?

Regards,
Felix

[psalidoiri]

Hi @felixmaisonneuve ,
It would be interesting to apply these changes to a stable branch and have a launch file in which both robots work. People who need to work or experiment with two Kinova robots would benefit from this improvement. For example, in our research center several people work with both robotic arms at the same time and it would be difficult to apply the modifications in each of their computers.

I wish this would happen soon,
Pablo S.

[felixmaisonneuve]

I created the bugfix/dual-arm-melodic branch that you can checkout.
Verify that the changes I made are correct and that it works. I have not tested it since I do not have a dual arm setup at the moment, but since you want this fix as soon as possible, I created the branch anyway based on the modifications you described in your previous comment.

Please tell me if I missed something or if this is not what you are looking for

[psalidoiri]

I just tested the changes in this new branch, and it works as desired.

Thanks,
Pablo S.