LIBRARY

PlanarMechanics.examples.two_joint_robot.JointController

Joint-level tracking controller: selects the reference position and velocity for one joint from vector-valued trajectory signals and computes a torque command using a PID controller with external derivative and feedforward inputs. The derivative input is formed from the velocity reference and a filtered velocity measurement.

Usage

MultibodyComponents.PlanarMechanics.examples.two_joint_robot.JointController(k=50.0, Ti=2.0, Td=2.0, y_max=200.0, T=0.0003)

Parameters:

Name	Description	Units	Default value
nu	Number of axes in the reference vectors	–	2
index	Index of the controlled joint in the reference vectors	–	1
k	Proportional gain	–	50.0
Ti	Integrator time constant	s	2.0
Td	Derivative gain	–	2.0
y_max	Maximum torque	–	200.0
T	Velocity filter time constant	s	0.0003

Connectors

• q - This connector represents a real signal as an input to a component (RealInput)

• qd - This connector represents a real signal as an input to a component (RealInput)

• w - This connector represents a real signal as an input to a component (RealInput)

• phi - This connector represents a real signal as an input to a component (RealInput)

• u_ff - This connector represents a real signal as an input to a component (RealInput)

• y - This connector represents a real signal as an output from a component (RealOutput)

Behavior

Source

"Joint-level tracking controller: selects the reference position and velocity for one joint from vector-valued trajectory signals and computes a torque command using a PID controller with external derivative and feedforward inputs. The derivative input is formed from the velocity reference and a filtered velocity measurement."
component JointController
  "Position reference vector"
  q = [RealInput() for i in 1:nu] {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 50, "x2": 50, "y2": 150, "rot": 0}
      },
      "tags": []
    }
  }
  "Velocity reference vector"
  qd = [RealInput() for i in 1:nu] {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 250, "x2": 50, "y2": 350, "rot": 0}
      },
      "tags": []
    }
  }
  "Measured joint velocity"
  w = RealInput() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 450, "x2": 50, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  "Measured joint angle"
  phi = RealInput() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 650, "x2": 50, "y2": 750, "rot": 0}
      },
      "tags": []
    }
  }
  "Feedforward torque"
  u_ff = RealInput() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 450, "y1": -50, "x2": 550, "y2": 50, "rot": 90}
      },
      "tags": []
    }
  }
  "Torque command"
  y = RealOutput() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 950, "y1": 450, "x2": 1050, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  q_sel = MultibodyComponents.Selector(nu = nu, index = index) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 50, "x2": 250, "y2": 150, "rot": 0}
      },
      "tags": []
    }
  }
  qd_sel = MultibodyComponents.Selector(nu = nu, index = index) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 250, "x2": 250, "y2": 350, "rot": 0}
      },
      "tags": []
    }
  }
  velfilt = BlockComponents.Continuous.FirstOrder(final T = T) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 450, "x2": 250, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  derr = BlockComponents.Math.Feedback() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 350, "y1": 250, "x2": 450, "y2": 350, "rot": 0}
      },
      "tags": []
    }
  }
  dgain = BlockComponents.Math.Gain(final k = Td) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 490, "y1": 250, "x2": 590, "y2": 350, "rot": 0}
      },
      "tags": []
    }
  }
  pid = BlockComponents.Continuous.LimPIDExternalDerivative(final k = k, final Ti = Ti, final y_max = y_max) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 700, "y1": 450, "x2": 800, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  "Number of axes in the reference vectors"
  structural parameter nu::Integer = 2
  "Index of the controlled joint in the reference vectors"
  structural parameter index::Integer = 1
  "Proportional gain"
  parameter k::Real = 50.0
  "Integrator time constant"
  parameter Ti::Time = 2.0
  "Derivative gain"
  parameter Td::Real = 2.0
  "Maximum torque"
  parameter y_max::Real = 200.0
  "Velocity filter time constant"
  parameter T::Time = 0.0003
relations
  # Initialize the velocity filter state explicitly; without this, the
  # overdetermined initialization system may park a nonzero residual in the
  # free filter state, producing a large spurious torque transient at t=0.
  initial velfilt.x = 0
  connect(q, q_sel.u)
  connect(qd, qd_sel.u)
  connect(q_sel.y, pid.u_s) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 640, "y": 100}, {"x": 640, "y": 477}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(phi, pid.u_m) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [
            {"x": 40, "y": 700},
            {"x": 40, "y": 650},
            {"x": 655, "y": 650},
            {"x": 655, "y": 523}
          ],
          "E": 2
        }
      ],
      "renderStyle": "standard"
    }
  }
  connect(qd_sel.y, derr.u1)
  connect(w, velfilt.u)
  connect(velfilt.y, derr.u2) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 400, "y": 500}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(derr.y, dgain.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(dgain.y, pid.u_d) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 610, "y": 300}, {"x": 610, "y": 610}, {"x": 750, "y": 610}],
          "E": 2
        }
      ],
      "renderStyle": "standard"
    }
  }
  connect(u_ff, pid.u_ff) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 500, "y": 40}, {"x": 750, "y": 40}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(pid.y, y)
