Question regarding PID values

[OmidRezayof]

Hi @mhubii,
I had a question regarding the PID controller in the filters.cpp file.
So, my understanding from KUKA's FRI is that, by sending an incremental command to the joint positions, the FRI will try its best to get to the desired joint values asap and before the next command arrives. Therefore, if we make sure that the incremental step is small enough for the robot to catch up, why would we need a PID controller?

And also, I'm guessing the PID values were set here and the p value has set to be 10 and i and d are 0? Are the p-i-d values set anywhere else?

[mhubii]

no worries. Yes, the PID values are set in the linked file. No, there is no other source for the PID values.

Since the communication runs asynchronously, we added a PID controller. This allows for some error when frequencies are not perfectly aligned. Hope this makes somewhat sense.

You can run this software without PID and it will run mostly well for open loop control, however, you might find some unsmooth execution as well.

[OmidRezayof]

Thanks for your reply @mhubii.
I had some chance to play more with the pose_control_node and pose_planning_node.

Meanwhile, I tried changing the p value here to p = 1 and running pose planning demo.
Although the commanded steps are relatively small, it looks like the robot is unable to follow the commands and will fall behind. Yet, keeping p = 10 will improve the robot's ability to keep up with the input commands. But why is this happening? My understanding from the FRI connection is that the robot can follow the commanded motions as fast as it can, and the motions that I'm commanding are very slow. So, where (and in which code) do you think the commanded movements are limited which makes us to put an extra p = 10 (which works like a command amplifier)? Or if you think this is a communication issue regarding the frequencies, do you have any guess where is this issue happening and how it can be eliminated?

[mhubii]

the PID is e.g. invoked here:

lbr_fri_ros2_stack/lbr_fri_ros2/src/interfaces/position_command.cpp

Line 47 in 18db6ff

