Faster following a desired trajectory

[ncovic1]

Hello To Whom It May Concern,

I am currently working on my PhD research, and I am using your xArm6 robot to test my recently developed real-time motion planning algorithms in dynamic environments on real-world scenarios. Accordingly, I need super fast response of the robot, since I consider obstacles' velocity even up to 1.6 [m/s]. I want to achieve better/faster following a desired trajectory. In the attachment real_time_planning_test1.pdf, I am sending figures where you can see both a desired and measured position and velocity for each robot's joint.

I read all previous issues on GitHub related to my question, yet there are few concerns I want to discuss about. Remark: When I mention "FILE" in text, it is related to "xarm_controller/config/xarm6_controllers.yaml" file.

1. Robot's firmware and software are updated to the last version 2.4.0. Is there any newer version?
2. I represented a desired trajectory as series of continuous quintic splines, which surely satisfy all kinematic constraints regarding robot's maximal velocity (3.14 [rad/s]), acceleration (20 [rad/s^2]) and jerk (500 [rad/s^3]). Then, I created a publisher in ROS2 in order to publish position and velocity points (message trajectory_msgs::msg::JointTrajectory) at high frequency of 250 [Hz], as you recommend, while using Robot Mode 1. I noticed that sending acceleration does not take any effect (adding acceleration to command_interfaces and to state_interfaces in the FILE). Why? Am I right? Moreover, I tried using Robot Mode 0, but worst results are obtained, as expected, since this mode is used only for positioning.
3. In the FILE, update_rate, state_publish_rate and action_monitor_rate are increased to 250 [Hz]. Then, I tried using Robot Mode 6 (results from the attachment), as you suggest to use it for online joint space replanning to new joint angles, while considering maximal joint velocity and acceleration. Figures in the attachment clearly reveal the tracking error even up to 0.2 [rad] considering position. Actually, the shape of measured and desired trajectory remains the same (at least for position), while we can clearly spot transport delay around 0.2 [s], which is relatively large for my purpose.
4. Using Robot Mode 6 turned out to be a little bit faster for trajectory following than using Robot Mode 1, but still I want to achieve better following. Are there any other better modes (e.g., Robot Mode 4) that can be suitable for my purpose?
5. Why it is not possible to set maximal values for velocity, acceleration and jerk for each joint individually when using Robot Mode 6?
6. Does PID regulator include feed-forward component by default? If not, can user set it, and which value to choose? If it has been already set, can user change it? What are your recommendations regarding feed-forward component?
7. I tried changing constraints (trajectory and goal) for each joint from the FILE, but it seems they do not take any effect. I am interested in the purpose of the mentioned parameters.
8. I see that you set interface_name only to position from the FILE. Can I make my own interface for velocity?

I really hope that you will clarify my concerns, since any kind of help will be kindly appreciated. Thank you in advance.

Best regards, Nermin Covic

[penglongxiang]

Hi @ncovic1, thanks for your feedback, please allow some more time for us to look into your issue and give the detailed reply. In order for us to reproduce, would you please provide more information? Like which launch file and arguments you use for start, and do you have a sample trajectory message list for us to take a look?

[ncovic1]

Thank you @penglongxiang for your willingness to help.

The launch file I am using is launch_file. As for the sample trajectory that I want to publish, you can take a look at the file real_time_planning_test1.log. The obtained results are real_time_planning_test1.pdf.

As for the code, it is relatively simple. I send a trajectory in the function publishingTrajectoryCallback from the file RealTimePlanningNode. In my code, Trajectory::getTrajectoryMaxTimeStep() is 0.004 [s] corresponding to 250 [Hz], and the function DP::spline_next->getPosition(t, i) gives i-th joint position at time instance t from a computed spline. Finally, with xarm_client.set_servo_angle, the current robot joints' position is published.

By the way, the mentioned spline surely satisfies all kinematic constraints on each joint's maximal velocity, acceleration and jerk, thus the robot should be capable of following it without any delay. In my example, I got a delay around 0.2 [s], which is relatively large for my purpose.

My real robot works very smooth and fast, and everything is great except that the transport delay (between measured and desired position) is produced, which occasionally causes unnecessary collision with environment.

If you have any more questions, feel free to ask. Best wishes, Nermin

[penglongxiang]

Hi @ncovic1, to answer your questions in initial post:

