How to Avoid Self Collisions using MoveIt?

Manipulators of various configurations
Post Reply
User avatar
MarkJennings
Posts: 8
Joined: Fri Jul 17, 2020 10:22 am
Contact:

How to Avoid Self Collisions using MoveIt?

Post by MarkJennings » Fri Jul 17, 2020 11:13 am

My lab, the Nuclear Robotics Group at UT Austin, built a custom Hebi arm a couple years ago for inspection and manipulation aboard a mobile robot base. Now, we are repurposing it for a different robot, and I am attempting to use the new and improved ROS API. Overall, things have gone pretty smoothly with the following steps:
  1. Define custom hrdf
  2. Get an xacro file with urdf_generator.py
  3. Use the MoveIt setup assistant to generate a config package. (thoroughly examine the allowable collision matrix to ensure there will not be unintended self collisions)
  4. Ensure everything works well in simulation
  5. Create gains and params files for real world control
At this point, I launch the arm_moveit_node to see that everything is working quite well! (video here)

However, there are instances where the arm crashes into itself, not following the MoveIt trajectory at all. (video here)

These videos was taken the day before lockdown. I thought maybe my gains were just off, so I continued to work on the robot in simulation, adding collision objects to the planning environment and incorporating joystick control. However, the collision issue still persists upon returning to the lab.

It seems the Hebi robot is not following trajectory actions very closely if at all. After tuning gains, the robot behavior is certainly better but it still crashes into itself occasionally.

Other kits have just one elbow joint which makes collisions less likely. However, I'd like to be able to fix this issue in software if possible. Any advice?
Last edited by MarkJennings on Wed Oct 14, 2020 2:25 pm, edited 1 time in total.
User avatar
hardik_singh
HEBI Official
Posts: 8
Joined: Thu Jul 09, 2020 10:49 am

Re: How to Avoid Self Collisions using MoveIt?

Post by hardik_singh » Fri Jul 17, 2020 11:53 am

Hi Mark!

Thanks for bringing this issue to our attention, we're looking into it now. It would help us debug the issue if you have a log file for those videos, or if you can send us a new log file from Scope of the arm not following the expected trajectory.

In the meantime, welcome to the forum! Excited to see someone from your lab on this platform. Whenever you get the chance, could you write a quick introduction for yourself in the Introductions topic of this forum as well? A short summary about yourself, the lab, and what you're working on would be great to see!
User avatar
MarkJennings
Posts: 8
Joined: Fri Jul 17, 2020 10:22 am
Contact:

Re: How to Avoid Self Collisions using MoveIt?

Post by MarkJennings » Fri Jul 17, 2020 12:08 pm

Absolutely! I'll try to recreate the issue and look into creating logs with ROS/Scope this weekend. Unfortunately I don't have time today to go into the lab, but I'll introduce myself on the forums :)
User avatar
MarkJennings
Posts: 8
Joined: Fri Jul 17, 2020 10:22 am
Contact:

Re: How to Avoid Self Collisions using MoveIt?

Post by MarkJennings » Fri Jul 31, 2020 5:15 pm

Hello again, sorry for the delay. I have since moved to a different lab space, so access to the Hebi arm has been difficult to come by recently.

This morning I had about 10 minutes to try and recreate the self collision errors I've been seeing. Unfortunately, everything ran great and I couldn't get it to crash into itself. This is normally a good thing :lol:

I used Scope to log everything (here's a link to download the .hebilog file). While it never had a self collision, I plotted deflections and noticed multiple spikes where it wasn't tracking properly. (Plot here)

I also recorded video if you want me to post that to YouTube. I tried to record my screen, but apparently the built-in screen recorder caps at 30 seconds by default.

Let me know if this is helpful or if you would like me to log anything else. I am using Ubuntu 18.04, ROS Melodic, with the latest firmware installed on each actuator.
Last edited by MarkJennings on Wed Oct 14, 2020 2:27 pm, edited 2 times in total.
User avatar
dave_rollinson
HEBI Official
Posts: 30
Joined: Tue Dec 31, 2019 11:58 am

Re: How to Avoid Self Collisions using MoveIt?

Post by dave_rollinson » Wed Aug 05, 2020 7:14 pm

Mark,

Thanks for attaching the log. It was only for one actuator, J1_shoulder1, but I think it gave some insight into what's going on. Occasionally the arm is being commanded to make very large moves in a short period of time. This might have something to do with a 0-2*pi wraparound that's not being checked correctly? I'm not sure.

A plot of the commanded / feedback positions in the the `herbie-log` is below. It shows a few places where the commanded positions change rapidly, leading to poor tracking until the arm converges again to a feasible trajectory. In the plot, dashed lines are commanded positions, solid lines are feedback. For much of the log, you can't see the dashed lines because the tracking is really good.
position_plot1.png
position_plot1.png (85.16 KiB) Viewed 1807 times
Zooming in on the first 'spike', it looks like the position is changing by roughly 2*pi, in about a 0.5 seconds. The trajectory generator looks like it's still working, just connecting positions that too far apart, with a duration that's too short. Again, dashed lines are commanded positions, solid lines are feedback.
position_plot2.png
position_plot2.png (49.58 KiB) Viewed 1807 times
Looking at the velocity plot of the same area, you can see the commanded velocity goes way outside if the feasible speed (it actually goes off to -35 rad/sec), but I cropped the plot to better see the feedback velocity. Again, dashed lines are commanded positions, solid lines are feedback.
velocity_plot1.png
velocity_plot1.png (47.86 KiB) Viewed 1807 times
Hope that helps,
-Dave
User avatar
MarkJennings
Posts: 8
Joined: Fri Jul 17, 2020 10:22 am
Contact:

Re: How to Avoid Self Collisions using MoveIt?

Post by MarkJennings » Thu Aug 06, 2020 10:35 am

Hey Dave,

That was really helpful. Just for the record, I can't see your images, and there is a note at the bottom saying "You do not have the required permissions to view the files attached to this post." However, I think I know what's causing my problems from just your text. (edit: I can see them when I am logged out, strange)

I'm not around the physical hardware to check right now, but I'm pretty sure joint limits were set in Scope by a previous user (or maybe the default is +-pi, not 100% sure). However, MoveIt had no knowledge of these joint limits, so it was likely giving the actuators commands outside of their range. I've updated joint_limits.yaml in the /config folder of my moveit_config and will test to see if that fixed my issue whenever I can get back in the lab. Fingers crossed!
User avatar
dave_rollinson
HEBI Official
Posts: 30
Joined: Tue Dec 31, 2019 11:58 am

Re: How to Avoid Self Collisions using MoveIt?

Post by dave_rollinson » Thu Aug 06, 2020 9:09 pm

Looking at the log again, I don't see any sign of the joint-level limits being set or reached while running. The only time the actuators don't track isn't because of them hitting any sort of set position limit, its just the commanded positions / velocities moving really fast and the actuator not being able to turn fast enough to keep up, note that last plot is velocity, not position. The limit you see in feedback is the actuator hitting its own internal velocity limit (to protect the motor from over-speeding), not a position limit..

The actuators by default don't have joint limits set, but its important to note that the actuators have +/- 4 revolutions of multi-turn absolute encoding. So as long as you run wiring in the thru-bores of the actuators you can set your joint limits to something big, like +/- 3 turns (+/- 6*pi) and you won't have to worry about wrapping up cables over time.

The issue appears to be the commands, I'm assuming coming from Move-It, being too fast to be tracked by the actuators. I'm not sure what you're using to choose how much time to take to move between waypoints but it seemed to look like you were commanding to go from something like +3 radians to -3 radians, but giving a time that was more in line for what would be noted to go from 3 radians to 3.3 radians if that makes sense. Again, that's just a guess. Let us know if we can help out further.

-Dave
User avatar
MarkJennings
Posts: 8
Joined: Fri Jul 17, 2020 10:22 am
Contact:

Re: How to Avoid Self Collisions using MoveIt?

Post by MarkJennings » Thu Aug 06, 2020 10:03 pm

I think I had joint limits working today with a not-so-elegant method and didn't see improvements in the log, so it seems you are right. (I'll leave that other thread up in case it helps anybody else, but it's not urgent to me anymore.) The commands from MoveIt are just assuming the robot can move much quicker than reality.

You noticed that:
it looks like the position is changing by roughly 2*pi, in about a 0.5 seconds
Here is my back-of-the-envelope idea why:
In actuator.xacro, the velocity limit of the 1st actuator (an X8-9) from the log I posted is set to 30 (radians per second I believe). By default, in joint_limits.yaml, MoveIt downscales velocity and acceleration limits by a factor of 10, making the velocity limit 3rad/sec. This is more in-line with what the log shows.

So my approach, which I probably won't be able to test for at least another week, will just be to decrease the default_velocity_scaling_factor and default_acceleration_scaling_factor in joint_limits.yaml. For our application, we want to run the robot arm slowly anyway since their will likely be lag between the operator and robot. Maybe this issue hasn't been apparent in the past since the design of other Hebi arms makes self-collisions much less likely?

I'll let you know how it goes!
User avatar
MarkJennings
Posts: 8
Joined: Fri Jul 17, 2020 10:22 am
Contact:

Re: How to Avoid Self Collisions using MoveIt?

Post by MarkJennings » Mon Aug 10, 2020 1:04 pm

That fixed it! I just set the max_velocity in joint_limits.yaml for each joint to 10% of the previous value. The tracking is excellent now, thank you for your help :D
Post Reply