LIBRARY
PlanarMechanics.examples.two_joint_robot.TwoJointPlanarRobotPID
Planar 2-joint serial robot tracking a jerk-bounded reference with per-joint JointController controllers. Computed-torque feedforward is obtained from a second instance of the arm acting as an inverse model: its joints are driven at the acceleration level by the reference trajectory through an InverseModelDriver, and the torques required to realize the reference motion are measured and fed to the feedforward inputs of the controllers.
Usage
MultibodyComponents.PlanarMechanics.examples.two_joint_robot.TwoJointPlanarRobotPID(k1=100.0, k2=50.0, Ti1=0.1, Ti2=0.1, Td1=1.0, Td2=1.0, y_max=200.0, T=0.005)
Parameters:
| Name | Description | Units | Default value |
|---|---|---|---|
k1 | Proportional gain for joint 1 | – | 100.0 |
k2 | Proportional gain for joint 2 | – | 50.0 |
Ti1 | Integrator time constant for joint 1 | s | 0.1 |
Ti2 | Integrator time constant for joint 2 | s | 0.1 |
Td1 | Derivative gain for joint 1 | – | 1.0 |
Td2 | Derivative gain for joint 2 | – | 1.0 |
y_max | Maximum torque of both controllers | – | 200.0 |
T | Velocity filter time constant of both controllers | s | 0.005 |
Behavior
julia
using MultibodyComponents #hide
using ModelingToolkit #hide
@variables k1 #hide
@variables k2 #hide
@variables Ti1 #hide
@variables Ti2 #hide
@variables Td1 #hide
@variables Td2 #hide
@variables y_max #hide
@variables T #hide
@named sys = MultibodyComponents.PlanarMechanics.examples.two_joint_robot.TwoJointPlanarRobotPID(k1=k1, k2=k2, Ti1=Ti1, Ti2=Ti2, Td1=Td1, Td2=Td2, y_max=y_max, T=T) #hide
let eqs = full_equations(sys); Base.length(eqs) > 25 ? nothing : eqs end #hide<< @example-block not executed in draft mode >>Source
dyad
"Planar 2-joint serial robot tracking a jerk-bounded reference with per-joint JointController controllers. Computed-torque feedforward is obtained from a second instance of the arm acting as an inverse model: its joints are driven at the acceleration level by the reference trajectory through an InverseModelDriver, and the torques required to realize the reference motion are measured and fed to the feedforward inputs of the controllers."
example component TwoJointPlanarRobotPID
world = MultibodyComponents.PlanarMechanics.World() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 1350, "y1": 220, "x2": 1250, "y2": 320, "rot": 0}
},
"tags": []
}
}
robot = TwoJointPlanarArm() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 1080, "y1": 220, "x2": 1180, "y2": 320, "rot": 0}
},
"tags": []
}
}
"Inverse model of the robot used to compute torque feedforward"
inverse_robot = TwoJointPlanarArm(render = false) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 1080, "y1": -70, "x2": 1180, "y2": 30, "rot": 0}
},
"tags": []
}
}
ptp = MultibodyComponents.KinematicPTPBoundedJerk(nout = 2, q0 = [0.0, 0.0], q1 = [pi / 2, -pi / 2], qd_max = [1.0, 1.0], qdd_max = [2.0, 2.0], qddd_max = [10.0, 10.0]) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -130, "y1": 600, "x2": -30, "y2": 700, "rot": 0}
},
"tags": []
}
}
ctrl1 = JointController(index = 1, final k = k1, final Ti = Ti1, final Td = Td1, y_max = y_max, T = T) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 470, "y1": 148, "x2": 570, "y2": 248, "rot": 0}
},
"tags": []
}
}
ctrl2 = JointController(index = 2, final k = k2, final Ti = Ti2, final Td = Td2, y_max = y_max, T = T) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 530, "y1": 610, "x2": 630, "y2": 710, "rot": 0}
},
"tags": []
}
}
inverse_drive = InverseModelDriver() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 500, "y1": -70, "x2": 600, "y2": 30, "rot": 0}
},
"tags": []
}
}
torque1 = RotationalComponents.Sources.TorqueSource() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 730, "y1": 150, "x2": 830, "y2": 250, "rot": 0}
},
"tags": []
}
}
torque2 = RotationalComponents.Sources.TorqueSource() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 730, "y1": 710, "x2": 830, "y2": 610, "rot": 0}
},
"tags": []
}
}
angle1 = RotationalComponents.Sensors.AngleSensor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 280, "y1": 230, "x2": 180, "y2": 330, "rot": 0}
},
"tags": []
}
}
angle2 = RotationalComponents.Sensors.AngleSensor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 290, "y1": 700, "x2": 190, "y2": 800, "rot": 0}
},
"tags": []
}
}
vel1 = RotationalComponents.Sensors.VelocitySensor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 280, "y1": 360, "x2": 180, "y2": 460, "rot": 0}
},
"tags": []
}
}
vel2 = RotationalComponents.Sensors.VelocitySensor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 290, "y1": 820, "x2": 190, "y2": 920, "rot": 0}
},
"tags": []
}
}
fixed = RotationalComponents.Components.Fixed() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 800, "y1": 430, "x2": 900, "y2": 530, "rot": 270}
},
"tags": []
}
}
fixed1 = RotationalComponents.Components.Fixed() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 530, "y1": 90, "x2": 570, "y2": 130, "rot": 0}
},
"tags": []
}
}
"Proportional gain for joint 1"
parameter k1::Real = 100.0
"Proportional gain for joint 2"
parameter k2::Real = 50.0
"Integrator time constant for joint 1"
parameter Ti1::Time = 0.1
"Integrator time constant for joint 2"
parameter Ti2::Time = 0.1
"Derivative gain for joint 1"
parameter Td1::Real = 1.0
"Derivative gain for joint 2"
parameter Td2::Real = 1.0
"Maximum torque of both controllers"
parameter y_max::Real = 200.0
"Velocity filter time constant of both controllers"
parameter T::Time = 0.005
relations
connect(robot.flange1, torque1.spline) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 970.5, "y": 244}, {"x": 970.5, "y": 200}], "E": 2}],
"renderStyle": "standard"
}
}
connect(torque1.support, fixed.spline) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 780, "y": 480}], "E": 2}],
"renderStyle": "standard"
}
}
connect(angle1.phi, ctrl1.phi) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 160, "y": 280}, {"x": 160, "y": 218}], "E": 2}],
"renderStyle": "standard"
}
}
connect(vel1.w, ctrl1.w) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 140, "y": 410}, {"x": 140, "y": 198}], "E": 2}],
"renderStyle": "standard"
}
}
connect(ctrl1.y, torque1.tau) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 648, "y": 198}, {"x": 648, "y": 199}], "E": 2}],
"renderStyle": "standard"
}
}
connect(angle2.spline, vel2.spline, robot.flange2, torque2.spline) {
"Dyad": {
"edges": [
{"S": -1, "M": [{"x": 330, "y": 750}], "E": 1},
{"S": 2, "M": [], "E": -1},
{"S": 3, "M": [{"x": 1040, "y": 295}], "E": -2},
{"S": -2, "M": [{"x": 1040, "y": 870}], "E": -1},
{"S": 4, "M": [], "E": -2}
],
"junctions": [{"x": 330, "y": 870}, {"x": 1040, "y": 660}],
"renderStyle": "standard"
}
}
connect(torque2.support, fixed.spline) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 780, "y": 480}], "E": 2}],
"renderStyle": "standard"
}
}
connect(ptp.q, ctrl2.q, ctrl1.q) {
"Dyad": {
"edges": [
{"S": 1, "M": [], "E": -1},
{"S": -1, "M": [{"x": 90, "y": 615}, {"x": 90, "y": 620}], "E": 2},
{"S": 3, "M": [{"x": 40, "y": 158}], "E": -1}
],
"junctions": [{"x": 40, "y": 615}],
"renderStyle": "standard"
}
}
connect(ptp.qd, ctrl2.qd, ctrl1.qd) {
"Dyad": {
"edges": [
{"S": 1, "M": [], "E": -1},
{"S": -1, "M": [{"x": 60, "y": 638}, {"x": 60, "y": 640}], "E": 2},
{"S": 3, "M": [{"x": 60, "y": 178}], "E": -1}
],
"junctions": [{"x": 60, "y": 638}],
"renderStyle": "standard"
}
}
connect(angle2.phi, ctrl2.phi) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 170, "y": 750}, {"x": 170, "y": 680}], "E": 2}],
"renderStyle": "standard"
}
}
connect(vel2.w, ctrl2.w) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 140, "y": 870}, {"x": 140, "y": 660}], "E": 2}],
"renderStyle": "standard"
}
}
connect(ctrl2.y, torque2.tau) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 648, "y": 661}, {"x": 648, "y": 660}], "E": 2}],
"renderStyle": "standard"
}
}
connect(vel1.spline, angle1.spline, robot.flange1) {
"Dyad": {
"edges": [
{"S": 1, "M": [{"x": 320, "y": 410}], "E": -1},
{"S": -1, "M": [], "E": 2},
{"S": 3, "M": [{"x": 970, "y": 244}, {"x": 970, "y": 280}], "E": -1}
],
"junctions": [{"x": 320, "y": 280}],
"renderStyle": "standard"
}
}
connect(ptp.qdd, inverse_drive.qdd) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 20, "y": 664}, {"x": 20, "y": -20}], "E": 2}],
"renderStyle": "standard"
}
}
connect(inverse_drive.flange2, inverse_robot.flange2) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 640, "y": 6}, {"x": 640, "y": 5}], "E": 2}],
"renderStyle": "standard"
}
}
connect(fixed1.spline, inverse_drive.support) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
connect(inverse_drive.tau2, ctrl2.u_ff) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 581, "y": 570}, {"x": 580, "y": 570}], "E": 2}],
"renderStyle": "standard"
}
}
connect(inverse_drive.tau1, ctrl1.u_ff) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 520, "y": 31}], "E": 2}],
"renderStyle": "standard"
}
}
connect(inverse_drive.flange1, inverse_robot.flange1) {
"Dyad": {
"renderStyle": "standard",
"edges": [{"S": 1, "M": [{"x": 840, "y": -48}, {"x": 840, "y": -46}], "E": 2}]
}
}
connect(robot.frame_a, world.frame_b, inverse_robot.frame_a) {
"Dyad": {
"edges": [
{"S": 1, "M": [], "E": -1},
{"S": -1, "M": [], "E": 2},
{"S": 3, "M": [{"x": 1210, "y": -20}], "E": -1}
],
"junctions": [{"x": 1210, "y": 270}],
"renderStyle": "standard"
}
}
u1: analysis_point(ctrl1.y, torque1.tau)
u2: analysis_point(ctrl2.y, torque2.tau)
ya1: analysis_point(angle1.phi, ctrl1.phi)
yv1: analysis_point(vel1.w, ctrl1.w)
ya2: analysis_point(angle2.phi, ctrl2.phi)
yv2: analysis_point(vel2.w, ctrl2.w)
uff1: analysis_point(inverse_drive.tau1, ctrl1.u_ff)
uff2: analysis_point(inverse_drive.tau2, ctrl2.u_ff)
endFlattened Source
dyad
"Planar 2-joint serial robot tracking a jerk-bounded reference with per-joint JointController controllers. Computed-torque feedforward is obtained from a second instance of the arm acting as an inverse model: its joints are driven at the acceleration level by the reference trajectory through an InverseModelDriver, and the torques required to realize the reference motion are measured and fed to the feedforward inputs of the controllers."
example component TwoJointPlanarRobotPID
world = MultibodyComponents.PlanarMechanics.World() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 1350, "y1": 220, "x2": 1250, "y2": 320, "rot": 0}
},
"tags": []
}
}
robot = TwoJointPlanarArm() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 1080, "y1": 220, "x2": 1180, "y2": 320, "rot": 0}
},
"tags": []
}
}
"Inverse model of the robot used to compute torque feedforward"
inverse_robot = TwoJointPlanarArm(render = false) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 1080, "y1": -70, "x2": 1180, "y2": 30, "rot": 0}
},
"tags": []
}
}
ptp = MultibodyComponents.KinematicPTPBoundedJerk(nout = 2, q0 = [0.0, 0.0], q1 = [pi / 2, -pi / 2], qd_max = [1.0, 1.0], qdd_max = [2.0, 2.0], qddd_max = [10.0, 10.0]) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": -130, "y1": 600, "x2": -30, "y2": 700, "rot": 0}
},
"tags": []
}
}
ctrl1 = JointController(index = 1, final k = k1, final Ti = Ti1, final Td = Td1, y_max = y_max, T = T) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 470, "y1": 148, "x2": 570, "y2": 248, "rot": 0}
},
"tags": []
}
}
ctrl2 = JointController(index = 2, final k = k2, final Ti = Ti2, final Td = Td2, y_max = y_max, T = T) {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 530, "y1": 610, "x2": 630, "y2": 710, "rot": 0}
},
"tags": []
}
}
inverse_drive = InverseModelDriver() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 500, "y1": -70, "x2": 600, "y2": 30, "rot": 0}
},
"tags": []
}
}
torque1 = RotationalComponents.Sources.TorqueSource() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 730, "y1": 150, "x2": 830, "y2": 250, "rot": 0}
},
"tags": []
}
}
torque2 = RotationalComponents.Sources.TorqueSource() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 730, "y1": 710, "x2": 830, "y2": 610, "rot": 0}
},
"tags": []
}
}
angle1 = RotationalComponents.Sensors.AngleSensor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 280, "y1": 230, "x2": 180, "y2": 330, "rot": 0}
},
"tags": []
}
}
angle2 = RotationalComponents.Sensors.AngleSensor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 290, "y1": 700, "x2": 190, "y2": 800, "rot": 0}
},
"tags": []
}
}
vel1 = RotationalComponents.Sensors.VelocitySensor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 280, "y1": 360, "x2": 180, "y2": 460, "rot": 0}
},
"tags": []
}
}
vel2 = RotationalComponents.Sensors.VelocitySensor() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 290, "y1": 820, "x2": 190, "y2": 920, "rot": 0}
},
"tags": []
}
}
fixed = RotationalComponents.Components.Fixed() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 800, "y1": 430, "x2": 900, "y2": 530, "rot": 270}
},
"tags": []
}
}
fixed1 = RotationalComponents.Components.Fixed() {
"Dyad": {
"placement": {
"diagram": {"iconName": "default", "x1": 530, "y1": 90, "x2": 570, "y2": 130, "rot": 0}
},
"tags": []
}
}
"Proportional gain for joint 1"
parameter k1::Real = 100.0
"Proportional gain for joint 2"
parameter k2::Real = 50.0
"Integrator time constant for joint 1"
parameter Ti1::Time = 0.1
"Integrator time constant for joint 2"
parameter Ti2::Time = 0.1
"Derivative gain for joint 1"
parameter Td1::Real = 1.0
"Derivative gain for joint 2"
parameter Td2::Real = 1.0
"Maximum torque of both controllers"
parameter y_max::Real = 200.0
"Velocity filter time constant of both controllers"
parameter T::Time = 0.005
relations
connect(robot.flange1, torque1.spline) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 970.5, "y": 244}, {"x": 970.5, "y": 200}], "E": 2}],
"renderStyle": "standard"
}
}
connect(torque1.support, fixed.spline) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 780, "y": 480}], "E": 2}],
"renderStyle": "standard"
}
}
connect(angle1.phi, ctrl1.phi) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 160, "y": 280}, {"x": 160, "y": 218}], "E": 2}],
"renderStyle": "standard"
}
}
connect(vel1.w, ctrl1.w) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 140, "y": 410}, {"x": 140, "y": 198}], "E": 2}],
"renderStyle": "standard"
}
}
connect(ctrl1.y, torque1.tau) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 648, "y": 198}, {"x": 648, "y": 199}], "E": 2}],
"renderStyle": "standard"
}
}
connect(angle2.spline, vel2.spline, robot.flange2, torque2.spline) {
"Dyad": {
"edges": [
{"S": -1, "M": [{"x": 330, "y": 750}], "E": 1},
{"S": 2, "M": [], "E": -1},
{"S": 3, "M": [{"x": 1040, "y": 295}], "E": -2},
{"S": -2, "M": [{"x": 1040, "y": 870}], "E": -1},
{"S": 4, "M": [], "E": -2}
],
"junctions": [{"x": 330, "y": 870}, {"x": 1040, "y": 660}],
"renderStyle": "standard"
}
}
connect(torque2.support, fixed.spline) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 780, "y": 480}], "E": 2}],
"renderStyle": "standard"
}
}
connect(ptp.q, ctrl2.q, ctrl1.q) {
"Dyad": {
"edges": [
{"S": 1, "M": [], "E": -1},
{"S": -1, "M": [{"x": 90, "y": 615}, {"x": 90, "y": 620}], "E": 2},
{"S": 3, "M": [{"x": 40, "y": 158}], "E": -1}
],
"junctions": [{"x": 40, "y": 615}],
"renderStyle": "standard"
}
}
connect(ptp.qd, ctrl2.qd, ctrl1.qd) {
"Dyad": {
"edges": [
{"S": 1, "M": [], "E": -1},
{"S": -1, "M": [{"x": 60, "y": 638}, {"x": 60, "y": 640}], "E": 2},
{"S": 3, "M": [{"x": 60, "y": 178}], "E": -1}
],
"junctions": [{"x": 60, "y": 638}],
"renderStyle": "standard"
}
}
connect(angle2.phi, ctrl2.phi) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 170, "y": 750}, {"x": 170, "y": 680}], "E": 2}],
"renderStyle": "standard"
}
}
connect(vel2.w, ctrl2.w) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 140, "y": 870}, {"x": 140, "y": 660}], "E": 2}],
"renderStyle": "standard"
}
}
connect(ctrl2.y, torque2.tau) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 648, "y": 661}, {"x": 648, "y": 660}], "E": 2}],
"renderStyle": "standard"
}
}
connect(vel1.spline, angle1.spline, robot.flange1) {
"Dyad": {
"edges": [
{"S": 1, "M": [{"x": 320, "y": 410}], "E": -1},
{"S": -1, "M": [], "E": 2},
{"S": 3, "M": [{"x": 970, "y": 244}, {"x": 970, "y": 280}], "E": -1}
],
"junctions": [{"x": 320, "y": 280}],
"renderStyle": "standard"
}
}
connect(ptp.qdd, inverse_drive.qdd) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 20, "y": 664}, {"x": 20, "y": -20}], "E": 2}],
"renderStyle": "standard"
}
}
connect(inverse_drive.flange2, inverse_robot.flange2) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 640, "y": 6}, {"x": 640, "y": 5}], "E": 2}],
"renderStyle": "standard"
}
}
connect(fixed1.spline, inverse_drive.support) {"Dyad": {"edges": [{"S": 1, "M": [], "E": 2}], "renderStyle": "standard"}}
connect(inverse_drive.tau2, ctrl2.u_ff) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 581, "y": 570}, {"x": 580, "y": 570}], "E": 2}],
"renderStyle": "standard"
}
}
connect(inverse_drive.tau1, ctrl1.u_ff) {
"Dyad": {
"edges": [{"S": 1, "M": [{"x": 520, "y": 31}], "E": 2}],
"renderStyle": "standard"
}
}
connect(inverse_drive.flange1, inverse_robot.flange1) {
"Dyad": {
"renderStyle": "standard",
"edges": [{"S": 1, "M": [{"x": 840, "y": -48}, {"x": 840, "y": -46}], "E": 2}]
}
}
connect(robot.frame_a, world.frame_b, inverse_robot.frame_a) {
"Dyad": {
"edges": [
{"S": 1, "M": [], "E": -1},
{"S": -1, "M": [], "E": 2},
{"S": 3, "M": [{"x": 1210, "y": -20}], "E": -1}
],
"junctions": [{"x": 1210, "y": 270}],
"renderStyle": "standard"
}
}
u1: analysis_point(ctrl1.y, torque1.tau)
u2: analysis_point(ctrl2.y, torque2.tau)
ya1: analysis_point(angle1.phi, ctrl1.phi)
yv1: analysis_point(vel1.w, ctrl1.w)
ya2: analysis_point(angle2.phi, ctrl2.phi)
yv2: analysis_point(vel2.w, ctrl2.w)
uff1: analysis_point(inverse_drive.tau1, ctrl1.u_ff)
uff2: analysis_point(inverse_drive.tau2, ctrl2.u_ff)
metadata {}
endTest Cases
No test cases defined.
Related
Examples
Experiments
Analyses
julia
<< @example-block not executed in draft mode >>