1. Currently the latest firmware version is 2.4.0. We will release 2.5.0 shortly.
2. After taking a brief look at your specified code. It seems that you are not using ROS controller but XArmROSClient, our API wrapper to control your real xArm. Actually with ROS JointTrajectoryController running, Our robot will not respond to trajectory_msgs::msg::JointTrajectory in Mode 6. Instead, it can respond with position joint interface in Mode 1, and velocity joint interface in Mode 4. In mode 1, only position is effective and it is immediately executed without additional trajectory interpolation, so it is the job of upper layer to ensure a smooth interpolation, I think your case should be OK with Mode 1. Mode 0 will not be suitable since it will finish each trajectory point with 0 velocity. Please take a look at our Mode explanations here.
3. the parameters in the FILE will not have any effect if you are using xArmROSClient but not ROS control related method.
4. Actually I thought the best mode for your application would be Mode 1 as stated in (2), since you can generate smooth trajectory and feed at a high frequency. Mode 6 will involve additional internal planning inside our control firmware with specified max velocity, acceleration and jerk, but it is relatively safe if command is sparse or not smooth.
5. In mode 6, we use a unified maximum value for all the joints. In joint velocity mode 4, user can specify target velocity for each individual joint, however, acceleration and jerk are still shared value for all. Please note if you use mode 4, only velocity will be the target and position will not be assured.
6. As far as I know, there is feed-forward part in our motor PID control, But I am not sure whether user is allowed to change the parameter, I will get back to you on this later on.
7. As mentioned previously, those parameters are used by ros controllers, the meaning of these parameters may be found in the source code of ros2-control and ros2-controllers. However, based on your provided code and control method, these will not have effect now. You can explore these parameters with our provided Moveit demo or moveit_servo demo.
8. For our Moveit related demo, if you specify "velocity_control:=true" in the launch file, the ros controller manager will use xarm6_velo_traj_controller (velocity controller) instead of xarm6_traj_controller (position controller), and the operation mode of real robot will be Mode 4 (joint velocity) instead of Mode 1 (servo position). Still, your implementation with xArmROSClient will not be using ros-controllers with these parameters.

As for the delay, Did you tried with a larger joint acceleration and jerk settings for Mode 6 operation? What are the current values configured? Additionally, would you mind to re-do your experiment test again and download the log immediately after that? I think it can better help for a closer diagnose. The log can be downloaded by UF studio: Settings -> My Device -> Log -> Download.

[ncovic1]

Thank you very much @penglongxiang for your prompt and detailed reply.

OK, as I see, the best option for me should be to use trajectory_msgs::msg::JointTrajectory in Mode 1, and then changing the parameters from the FILE should take effect (if I understood correctly). Actually, I have such kind of implementation here, but I have put my focus all time on Mode 6. Unfortunately, I did not try to change the mentioned parameters from the FILE while being in Mode 1. I have only changed them while being in Mode 6. I will do it and notify you about the obtained results.

I have one question when using Mode 1. Is the same result achieved when I send position points using set_servo_angle_j(position) from xarm_api::XArmROSClient at high frequency, and when I push several future position points (with time instances) to trajectory_msgs::msg::JointTrajectory message (something like in function addPoint)?

Please let me know more details regarding feed-forward component within PID regulators. It is very important information to me.

I did not try to set a larger joint acceleration and jerk settings in Mode 6. I used maximal velocity (3.14 [rad/s]), acceleration (20 [rad/s^2]) and jerk (500 [rad/s^3]). I can try larger values and see what happens.

I will re-do my experiment test again and download the log, and I will notify you about my progress.

Thank you again. Nermin

[penglongxiang]

Hi @ncovic1, there are some more points to clarify.

1. Those parameters in the FILE will only take effect when you send your command in trajectory_msgs::msg::JointTrajectory format, that will be further processed/interpolated by ros2 controller (JointTrajectoryController with position or velocity interface) with the parameters specified in the FILE. However from your implementation code I did not see the ros message format being published, if I am not missing anything. The way in your implementation will bypass ros controller and directly command the xArm (by our ROS API) with your generated trajectory target in your self-controlled frequency.
2. In Mode 1 our firmware will not do further interpolation, smoothing or velocity control, it is the task of upper layer. After proper trajectory generation / motion planning with controlled velocity and acceleration, the upper layer will have to do a discretization with desired control frequency and only send the target position at that time-spot to xArm firmware. This is exactly what ros controller did as an upper layer, so it can work with xArm in Mode 1 with position interface.
3. In Mode 6, our firmware will do further trajectory planning with specified velocity and acceleration, so the upper layer will not have to do complicated task of trajectory planning as mentioned above. However, it is not a proper mode working with ros-controllers (as mentioned previously ros controller interface with xArm in Mode 1 or 4). In your current implementation, it works since you are directly interfacing with xArm without ROS control in the middle. You may not have to send command in such a high frequency as 250Hz, since the firmware will do additional planning anyway in mode 6, the actual max speed will be determined by the speed/acceleration specified in set_servo_angle() interface.
4. The reason for suggesting Mode 1 for your application is, you can do exactly what ros-control does (motion planning and trajectory discretization at a fixed high speed), so indeed you do not need ros-controller in the middle (as well as parameters in the FILE). If you can generate proper trajectory, all you need to do is changing the mode to 1 in your current implementation and send proper target position for that time-spot with set_servo_angle_j() in a fixed high frequency (250 HZ). It is also important to ensure a good real-time performance for your computer and a reliable communication connection (cabled) with xArm hardware.

