Draft: Tobi Dance Branch

Control_Toolkit_ASF_Template/Controllers/controller_barebone.py

@@ -9,7 +9,8 @@
 import os
 
 from Control_Toolkit.Controllers import template_controller
-from others.globals_and_utils import create_rng
+from others.globals_and_utils import create_rng, update_attributes
+
 
 # TODO: You can load and access config files here, like this:
 # config = yaml.load(open("config.yml", "r"), Loader=yaml.FullLoader)
@@ -32,9 +33,10 @@ def configure(self):
         pass
 
     def step(self, s: np.ndarray, time=None, updated_attributes: "dict[str, TensorType]" = {}):
+
         # The controller has to adapt when environment-related attributes such as target positions change
         # Updated targets etc. are passed as a dictionary updated_attributes
-        self.update_attributes(updated_attributes)  # After this call, updated attributes are available as self.<<attribute_name>>
+        update_attributes(updated_attributes,self)  # After this call, updated attributes are available as self.<<attribute_name>>
 
         # TODO: Implement your controller here
         # Examples:

Control_Toolkit_ASF_Template/Controllers/controller_do_mpc.py

@@ -9,7 +9,7 @@
 from CartPole.cartpole_model import Q2u, cartpole_ode_namespace
 from CartPole.state_utilities import cartpole_state_vector_to_namespace
 from Control_Toolkit.Controllers import template_controller
-from others.globals_and_utils import create_rng
+from others.globals_and_utils import create_rng, update_attributes
 from SI_Toolkit.computation_library import NumpyLibrary, TensorType
 
 
@@ -137,7 +137,7 @@ def tvp_fun(self, t_ind):
 
 
     def step(self, s: np.ndarray, time=None, updated_attributes: "dict[str, TensorType]" = {}):
-        self.update_attributes(updated_attributes)
+        update_attributes(updated_attributes,self)
 
         s = cartpole_state_vector_to_namespace(s)
 

Control_Toolkit_ASF_Template/Controllers/controller_do_mpc_discrete.py

@@ -11,6 +11,7 @@
 from CartPole.state_utilities import cartpole_state_vector_to_namespace
 from Control_Toolkit.Controllers import template_controller
 from SI_Toolkit.computation_library import NumpyLibrary, TensorType
+from others.globals_and_utils import update_attributes
 
 
 def mpc_next_state(s, u, dt):
@@ -148,7 +149,7 @@ def tvp_fun(self, t_ind):
         return self.tvp_template
 
     def step(self, s: np.ndarray, time=None, updated_attributes: "dict[str, TensorType]" = {}):
-        self.update_attributes(updated_attributes)
+        update_attributes(updated_attributes,self)
 
         s = cartpole_state_vector_to_namespace(s)
 

Control_Toolkit_ASF_Template/Controllers/controller_lqr.py

@@ -14,7 +14,7 @@
 from CartPole.state_utilities import (ANGLE_IDX, ANGLED_IDX, POSITION_IDX,
                                       POSITIOND_IDX)
 from Control_Toolkit.Controllers import template_controller
-from others.globals_and_utils import create_rng
+from others.globals_and_utils import create_rng, update_attributes
 
 config = yaml.load(open("config.yml", "r"), Loader=yaml.FullLoader)
 actuator_noise = config["cartpole"]["actuator_noise"]
@@ -81,7 +81,7 @@ def configure(self):
         self.eigVals = eigVals
 
     def step(self, s: np.ndarray, time=None, updated_attributes: "dict[str, TensorType]" = {}):
-        self.update_attributes(updated_attributes)
+        update_attributes(updated_attributes,self)
 
         state = np.array(
             [[s[POSITION_IDX] - self.target_position], [s[POSITIOND_IDX]], [s[ANGLE_IDX]], [s[ANGLED_IDX]]])

Control_Toolkit_ASF_Template/Cost_Functions/EnvironmentName/cost_function_barebone.py

@@ -9,7 +9,7 @@
 
 
 # TODO: Load constants from the cost config file, like this:
-config = yaml.load(open(os.path.join("Control_Toolkit_ASF", "config_cost_function.yml"), "r"), Loader=yaml.FullLoader)
+config = yaml.load(open(os.path.join("Control_Toolkit_ASF", "config_cost_functions.yml"), "r"), Loader=yaml.FullLoader)
 
 # TODO: Rename parent folder from EnvironmentName to the actual name of you environment
 # TODO: Load constants like this:
@@ -37,7 +37,7 @@ def get_terminal_cost(self, terminal_states: TensorType):
         # return terminal_cost
         pass
 
-    # all stage costs together
+    # all stage costs together. A 'stage' is one timestep of a rollout.
     def get_stage_cost(self, states: TensorType, inputs: TensorType, previous_input: TensorType):
         # Shape of states: [batch_size, mpc_horizon, num_states]
         # TODO: Compute stage cost

Controllers/__init__.py

@@ -1,20 +1,21 @@
 import os
 from abc import ABC, abstractmethod
