NMPC Swing Up Controller based on Crocoddyl

furuta/controls/controllers.py

@@ -1,27 +1,18 @@
+import typing as tp
 from abc import ABC, abstractmethod
 
+import crocoddyl
+import mim_solvers
 import numpy as np
+import pinocchio as pin
 from simple_pid import PID
 
 
 class Controller(ABC):
     @abstractmethod
-    def compute_command(self, position: float):
+    def compute_command(self, state):
         pass
 
-    @staticmethod
-    def build_controller(parameters: dict):
-        controller_type = parameters["controller_type"]
-        # match controller_type:
-        #     case "PIDController":
-        #         return PIDController(parameters)
-        #     case _:
-        #         raise ValueError(f"Invalid controller type: {controller_type}")
-        if controller_type == "PIDController":
-            return PIDController(parameters)
-        else:
-            raise ValueError(f"Invalid controller type: {controller_type}")
-
 
 class PIDController(Controller):
     def __init__(self, parameters):
@@ -43,3 +34,138 @@ def __init__(self, parameters):
 
     def compute_command(self, position: float):
         return self.pid(position)
+
+
+class SwingUpController(Controller):
+    class FurutaActuationModel(crocoddyl.ActuationModelAbstract):
+        # TODO : Implement in c++ if too slow
+        def __init__(self, state):
+            nu = 1  # Control dimension
+            crocoddyl.ActuationModelAbstract.__init__(self, state, nu=nu)
+
+        def calc(self, data, x, u):
+            assert len(data.tau) == 2
+            # Map the control dimensions to the joint torque
+            data.tau[0] = u
+            data.tau[1] = 0  # Underactuated joint
+
+        def calcDiff(self, data, x, u):
+            # Specify the actuation jacobian
+            data.dtau_du[0] = 1
+            data.dtau_du[1] = 0
+
+    def __init__(self, robot: pin.RobotWrapper, parameters: tp.Dict, xref: np.ndarray):
+
+        # Read parameters
+        control_freq: float = parameters["control_frequency"]
+        t_final: float = parameters["t_final"]
+        self.u_lim: float = parameters["u_lim"]
+        constraints_type: str = parameters["constraints_type"]
+        self.solver_type: str = parameters["solver_type"]
+
+        # Time variables
+        dt = 1 / control_freq  # Time step
+        self.T = int(t_final * control_freq)
+
+        # Instantiate robot as a pinocchio RobotWrapper
+        self.robot = robot
+
+        # State
+        state = crocoddyl.StateMultibody(robot.model)
+
+        # Actuation model
+        actuationModel = self.FurutaActuationModel(state)
+
+        # State Residual
+        stateResidual = crocoddyl.ResidualModelState(state, xref=xref, nu=actuationModel.nu)
+        stateCostModel = crocoddyl.CostModelResidual(state, stateResidual)
+
+        # Control Residual
+        controlResidual = crocoddyl.ResidualModelControl(state, nu=actuationModel.nu)
+        if constraints_type == "SOFT":
+            bounds = crocoddyl.ActivationBounds(np.array([self.u_lim]), np.array([self.u_lim]))
+            activation = crocoddyl.ActivationModelQuadraticBarrier(bounds)
+            controlCost = crocoddyl.CostModelResidual(
+                state, residual=controlResidual, activation=activation
+            )
+        else:
+            controlCost = crocoddyl.CostModelResidual(state, residual=controlResidual)
+
+        # Constraints
+        constraintManager = crocoddyl.ConstraintModelManager(state, nu=actuationModel.nu)
+        if constraints_type == "HARD":
+            constraint = crocoddyl.ConstraintModelResidual(
+                state,
+                controlResidual,
+                np.array([-self.u_lim, -self.u_lim]),
+                np.array([self.u_lim, self.u_lim]),
+            )
+            constraintManager.addConstraint("control_constraint", constraint)
+
+        # Running cost
+        runningCostModel = crocoddyl.CostModelSum(state, nu=actuationModel.nu)
+        # runningCostModel.addCost("state_cost", cost=stateCostModel, weight=1e-6)
+        # runningCostModel.addCost("control_cost", cost=controlCost, weight=1e-3)
+
+        # Terminal cost
+        terminalCostModel = crocoddyl.CostModelSum(state, nu=actuationModel.nu)
+        terminalCostModel.addCost("state_cost", cost=stateCostModel, weight=self.T)
+
+        # IAM
+        self.runningModel = crocoddyl.IntegratedActionModelEuler(
+            crocoddyl.DifferentialActionModelFreeFwdDynamics(
+                state, actuationModel, runningCostModel, constraintManager
+            ),
+            dt,
+        )
+        self.terminalModel = crocoddyl.IntegratedActionModelEuler(
+            crocoddyl.DifferentialActionModelFreeFwdDynamics(
+                state, actuationModel, terminalCostModel
+            ),
+            0.0,
+        )
+
+        # Initial state
+        q0 = np.zeros((state.nq + state.nv,))
+
+        # Create the shooting problem
+        self.create_problem(q0)
+
+    def create_problem(self, state: np.ndarray):
+        self.problem = crocoddyl.ShootingProblem(
+            state, [self.runningModel] * self.T, self.terminalModel
+        )
+
+    def get_warm_start(self) -> tp.Tuple[np.ndarray, np.ndarray]:
+        xs = self.solver.xs
+        us = self.solver.us
+        xs[:-1] = xs[1:]
+        xs[-1] = self.solver.xs[-1]
+        us[:-1] = us[1:]
+        us[-1] = self.solver.us[-1]
+        return xs, us
+
+    def compute_command(
+        self, state: np.ndarray, max_iter: float = 500, x_ws=[], u_ws=[], callback=True
+    ) -> float:
+        self.create_problem(state)
+        if self.solver_type == "SQP":
+            self.solver = mim_solvers.SolverSQP(self.problem)
+        elif self.solver_type == "FDDP":
+            self.solver = crocoddyl.SolverFDDP(self.problem)
+        else:
+            raise ValueError(f"Invalid solver type: {self.solver_type}")
+        if callback:
+            callbacks = []
+            callbacks.append(crocoddyl.CallbackVerbose())
+            self.solver.setCallbacks(callbacks)
+        self.solver.solve(x_ws, u_ws, max_iter, False, 1e-5)
+        # Clamp the control signal
+        u = np.clip(self.solver.us[0][0], -self.u_lim, self.u_lim)
+        return u
+
+    def get_trajectoy(self) -> np.ndarray:
+        return self.solver.xs
+
+    def get_command(self) -> np.ndarray:
+        return self.solver.us