[ncovic1]

Hi @penglongxiang. Thank you for your prompt reply.

1. Of course, I do have an implementation for publishing trajectory_msgs::msg::JointTrajectory messages in (line 231 from computeTrajectory function, where Trajectory::publish function is implemented here, which I mentioned before. However, this is not a problem, publishing points works very well. The main problem is following a desired trajectory. Today, I have tried reducing position tolerance constraints (only trajectory from 1.0 to 0.1, while goal remained at 0.01) for each joint from the FILE, thus the robot has started shaking and the following messages have been displayed: As we can see, the robot is holding position when the state tolerance is violated. When I set all constraints (trajectory and goal) to zero as proposed here, the robot works smooth, but with delayed response (between 0.1 and 0.2 [sec] depending on velocity). I have an idea of implementing some kind of my own compensation than can emulate feed-forward component. In other words, I can send a modified reference trajectory which already contains feed-forward term, and which will hopefully reduce the tracking error.

2. OK, that is clear now. I am only curious about the velocity and acceleration in waypoints? Are they zero, or are they estimated by knowing next position and required time for reaching it?

3. OK, that is also clear now. You are "wasting" several dozens of milliseconds for a trajectory (spline) computation between current and next point, while satisfying all kinematic constraints on maximal velocity, acceleration and even jerk. Therefore, delay is inevitable. I am not sure how much time your functions for a spline computing consumes, but mine implementation takes approximately less than 1 [ms]. You can take a look and use it if needed.

4. Today, I have noticed that sending points in my current implementation of publishingTrajectoryCallback function was not regular at all. After I have printed time stamps of points that are sent/published, I have noticed irregular (not equidistant) time intervals between adjacent points. The time interval between samples (discretization step) should be 4 [ms] when using 250 [Hz], but it varied sometimes even up to 20 [ms]. OK, that was my fault, since I have not enabled this callback to be executed concurrently with other callbacks. In order to solve this, I added rclcpp::executors::MultiThreadedExecutor executor here, so that all callbacks from my real_time_planning_node node (they are defined here and here, but they are not relevant now) can be executed concurrently. Finally, it worked for all my callbacks, except for callbacks from your XArmROSClient client. After including this client, I have got the following error: I repeat, when I do not use rclcpp::executors::MultiThreadedExecutor executor, no errors occur, but then I do not have regular time intervals between samples, thus the robot may act strange (shake or follow a desired trajectory with huge error).

Thank you again in advance, and I hope to hear your response. Best regards, Nermin

[penglongxiang]

(2) The position command will be executed immediately with maximum allowed joint speed (180 deg/s). So if the command is far away, error may occur.

(4) There seems to be conflicts under MultiThreadedExecutor case, since there is underlying rclcpp::spin_until_future_complete(node_) call (refer here) when xarm_ros_client instance calls for a service. Not sure if there will be problems if this get executed multiple times concurrently. We will take a further look at this issue.

BTW, could you please check your xArm's Serial Number and tell us? It is labled on robot base, or can be checked in UF Studio system information.

[ncovic1]

Of course, here are the information from UF studio:

[penglongxiang]

Hi @ncovic1, thanks for your info. By the way, could you try adding a callback_group to XArmROSClient with rclcpp::CallbackGroupType::MutuallyExclusive type explicitly, something like adding:

rclcpp::CallbackGroup::SharedPtr callback_group = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); 

in XArmROSClient::init() function. And specify this callback_group for client_set_servo_angle_j_ creation here?

I am not sure if the Reentrant callback_group specified in the BaseNode will be inherited by xarm_ros_client node, and while executing in parallel, the underlying spin_until_future_complete() will report that error.

[penglongxiang]

Hi @ncovic1 we made some changes to XArmROSClient logic and removed spin_until_future_complete() inside, and tested with a similar structure with your application without runtime error. You may try it first if interested by replacing the xarm_ros_client.h and xarm_ros_client.cpp files inside xarm_api package with those in the uploaded zip file. xarm_ros_client.zip

This new logic would be updated to this repository after more test and optimization later. Please ignore my suggestion in last comment, it will not work.

[ncovic1]

Thank you @penglongxiang. I will try it today, and notify you about the results.

[ncovic1]

Dear @penglongxiang,

Considering your new files, xarm_ros_client.h and xarm_ros_client.cpp, runtime error does not occur, thus code runs flawlessly (all my callbacks run in parallel), and everything looks great except the real robot. Unfortunately, it starts moving for a few seconds, and then suddenly the following controller error appears (of course that my robot does not collide when the error appears): despite I send position points at 250 [Hz] (every 4 [ms]), which you can clearly observe from time stamps of the message Inside publishingTrajectoryCallback... in the following figure:

