Added the fixed step ode1 solver in the FD tool set, added a silent m…

…ode for the controller simulator so that no info will be printed.
darwinlau · May 7, 2018 · fffb2e9 · fffb2e9
1 parent 6d3dfeb
commit fffb2e9
Show file tree

Hide file tree

Showing 6 changed files with 60 additions and 5 deletions.
diff --git a/data/model_config/models/MCDM/BMArm/BMArm_trajectories.xml b/data/model_config/models/MCDM/BMArm/BMArm_trajectories.xml
@@ -323,5 +323,44 @@
           </point> 
         </points>
       </quintic_spline_trajectory>
+      <quintic_spline_trajectory id="star_m" time_step="0.005" time_definition="relative">
+        <points>
+          <point>
+            <y>0 0.575 0.0</y>
+            <y_dot>0.0 0.0 0.0</y_dot>
+            <y_ddot>0.0 0.0 0.0</y_ddot>
+          </point>            
+          <point time="0.5">
+            <y>0.075 0.575 0.0</y>
+            <y_dot>0.0 0.0 0.0</y_dot>
+            <y_ddot>0.0 0.0 0.0</y_ddot>
+          </point>  
+          <point time="1.0">
+            <y>-0.046 0.575 -0.088</y>
+            <y_dot>0.0 0.0 0.0</y_dot>
+            <y_ddot>0.0 0.0 0.0</y_ddot>
+          </point> 
+          <point time="1.0">
+            <y>0.0 0.575 0.054</y>
+            <y_dot>0.0 0.0 0.0</y_dot>
+            <y_ddot>0.0 0.0 0.0</y_ddot>
+          </point> 
+          <point time="1.0">
+            <y>0.046 0.575 -0.088</y>
+            <y_dot>0.0 0.0 0.0</y_dot>
+            <y_ddot>0.0 0.0 0.0</y_ddot>
+          </point> 
+          <point time="1.0">
+            <y>-0.075 0.575 0.0</y>
+            <y_dot>0.0 0.0 0.0</y_dot>
+            <y_ddot>0.0 0.0 0.0</y_ddot>
+          </point> 
+          <point time="0.5">
+            <y>0 0.575 0.00</y>
+            <y_dot>0.0 0.0 0.0</y_dot>
+            <y_ddot>0.0 0.0 0.0</y_ddot>
+          </point> 
+        </points>
+      </quintic_spline_trajectory>
     </operational_trajectories>
 </trajectories>
diff --git a/dependencies/Fixed_Step_ODE_Solvers/ode1.m b/dependencies/Fixed_Step_ODE_Solvers/ode1.m
@@ -1,4 +1,4 @@
-function Y = ode1(odefun,tspan,y0,varargin)
+function [fake_output, Y] = ode1(odefun,tspan,y0,varargin)
 %ODE1  Solve differential equations with a non-adaptive method of order 1.
 %   Y = ODE1(ODEFUN,TSPAN,Y0) with TSPAN = [T1, T2, T3, ... TN] integrates 
 %   the system of differential equations y' = f(t,y) by stepping from T0 to 
@@ -54,3 +54,4 @@
   Y(:,i+1) = Y(:,i) + h(i)*feval(odefun,tspan(i),Y(:,i),varargin{:});
 end
 Y = Y.';
+fake_output = [];
diff --git a/src/Analysis/ForwardDynamics/FDSolverType.m b/src/Analysis/ForwardDynamics/FDSolverType.m
@@ -13,5 +13,6 @@
         ODE23T
         ODE23TB
         ODE4
+        ODE1
     end
 end
diff --git a/src/Analysis/ForwardDynamics/ForwardDynamics.m b/src/Analysis/ForwardDynamics/ForwardDynamics.m
@@ -44,6 +44,8 @@
                     solverFn = @(f,t,y0) ode23tb(f,t,y0);
                 case FDSolverType.ODE4
                     solverFn = @(f,t,y0) ode4(f,t,y0);
+                case FDSolverType.ODE1
+                    solverFn = @(f,t,y0) ode1(f,t,y0);
             end
             [~, y_out] = solverFn(@(~,y) ForwardDynamics.eom(0, y, model, cable_forces_active, cable_indices_active, w_ext), [0 dt], y0);
             %[~, y_out] = ode113(@(~,y) ForwardDynamics.eom(0, y, model, cable_forces, w_ext), [0 dt], y0);

