diff --git a/+acrobot/acrobot.m b/+acrobot/acrobot.m index c3b957a..ea5e88b 100644 --- a/+acrobot/acrobot.m +++ b/+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)); diff --git a/+acrobot/acrobot_state_estimator.m b/+acrobot/acrobot_state_estimator.m index f5cb797..8c558c6 100644 --- a/+acrobot/acrobot_state_estimator.m +++ b/+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(~) diff --git a/+acrobot/acrobot_state_estimator_helper.m b/+acrobot/acrobot_state_estimator_helper.m new file mode 100644 index 0000000..38a37ff --- /dev/null +++ b/+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 + diff --git a/+acrobot/acrobot_walker.m b/+acrobot/acrobot_walker.m index 7fdbf73..5f65b6b 100644 --- a/+acrobot/acrobot_walker.m +++ b/+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 diff --git a/acrobot_sim.slx b/acrobot_sim.slx index aa66cbd..4254de7 100644 Binary files a/acrobot_sim.slx and b/acrobot_sim.slx differ diff --git a/acrobot_sim.slx.original b/acrobot_sim.slx.original index a377905..045bc3e 100644 Binary files a/acrobot_sim.slx.original and b/acrobot_sim.slx.original differ