Skip to content

Commit 07671c7

Browse files
conconmanSylvainBertrand
conconman
authored and
SylvainBertrand
committed
Adjusted sim to new transfer function class
1 parent 8e8b489 commit 07671c7

File tree

2 files changed

+48
-33
lines changed

2 files changed

+48
-33
lines changed

‎example-simulations/src/main/java/us/ihmc/exampleSimulations/yoFilteredDouble/emptyRobotSCS/YoFilteredDoubleController.java‎

Lines changed: 47 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
1-
package us.ihmc.exampleSimulations.yoFilteredDouble;
1+
package us.ihmc.exampleSimulations.yoFilteredDouble.emptyRobotSCS;
22

3+
import us.ihmc.robotics.math.filters.ContinuousTransferFunction;
34
import us.ihmc.robotics.math.filters.TransferFunctionDiscretizer;
45
import us.ihmc.robotics.math.filters.YoFilteredDouble;
56
import us.ihmc.robotics.math.trajectories.generators.TrajectoryGenerator;
@@ -47,7 +48,7 @@ public class YoFilteredDoubleController implements RobotController {
4748

4849
// Notch Filter Parameters
4950
private final double wn = 60*2*Math.PI;
50-
private final double Q = 5.0;
51+
private final double Q = 1.0;
5152

5253
// PID Parameters
5354
private final double Kp = 15.0;
@@ -66,30 +67,45 @@ public YoFilteredDoubleController(YoRegistry registry, double dt_ns) {
6667
this.controllerTime = new YoDouble("controllerTime", registry);
6768

6869
// 2.0 Build Filter Objects.
69-
TransferFunctionDiscretizer FOLPFilter = new TransferFunctionDiscretizer(1.0,
70-
new double[] {1.0},
71-
new double[] { 1/Tau_folp, 1.0},
72-
(1000000000.0 / ((double) dt_ns)));
73-
TransferFunctionDiscretizer LP_2nd_Order_Butter_Filter = new TransferFunctionDiscretizer(1.0,
74-
new double[] {wc*wc},
75-
new double[] { 1.0, Math.sqrt(2)*wc, wc*wc},
76-
(1000000000.0 / ((double) dt_ns)));
77-
TransferFunctionDiscretizer Notch_Filter = new TransferFunctionDiscretizer(1.0,
78-
new double[] { 1.0, 0.0, wn*wn},
79-
new double[] { 1.0, wn/Q, wn*wn},
80-
(1000000000.0 / ((double) dt_ns)));
81-
TransferFunctionDiscretizer MultiorderComplex_Filter = new TransferFunctionDiscretizer(1.0,
82-
new double[] { 196.919515374308, 21033.790696845190, 427573.897431703983, 18317222.932339027524 },
83-
new double[] { 1.000000000000, 382.156022138851, 60851.343857079330, 3875784.585037478711 },
84-
(1000000000.0 / ((double) dt_ns)));
85-
TransferFunctionDiscretizer PID_Filter = new TransferFunctionDiscretizer(1.0,
86-
new double[] {(Kp + Tau*Kd), (Tau*Kp + Ki), Ki*Tau},
87-
new double[] { 1.0, Tau, 0.0},
88-
(1000000000.0 / ((double) dt_ns)));
89-
TransferFunctionDiscretizer Lead_Lag_Compensator_Filter = new TransferFunctionDiscretizer(k,
90-
new double[] {1.0, z},
91-
new double[] { 1.0, p},
92-
(1000000000.0 / ((double) dt_ns)));
70+
ContinuousTransferFunction tf1 = new ContinuousTransferFunction("First Order Low-Pass Filter",
71+
1.0,
72+
new double[] {1.0},
73+
new double[] { 1/Tau_folp, 1.0});
74+
TransferFunctionDiscretizer FOLPFilter = new TransferFunctionDiscretizer(tf1, 1000.0);
75+
76+
ContinuousTransferFunction tf2 = new ContinuousTransferFunction("Second Order Low-Pass Butterworth Filter",
77+
1.0,
78+
new double[] {wc*wc},
79+
new double[] { 1.0, Math.sqrt(2)*wc, wc*wc});
80+
81+
TransferFunctionDiscretizer LP_2nd_Order_Butter_Filter = new TransferFunctionDiscretizer(tf2, 1000.0);
82+
83+
ContinuousTransferFunction tf3 = new ContinuousTransferFunction("Second Order Notch Filter",
84+
1.0,
85+
new double[] { 1.0, 0.0, wn*wn},
86+
new double[] { 1.0, wn/Q, wn*wn});
87+
88+
TransferFunctionDiscretizer Notch_Filter = new TransferFunctionDiscretizer(tf3, 1000.0);
89+
90+
ContinuousTransferFunction tf4 = new ContinuousTransferFunction("Complex Multi-Order Filter",
91+
1.0,
92+
new double[] { 196.919515374308, 21033.790696845190, 427573.897431703983, 18317222.932339027524 },
93+
new double[] { 1.000000000000, 382.156022138851, 60851.343857079330, 3875784.585037478711 });
94+
TransferFunctionDiscretizer MultiorderComplex_Filter = new TransferFunctionDiscretizer(tf4, 1000.0);
95+
96+
ContinuousTransferFunction tf5 = new ContinuousTransferFunction("PID Controller",
97+
1.0,
98+
new double[] {(Kp + Tau*Kd), (Tau*Kp + Ki), Ki*Tau},
99+
new double[] { 1.0, Tau, 0.0});
100+
101+
TransferFunctionDiscretizer PID_Filter = new TransferFunctionDiscretizer(tf5, 1000.0);
102+
103+
ContinuousTransferFunction tf6 = new ContinuousTransferFunction("Lead-Lag Controller",
104+
k,
105+
new double[] {1.0, z},
106+
new double[] { 1.0, p});
107+
108+
TransferFunctionDiscretizer Lead_Lag_Compensator_Filter = new TransferFunctionDiscretizer(tf6, 1000.0);
93109

94110

95111
// 3.0 Create new filtered doubles based on the Filter objects.
@@ -103,7 +119,6 @@ public YoFilteredDoubleController(YoRegistry registry, double dt_ns) {
103119
RefTraj_nonjump_Filtered_Var = new YoFilteredDouble("RefTraj_nonjump_Filtered_Var", registry, LP_2nd_Order_Butter_Filter, false); // Set false to avoid jump
104120
RefTraj2_jump_Filtered_Var = new YoFilteredDouble("RefTraj2_jump_Filtered_Var", registry, Lead_Lag_Compensator_Filter, true);
105121
RefTraj2_nonjump_Filtered_Var = new YoFilteredDouble("RefTraj2_nonjump_Filtered_Var", registry, Lead_Lag_Compensator_Filter, false); // Set false to avoid jump
106-
107122

108123
System.out.println("1st Order LP Input Coeffs: " + FOLPFilter.getInputCoefficients().toString());
109124
System.out.println("1st Order LP Output Coeffs: " + FOLPFilter.getOutputCoefficients().toString());
@@ -124,9 +139,9 @@ public YoFilteredDoubleController(YoRegistry registry, double dt_ns) {
124139
System.out.println("Lead-Lag Controller Output Coeffs: " + Lead_Lag_Compensator_Filter.getOutputCoefficients().toString());
125140

126141
// 4.0 Initialize Input Chirp Trajectory.
127-
chirp = new TrajectoryGenerator("chirpSignal", registry, Trajectory.CHIRP, ChirpType.EXPONENTIAL, 100.0, 1.0, 0.00001, 500.0);
128-
chirp_lin = new TrajectoryGenerator("chirpSignal_lin", registry, Trajectory.CHIRP, ChirpType.LINEAR, 10.0, 1.0, 0.01, 2.5);
129-
chirp_exp = new TrajectoryGenerator("chirpSignal_exp", registry, Trajectory.CHIRP, ChirpType.EXPONENTIAL, 10.0, 1.0, 0.01, 2.5);
142+
chirp = new TrajectoryGenerator("chirpSignal", registry, Trajectory.CHIRP, ChirpType.EXPONENTIAL, 20.0, 1.0, 0.00001, 100.0);
143+
chirp_lin = new TrajectoryGenerator("chirpSignal_lin", registry, Trajectory.CHIRP, ChirpType.LINEAR, 20.0, 1.0, 0.00001, 2.5);
144+
chirp_exp = new TrajectoryGenerator("chirpSignal_exp", registry, Trajectory.CHIRP, ChirpType.EXPONENTIAL, 20.0, 1.0, 0.00001, 2.5);
130145
sinusoid = new TrajectoryGenerator("sinusoid", registry, Trajectory.SINE, 1.0, 100, 0.0, 0.0, 5.0);
131146
input_lin = new YoDouble("input_lin",registry);
132147

@@ -136,6 +151,7 @@ public YoFilteredDoubleController(YoRegistry registry, double dt_ns) {
136151
input = new YoDouble("input",registry);
137152
chirpFreqHz = new YoDouble("chirpFreqHz", registry);
138153
sinusoid_input = new YoDouble("sinusoid_input", registry);
154+
139155
}
140156

141157
@Override
@@ -169,8 +185,7 @@ public void doControl() {
169185
RefTraj_nonjump_Filtered_Var.set(sinusoid_input.getDoubleValue());
170186
RefTraj2_jump_Filtered_Var.set(sinusoid_input.getDoubleValue());
171187
RefTraj2_nonjump_Filtered_Var.set(sinusoid_input.getDoubleValue());
172-
chirpFreqHz.set(chirp.getFreqCheckRad()/(2*Math.PI));
173-
188+
chirpFreqHz.set(chirp.getFreqCheckRad()/(2*Math.PI));
174189
}
175190

176191
}

‎example-simulations/src/main/java/us/ihmc/exampleSimulations/yoFilteredDouble/emptyRobotSCS/YoFilteredDoubleSimulation.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
package us.ihmc.exampleSimulations.yoFilteredDouble;
1+
package us.ihmc.exampleSimulations.yoFilteredDouble.emptyRobotSCS;
22

33
import us.ihmc.simulationconstructionset.Robot;
44
import us.ihmc.simulationconstructionset.SimulationConstructionSet;

0 commit comments

Comments
 (0)