Pull request code refactoring #3
Conversation
- now abstract controller moved in controller.py; - added class safe_set that manages and contains the safe set constraints, both in case of network and analytic set - changed initial operations to generate robot joints list - added EE jacobianmember - now torque bounds moved in URDF description - added robot capsule setup - method checkSafeConstraint general, it changes based on particular safe set class - removed setNNmodel, moved in set_safe.py -check collision method moved from controller to model - method that generates the list of external constraints and bounds (due to collisions pairs) - method that generates casadi fun of collision constraints
- added automatic external constraints assignment, using generateNLconstraints from AdamModel - terminal and running constraints safe set methods now manages both the case of NN constraint and analytic set constraints - reset horizon method overloaded by child classes if needed - added parallel controller
- removed obstacles and capsules as arguments - added inverse kinematics problem - check collision same as that in acados controller (method of adam model used) - general collisions constraint assignment (from attribute in model)
- added capsules, obstacles and collision pairs assignment. Now these are defined in config.yaml and are managed in class Parameters - only gelu network used now - frame name moved in config
… getters of constraints experession, constraints casadi function and bounds
- added number of controlled dofs and number of safe set's dof - flag to use net (setting to true) or analytic set(false), to not use it set null - added flag to use or not the capsules defined (if you want to use only ee-obtacles collision pairs)
…functions, in such a way that same expressions of optimal control problem expressions used
…, inertia and center of mass used for dynamics integration
There was a problem hiding this comment.
Thanks for the PR @EliasFontanari , it seems you're moving in the right direction overall. However, there are still a few things to fix before merging this. See my detalied comments below.
There was a problem hiding this comment.
Please remove the folder fr3_description as it should not be a part of this repository. It should simply be mentioned on the README as an optional dependency (I guess it's already in another repo somewhere on github).
| super().__init__(model) | ||
|
|
||
| # constraints bounded | ||
| self.constraint_bound = 'zl' |
There was a problem hiding this comment.
what does "zl" means? Add a comment to explain
There was a problem hiding this comment.
I this we could get rid of this information if we add the slack variable on both sides of the inequality, can't we?
There was a problem hiding this comment.
That variable indicates if the cost of the slack has to be set on the lower or upper bounds' slacks, in case of soft safe set constraint. A way to get rid of this information could be to have a convention, as for example having always the constraints lower bounded.
There was a problem hiding this comment.
I think adding the slack on both sides of the inequality is better because it allows for more flexibility. Can't we do that? Instead of having:
lb <= g(x) <= ub
we can have:
lb-s <= g(x) <= ub + s
with s>=0.
src/safe_mpc/safe_set.py
Outdated
| device='cpu', | ||
| name=f'{self.model.params.urdf_name}_model', | ||
| build_dir=f'{self.model.params.GEN_DIR}nn_{self.model.params.urdf_name}') | ||
| self.nn_model = cs.if_else(self.model.p[4] > 0, |
There was a problem hiding this comment.
The fact that the safe-set constraint is deactivated based on the value of a parameter should not appear in this class, but it should be introduced in the controller (actually only Receding and Parallel need this)
|
|
||
| # Cost | ||
| self.Q = self.model.params.Q if not(self.model.params.track_traj) else 5e2 * np.eye(3) | ||
| self.R = self.model.params.R * np.eye(self.model.nu) |
There was a problem hiding this comment.
I think the cost function should not be defined in the AbstractController because the cost function depends on the specific test that you wanna do, so it should be defined by the user in the test script. Also you should avoid hard-coded values such as this 5e2. We could add to AbstractController a method compute_cost that needs to be defined in the child classes.
Code refactoring, implementing discussed changes:
ostacles, capsules and collision pairs can be set in configuration file;
all check constraints function moved in model, and they are the same expressions used as constraints in controller's optimal control problem;
model (env_model.py) and controller now are separated;
added safe_set class, containing constraints of the network set or of the analytic set;
added trajectory tracking;
added system dynamics uncertainty ( automatically a new urdf file is created with mass, inertia and center of mass position alterated, and the new urdf is used for the dynamics of the integrator). Possible, but not done yet, also to implement a randomization directly with adam.