Skip to content

Commit

Permalink
Merge pull request #20 from roahmlab/develop
Browse files Browse the repository at this point in the history 
Version push to v0.2.0
  • Loading branch information
@BuildingAtom
BuildingAtom authored Oct 19, 2023
2 parents 188fae3 + 27854a9 commit b273b4e
Show file tree
Hide file tree
Showing 99 changed files with 6,500 additions and 1,412 deletions.
24 changes: 18 additions & 6 deletions +armour/+agent/@ArmourMexController/ArmourMexController.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,25 @@

methods
function reset(self, varargin)
[email protected](self, varargin{:})
parser = inputParser;
parser.KeepUnmatched = true;
parser.PartialMatching = false;
parser.StructExpand = false;
parser.addParameter('robot_file', "kinova_without_gripper.txt");
parser.parse(varargin{:})

robot_file = parser.Results.robot_file;
passthrough_args = namedargs2cell(parser.Unmatched);

[email protected](self, passthrough_args{:})

% Default model
robot_file = fullfile(basepath(self), "kinova_without_gripper.txt");
% robot_file = fullfile(basepath(self), "kinova_without_gripper.txt");
if ~robot_file.startsWith('/')
robot_file = fullfile(basepath(self), robot_file);
end
robot_file = char(robot_file);
warning('Current robust controller is hardcoded to use kinova_without_gripper!');
self.vdisp(['Using robot file: ', robot_file], "INFO");

model_uncertainty = 0.03;
warning('Current robust controller is hardcoded to use model uncertainty of 0.03 for all!');
Expand Down Expand Up @@ -53,9 +66,8 @@ function reset(self, varargin)
velocity = z(self.robot_state.velocity_indices);

% Prepare trajectory
trajectory = self.trajectories{end};
startTime = trajectory.startState.time;
target = trajectory.getCommand(startTime + t);
startTime = self.robot_state.get_state().time;
target = self.trajectories.getCommand(startTime + t);