furuta/controls/filters.py

@@ -0,0 +1,17 @@
+from scipy.signal import butter, lfilter, lfilter_zi
+
+
+class VelocityFilter:
+    def __init__(
+        self, order: int, cutoff: float, control_frequency: float, init_vel: float
+    ) -> None:
+        nyq_freq = control_frequency / 2
+        normal_cutoff = cutoff / nyq_freq
+        assert normal_cutoff < 1, "cutoff frequency should be smaller than control_frequency / 2"
+        self.b, self.a = butter(N=order, Wn=normal_cutoff)
+        zi = lfilter_zi(self.b, self.a)
+        _, self.z = lfilter(self.b, self.a, x=[init_vel], zi=zi * init_vel)
+
+    def __call__(self, new_vel: float) -> float:
+        filtered_vel, self.z = lfilter(self.b, self.a, x=[new_vel], zi=self.z)
+        return filtered_vel[0]

furuta/robot.py

@@ -10,10 +10,10 @@
 @dataclass
 class RobotModel:
     robot = None
-    # robot = pin.RobotWrapper.BuildFromURDF(
-    #     "robot/hardware/URDF/robot.urdf",
-    #     package_dirs=[str(Path("robot/hardware/URDF/STL/").absolute())],
-    # )
+    robot = pin.RobotWrapper.BuildFromURDF(
+        "robot/hardware/v2/robot.urdf",
+        package_dirs=[str(Path("robot/hardware/v2/stl/").absolute())],
+    )
 
 
 class Robot:

furuta/viewer.py

@@ -52,7 +52,7 @@ def display(cls, state: np.ndarray) -> np.ndarray:
         return cls.viewer.get_screenshot(requested_format="RGB")
 
     def close(cls):
-        cls.viewer.close()
+        cls.viewer.stop()
 
 
 class Viewer2D(AbstractViewer):

robot/hardware/v2/config.json

@@ -0,0 +1,8 @@
+{
+    "documentId": "5d10de487316a32e06c68c4c",
+    "outputFormat": "urdf",
+    "assemblyName": "big_motor_assembly",
+    "mergeSTLs": "collision",
+    "simplifySTLs": "no",
+    "maxSTLSize": 3
+}

robot/hardware/CAD/furuta_v2.step → robot/hardware/v2/furuta_v2.step

@@ -1,7 +1,7 @@
 ISO-10303-21;
 HEADER;
 /* Generated by software containing ST-Developer
- * from STEP Tools, Inc. (www.steptools.com) 
+ * from STEP Tools, Inc. (www.steptools.com)
  */
 
 FILE_DESCRIPTION(