Introduce Controller and MPC classes to ensure modularity and avoid global config files. #12
Conversation
Danfoa
changed the title
|
After discussing with @giulioturrisi this morning another thin TO DO is to eliminated the LegAttr Class from inside the controllerClass. I have fixed this I will push in the next few days |
|
I tested the nominal mpc with different trots and it works fine. |
…d of calling on config, added full stance to gai gernrator
|
this new commit contains:
I need to do some clean up and rerun the simulation almost everything seemed to work next commit should be the last one. |
There was a problem hiding this comment.
Hi @Giovadess, I think removing the leader variables in the simulation script, should make this PR ready to be merged.
Note that in future PRs we should start doing the PR not to the main but a devel branch which we periodically but not so often merge to main. This because multiple people are using the same codebase and updating the main frequently might not be so nice from the user perspective.
| self.swing_generator = simulation_parameters['swing_generator'] | ||
| self.position_gain_fb = simulation_parameters['swing_position_gain_fb'] | ||
| self.velocity_gain_fb = simulation_parameters['swing_velocity_gain_fb'] | ||
| self.swing_integral_gain_fb=simulation_parameters['swing_integral_gain_fb'] | ||
| self.step_height =simulation_parameters['step_height'] |
There was a problem hiding this comment.
Since these attributes are class instance attributes of the SwingController, its better if we do not store them as parameters of the control class. This in case the SwingController parameters are changed dynamically, and these values become outdated.
So if one wants to acces pos_gain_fb from the control instance once can simply do smth like: controller.stc.pos_gain_fb
| self.swing_generator = simulation_parameters['swing_generator'] | |
| self.position_gain_fb = simulation_parameters['swing_position_gain_fb'] | |
| self.velocity_gain_fb = simulation_parameters['swing_velocity_gain_fb'] | |
| self.swing_integral_gain_fb=simulation_parameters['swing_integral_gain_fb'] | |
| self.step_height =simulation_parameters['step_height'] | |
| self.swing_generator = simulation_parameters['swing_generator'] | |
| self.position_gain_fb = simulation_parameters['swing_position_gain_fb'] | |
| self.velocity_gain_fb = simulation_parameters['swing_velocity_gain_fb'] | |
| self.swing_integral_gain_fb=simulation_parameters['swing_integral_gain_fb'] | |
| self.step_height =simulation_parameters['step_height'] |
| self.velocity_gain_fb = simulation_parameters['swing_velocity_gain_fb'] | ||
| self.swing_integral_gain_fb=simulation_parameters['swing_integral_gain_fb'] | ||
| self.step_height =simulation_parameters['step_height'] | ||
| self.simulation_dt=simulation_parameters['dt'] |
There was a problem hiding this comment.
We might change this name to dynamics_dt @giulioturrisi?
To consider the simulation and real-hardware scenarios.
| gait_name=simulation_parameters['gait'] | ||
| gait_params=simulation_parameters['gait_params'][gait_name] | ||
| self.gait_type, self.duty_factor, self.step_frequency = gait_params['type'], gait_params['duty_factor'], gait_params['step_freq'] |
There was a problem hiding this comment.
Similar to the swing parameters, these attributes should only be class attributes of the PGG
| gait_name=simulation_parameters['gait'] | |
| gait_params=simulation_parameters['gait_params'][gait_name] | |
| self.gait_type, self.duty_factor, self.step_frequency = gait_params['type'], gait_params['duty_factor'], gait_params['step_freq'] | |
| gait_name=simulation_parameters['gait'] | |
| gait_params=simulation_parameters['gait_params'][gait_name] | |
| self.gait_type, self.duty_factor, self.step_frequency = gait_params['type'], gait_params['duty_factor'], gait_params['step_freq'] |
| self.stance_time = (1 / self.step_frequency) * self.duty_factor | ||
| self.swing_period = (1 - self.duty_factor) * (1 / self.step_frequency) # + 0.07 |
There was a problem hiding this comment.
Should these be @properties of the PGG class? @giulioturrisi
| self.stance_time = (1 / self.step_frequency) * self.duty_factor | |
| self.swing_period = (1 - self.duty_factor) * (1 / self.step_frequency) # + 0.07 | |
| self.stance_time = (1 / self.step_frequency) * self.duty_factor | |
| self.swing_period = (1 - self.duty_factor) * (1 / self.step_frequency) # + 0.07 |
Properties as it is expected the user cant modify these values at will.
| self.warm_start=mpc_parameters['use_warm_start'] | ||
| self.use_integrator=mpc_parameters['use_integrators'] | ||
| self.alpha_integrator=mpc_parameters['alpha_integrator'] | ||
| self.inttegrator_cap=mpc_parameters['integrator_cap'] | ||
| self.use_foothold_optimization=mpc_parameters['use_foothold_optimization'] | ||
| self.use_foothold_constraints=mpc_parameters['use_foothold_constraints'] | ||
| self.use_RTI=mpc_parameters['use_RTI'] | ||
| self.as_rti_type=mpc_parameters['as_rti_type'] | ||
| self.as_rti_iter=mpc_parameters['as_rti_iter'] | ||
| self.use_DDP=mpc_parameters['use_DDP'] | ||
| self.num_qp_iterations=mpc_parameters['num_qp_iterations'] | ||
| self.solver_mode=mpc_parameters['solver_mode'] | ||
| self.use_zmp_stability=mpc_parameters['use_zmp_stability'] | ||
| self.trot_stability_margin=mpc_parameters['trot_stability_margin'] | ||
| self.pace_stability_margin=mpc_parameters['pace_stability_margin'] | ||
| self.crawl_stability_margin=mpc_parameters['crawl_stability_margin'] | ||
|
|
||
| self.external_wrenches_compensation=mpc_parameters['external_wrenches_compensation'] | ||
| self.external_wrenches_compensation_num_steps=mpc_parameters['external_wrenches_compensation_num_step'] | ||
| self.passive_arm_compensation=mpc_parameters['passive_arm_compensation'] | ||
|
|
||
| self.use_nonuniform_discretization=mpc_parameters['use_nonuniform_discretization'] | ||
| self.dt_fine_grained = mpc_parameters['dt_fine_grained'] | ||
| self.horizon_fine_grained = mpc_parameters['horizon_fine_grained'] | ||
|
|
||
| self.use_input_prediction=mpc_parameters['use_input_prediction'] | ||
| self.use_static_stability=mpc_parameters['use_static_stability'] | ||
| self.controller = Acados_NMPC_Nominal() | ||
|
|
There was a problem hiding this comment.
All these variables should be attributes of the MPC (pased on the constructor [...] Acados_NMPC_Nominal(**mpc_params)) not the SRBController
| self.warm_start=mpc_parameters['use_warm_start'] | |
| self.use_integrator=mpc_parameters['use_integrators'] | |
| self.alpha_integrator=mpc_parameters['alpha_integrator'] | |
| self.inttegrator_cap=mpc_parameters['integrator_cap'] | |
| self.use_foothold_optimization=mpc_parameters['use_foothold_optimization'] | |
| self.use_foothold_constraints=mpc_parameters['use_foothold_constraints'] | |
| self.use_RTI=mpc_parameters['use_RTI'] | |
| self.as_rti_type=mpc_parameters['as_rti_type'] | |
| self.as_rti_iter=mpc_parameters['as_rti_iter'] | |
| self.use_DDP=mpc_parameters['use_DDP'] | |
| self.num_qp_iterations=mpc_parameters['num_qp_iterations'] | |
| self.solver_mode=mpc_parameters['solver_mode'] | |
| self.use_zmp_stability=mpc_parameters['use_zmp_stability'] | |
| self.trot_stability_margin=mpc_parameters['trot_stability_margin'] | |
| self.pace_stability_margin=mpc_parameters['pace_stability_margin'] | |
| self.crawl_stability_margin=mpc_parameters['crawl_stability_margin'] | |
| self.external_wrenches_compensation=mpc_parameters['external_wrenches_compensation'] | |
| self.external_wrenches_compensation_num_steps=mpc_parameters['external_wrenches_compensation_num_step'] | |
| self.passive_arm_compensation=mpc_parameters['passive_arm_compensation'] | |
| self.use_nonuniform_discretization=mpc_parameters['use_nonuniform_discretization'] | |
| self.dt_fine_grained = mpc_parameters['dt_fine_grained'] | |
| self.horizon_fine_grained = mpc_parameters['horizon_fine_grained'] | |
| self.use_input_prediction=mpc_parameters['use_input_prediction'] | |
| self.use_static_stability=mpc_parameters['use_static_stability'] | |
| self.controller = Acados_NMPC_Nominal() | |
| self.warm_start=mpc_parameters['use_warm_start'] | |
| self.use_integrator=mpc_parameters['use_integrators'] | |
| self.alpha_integrator=mpc_parameters['alpha_integrator'] | |
| self.inttegrator_cap=mpc_parameters['integrator_cap'] | |
| self.use_foothold_optimization=mpc_parameters['use_foothold_optimization'] | |
| self.use_foothold_constraints=mpc_parameters['use_foothold_constraints'] | |
| self.use_RTI=mpc_parameters['use_RTI'] | |
| self.as_rti_type=mpc_parameters['as_rti_type'] | |
| self.as_rti_iter=mpc_parameters['as_rti_iter'] | |
| self.use_DDP=mpc_parameters['use_DDP'] | |
| self.num_qp_iterations=mpc_parameters['num_qp_iterations'] | |
| self.solver_mode=mpc_parameters['solver_mode'] | |
| self.use_zmp_stability=mpc_parameters['use_zmp_stability'] | |
| self.trot_stability_margin=mpc_parameters['trot_stability_margin'] | |
| self.pace_stability_margin=mpc_parameters['pace_stability_margin'] | |
| self.crawl_stability_margin=mpc_parameters['crawl_stability_margin'] | |
| self.external_wrenches_compensation=mpc_parameters['external_wrenches_compensation'] | |
| self.external_wrenches_compensation_num_steps=mpc_parameters['external_wrenches_compensation_num_step'] | |
| self.passive_arm_compensation=mpc_parameters['passive_arm_compensation'] | |
| self.use_nonuniform_discretization=mpc_parameters['use_nonuniform_discretization'] | |
| self.dt_fine_grained = mpc_parameters['dt_fine_grained'] | |
| self.horizon_fine_grained = mpc_parameters['horizon_fine_grained'] | |
| self.use_input_prediction=mpc_parameters['use_input_prediction'] | |
| self.use_static_stability=mpc_parameters['use_static_stability'] | |
| self.controller = Acados_NMPC_Nominal() | |
| # Get current Rigidbody state | ||
| base_lin_vel = env.base_lin_vel(frame='world') | ||
| base_ang_vel = env.base_ang_vel(frame='world') | ||
| base_ori_euler_xyz_leader = env.base_ori_euler_xyz |
There was a problem hiding this comment.
@Giovadess I believe these leader variables are specific to your development and not to the "main branch", right ?
Code working fine in simulation.
TO DO: