Unexplained slowdown near singularity

[bjwindham]

On occasion when using Moveit and OMPL to generate paths for my GP88, a valid path will be found, and I'll send the plan to the robot. All that will happen is that the visualization in RVIZ will move very slowly while my actual robot won't move at all and I just get stuck with a message in my terminal stating "Execute request accepted". I think the culprit is that the trajectory created by OMPL is over the size limit.

If I turn up the velocity scaling factor, thus increasing available speed, it becomes less and less of an issue. However, I have a lot of expensive and fragile equipment attached to my end effector, so reducing speed to comfortable level is ideal.

As I've turned up the velocity I've noticed that I'm having an issue where when my GP88 is near-singular, I get a noticeable slowdown. I'm using OMPL so I assume that all planning takes place in joint space thus, there should be no issue with singularities. The only thing I can think of is that since I do have constraints on my end effector (they're pretty liberal), maybe that could be part of it? Or is there some logic on the Yaskawa side that limits joint velocities when near singular?

I've been chasing the issue for a couple days now. Has anyone seen a similar issue or have insight as to why this may be happening? I also checked to see if I had any limitations from the FSU and there is nothing listed on my teach pendant.

Below is a google drive link so you can see the speed being limited when the manipulator is near-singular.

I appreciate any insight that anyone can provide.

https://photos.app.goo.gl/yjnhg3V4AVZzf9jh8

[gavanderhoorn]

Could you please attach an example of a trajectory which has the slowdown you observe?

An example of a trajectory which results in the "stuck terminal" would also be nice, as we could check a couple of things.

---

Edit: oh and please also attach a copy of a debug listener log of a session which has the slowdown.

[bjwindham]

I will try to get you this information. I'm still very new to ROS2 and C++ and I don't know exactly how to pull out the trajectory information from the plan returned from moveit::planning_interface::MoveGroupInterface::Plan. I will try to figure it out today.

[gavanderhoorn]

The Plan struct contains the trajectory in one of the fields (docs). That'd be a RobotTrajectory, which has the JointTrajectory in one of its members.

[bjwindham]

gavanderhoorn, I have a .txt file with the trajectories in it. Note that there are 3 trajectories, and that if "failed" on the last one. By "failed", I mean, came up with a valid plan moved in RVIZ (very slowly) and my actual hardware made no movement. It seems like when the time to complete a trajectory is greater than 12.7 seconds, it fails. Attached is the .txt of trajectories.
GP88_trajectories.txt

[bjwindham]

@gavanderhoorn, I'm currently trying to figure out how to perform that test without asking a bunch of questions on here. As stated before, I'm very new to C++ and it's been a bit of a learning curve for me. I'm going to get one of our software guys to help me out. I haven't forgotten about the thread. I'll report back when I figure it out.

[gavanderhoorn]

Thanks for the update @bjwindham.

I'm currently trying to figure out how to perform that test without asking a bunch of questions on here.

you might want to post on either robotics.stackexchange.com or the MoveIt 2 discussion board. They may be able to help you extract just the needed bits of that test.

You really only need to retime the trajectory with the specific time-parameterisation algorithm while specifying the desired nr of points.

[gavanderhoorn]

@bjwindham: did you in the end figure this out?

[gavanderhoorn]

Friendly ping @bjwindham

[bjwindham]

@gavanderhoorn

Apologies for the radio silence. I was able to accomplish what I needed to using quick workarounds which I'll explain below. As a note, the reason for the excessively long trajectories, was due to the constraints on the end effector and being near singular. Usually the issues associated with singular configurations (a non-invertable Jacobian) are not present when planning in joint space. However, in order for my manipulator to perform these movements and meet my orientation constraints, high joint speeds were necessary, specifically on the r axis and t axis, and since I had limited my joint speeds, the desired trajectories weren't achievable in under 20 seconds (or 200 trajectory points). The short term fix for this was to limit the joint rates for the first three joints, while giving the last three access to their full range of velocities. Limiting the first three axes prevented large linear velocities of the TCP that were making people "nervous". Now that the demo that all of this was for is complete, I'm actively working on reparameterizing the velocities in order to be able to limit them to at most 199 trajectory points. I'll keep you updated, and again, apologies for being absent.

[gavanderhoorn]

Slightly off-topic, but: @ted-miller:

This test shows how to instantiate invoke TOTG and use it to re-parameterise a RobotTrajectory while specifying a maximum/fixed number of trajectory points.

I haven't tested it, but if this works, this might be something to add to our documentation.

It would essentially allow "any" trajectory to be resampled to always be encoded in a specific number of points, instead of whatever MoveIt calculates it needs.

Still wouldn't be ideal, but given the constraints we currently have, would at least be an improvement.