metadata {
  "Dyad": {"icons": {"default": "dyad://MultibodyComponents/JointController.svg"}}
}
end

Flattened Source

"Joint-level tracking controller: selects the reference position and velocity for one joint from vector-valued trajectory signals and computes a torque command using a PID controller with external derivative and feedforward inputs. The derivative input is formed from the velocity reference and a filtered velocity measurement."
component JointController
  "Position reference vector"
  q = [RealInput() for i in 1:nu] {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 50, "x2": 50, "y2": 150, "rot": 0}
      },
      "tags": []
    }
  }
  "Velocity reference vector"
  qd = [RealInput() for i in 1:nu] {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 250, "x2": 50, "y2": 350, "rot": 0}
      },
      "tags": []
    }
  }
  "Measured joint velocity"
  w = RealInput() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 450, "x2": 50, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  "Measured joint angle"
  phi = RealInput() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": -50, "y1": 650, "x2": 50, "y2": 750, "rot": 0}
      },
      "tags": []
    }
  }
  "Feedforward torque"
  u_ff = RealInput() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 450, "y1": -50, "x2": 550, "y2": 50, "rot": 90}
      },
      "tags": []
    }
  }
  "Torque command"
  y = RealOutput() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 950, "y1": 450, "x2": 1050, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  q_sel = MultibodyComponents.Selector(nu = nu, index = index) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 50, "x2": 250, "y2": 150, "rot": 0}
      },
      "tags": []
    }
  }
  qd_sel = MultibodyComponents.Selector(nu = nu, index = index) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 250, "x2": 250, "y2": 350, "rot": 0}
      },
      "tags": []
    }
  }
  velfilt = BlockComponents.Continuous.FirstOrder(final T = T) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 150, "y1": 450, "x2": 250, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  derr = BlockComponents.Math.Feedback() {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 350, "y1": 250, "x2": 450, "y2": 350, "rot": 0}
      },
      "tags": []
    }
  }
  dgain = BlockComponents.Math.Gain(final k = Td) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 490, "y1": 250, "x2": 590, "y2": 350, "rot": 0}
      },
      "tags": []
    }
  }
  pid = BlockComponents.Continuous.LimPIDExternalDerivative(final k = k, final Ti = Ti, final y_max = y_max) {
    "Dyad": {
      "placement": {
        "diagram": {"iconName": "default", "x1": 700, "y1": 450, "x2": 800, "y2": 550, "rot": 0}
      },
      "tags": []
    }
  }
  "Number of axes in the reference vectors"
  structural parameter nu::Integer = 2
  "Index of the controlled joint in the reference vectors"
  structural parameter index::Integer = 1
  "Proportional gain"
  parameter k::Real = 50.0
  "Integrator time constant"
  parameter Ti::Time = 2.0
  "Derivative gain"
  parameter Td::Real = 2.0
  "Maximum torque"
  parameter y_max::Real = 200.0
  "Velocity filter time constant"
  parameter T::Time = 0.0003
relations
  # Initialize the velocity filter state explicitly; without this, the
  # overdetermined initialization system may park a nonzero residual in the
  # free filter state, producing a large spurious torque transient at t=0.
  initial velfilt.x = 0
  connect(q, q_sel.u)
  connect(qd, qd_sel.u)
  connect(q_sel.y, pid.u_s) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 640, "y": 100}, {"x": 640, "y": 477}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(phi, pid.u_m) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [
            {"x": 40, "y": 700},
            {"x": 40, "y": 650},
            {"x": 655, "y": 650},
            {"x": 655, "y": 523}
          ],
          "E": 2
        }
      ],
      "renderStyle": "standard"
    }
  }
  connect(qd_sel.y, derr.u1)
  connect(w, velfilt.u)
  connect(velfilt.y, derr.u2) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 400, "y": 500}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(derr.y, dgain.u) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
  connect(dgain.y, pid.u_d) {
    "Dyad": {
      "edges": [
        {
          "S": 1,
          "M": [{"x": 610, "y": 300}, {"x": 610, "y": 610}, {"x": 750, "y": 610}],
          "E": 2
        }
      ],
      "renderStyle": "standard"
    }
  }
  connect(u_ff, pid.u_ff) {
    "Dyad": {
      "edges": [{"S": 1, "M": [{"x": 500, "y": 40}, {"x": 750, "y": 40}], "E": 2}],
      "renderStyle": "standard"
    }
  }
  connect(pid.y, y)
metadata {
  "Dyad": {"icons": {"default": "dyad://MultibodyComponents/JointController.svg"}}
}
end

Test Cases

No test cases defined.

Related

• Examples

• Experiments

• Analyses