Bstn branch

+acrobot/acrobot.m

@@ -35,12 +35,12 @@
         B = [0; 1];
 
     end
-    properties
+    properties(Access = public)
         % Physical Parameters
         g = 9.81;
 
         % Mechanical Parameters
-        leg_length;
+        leg_length = 0;
         foot_radius = 0.018;
         angle_limit = pi/20;
         motor_friction = 0.01;
@@ -55,9 +55,9 @@
         % Curves
         top_clip = 0;
         bottom_clip = 10;
-        pre_c;  % Pre first step
-        c1;     % First Step
-        c2;     % Second Step
+        pre_c = 0;  % Pre first step
+        c1 = 0;     % First Step
+        c2 = 0;     % Second Step
     end
 
     methods
@@ -79,14 +79,14 @@
             obj.c1 = acrobot.curve(pi/9.3, pi*0.25, 1.40, 0.48, [0.0960804536593042,-0.211617429208404,0.659358736156863,0.211867160150805]); % Third Step
             obj.c2 = acrobot.curve(pi/9.5, pi*0.25, 1.40, 0.48, [0.0305427320850607,-0.210190202255343,0.549640602180841,0.120949051732730]); % Second Step
 
-            % Create Robot Equation handles
+%             % Create Robot Equation handles
             obj.solveRoboticsEquation();
-
-            % Solve for the curve for both legs
+%             
+%             % Solve for the curve for both legs
             obj.calcRobotStates();
-            obj.updateCurves();
+%             obj.updateCurves();
         end
-
+%         
         function mass = lmass(obj, num)
             if rem(obj.step_count,2) == 1
                 if num == 2
@@ -158,7 +158,7 @@
 
         function calcRobotStates(obj)
             % Post impact for one foot is pre-impact for next foot
-
+            temp = obj.step_count;
             % Heavy foot on the ground
             c1_qp = [(pi + obj.c2.beta)/2; pi - obj.c2.beta]; % Joint angles post impact
             c1_qm = [(pi - obj.c1.beta)/2; obj.c1.beta - pi]; % Joint angles pre impact
@@ -183,6 +183,11 @@ function calcRobotStates(obj)
             % Then again ground to ground
             obj.c2.xm = [c1_qm; c1_w];
             obj.c2.xp = [c1_qp; c1_v];
+
+            obj.step_count = temp;
+            if obj.step_count ~= 0
+                step_count;
+            end
         end
 
         % objective [dist to final point; velocity to final point]
@@ -324,7 +329,7 @@ function calcHolonomicCurves(obj, curve, optimize)
             rend = obj.calc_rend(obj.leg_length, obj.leg_length, qm(1), qm(2));
             rend = rend + [cos(impact_angle) * 0.01; sin(impact_angle) * 0.01];
             qm_pre = obj.calc_qd(obj.leg_length, obj.leg_length, rend(1), rend(2));
-            w = unit(qm - qm_pre) * impact_velocity;
+            w = (qm - qm_pre)/norm(qm-qm_pre) * impact_velocity;
 
             % Post impact calculations
             De = obj.calc_De(obj.linertia(1), obj.linertia(2), obj.leg_length, obj.lcom(1), obj.lcom(2), obj.lmass(1), obj.lmass(2), qm(1), qm(2));

+acrobot/acrobot_state_estimator.m

@@ -20,9 +20,9 @@
         leg_length = 0.335;
         state = [0;0;0;0];
         cycleCount = 0;
-        timeout = false;
+        timeOut = false;
         max_velocity_change = 10;
-
+        step_count = 0;
     end
 
     methods
@@ -46,9 +46,9 @@ function setupImplPublic(obj)
             if (mod(step, 2) == 1)
                 qm = 2*pi - qm;
             end