% Get the control inputs from the mex controller
[u, tau, v] = updateKinovaController( ...
Expand Down
10 changes: 5 additions & 5 deletions +armour/+agent/ArmKinematics.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,11 @@
arguments
arm_info armour.agent.ArmourAgentInfo
arm_state_component armour.agent.ArmourAgentState
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.verbose_level
options.name
end
self.mergeoptions(optionsStruct, options);
self.mergeoptions(optionsStruct.options, options);

self.arm_info = arm_info;
self.arm_state = arm_state_component;
Expand All @@ -36,11 +36,11 @@
function reset(self, optionsStruct, options)
arguments
self
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.verboseLevel
options.name
end
options = self.mergeoptions(optionsStruct, options);
options = self.mergeoptions(optionsStruct.options, options);

self.set_vdisplevel(options.verboseLevel);
self.name = options.name;
Expand Down Expand Up @@ -76,7 +76,7 @@ function reset(self, optionsStruct, options)
end

% interpolate the state for the corresponding time
j_vals = self.arm_state.get_state(t).q;
j_vals = self.arm_state.get_state(t).position;
else
% assume a configuration was put in
q = time_or_config;
Expand Down
8 changes: 4 additions & 4 deletions +armour/+agent/ArmourAgentInfo.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,15 +56,15 @@
arguments
robot rigidBodyTree
params
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.M_min_eigenvalue
options.gravity
options.joint_velocity_limits
options.joint_torque_limits
options.transmission_inertia
options.buffer_dist
end
self.mergeoptions(optionsStruct, options);
self.mergeoptions(optionsStruct.options, options);

% we need the robot to have the column dataformat to work with
% our code, so set that. Gravity is an option so set that
Expand All @@ -81,15 +81,15 @@
function reset(self, optionsStruct, options)
arguments
self
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.M_min_eigenvalue
options.gravity
options.joint_velocity_limits
options.joint_torque_limits
options.transmission_inertia
options.buffer_dist
end
options = self.mergeoptions(optionsStruct, options);
options = self.mergeoptions(optionsStruct.options, options);
if isempty(options.joint_velocity_limits)
error("Must pass in joint_velocity_limits externally!")
end
Expand Down
27 changes: 15 additions & 12 deletions +armour/+agent/ArmourAgentState.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,13 +40,13 @@
function self = ArmourAgentState(arm_info, optionsStruct, options)
arguments
arm_info armour.agent.ArmourAgentInfo
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.initial_position
options.initial_velocity
options.verboseLevel
options.name
end
self.mergeoptions(optionsStruct, options);
self.mergeoptions(optionsStruct.options, options);

% Setup
self.entity_info = arm_info;
Expand All @@ -58,13 +58,13 @@
function reset(self, optionsStruct, options)
arguments
self
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.initial_position
options.initial_velocity
options.verboseLevel
options.name
end
options = self.mergeoptions(optionsStruct, options);
options = self.mergeoptions(optionsStruct.options, options);

% Set component dependent properties
self.n_states = 2 * self.entity_info.num_q;
Expand Down Expand Up @@ -149,22 +149,25 @@ function random_init(self, options)
end
end

function state = get_state(self, time)
function state_out = get_state(self, time)
arguments
self
time = self.time(end);
self armour.agent.ArmourAgentState
time(1,:) double = self.time(end);
end
state = rtd.entity.states.ArmRobotState(self.position_indices, self.velocity_indices);

% Default to the last time and state
state.time = time;
state.state = repmat(self.state(:,end),1,length(time));
state_data = repmat(self.state(:,end),1,length(time));

% If we can and need to interpolate the state, do it
mask = time <= self.time(end);
if length(self.time) > 1 && any(mask)
state.state(:,mask) = interp1(self.time, self.state.', time(mask)).';
state_data(:,mask) = interp1(self.time, self.state.', time(mask)).';
end

state_out(length(time)) = rtd.entity.states.ArmRobotStateInstance();
state_out.setTimes(time);
state_out.setStateSpace(state_data, ...
position_idxs=self.position_indices, ...
velocity_idxs=self.velocity_indices);
end

% function state = get_step_states(self, step)
Expand Down
35 changes: 17 additions & 18 deletions +armour/+agent/ArmourController.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@
robot_state = armour.agent.ArmourAgentState.empty()

n_inputs uint32 = 0

trajectories
end

% Extra properties we define
Expand All @@ -23,8 +25,6 @@
alpha_constant double
V_max double
r_norm_threshold double

trajectories = {}
end

methods (Static)
Expand All @@ -46,7 +46,7 @@
arguments
arm_info armour.agent.ArmourAgentInfo
arm_state_component armour.agent.ArmourAgentState
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.use_true_params_for_robust
options.use_disturbance_norm
options.Kr
Expand All @@ -56,7 +56,7 @@
options.verboseLevel
options.name
end
self.mergeoptions(optionsStruct, options);
self.mergeoptions(optionsStruct.options, options);

% Set base variables
self.robot_info = arm_info;
Expand All @@ -69,7 +69,7 @@
function reset(self, optionsStruct, options)
arguments
self
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.use_true_params_for_robust
options.use_disturbance_norm
options.Kr
Expand All @@ -79,7 +79,7 @@ function reset(self, optionsStruct, options)
options.verboseLevel
options.name
end
options = self.mergeoptions(optionsStruct, options);
options = self.mergeoptions(optionsStruct.options, options);

% Set component dependent variables
self.n_inputs = self.robot_info.num_q;
Expand All @@ -106,18 +106,19 @@ function reset(self, optionsStruct, options)

% Create the initial trajectory
initial_traj = armour.trajectory.ZeroHoldArmTrajectory(self.robot_state.get_state);
self.trajectories = {initial_traj};
self.trajectories.setInitialTrajectory(initial_traj);
self.trajectories.clear()
end


function setTrajectory(self, trajectory)
arguments
self
trajectory rtd.planner.trajectory.Trajectory
trajectory rtd.trajectory.Trajectory
end
% Add the trajectory if it is valid
if trajectory.validate()
self.trajectories = [self.trajectories, {trajectory}];
self.trajectories.setTrajectory(trajectory);
end
end

Expand All @@ -135,9 +136,8 @@ function setTrajectory(self, trajectory)
velocity = z(self.robot_state.velocity_indices);

% Prepare trajectory
trajectory = self.trajectories{end};
startTime = trajectory.startState.time;
target = trajectory.getCommand(startTime + t);
startTime = self.robot_state.get_state().time;
target = self.trajectories.getCommand(startTime + t);

% error terms
err = target.position - position;
Expand Down Expand Up @@ -272,17 +272,16 @@ function setTrajectory(self, trajectory)
t_check = t_input(1):t_check_step:t_input(end);
u_check = self.robot_state.get_state(t_check);
% Get the reference trajectory
trajectory = self.trajectories{end};
reference_trajectory = arrayfun(@(t)trajectory.getCommand(t), t_check);
u_pos_ref = [reference_trajectory.q];
u_vel_ref = [reference_trajectory.q_dot];
reference_trajectory = arrayfun(@(t)self.trajectories.getCommand(t), t_check);
u_pos_ref = [reference_trajectory.position];
u_vel_ref = [reference_trajectory.velocity];

% check bound satisfaction
self.vdisp('Running ultimate bound check!', 'INFO');

% Absolute difference
u_pos_diff = abs(u_pos_ref - u_check.q);
u_vel_diff = abs(u_vel_ref - u_check.q_dot);
u_pos_diff = abs(u_pos_ref - [u_check.position]);
u_vel_diff = abs(u_vel_ref - [u_check.velocity]);
position_exceeded = u_pos_diff > self.ultimate_bound_position;
velocity_exceeded = u_vel_diff > self.ultimate_bound_velocity;

Expand Down
12 changes: 7 additions & 5 deletions +armour/+agent/ArmourDynamics.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@
arm_info armour.agent.ArmourAgentInfo
arm_state_component armour.agent.ArmourAgentState
controller_component armour.agent.ArmourController
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.time_discretization
options.measurement_noise_points
options.measurement_noise_pos_sigma
Expand All @@ -61,7 +61,7 @@
options.verboseLevel
options.name
end
self.mergeoptions(optionsStruct, options);
self.mergeoptions(optionsStruct.options, options);

% set base variables
self.robot_info = arm_info;
Expand All @@ -74,7 +74,7 @@
function reset(self, optionsStruct, options)
arguments
self
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.time_discretization
options.measurement_noise_points
options.measurement_noise_pos_sigma
Expand All @@ -83,7 +83,7 @@ function reset(self, optionsStruct, options)
options.verboseLevel
options.name
end
options = self.mergeoptions(optionsStruct, options);
options = self.mergeoptions(optionsStruct.options, options);

% if we're going to log, set it up
if options.log_controller
Expand Down Expand Up @@ -118,7 +118,9 @@ function move(self, t_move)

% get the current state
state = self.robot_state.get_state;
zcur = state.state;
zcur = state.getStateSpace( ...
position_idxs=self.robot_state.position_indices, ...
velocity_idxs=self.robot_state.velocity_indices);

% call the ode solver to simulate agent
[tout,zout] = self.integrator(@(t,z) self.dynamics(t,z),...
Expand Down
2 changes: 1 addition & 1 deletion +armour/+agent/ArmourPatchCollision.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@ function create_collision_check_patch_data(self)
q = options.q;
if ~isempty(options.time)
state = self.arm_state.get_state(options.time);
q = state.q;
q = state.position;
end

[R,T] = self.kinematics.get_link_rotations_and_translations(q) ;
Expand Down
8 changes: 4 additions & 4 deletions +armour/+agent/ArmourPatchVisual.m
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@
arm_info armour.agent.ArmourAgentInfo
arm_state_component armour.agent.ArmourAgentState
kinematics_component armour.agent.ArmKinematics
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.face_color
options.face_opacity
options.edge_color
Expand All @@ -53,7 +53,7 @@
options.verboseLevel
options.name
end
self.mergeoptions(optionsStruct, options);
self.mergeoptions(optionsStruct.options, options);

self.arm_info = arm_info;
self.arm_state = arm_state_component;
Expand All @@ -65,7 +65,7 @@
function reset(self, optionsStruct, options)
arguments
self
optionsStruct struct = struct()
optionsStruct.options struct = struct()
options.face_color
options.face_opacity
options.edge_color
Expand All @@ -76,7 +76,7 @@ function reset(self, optionsStruct, options)
options.verboseLevel
options.name
end
options = self.mergeoptions(optionsStruct, options);
options = self.mergeoptions(optionsStruct.options, options);

% Set up verbose output
self.name = options.name;
Expand Down
Loading

0 comments on commit b273b4e

Please sign in to comment.