j2s6s200 integration

[wxmerkt]

This PR adds generic features for Gazebo simulation as well as for integrating Kinova arms into more complex robotic systems. In particular, it includes:

1. Adds Gazebo support for two-finger hands (in general, new argument has2finger which defaults to false, i.e., 3-finger hand).
2. Allows remapping of robot_description in kinova_control.launch. Defaults to robot_description as currently.
3. Adds control configuration for j2s6s200 (adapted from j2s6s300 and removed the controllers for the third finger).
4. Adds two new xacro for integrating the j2s6s200/j2s6s300 without the world link from _standalone. These are xacro with allow specification of the desired base/mount link. This is required for complex Gazebo integrations and some planners that do not like explicit world links.
5. Changes j2s6s200 joint types for continuous joints with joint limits to revolute. This relates to Continuous joints with joint limits #289. I am happy to revert this one and instead remove the upper and lower position limits. Either is fine for me :-).

[HappySamuel]

#316

Hi @wxmerkt

I am trying out the j2s6s200 integration, but i faced the robot arm exploding when loading it with a mobile robot model and theirs' controllers after unpause gazebo. Do you have a clue on how shall i solve this issue?

Loading j2s6s200 alone, have no issue.

[wxmerkt]

Hi @HappySamuel,
Can you check the initial joint angles? When loading with a combined robot I often notice the Kinova to be spawned with joints far outside the limits causing large forces. Your command in #316 with a paused simulator should prevent this but it might still happen.

Please also note that I am also using a modified set of ROS-Control controllers with fixes for initial states and large joint limits in my experiments (ros-controls/ros_controllers#504, a variant of ros-controls/ros_controllers#509). This is fine for heavy bases like yours but the default set of Gazebo configs have very high gains so it would cause issues if you were to have a light-weight base.

Hope this helps
Wolfgang

[HappySamuel]

Hi @wxmerkt

I tried following scenarios:
(1) If loaded effort_joint_trajectory_controller only, i observed that the fingers will no longer link with the arm properly. And the arm itself is keep on shaking. It caused the Mobile robot also shaking.

(2) If loaded with joint_x_position_controller only, i observed that the fingers are still no longer link with the arm properly. And the arm is still shaking.

As you mentioned, this could be caused by the default P gain that kinova-ros suggested.

The PID values i used is as shown below:

effort_joint_trajectory_controller:
constraints:
goal_time: 1.0
j2s6s200_joint_1:
goal: 0.02
trajectory: 0.05
j2s6s200_joint_2:
goal: 0.02
trajectory: 0.05
j2s6s200_joint_3:
goal: 0.02
trajectory: 0.05
j2s6s200_joint_4:
goal: 0.02
trajectory: 0.05
j2s6s200_joint_5:
goal: 0.02
trajectory: 0.05
j2s6s200_joint_6:
goal: 0.02
trajectory: 0.05
stopped_velocity_tolerance: 0.02
gains:
j2s6s200_joint_1:
d: 0
i: 0
i_clamp: 10
p: 5000
j2s6s200_joint_2:
d: 0
i: 0
i_clamp: 10
p: 5000
j2s6s200_joint_3:
d: 0
i: 0
i_clamp: 10
p: 5000
j2s6s200_joint_4:
d: 0
i: 0
i_clamp: 10
p: 500
j2s6s200_joint_5:
d: 0
i: 0
i_clamp: 10
p: 200
j2s6s200_joint_6:
d: 0
i: 0
i_clamp: 10
p: 500

Can you share the PID values that you used, which doesn't have these kind of issues??

Secondly, do i need to modify the ros_controllers as per you suggested?

Best,
Samuel

[HappySamuel]

Hi @wxmerkt

I have found the root cause of causing this explosion / severe jittering of joints in Gazebo. The reason is due to the default PID value of robot arm that Kinova provided only applicable to default Gazebo physics settings on more complex robots.

Default Setting:

<physics name='default_physics' default='0' type='ode'>
       <max_step_size>0.001</max_step_size> <!-- time for each measurement taken -->
       <real_time_factor>1</real_time_factor>
       <real_time_update_rate>1000</real_time_update_rate>
</physics>

My Setting:

<physics name='default_physics' default='0' type='ode'>
       <max_step_size>0.005</max_step_size> <!-- time for each measurement taken -->
       <real_time_factor>1</real_time_factor>
       <real_time_update_rate>200</real_time_update_rate>
</physics>

Although my physics configuration will force the simulation to run at the real_time_factor=1 , which is good especially when your laptop CPU is not the high end specs. With this method, you can still have the robot navigation working at real world speed.
However, this bring disaster to the robotics arm simulation, the arm will explode if you attach onto another robot.

According to my findout, if the max_step_size is larger than 0.001, the robot arm will start jittering. If the value go above 0.003, the finger will start to jittering apart from the joints.

Questions:
(1) Why is the max_step_size will affect the robot arm simulation?
(2) What shall i do to solve this exploding / jittering issue if want to apply my physics config (to ensure mobile robot + robot arm always simulated at real time speed.

Best,
Samuel

[felixmaisonneuve]

Hi @wxmerkt,

Thank you for your work. I understand this thread has been inactive for almost a year, but here are my thoughts on this PR.

First, I like that you renamed the robot_state_publisher to $(arg kinova_robotName)_robot_state_publisher and you added the possibility to remap the robot_description. This allows easier integration of other robots within the simulation and reduce the change of to nodes being initialized qith the same name.

However, I will ask you to remove the modifications you did to j2s6s200.xacro file. The joints are continous and should not be change to revolute. We can always discuss this in the issue #289 you created.

Also, I am not sure what j2s6s200_integration.xacro and j2s6s300_integration.xacro are for. Can you explain it to me? You don't seem to reference them anywhere, even in your branch. I noticed they slightly differ from the $(arg kinova_robotName)_standalone.xacro, but I haven't tested them. I don't really know what they bring and I don't understand why they are never used.

Finally, you modified kinova_control.launch a bit, but you didn't follow those modifications to robot_launch.launch, which is the file we use to launch the simulation (and the one calling kinova_control.launch).
It would be great if you could also update robot_launch.launch like I did here (which is a branch based on your modifications for our noetic-devel branch)

Thank you for contribution

Regards,
Felix

[wxmerkt]

Hi @felixmaisonneuve,
Thank you very much for your time and going through this pull request.

As for the revolute vs continuous joint discussion, I've reverted the changes in this PR. I believe we should amend the macro, however, to omit joint limits that do not exist for continuous joints if the joint type is continuous to conclude #289.

I've cherry-picked your commit 0f4c3b4 to this branch - thanks for spotting this omission!

The reason for adding the _integration.xacro files is that - for control purposes when integrating Kinova robots in larger systems such as mobile manipulation platforms - we need to be able to create stand-alone URDFs for which we can configure the arm-root link (as it is defined in the full robot model for instance) - and that do not contain the root and world link defined in the _standalone.xacro URDFs as e.g. world is only required for Gazebo I believe. In our case, for instance, we have been integrating a Jaco on top of a quadruped and I believe this may be similar for other integrations as in this thread above. If you'd like, I am happy to move this xacro file to our repository instead.

I believe the PR should be now good to go - please let me know if I missed anything.

Regards,
Wolfgang

[felixmaisonneuve]

Thank you @wxmerkt for doing the modifications.

I will merge this PR in our melodic-devel branch

Your work is greatly appreciated,
Felix