-from typing import Tuple
+from typing import Tuple, Union
 
 import numpy as np
 import yaml
 from Control_Toolkit.others.globals_and_utils import get_logger
 from SI_Toolkit.computation_library import (ComputationLibrary, NumpyLibrary,
                                             PyTorchLibrary, TensorFlowLibrary,
                                             TensorType)
+from others.globals_and_utils import load_or_reload_config_if_modified
 
-config_cost_function = yaml.load(open(os.path.join("Control_Toolkit_ASF", "config_cost_function.yml")), Loader=yaml.FullLoader)
+config_cost_function = yaml.load(open(os.path.join("Control_Toolkit_ASF", "config_cost_functions.yml")), Loader=yaml.FullLoader)
 logger = get_logger(__name__)
 
 """
 For a controller to be found and imported by CartPoleGUI/DataGenerator it must:
-1. Be in Controller folder
+1. Be in Controllers folder
 2. Have a name starting with "controller_"
 3. The name of the controller class must be the same as the name of the file.
 4. It must have __init__ and step methods
@@ -26,7 +27,7 @@
 class template_controller(ABC):
     _has_optimizer = False
     # Define the computation library in your controller class or in the controller's configuration:
-    _computation_library: "type[ComputationLibrary]" = None
+    _computation_library: ComputationLibrary = None
 
     def __init__(
         self,
@@ -38,14 +39,13 @@ def __init__(
         initial_environment_attributes: "dict[str, TensorType]",
     ):
         # Load controller config and select the entry for the current controller
-        config_controllers = yaml.load(
-            open(os.path.join("Control_Toolkit_ASF", "config_controllers.yml")),
-            Loader=yaml.FullLoader
-        )
+        (config_controllers,_) = load_or_reload_config_if_modified(os.path.join("Control_Toolkit_ASF", "config_controllers.yml")) # ignore the _ changes return since this is initial call
         # self.controller_name is inferred from the class name, which is the class being instantiated
         # Example: If you create a controller_mpc, this controller_template.__init__ will be called
         # but the class name will be controller_mpc, not template_controller.
-        self.config_controller = dict(config_controllers[self.controller_name])
+        config_key=self.controller_name
+        self.config_controller = config_controllers[config_key]
+        # add timestep .dt to all controllers here
         self.config_controller["dt"] = dt
 
         # Set computation library
@@ -78,11 +78,20 @@ def __init__(
         self.num_control_inputs = num_control_inputs
         self.control_limits = control_limits
         self.action_low, self.action_high = self.control_limits
+
+        # todo these are special for cartpole but we would need a base cartpole controller class to put them there
+        # self.target_position=None
+        # self.target_equilibrium=None
 
         # Set properties like target positions on this controller
         for property, new_value in initial_environment_attributes.items():
             setattr(self, property, self.computation_library.to_variable(new_value, self.computation_library.float32))
-
+
+        # set all controller config numerical values as float variables in the computation space, e.g. tensorflow, so they can be updqted during runtime
+        for property, value in self.config_controller.items():
+            if value is float or value is int:
+                setattr(self, property, self.computation_library.to_variable(value, self.computation_library.float32))
+
         # Initialize control variable
         self.u = 0.0
 
@@ -103,12 +112,18 @@ def configure(self, **kwargs):
         # In your controller, implement any additional initialization steps here
         pass
 
-    def update_attributes(self, updated_attributes: "dict[str, TensorType]"):
-        for property, new_value in updated_attributes.items():
-            self.computation_library.assign(getattr(self, property), self.lib.to_tensor(new_value, self.lib.float32))
-
     @abstractmethod
-    def step(self, s: np.ndarray, time=None, updated_attributes: "dict[str, TensorType]" = {}):
+    def step(self, s: np.ndarray, time=None, updated_attributes: "dict[str, Union[TensorType,float]]" = dict()):
+        """
+        Execute one timestep of control.
+
+        :param s: the state array, dimensions are TODO add dimension to help users
+        :param time: the time in seconds
+        :param updated_attributes: dict of updated attributes
+
+        :returns: the next control action u e.g. a normed control input in the range [-1,1] TODO is this correct?
+
+        """
         ### Any computations in order to retrieve the current control. Such as:
         ## If the environment's target positions etc. change, copy the new attributes over to this controller so the cost function knows about it:
         # self.update_attributes(updated_attributes)
@@ -134,10 +149,17 @@ def controller_report(self):
     def controller_reset(self):
         raise NotImplementedError
 
-    @property
+    @property # decorates the controller so it has the field .controller_name that gets its short name which is the key in the .yml config file
     def controller_name(self):
+        """ Generates standard name for this controller, but use this method like it were a field.
+
+        :returns: the short name which is the key to the controller in the config_controller.yml file, e.g. 'cartpole_mppi'
+
+        """
         name = self.__class__.__name__
         if name != "template_controller":
+            if 'controller_' not in name:
+                raise AttributeError(f'this controller named "{name}" does not contain "controller_". Controllers should start or contain "controller_" and the key in the config_controllers.yml file should follow the underscore')
             return name.replace("controller_", "").replace("_", "-").lower()
         else:
             raise AttributeError()
@@ -147,7 +169,7 @@ def controller_data_for_csv(self):
         return {}
 
     @property
-    def computation_library(self) -> "type[ComputationLibrary]":
+    def computation_library(self) -> ComputationLibrary:
         if self._computation_library == None:
             raise NotImplementedError("Controller class needs to specify its computation library")
         return self._computation_library

Controllers/cartpole_trajectory_generator.py

@@ -0,0 +1,82 @@
+# stub for genrating desired future trajectory of cartpole
+import numpy
+import numpy as np
+from torch import TensorType
+
+from Control_Toolkit.Controllers import template_controller
+from Control_Toolkit.others.globals_and_utils import get_logger
+from SI_Toolkit.computation_library import ComputationLibrary
+from CartPole import state_utilities
+
+period=1
+
+log=get_logger(__name__)
+
+class cartpole_trajectory_generator:
+    """ Generates target state trajectory for the cartpole """
+    def __init__(self, lib:ComputationLibrary, controller:template_controller=None):
+        """ Construct the trajectory generator.
+
+        :param lib: the computation library, e.g. tensorflow
+        :param horizon: the MPC horizon in timesteps
+        """
+        self.lib = lib
+        self.controller:template_controller=controller
+
+    def step(self, time: float, horizon: int, dt:float) -> TensorType:
+        """ Computes the desired future state trajectory at this time.
+
+        :param time: the scalar time in seconds
+        :param horizon: the number of horizon steps
+        :param dt: the timestep in seconds
+
+        :returns: the target state trajectory of cartpole.
+        It should be a Tensor with NaN as at least first entries for don't care states, and otherwise the desired state values.
+
+        """
+
+        traj=np.zeros((state_utilities.NUM_STATES, horizon)) # must be numpy here because tensor is immutable
+        traj[:]=self.lib.nan # set all states undetermined
+
+        cost_function=self.controller.cost_function_wrapper.cost_function # use cost_function to access attributes (fields) set in config_cost_functions.yml
+        controller=self.controller # use controller to access attributes set in config_optimizers
+
+        policy=cost_function.policy
+        if policy is None:
+            raise RuntimeError(f'set policy in config_cost_functions.yml')
+
+        if policy == 'spin': # spin pole CW or CCW depending on target_equilibrium up or down
+            traj[state_utilities.POSITION_IDX] = controller.target_position
+            # traj[state_utilities.ANGLE_COS_IDX, :] = controller.target_equilibrium
+            # traj[state_utilities.ANGLE_SIN_IDX, :] = 0
+            # traj[state_utilities.ANGLE_IDX, :] = self.lib.pi * controller.target_equilibrium
+            traj[state_utilities.ANGLED_IDX, :] = 1000*controller.target_equilibrium # 1000 rad/s is arbitrary, not sure if this is best target
+            # traj[state_utilities.POSITIOND_IDX, :] = 0
+        elif policy == 'balance': # balance upright or down at desired cart position
+            traj[state_utilities.POSITION_IDX] = controller.target_position
+            target_angle=self.lib.pi * (1-controller.target_equilibrium)/2 # either 0 for up and pi for down
+            traj[state_utilities.ANGLE_COS_IDX, :] = np.cos(target_angle)
+            traj[state_utilities.ANGLE_SIN_IDX, :] = np.sin(target_angle)
+            traj[state_utilities.ANGLE_IDX, :] = target_angle
+            traj[state_utilities.ANGLED_IDX, :] = 0
+            traj[state_utilities.POSITIOND_IDX, :] = 0
+        elif policy == 'shimmy': # cart follows a desired cart position shimmy while keeping pole up or down
+            shimmy_period=cost_function.shimmy_period # seconds
+            shimmy_amp=cost_function.shimmy_amp # meters
+            endtime=time+horizon*dt
+            times=np.linspace(time,endtime,num=horizon)
+            shimmy=shimmy_amp*np.sin((2*np.pi/shimmy_period)*times)
+            shimmyd=np.gradient(shimmy,dt)
+            traj[state_utilities.POSITION_IDX] = controller.target_position+shimmy
+            target_angle=self.lib.pi * (1-controller.target_equilibrium)/2 # either 0 for up and pi for down
+            traj[state_utilities.ANGLE_COS_IDX, :] = np.cos(target_angle)
+            traj[state_utilities.ANGLE_SIN_IDX, :] = np.sin(target_angle)
+            traj[state_utilities.ANGLE_IDX, :] = target_angle
+            # traj[state_utilities.ANGLED_IDX, :] = 0
+            traj[state_utilities.POSITIOND_IDX, :] = shimmyd
+        else:
+            log.error(f'cost policy "{policy}" is unknown')
+
+        # traj=self.lib.to_variable(traj, self.lib.float32)
+
+        return traj