I repeat that my trajectory surely satisfies all constraints on maximal velocity, acceleration and jerk, which in this case were reduced to 1 [rad/s], 10 [rad/s²] and 100 [rad/s³], respectively, but controller error has still appeared.

Btw, I have a question that I forgot to ask you. When I try publishing points using trajectory_msgs::msg::JointTrajectory message, sometimes (maybe every two minutes while the robot is working), the following controller error appears: despite all joint velocities are within their limits (used 1.5 [rad/s] for each joint). Can I disable this linear speed checking, since it is not relevant to my application? I only consider joint constraints.

Thanks in advance. Nermin

[penglongxiang]

Hi @ncovic1 I am not quite sure about the first C31 error cause. I may have to check the log from your controller box. Would you please reproduce this error and then stop the program and download the log package immediately? Since the servo command is in high frequency and the log history is limited. My random guess would be received trajectory not being smooth due to some jerk or communication delay, I will have to check the log for a final diagnose. In fact you can try with a lower update frequency, and the hardware can still perform good with 125Hz or even 100Hz control, with lower frequency from user layer the real-time performance stress will be less and maybe it will be less error prone.

As for the second question about C60 error, it happens because our controller firmware will monitor the actual linear speed in servoj mode, in case it is driven too fast and become unsafe. Currently this configuration can only be modified through SDK interface set_linear_spd_limit_factor(), this function takes a factor input as the multiplier of nominal max linear speed (1000 mm/s), default value is 1.2, It may be violated when arm is stretched out, you can set this limit to a larger value, but be careful for the execution. Here is the sample code for setting this factor in Python SDK (use 1.5 as example):

from xarm.wrapper import XArmAPI

ip = '192.168.1.xxx'  # your controller IP address
arm = XArmAPI(ip)
ret = arm.get_linear_spd_limit_factor()
print('BEFORE setting, linear_spd_limit_factor is: {}'.format(ret[1]))
ret = arm.set_linear_spd_limit_factor(1.5)
print('set_linear_spd_limit_factor, code={}'.format(ret))
arm.save_conf() # call save_conf if you want to keep this configuration after system reboot
ret = arm.get_linear_spd_limit_factor()
print('AFTER setting, linear_spd_limit_factor is: {}'.format(ret[1]))

[ncovic1]

Hello @penglongxiang,

You can download my log file here. I hope you will find out what causes the problem.

I tried reducing the update frequency to 100 Hz, but still got the same C31 controller error. The following figures (for 100 Hz and 250 Hz) reveal that position commands are really sent in real-time:

Moreover, you can inspect these fig files for more details. The error occurs after cca 1 [s].

I believe you can find a solution. Best wishes, Nermin

[penglongxiang]

Hi @ncovic1, I found something strange in the log. It seems that your command always comes duplicated, that is, the controller will receive two identical command every cycle (4ms or 10ms), here is a snippet of received command log:

you may notice the duplication by the time info on the left side. If commands comes too close, the small time interval may cause incorrect estimation of command acceleration and thus incorrect theoretical joint torque estimation. We will add some logic to prevent this situation in the future.

Meanwhile I looked back to your launch file, you might have triggered two "uf_driver"nodes since both robot_moveit_servo_launch and robot_driver_launch will launch one driver node. That may result in two controlling sources and conflicted robot commands. Would you try just using one of the two parts, like removing 'robot_moveit_servo_launch' part if you are not using moveit for additional planning and controlling, and specifying additional parameter 'show_rviz' : 'true' to the robot_driver_launch part if you need rviz showing the robot status. Also try with 100Hz or 125Hz command frequency to ensure a more stable command acceleration estimation.

I will continue to see how to optimize the higher frequency servo control situation. The above suggestion is for avoiding the C31 error under current controller firmware logic.

[ncovic1]

Dear @penglongxiang,

Thank you very much for your prompt reply and for your hopefully found solution. I am on vacation this week, thus I will try your suggestions next week.

Best regards, Nermin

[ncovic1]

Hi @penglongxiang,

You are absolutely right, my launch file triggers two uf_driver nodes causing conflicts.

Now, Robot Mode 1 works flawlessly, as you can see in the following figure: Mode1_following_pos.pdf

Still there is delay of 0.1 [s] of the response signal - joint position. However, if I send my desired trajectory in advance of 0.1 [s] (since my trajectory is completely known), I can achieve very satisfying following (without any delay), as you can see in the following figure: perfect_following_pos_ff_0.1.pdf

Thank you again for your very kind and selfless support! I think that you can close this issue as resolved. Best regards, Nermin

[penglongxiang]

Glad to know it works. I will close this issue for now.