joint_position_pid_.compute(

a small value for P will introduce lag.

If you e.g. replace the code above and set:

command_.joint_position = command_target_.joint_position

all commands will be directly forwarded.

I am, however, beginning to understand your question. The gains are currently multiplied by the sample time (the config file is therefore not the only source). This is probably incorrect:

lbr_fri_ros2_stack/lbr_fri_ros2/src/filters.cpp

Line 64 in 18db6ff

pid.initPid(pid_parameters.p * dt, pid_parameters.i * dt, pid_parameters.d * dt,

[OmidRezayof]

Thanks @mhubii. I replaced the line with command_.joint_position = command_target_.joint_position and tried to directly send the commands to the robot.
To better express my question, please take a look at the code below:

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose
import math
import copy
import time
import numpy as np

class Move2Cart(Node):
    def __init__(self):
        super().__init__('move_2_cart')
        self.is_init = False
        self.curr_pose = Pose()
        self.communication_time = 0.01  # 10 ms
        self.pose_pub = self.create_publisher(Pose, 'command/pose', 1)
        self.pose_sub = self.create_subscription(Pose, 'state/pose', self.on_pose, 1)
        

    def on_pose(self, msg):
        
        if not self.is_init:
            self.curr_pose = msg
            self.is_init = True
            self.initial_pose = msg
            self.flag = 1
            self.flag2 = 1
        else:
            self.curr_pose = msg
            
            goal_pose = Pose()
            goal_pose.position = copy.deepcopy(self.initial_pose.position)
            goal_pose.position.x = self.initial_pose.position.x 
            goal_pose.position.y = self.initial_pose.position.y 
            goal_pose.position.z = self.initial_pose.position.z - 0.05 
            goal_pose.orientation = self.initial_pose.orientation
            command_pose = self.move_command(goal_pose, 0.01)

            if (command_pose == False):
                if(self.flag == 1): 
                    current_time_ms = int(round(time.time() * 1000))  # Current time in milliseconds
                    print(f'Time (ms): {current_time_ms}')
                    self.flag = 0    

            elif (command_pose.position.x > 0.6 or command_pose.position.x < 0.4 or 
                command_pose.position.y > 0.06 or command_pose.position.y < -0.06 or 
                command_pose.position.z > 0.6 or command_pose.position.z < 0.4):
                print('Failed, not executable')

            else:                
                self.pose_pub.publish(command_pose)
                current_time_ms = int(round(time.time() * 1000))  # Current time in milliseconds
                if(self.flag2 == 1):
                    print(f'Time (ms): {current_time_ms}')
                    self.flag2=0


    def move_command(self, goal_pose, lin_vel): # TODO: Rotational velocity
        command_pose = Pose()

        translation_vec = np.asarray([goal_pose.position.x - self.curr_pose.position.x, 
                                      goal_pose.position.y - self.curr_pose.position.y, 
                                      goal_pose.position.z - self.curr_pose.position.z])
        vel_vec = (translation_vec / np.linalg.norm(translation_vec)) * lin_vel

        if(np.linalg.norm(translation_vec)<0.001):
            return False

        command_pose.position.x = self.curr_pose.position.x + vel_vec[0] * self.communication_time
        command_pose.position.y = self.curr_pose.position.y + vel_vec[1] * self.communication_time
        command_pose.position.z = self.curr_pose.position.z + vel_vec[2] * self.communication_time
        command_pose.orientation = self.initial_pose.orientation

        return command_pose

def main(args=None):
    rclpy.init(args=args)
    node = Move2Cart()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

This is a very basic code (and not written very professional because it was intended for test purposes (: ) which moves the robot -5 cm in z direction (world frame). When the lin_vel is set to 0.01 (m/s) we expect the motion to be completed in 5 seconds. However, it takes 50 seconds for the motion to get complete. And if I change the lin_vel to 0.1 (m/s) now the motion gets completed in 5 seconds.
So my guess is, somewhere in one of the codes, the commanding values are getting divided by 10?
I just couldn't find where is this issue happening, cuz with this slow movement, the robot should almost follow the desired motion perfectly. Do you have any idea where is this division/lag coming from?

[mhubii]

thanks for sharing the code, will test it out tomorrow, hope we can fix this properly for this driver

[mhubii]

okay the PID values should now make more sense, see e.g.

lbr_fri_ros2_stack/lbr_ros2_control/config/lbr_system_parameters.yaml

Line 10 in cc03bb0

pid_p: 0.1 # P gain for the joint position (useful for asynchronous control)

I ran your code and exactly what you describe happens. Velocity of

• 0.01 takes ~50 seconds (should take 5)
• 0.1 takes ~5 seconds

When the p gain is set to 1, velocity is correctly executed but also unsmooth motion is observed.

[mhubii]

thank you again for pointing this out @OmidRezayof. So what is missing now is a controller that controls the end-effector velocity smoothly. Maybe we close this issue and continue on the somewhat related #178

[OmidRezayof]

Thank you @mhubii.
I tried some different combinations and this is the results I get:

In a nut shell, bypassing the PID controller as you suggested here will introduce noisy execution and still the motion isn't performed at the correct time. For the P value, I found that for p=12.25 the motion time is nearly 10 times slower, therefore by increasing the commanded speed in the code, I could get the motion to be done in correct timing and smoothly. This is the final code that was working (i.e. move the robot to a cartesian position while keeping the orientation constant). Note that you have to set the velocity exactly 10 times higher than the velocity in m/s:

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose
import math
import copy
import time
import numpy as np

class Move2Cart(Node):
    def __init__(self):
        super().__init__('move_2_cart')
        self.is_init = False
        self.curr_pose = Pose()
        self.communication_time = 0.01  # 10 ms
        self.pose_pub = self.create_publisher(Pose, 'command/pose', 1)
        self.pose_sub = self.create_subscription(Pose, 'state/pose', self.on_pose, 1)
        

    def on_pose(self, msg):
        
        if not self.is_init:
            self.curr_pose = msg
            self.is_init = True
            self.initial_pose = msg
            self.flag = 1
            self.flag2 = 1
        else:
            self.curr_pose = msg
            
            goal_pose = Pose()
            goal_pose.position = copy.deepcopy(self.initial_pose.position)
            goal_pose.position.x = 0.5
            goal_pose.position.y = 0.0
            goal_pose.position.z = 0.5
            goal_pose.orientation = self.initial_pose.orientation
            command_pose = self.move_command(goal_pose, 0.1)

            if (command_pose == False):
                if(self.flag == 1): 
                    current_time_ms = int(round(time.time() * 1000))  # Current time in milliseconds
                    print(f'Time (ms): {current_time_ms}')
                    self.flag = 0    

            elif (command_pose.position.x > 0.6 or command_pose.position.x < 0.4 or 
                command_pose.position.y > 0.06 or command_pose.position.y < -0.06 or 
                command_pose.position.z > 0.6 or command_pose.position.z < 0.25):
                print('Failed, not executable')

            else:                
                self.pose_pub.publish(command_pose)
                current_time_ms = int(round(time.time() * 1000))  # Current time in milliseconds
                if(self.flag2 == 1):
                    print(f'Time (ms): {current_time_ms}')
                    self.flag2=0


    def move_command(self, goal_pose, lin_vel): # TODO: Rotational velocity
        command_pose = Pose()

        translation_vec = np.asarray([goal_pose.position.x - self.curr_pose.position.x, 
                                      goal_pose.position.y - self.curr_pose.position.y, 
                                      goal_pose.position.z - self.curr_pose.position.z])
        vel_vec = (translation_vec / np.linalg.norm(translation_vec)) * lin_vel

        if(np.linalg.norm(translation_vec)<0.001):
            return False

        command_pose.position.x = self.curr_pose.position.x + vel_vec[0] * self.communication_time
        command_pose.position.y = self.curr_pose.position.y + vel_vec[1] * self.communication_time
        command_pose.position.z = self.curr_pose.position.z + vel_vec[2] * self.communication_time
        command_pose.orientation = self.initial_pose.orientation

        return command_pose

def main(args=None):
    rclpy.init(args=args)
    node = Move2Cart()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

This (i.e. setting p = 12.25 and commanding the velocity 10 times higher) is definitely not a neat solution but might be a quick solution.
I'm also writing a code for including orientation changes as well. Will share that when it gets done. It may be a good demo to get added to the lbr_demos.

[OmidRezayof]

@mhubii hope you're great.
I wrote a demo for velocity control in Cartesian space which can be useful. Based on CONTRIBUTING.md , I have to request for a new branch I guess and create a PR against that branch?
I would appreciate if you create the branch : )

[mhubii]

hi @OmidRezayof , sounds awesome! Sorry for the late response.

Created a branch for you @ dev-humble-cartesian-velocity: https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/dev-humble-cartesian-velocity

Looking forward to the PR!

[OmidRezayof]

Hi! PR created. I'm so sorry but I couldn't figure out how to aim the PR towards the created branch. :(
Hope you accept my apologies.

[OmidRezayof]

Hi @mhubii. Did you find out about why the robot exhibits jerky movements when the p value is set to 1 in the pid parameters? You mentioned it here and it also happens on my robot.
I was wondering if we know why is this happening or how we can solve it?

[mhubii]

when p=1 then the robot executes exactly what you ask it to execute. If trajectories are not smooth for some reason, then that behavior will also be replicated on the robot.

Possible solutions are:

• Avoid commanding over topics
• Don't set p=1
• Increase update_rate:
  

  lbr_fri_ros2_stack/lbr_ros2_control/config/lbr_controllers.yaml

  Line 5 in 3c8e55b

  update_rate: 100

[mhubii]

Closing this issue for now. Feel free to open new issues please, but I think your original finding is now fixed

Keeping #178 and #192