Feedback wanted: how to update joint limits dynamically

AndyZe commented 2 years ago

I have a use case where the robot is picking a heavy payload, and it should move more slowly while carrying the payload. We are hoping to implement runtime configurable velocity & acceleration limits for this. The vel & accel (VA) limits are currently stored in the planning scene (moveit::core::RobotModelConstPtr robot_model_). Note, it's const :(

The RobotModel stores the PVAJ limits in a JointBoundsVector. Or you can get the bounds for any individual joint.

I have about a week to implement this, not oodles and oodles of time.

Where are VA limits used?

Grepping through the codebase, it looks like these are the pieces that actually use VA limits. It's not so many places.

    Pilz joint_limits_aggregator
    time-parameterization algorithms in moveit_core/trajectory_processing
    RobotState isValidVelocityMove() --> Probably could delete. Not used internally
    Servo

How would (Servo / MTC / time-parameterization plugins) work if the VA limits are updated?

MTC already has setRobotModel() and loadRobotModel() functions so I don't think it would have to change
Time-parameterization plugins: won't need any change because they get a new copy of the RobotModel every time they're called
Servo uses the RobotModel in the constructor. I could probably add a reinitialize service call that would stop motion then re-initialize a few things.
Pilz: I think this would be similar to Servo that a few things need to be re-initialized if the RobotModel changes.

Here are some ideas I'm hoping to get feedback on.

Proposal 1 - non-const RobotModel pointer

Just make the RobotModel pointer in the Planning Scene non-const so it can be modified. Add a setLimits() method to the RobotModel. Add a MoveGroup service call to trigger this.

Proposal 2 - Store the vel/accel/jerk somewhere besides the RobotModel

This would be a big refactor, I think. Position limits should prob stay in the RobotModel because they are used in many, many places in the codebase.

Proposal 3 - Multiple planning groups

Don't change the MoveIt codebase much. Just use multiple planning groups, each with their own joint_limits.yaml

For us, that would still require a new reinitialize service in Servo.

henningkayser commented 2 years ago

Proposal 1 would arguably be the most difficult as it would require proper synchronization. You would basically solve #208 with that.

Proposal 3 might not work because the limits are not stored in the groups afaik.

I'd go for proposal 2, ideally in a way that supports overriding defaults in the RobotModel dynamically. It would be really useful if this would also support other runtime configurations like constraints and planner settings :thinking:

rhaschke commented 2 years ago

Sorry for the late reply, I was on holidays. The joint limits in RobotModel are considered maximal limits. If you want smaller values, you can already do so, specifying scaling factors for the time-parameterization plugins, can't you?

AndyZe commented 2 years ago

@rhaschke That makes sense. We do need scaling factors for each joint, however. So the new TOTG function would be something like this:

  bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const std::vector<std::string>& joint_names,
                         const std::vector<double>& velocity_limits, const std::vector<double>& acceleration_limits,
                         const std::vector<double>& jerk_limits) const override;

It's also important to us to modify these limits at runtime from MTC, so would you support adding functions like these to MTC? (I guess this would be for MoveTo, MoveRelative, and Connect stages):

void setJerkLimits(const std::vector<std::string>& joint_names, const std::vector<double>& jerk_limits);

mechwiz commented 2 years ago

Hey all, just weighing in here since this is something I think will be useful. Instead of having one argument being the vector of joint_names and then the actual limits arguments matching the size and order, I think it would be better to use a map of joint names to limit values like below. This way you can specify specific limits for specific joints. Any limits left out will just use what's stored in the robot model.

  bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory,
                         const std::map<std::string, double>& velocity_limits, const std::map<std::string, double>& acceleration_limits,
                         const std::map<std::string, double>& jerk_limits) const override;

tylerjw commented 2 years ago

I'm just here to say that if you can do this without creating more mutable data in moveit that would be really good. The more mutable things you have the more synchronization you need and that creates opportunities for bottlenecks and race conditions. There are techniques to create immutable data structures that can be inexpensively copied and that is how you mutate it safely. Here is a talk on these data structures: https://www.youtube.com/watch?v=sPhpelUfu8Q

mechwiz commented 2 years ago

How would this work for servo? I don't believe servo uses the time-parameterization plugins since it's not planning a trajectory. We would still need to update the robotmodel somehow for this to work in servo no?

rhaschke commented 2 years ago

It's also important to us to modify these limits at runtime from MTC, so would you support adding functions like these to MTC?

Sure, when we extend the time-parameterization API, this should be reflected in higher-level API as well, including MTC.

moveit / moveit2