-
+%             pos = (quat2eul(pos'))';
             % q1 & q1_dot
-            roll = pos(2);
+            roll = pos(3);
             q1 = roll + pi/2;
             q1_dot = (q1 - obj.state(1))/obj.sample_time;
 
@@ -63,16 +63,21 @@ function setupImplPublic(obj)
                 q2_dot = obj.state(4);
             end
 
+            collision = 0;
+
+
             rH = obj.leg_length * [cos(q1); sin(q1)];
             rc2 = rH + obj.leg_length * [cos(q1+q2); sin(q1+q2)];
             dist_to_floor = rc2(2);
             if dist_to_floor < 0
+                obj.timeOut = true;
                 collision = 1;
-            else
-                collision = 0;
+                obj.step_count = obj.step_count + 1;
             end
-
+
+
             state = [q1;q2;q1_dot;q2_dot];
+%             state = [ pos(1); pos(2) ; pos(3); 0];
             obj.state = state;
         end
     end
@@ -83,8 +88,15 @@ function setupImpl(obj)
             obj.setupImplPublic();
         end
 
-        function [state, collision] = stepImpl(obj, step, pos, acc, motor_step)
-            [state, collision] = obj.stepImplPublic(step, pos, acc, motor_step);
+        function [state, collision] = stepImpl(obj,pos1, pos2, motor_step)
+             if mod(obj.step_count, 2) == 0
+                pos = pos1;
+            else
+                pos = pos2;
+             end
+            %placeholder for acc
+            acc = 0;
+            [state, collision] = obj.stepImplPublic(obj.step_count, pos,acc, motor_step);
         end
 
         function [s1, s2] = getOutputSizeImpl(~)
@@ -94,7 +106,7 @@ function setupImpl(obj)
 
         function [d1, d2] = getOutputDataTypeImpl(~)
             d1 = 'double';
-            d2 = 'boolean';
+            d2 = 'double';
         end
 
         function [c1, c2] = isOutputComplexImpl(~)

+acrobot/acrobot_state_estimator_helper.m

@@ -0,0 +1,84 @@
+classdef acrobot_state_estimator_helper < matlab.System
+    % Public, tunable properties
+    properties
+        sample_time = 0.01;
+    end
+
+    % Public, non-tunable properties
+    properties(Nontunable)
+
+    end
+
+    properties(DiscreteState)
+
+
+    end
+
+    % Pre-computed constants
+    properties(Access = private)
+        cycle = 0;
+    end
+
+    methods
+        % Constructor
+        function obj = acrobot_state_estimator_helper(varargin)
+            % Support name-value pair arguments when constructing object
+            setProperties(obj,nargin,varargin{:})
+        end
+    end
+
+    methods(Access = public)
+        function setupImplPublic(obj)
+            % Perform one-time calculations, such as computing constants
+        end
+    end
+
+    methods(Access = protected)
+        %% Common functions
+        function setupImpl(obj)
+            obj.setupImplPublic();
+        end
+
+        function [step, pos, acc] = stepImpl(obj, step_count, pos1, acc1, pos2, acc2)
+                step = step_count;
+            if mod(cast(step, 'int8'), 2) == 0
+                pos = pos2;
+                acc = acc2;
+            else
+                pos = pos1;
+                acc = acc1;
+            end
+            obj.cycle = obj.cycle + 1;
+        end
+
+        function [s1, s2, s3] = getOutputSizeImpl(~)
+            s1 = 1;
+            s2 = [4,1];
+            s3 = [3,1];
+        end
+
+        function [d1, d2, d3] = getOutputDataTypeImpl(~)
+            d1 = 'double';
+            d2 = 'double';
+            d3 = 'double';
+        end
+
+        function [c1, c2, c3] = isOutputComplexImpl(~)
+            c1 = false;
+            c2 = false;
+            c3 = false;
+        end
+
+        function [c1, c2, c3] = isOutputFixedSizeImpl(~)
+            c1 = true;
+            c2 = true;
+            c3 = true;
+        end
+
+        function sts = getSampleTimeImpl(obj)
+            sts = createSampleTime(obj,'Type','Discrete',...
+              'SampleTime',obj.sample_time,'OffsetTime',0.0);
+        end
+    end 
+end
+

+acrobot/acrobot_walker.m

@@ -35,9 +35,10 @@ function setupImpl(obj)
             obj.tau = [0;0];
         end
 
-        function tau = stepImpl(obj,state)
-            obj.x = state;
-            obj.show(0);
+        function tau = stepImpl(obj, state)
+%            
+%             obj.x = state;
+%             obj.show(0);
             tau = 0;
         end
 
@@ -47,14 +48,17 @@ function setupImpl(obj)
 
         function d1 = getOutputDataTypeImpl(~)
             d1 = 'double';
+
         end
 
         function c1 = isOutputComplexImpl(~)
             c1 = false;
+
         end
 
         function c1 = isOutputFixedSizeImpl(~)
             c1 = true;
+
         end
     end
 end