Tips for robots with wildly different acceleration/velocity limits

[peter-mitrano-bg]

I'm working with a robot that has a base joint that is very slow (2 rad/s^2 acceleration, 2 rad/s velocity) but the other 6 joints are faster (20-200 rad/s^2 and 6-12 rad/s)

I noticed that trajopt fails to solve reliably in this case. To reproduce the issue with the ur5e, change the acceleration limits to this:

      max_acceleration: [1.2, 12, 12, 12, 12, 12]

Then run this:

motion_gen_reacher.py --robot ur5e.yml

And try moving the block around. You can see it often fails to file a plan, claiming the traj-opt or traj-opt fine tuning failed.
Moving it left/right fails often. Moving up and down works great (because base joint does not get used much)
My hypothesis that what's happening is the IK solver is returning goal config that all require moving the base joint, but there just isn't enough time to get there while obeying the low acceleration limits.

How should we go about configuring the solver to handle this case?

[peter-mitrano-bg]

After further testing, this seems to be a problem caused by low acceleration period, not a difference in acceleration. Setting all the joints to low acceleration also prevents planning. It seems that increasing these arguments to the MotionGenConfig help:

maximum_trajectory_dt=1.0,  # making this bigger helps
trajopt_tsteps=100, # also bigger seems to help

I'm still struggling to have it find solutions though. Those params help, but they don't fully solve the problem.

[balakumar-s]

Ideally, you wouldn't need to increase trajopt_tsteps. Large trajopt_tsteps (>32) are only needed if the robot needs to change direction of motion very often in a trajectory and 32 waypoints are not sufficient ( we haven't found a problem that needs more than 32).

You are correct that you need to increase maximum_trajectory_dt as that is what we use to check if optimization is successful. We found that maximum_trajectory_dt was a good heuristic to measure if the trajectory is smooth enough.

The bound cost would push the joint-0 motion to be within limits while enabling other joints to move more to reach the Cartesian Pose (if there is sufficient manipulability). In your arm are all 7 dof revolute joints or base joint is linear (trying to understand manipulability) ?

[peter-mitrano-bg]

All dof are revolute, and actually there are two base joints that are both quite slow, but I have also been testing with just one base joint. Regardless, I am certain manipulability is not the problem, because planning with those base joints locked actually works pretty well.

[peter-mitrano-bg]

I've done a bit more digging, and I've found cases where there is at least one or two IK solutions that do not move the base joints (good!) but that the solver still fails. To me this suggests that some combination of dt and bounds costs are not working well. Perhaps those seeds are failing to converge for some other reason, but at least I can say there are seeds where the torso doesn't really move, and those should be able to produce valid solutions... if I had everything else configured correctly.

[balakumar-s]

Another way to debug this would be to set self_collision_check=False and self_collision_opt=False, followed by having a very simple world model to see if then it succeeds.

[peter-mitrano-bg]

Closing this because through a combination of tweaks to self collision and joint limits, changing the test cases, and most importantly setting maximum_trajectory_dt to something bigger than the default, like 0.2, I have all my test cases working currently. Ill re-open if the problem re-emerges!