diff --git a/src/Simulation/Control/ControllerSimulator.m b/src/Simulation/Control/ControllerSimulator.m
@@ -69,6 +69,7 @@
         l_0_ctrl            % initial cabel lengths in the environment (used to restore the absolute lengths when relative encoder is used)
 
         rounding_error_guard % used to cut the effect of rounding error
+
     end
 
     methods
@@ -343,8 +344,10 @@ function run(obj, ref_trajectory, q0, q0_dot, q0_ddot)
             %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
             first_cycle = true;
             for t = 1:length(obj.timeVector)
-                CASPR_log.Info(sprintf('Time : %f', obj.timeVector(t)));
-                CASPR_log.Info(sprintf('Completion Percentage: %3.2f%%',100*t/length(obj.timeVector)));
+                if (~obj.simopt.is_silent_mode)
+                    CASPR_log.Info(sprintf('Time : %f', obj.timeVector(t)));
+                    CASPR_log.Info(sprintf('Completion Percentage: %3.2f%%',100*t/length(obj.timeVector)));
+                end
 
                 % extract current time to make component update decisions
                 current_time = obj.timeVector(t);
@@ -550,11 +553,11 @@ function forwardDynamicUpdate(obj, skip_FD)
                 % compiled mode
                 if (obj.model.modelMode ~= ModelModeType.COMPILED)
                     obj.stiffness{obj.sim_counter + 1}     	 =   obj.model.K;
-                    obj.stiffness{obj.sim_counter + 1}
                 end
             else
                 % update sim_trajectory with new state from FD
                 % algorithm
+                if (obj.dyn)
                 [obj.trajectory.q{obj.sim_counter + 1}, obj.trajectory.q_dot{obj.sim_counter + 1}, obj.trajectory.q_ddot{obj.sim_counter + 1}, obj.true_model] = ...
                     obj.fdSolver.compute(obj.trajectory.q{obj.sim_counter}, obj.trajectory.q_dot{obj.sim_counter}, obj.cableForces{obj.ctrl_counter}, obj.true_model.cableModel.cableIndicesActive, obj.w_ext, obj.timeVector(obj.sim_counter + 1) - obj.timeVector(obj.sim_counter), obj.true_model);
                 obj.trajectory.q_ddot{obj.sim_counter + 1}	=   obj.trajectory.q_ddot{obj.sim_counter};
@@ -567,7 +570,6 @@ function forwardDynamicUpdate(obj, skip_FD)
                 % compiled mode
                 if (obj.model.modelMode ~= ModelModeType.COMPILED)
                     obj.stiffness{obj.sim_counter + 1}     	 =   obj.model.K;
-                    obj.stiffness{obj.sim_counter + 1}
                 end
             end
             obj.sim_counter = obj.sim_counter + 1;

diff --git a/src/Simulation/Control/ControllerSimulatorOptions.m b/src/Simulation/Control/ControllerSimulatorOptions.m
@@ -17,6 +17,7 @@
 %       EnableObserver              ->  enable_observer
 %       FKDebugging                 ->  forward_kinematics_debugging
 %       IsOpSpaceControl            ->  is_operational_space_control
+%       IsSilentMode                ->  is_silent_mode
 %   The syntex for constructor call:
 %       opt = ControllerSimulatorOptions()
 %           -->  use the default options
@@ -39,6 +40,7 @@
         enable_observer                     % if true observer is assumed to be passed to the simulator and enabled
         forward_kinematics_debugging        % if true, simulator will save all available FK data for future debugging
         is_operational_space_control        % if true the simulator will be cofigured to perform operational space control
+        is_silent_mode                   % if true nothing will be printed out when this flag is true (e.g. Compeletion Percentage)
     end
     methods
         % constructors
@@ -79,6 +81,7 @@ function defaultOptions(obj)
             obj.enable_observer                 =   false;
             obj.forward_kinematics_debugging    =   false;
             obj.is_operational_space_control    =   false;
+            obj.is_silent_mode                  =   false;
         end
 
         % process option input
@@ -152,6 +155,13 @@ function inputInterpretation(obj, input_category, input_value)
                     obj.is_operational_space_control     =   false;
                 end
             end
+            if (strcmp(input_category, 'IsSilentMode'))
+                if (strcmp(input_value, 'true'))
+                    obj.is_silent_mode      =   true;
+                else
+                    obj.is_silent_mode      =   false;
+                end
+            end
         end
     end
 end
-Original file line number
+Diff line change
@@ Expand Up / @@ -13,5 +13,6 @@ @@
             ODE23T
             ODE23TB
             ODE4
+            ODE1
         end
     end