Improve Gazebo PID parameters for all robots

[matteolucchi]

This is a work in progress. I am working on improving the Gazebo PID parameters for all the robots.

This is based on #525 as for now I will focus on tuning them with the standard cylinder approximation of the links. Once we are sure that the intertia matrices of #504 are correct and this will be merged I will check again if the performance is still ok or if the PID params need re tuning.

Update PID parameters of:

• ur3
• ur5
• ur10
• ur3e
• ur5e
• ur10e
• ur16e

I am tuning the params at the best of my knowledge, if some has any advice on how to this better let me know.

This is what I did for the UR10:

I wrote a small ROS node to publish sinusoidal or step inputs to the different joints of the robot, it looks ugly but it does the job:

#!/usr/bin/env python
import math, time
import rospy
from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
from std_msgs.msg import Header

class JT:
    def __init__(self):
        rospy.init_node('joint_trajectory_pub')
        self.rate = rospy.Rate(50)

        # Publisher to JointTrajectory robot controller
        self.jt_pub = rospy.Publisher('/eff_joint_traj_controller/command', JointTrajectory, queue_size=10)


    def joint_trajectory_publisher(self):

        i =0 

        while not rospy.is_shutdown():

            msg = JointTrajectory()
            msg.header = Header()
            # msg.joint_names = ["elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", \
                                # "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]
            msg.joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", \
                                "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]                                
            msg.points=[JointTrajectoryPoint()]
            
            # shoulder_pan_joint
            # sinewave =  0.5 * math.sin(2 * math.pi * 0.5  *time.time())
            # msg.points[0].positions = [sinewave,0.0,0.0,0.0,0.0,0.0]

            # shoulder lift joint
            # sinewave =  -1.57 + 0.5 * math.sin(2 * math.pi * 0.5  *time.time())
            # msg.points[0].positions = [0.0,0.0,sinewave,0.0,0.0,0.0]

            # elbow joint
            # sinewave =  0.5 * math.sin(2 * math.pi * 0.5  *time.time())
            # msg.points[0].positions = [0.0,-1.57,sinewave,0.0,0.0,0.0]

            # step function shoulder_pan_joint
            if i > 20 and i < 500:
                msg.points[0].positions = [0.5,0.0,0,0.0,0.0,0.0,0.0]
            else:
                msg.points[0].positions = [0.0,0.0,0.0,0.0,0.0,0.0]

            # step function shoulder_lift_joint
            # if i > 20 and i < 500:
            #     msg.points[0].positions = [0.0,-0.5,0,0.0,0.0,0.0,0.0]
            # else:
            #     msg.points[0].positions = [0.0,0.0,0.0,0.0,0.0,0.0]

            # # step function elbow_joint
            # if i > 20 and i < 500:
            #     msg.points[0].positions = [0.0,-1.57,0.5,0.0,0.0,0.0]
            # else:
            #     msg.points[0].positions = [0.0,-1.57,0.0,0.0,0.0,0.0]

            # step function wrist_1_joint
            # if i > 20 and i < 500:
            #     msg.points[0].positions = [0.0,-1.57,0.0,1.0,0.0,0.0]
            # else:
            #     msg.points[0].positions = [0.0,-1.57,0.0,0.0,0.0,0.0]

            # step function wrist_2_joint
            # if i > 20 and i < 500:
            #     msg.points[0].positions = [0.0,-1.57,0.0,0.0,1.0,0.0]
            # else:
            #     msg.points[0].positions = [0.0,-1.57,0.0,0.0,0.0,0.0]

            # step function wrist_3_joint
            # if i > 20 and i < 500:
            #     msg.points[0].positions = [0.0,-1.57,0.0,0.0,0.0,1.0]
            # else:
            #     msg.points[0].positions = [0.0,-1.57,0.0,0.0,0.0,0.0]

            msg.points[0].time_from_start = rospy.Duration.from_sec(0.1)
            self.jt_pub.publish(msg)
            self.rate.sleep()
            i = i + 1


if __name__ == '__main__':
    try:
        ch = JT()
        ch.joint_trajectory_publisher()
    except rospy.ROSInterruptException:
        pass

I then tried my best to improve the response of the controller to sinewaves and step inputs. Here is what I got so far for the UR10:

The wrist_3_joint was not able to mantain the position at all:
wrist_3 error response to a 1.0 step input with previous pid params

wrist_3 error response to a 1.0 step input with proposed pid params

The wrist_2_joint has now a response with smaller amplitude and the oscillation is dampened faster
wrist_2 error response to a 1.0 step input with previous pid params

wrist_2 error response to a 1.0 step input with proposed pid params

Wrist 1 was not modified.

The elbow step response oscillation is now dampened faster
elbow error response to a 1.0 step input with previous pid params

elbow error response to a 1.0 step input with proposed pid params

The shoulder_lift step response oscillation is now dampened faster

shoulder_lift error response to a 1.0 step input with previous pid params

shoulder_lift error response to a 1.0 step input with proposed pid params

The shoulder_pan step response oscillation is now dampened faster, I apologize but I lost the plots of it.

I am sorry if the plots are not the best, and maybe a bit confusing given the different axis scales but I hope it shows the improvement.

@gavanderhoorn @fmauch let me know what do you think about this.

[matteolucchi]

I finished to tune the pid parameters for all the models, I am quite happy with the performance now.
I think this should be merged at the same time of #525 to fix some bad controllers behaviors.

Once this is merged and we are sure about the new inertia values in #504 I can check again if the performance is still good or if the PID params need retuning.

[fmauch]

Thanks @matteolucchi also for documenting your testing method. It will be very interesting to see, how the changes from #504 change that outcome.

[ipa-nhg]

Great contribution. Thanks a lot @matteolucchi

I made a preliminary test on simulation and it works as expected 🎉

[matteolucchi]

Hi, what is missing for this to be merged?

[ipa-nhg]

ping @gavanderhoorn, could you please take a look at the changes proposed here?

[RobertWilbrandt]

This is a great contribution, thanks @matteolucchi ! I'll close this now as it was merged through #619