diff --git a/+armour/+agent/@ArmourMexController/ArmourMexController.m b/+armour/+agent/@ArmourMexController/ArmourMexController.m
index 6ae397f9..d2616929 100644
--- a/+armour/+agent/@ArmourMexController/ArmourMexController.m
+++ b/+armour/+agent/@ArmourMexController/ArmourMexController.m
@@ -7,12 +7,25 @@
methods
function reset(self, varargin)
- reset@armour.agent.ArmourController(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);
+
+ reset@armour.agent.ArmourController(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!');
@@ -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( ...
diff --git a/+armour/+agent/ArmKinematics.m b/+armour/+agent/ArmKinematics.m
index b59759cc..ec94761e 100644
--- a/+armour/+agent/ArmKinematics.m
+++ b/+armour/+agent/ArmKinematics.m
@@ -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;
@@ -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;
@@ -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;
diff --git a/+armour/+agent/ArmourAgentInfo.m b/+armour/+agent/ArmourAgentInfo.m
index 4a718951..f1a7f44f 100644
--- a/+armour/+agent/ArmourAgentInfo.m
+++ b/+armour/+agent/ArmourAgentInfo.m
@@ -56,7 +56,7 @@
arguments
robot rigidBodyTree
params
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.M_min_eigenvalue
options.gravity
options.joint_velocity_limits
@@ -64,7 +64,7 @@
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
@@ -81,7 +81,7 @@
function reset(self, optionsStruct, options)
arguments
self
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.M_min_eigenvalue
options.gravity
options.joint_velocity_limits
@@ -89,7 +89,7 @@ function reset(self, optionsStruct, options)
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
diff --git a/+armour/+agent/ArmourAgentState.m b/+armour/+agent/ArmourAgentState.m
index 73170e30..e09e326e 100644
--- a/+armour/+agent/ArmourAgentState.m
+++ b/+armour/+agent/ArmourAgentState.m
@@ -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;
@@ -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;
@@ -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)
diff --git a/+armour/+agent/ArmourController.m b/+armour/+agent/ArmourController.m
index 0b6d6710..8dbc3510 100644
--- a/+armour/+agent/ArmourController.m
+++ b/+armour/+agent/ArmourController.m
@@ -11,6 +11,8 @@
robot_state = armour.agent.ArmourAgentState.empty()
n_inputs uint32 = 0
+
+ trajectories
end
% Extra properties we define
@@ -23,8 +25,6 @@
alpha_constant double
V_max double
r_norm_threshold double
-
- trajectories = {}
end
methods (Static)
@@ -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
@@ -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;
@@ -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
@@ -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;
@@ -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
@@ -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;
@@ -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;
diff --git a/+armour/+agent/ArmourDynamics.m b/+armour/+agent/ArmourDynamics.m
index b71e6865..0d928673 100644
--- a/+armour/+agent/ArmourDynamics.m
+++ b/+armour/+agent/ArmourDynamics.m
@@ -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
@@ -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;
@@ -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
@@ -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
@@ -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),...
diff --git a/+armour/+agent/ArmourPatchCollision.m b/+armour/+agent/ArmourPatchCollision.m
index 70d4a858..b73b8af1 100644
--- a/+armour/+agent/ArmourPatchCollision.m
+++ b/+armour/+agent/ArmourPatchCollision.m
@@ -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) ;
diff --git a/+armour/+agent/ArmourPatchVisual.m b/+armour/+agent/ArmourPatchVisual.m
index 4338daf6..1b9e28ae 100644
--- a/+armour/+agent/ArmourPatchVisual.m
+++ b/+armour/+agent/ArmourPatchVisual.m
@@ -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
@@ -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;
@@ -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
@@ -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;
diff --git a/+armour/+deprecation/RandomArmConfigurationGoal.m b/+armour/+deprecation/RandomArmConfigurationGoal.m
index 7442b58b..ee54d51e 100644
--- a/+armour/+deprecation/RandomArmConfigurationGoal.m
+++ b/+armour/+deprecation/RandomArmConfigurationGoal.m
@@ -48,13 +48,13 @@
arguments
collision_system
arm_agent
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.time_discretization
options.face_color
options.face_opacity
options.edge_color
options.edge_opacity
- options.edge_width
+ options.edge_style
options.start_position
options.goal_position
options.min_dist_start_to_goal
@@ -63,7 +63,7 @@
options.verboseLevel
options.name
end
- self.mergeoptions(optionsStruct, options);
+ self.mergeoptions(optionsStruct.options, options);
self.collision_system = collision_system;
self.arm_agent = arm_agent;
@@ -74,13 +74,13 @@
function reset(self, optionsStruct, options)
arguments
self
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.time_discretization
options.face_color
options.face_opacity
options.edge_color
options.edge_opacity
- options.edge_width
+ options.edge_style
options.start_position
options.goal_position
options.min_dist_start_to_goal
@@ -89,7 +89,7 @@ function reset(self, optionsStruct, options)
options.verboseLevel
options.name
end
- options = self.mergeoptions(optionsStruct, options);
+ options = self.mergeoptions(optionsStruct.options, options);
self.time_discretization = options.time_discretization;
% Set up verbose output
@@ -109,7 +109,7 @@ function reset(self, optionsStruct, options)
self.goal_radius = options.goal_radius;
if isempty(self.start_position)
- self.start_position = self.arm_agent.state.get_state().q;
+ self.start_position = self.arm_agent.state.get_state().position;
end
if isempty(self.min_dist_start_to_goal)
pos_limits = [self.arm_agent.info.joints.position_limits];
@@ -151,7 +151,7 @@ function reset(self, optionsStruct, options)
% Accumulate the return
goal = false;
- get_pos = @(t)self.arm_agent.state.get_state(t).q;
+ get_pos = @(t)self.arm_agent.state.get_state(t).position;
for t_check = t_vec
goal = goal || all(abs(get_pos(t_check) - self.goal_position) <= self.goal_radius, 'all');
end
@@ -202,13 +202,13 @@ function createGoal(self, position)
end
% compute distance to the goal & update time
- proposed_goal = self.arm_agent.state.get_state().q;
+ proposed_goal = self.arm_agent.state.get_state().position;
proposed_goal_dist = norm(self.start_position - proposed_goal);
end
if isempty(proposed_goal)
self.vdisp('Failed to find collision free goal! Using random goal', 'GENERAL');
% Position is already randomized
- proposed_goal = self.arm_agent.state.get_state().q;
+ proposed_goal = self.arm_agent.state.get_state().position;
end
% save and update
self.goal_position = proposed_goal;
@@ -219,12 +219,17 @@ function createGoal(self, position)
function resetVisual(self)
% Generate a visual
+ prev_fig = get(groot,'CurrentFigure');
+ temp_fig = figure;
self.arm_agent.visual.plot_links(self.goal_position);
% Extract the data to plot
faces = {self.arm_agent.visual.plot_data.links.Faces};
vertices = {self.arm_agent.visual.plot_data.links.Vertices};
self.link_plot_data.faces = faces;
self.link_plot_data.vertices = vertices;
+ % restore visual
+ close(temp_fig);
+ set(groot,'CurrentFigure',prev_fig);
end
function plot(self,options)
diff --git a/+armour/+legacy/create_jrs_online.m b/+armour/+legacy/create_jrs_online.m
deleted file mode 100644
index 7209c813..00000000
--- a/+armour/+legacy/create_jrs_online.m
+++ /dev/null
@@ -1,263 +0,0 @@
-function [Q_des, Qd_des, Qdd_des, Q, Qd, Qd_a, Qdd_a, R_des, R_t_des, R, R_t, jrs_info] = create_jrs_online(q, dq, ddq, joint_axes, taylor_degree, traj_type, add_ultimate_bound, LLC_info)
-% patrick 20220330
-% implementing jrs computation that avoids going through CORA"s reachability toolbox
-% and relies only on PZ arithmetic.
-% can probably be greatly sped up by a lot (all?) computations offline.
-% implement for 2 different trajectory parameterizations:
-% original ARMTD: q(t) = q_0 + \dot{q}_0*t + 1/2*k*t^2
-% Bernstein: q(t) = \sum_i=0^3 \beta_i B_i(t)
-
-% patrick 20221027
-% moved file from dynamics_dev to armtd_dev
-% continue development on armtd_dev!
-
-% adam 20230206
-% copied into legacy subpackage of armour in rtd-code repository from
-% commit df7cc3e and adapted
-%
-import armour.pz_roahm.*
-import armour.legacy.*
-
-%% setup
-if ~exist('add_ultimate_bound', 'var')
- add_ultimate_bound = false;
-end
-
-make_gens_independent = true; % treat time and error as independent generators
-% (we probably don't want to do this until after cos/sin has been taken to get the
-% JRSs as tight as possible.)
-if ~make_gens_independent
- disp('Treating time and tracking error as dependent generators. Make sure to remove them before creating constraints.');
-end
-
-q = q(:);
-dq = dq(:);
-n_q = length(q);
-
-if ~exist('ddq', 'var')
- ddq = zeros(n_q, 1);
-end
-
-if ~exist('joint_axes', 'var')
- joint_axes = [zeros(2, length(q)); ones(1, length(q))]; % default assumes all rotations about z axis
-end
-
-if ~exist('taylor_degree', 'var')
- taylor_degree = 1; % default uses degree 1 taylor expansion
-end
-
-if ~exist('traj_type', 'var')
- traj_type = 'orig'; % use original traj parameterization by default
-end
-
-% if ~exist('bernstein_waypoint', 'var')
- bernstein_center = zeros(size(q));
-% else
-% bernstein_center = bernstein_waypoint - q;
-% end
-
-if ~exist('LLC_info', 'var')
- % hard coded ultimate bound if not passed in
- ultimate_bound = 0.0191;
- k_r = 10; % assuming K_r = k_r*eye(n_q)
-else
- if add_ultimate_bound
- ultimate_bound = LLC_info.ultimate_bound;
- k_r = LLC_info.Kr;
- else
- k_r = 0;
- end
-end
-
-bernstein_final_range = pi/36*ones(n_q, 1);
-% bernstein_final_range = [pi/24; pi/72; pi/24; pi/72; pi/72; pi/72; pi/72];
-
-t_f = 1;
-t_p = 0.5;
-dt = 0.01;
-n_t = t_f/dt;
-n_t_p = t_p/t_f*n_t; % last time step of "planning" phase before braking phase
-
-% adjust range that k \in [-1, 1] corresponds to...
-n_k = n_q;
-c_k_orig = zeros(n_k, 1);
-g_k_orig = min(max(pi/24, abs(dq/3)), pi/3); % added critical abs() here!!
-% g_k = ones(n_q, 1);
-
-%% initialize
-id = [];
-id_add = [];
-id_names = [];
-
-T = cell(n_t, 1);
-K = cell(n_t, 1);
-Q_des = cell(n_t, 1);
-Qd_des = cell(n_t, 1);
-Qdd_des = cell(n_t, 1);
-Q = cell(n_t, 1);
-Qd = cell(n_t, 1);
-Qd_a = cell(n_t, 1);
-Qdd_a = cell(n_t, 1);
-R_des = cell(n_t, 1);
-R_t_des = cell(n_t, 1);
-R = cell(n_t, 1);
-R_t = cell(n_t, 1);
-
-%% get PZ versions of k, time, and error:
-% get traj param PZs
-switch traj_type
- case 'orig'
- for j = 1:n_q
- [id, id_names, id_add] = update_id(id, id_names, 1, ['k', num2str(j)]);
- K{j} = polyZonotope_ROAHM(0, 1, [], 1, id_add);
- end
- case 'bernstein'
- for j = 1:n_q
- [id, id_names, id_add] = update_id(id, id_names, 1, ['k', num2str(j)]);
- % K{j} = polyZonotope_ROAHM(q(j), bernstein_final_range, [], 1, id_add);
- K{j} = polyZonotope_ROAHM(0, 1, [], 1, id_add);
- end
-end
-
-% get time subinterval PZs
-[id, id_names, id_add] = update_id(id, id_names, n_t, ['t1']);
-for i = 1:n_t
- T{i} = polyZonotope_ROAHM(dt*(i-1) + dt/2, dt/2, [], 1, id_add(i, :));
-end
-
-% get ultimate bound PZs
-if add_ultimate_bound
- for j = 1:n_q
- [id, id_names, id_add] = update_id(id, id_names, 1, ['e1']);
- E_p{j} = polyZonotope_ROAHM(0, ultimate_bound/k_r, [], 1, id_add); % position
- [id, id_names, id_add] = update_id(id, id_names, 1, ['e2']);
- E_v{j} = polyZonotope_ROAHM(0, 2*ultimate_bound, [], 1, id_add); % velocity
- end
-else
- for j = 1:n_q
- E_p{j} = 0;
- E_v{j} = 0;
- end
-end
-
-%% multiply to get JRS
-% some preliminaries for traj computation:
-switch traj_type
-case 'orig'
- for j = 1:n_q
- Qd_plan{j} = dq(j) + (c_k_orig(j) + g_k_orig(j).*K{j}).*t_p;
- Qdd_brake{j} = (-1).*Qd_plan{j}.*(1/(t_f - t_p));
- Q_plan{j} = q(j) + dq(j).*t_p + 0.5.*(c_k_orig(j) + g_k_orig(j).*K{j}).*t_p^2;
- end
-case 'bernstein'
- for j = 1:n_q
- q1{j, 1} = q(j) + bernstein_center(j) + bernstein_final_range(j).*K{j}; % final position is initial position +- k \in [-1, 1]
- dq1 = 0;
- ddq1 = 0;
- beta{j} = match_deg5_bernstein_coefficients({q(j); dq(j); ddq(j); q1{j}; dq1; ddq1});
- alpha{j} = bernstein_to_poly(beta{j}, 5);
- end
-end
-
-% main loop:
-for i = 1:n_t
- for j = 1:n_q
- switch traj_type
- case 'orig'
- if i <= n_t_p
- Q_des{i}{j, 1} = q(j) + dq(j).*T{i} + 0.5.*(c_k_orig(j) + g_k_orig(j).*K{j}).*T{i}.*T{i};
- Qd_des{i}{j, 1} = dq(j) + (c_k_orig(j) + g_k_orig(j).*K{j}).*T{i};
- Qdd_des{i}{j, 1} = (c_k_orig(j) + g_k_orig(j).*K{j});
- else
- Q_des{i}{j, 1} = Q_plan{j} + Qd_plan{j}.*(T{i} - t_p) + 0.5.*Qdd_brake{j}.*(T{i} - t_p).*(T{i} - t_p);
- Qd_des{i}{j, 1} = Qd_plan{j} + Qdd_brake{j}.*(T{i} - t_p);
- Qdd_des{i}{j, 1} = Qdd_brake{j};
- end
- case 'bernstein'
-% error('not implemented yet');
- Q_des{i}{j, 1} = 0;
- Qd_des{i}{j, 1} = 0;
- Qdd_des{i}{j, 1} = 0;
- for k = 0:5
- Q_des{i}{j, 1} = Q_des{i}{j, 1} + alpha{j}{k+1}.*T{i}.^k;
- if k > 0
- Qd_des{i}{j, 1} = Qd_des{i}{j, 1} + k*alpha{j}{k+1}.*T{i}.^(k-1);
- end
- if k > 1
- Qdd_des{i}{j, 1} = Qdd_des{i}{j, 1} + (k)*(k-1)*alpha{j}{k+1}.*T{i}.^(k-2);
- end
- end
- end
-
- % add tracking error
- Q{i}{j, 1} = Q_des{i}{j, 1} + E_p{j};
- Qd{i}{j, 1} = Qd_des{i}{j, 1} + E_v{j};
- Qd_a{i}{j, 1} = Qd_des{i}{j, 1} + k_r.*E_p{j};
- Qdd_a{i}{j, 1} = Qdd_des{i}{j, 1} + k_r.*E_v{j};
-
- % get rotation matrices:
- [R_des{i}{j, 1}, R_t_des{i}{j, 1}] = get_pz_rotations_from_q(Q_des{i}{j, 1}, joint_axes(:, j), taylor_degree);
- [R{i}{j, 1}, R_t{i}{j, 1}] = get_pz_rotations_from_q(Q{i}{j, 1}, joint_axes(:, j), taylor_degree);
- end
-end
-
-%% throw away time/error generators and compress independent generators to just one (if 1D):
-if make_gens_independent
- for i = 1:n_t
- for j = 1:n_q
- k_id = id(j);
- Q_des{i}{j, 1} = remove_dependence_and_compress(Q_des{i}{j, 1}, k_id);
- Qd_des{i}{j, 1} = remove_dependence_and_compress(Qd_des{i}{j, 1}, k_id);
- Qdd_des{i}{j, 1} = remove_dependence_and_compress(Qdd_des{i}{j, 1}, k_id);
- Q{i}{j, 1} = remove_dependence_and_compress(Q{i}{j, 1}, k_id);
- Qd{i}{j, 1} = remove_dependence_and_compress(Qd{i}{j, 1}, k_id);
- Qd_a{i}{j, 1} = remove_dependence_and_compress(Qd_a{i}{j, 1}, k_id);
- Qdd_a{i}{j, 1} = remove_dependence_and_compress(Qdd_a{i}{j, 1}, k_id);
- R_des{i}{j, 1} = remove_dependence_and_compress_mat(R_des{i}{j, 1}, k_id);
- R_t_des{i}{j, 1} = remove_dependence_and_compress_mat(R_t_des{i}{j, 1}, k_id);
- R{i}{j, 1} = remove_dependence_and_compress_mat(R{i}{j, 1}, k_id);
- R_t{i}{j, 1} = remove_dependence_and_compress_mat(R_t{i}{j, 1}, k_id);
-
- end
- end
-
- % remove all ids except for k
- id(n_q+1:end, :) = [];
- id_names(n_q+1:end, :) = [];
-end
-
-% save info
-jrs_info.id = id;
-jrs_info.id_names = id_names;
-jrs_info.k_id = (1:n_q)';
-jrs_info.n_t = n_t;
-jrs_info.n_q = n_q;
-jrs_info.n_k = n_k;
-jrs_info.c_k_orig = c_k_orig;
-jrs_info.g_k_orig = g_k_orig;
-jrs_info.c_k_bernstein = bernstein_center;
-jrs_info.g_k_bernstein = bernstein_final_range;
-
-end
-
-function B = remove_dependence_and_compress(A, k_id)
-import armour.pz_roahm.*
- k_id_idx = (A.id == k_id);
- k_slc_idx = (A.expMat(k_id_idx, :) ~= 0 & all(A.expMat(~k_id_idx, :) == 0, 1)); % should only be one!
- if length(find(k_slc_idx)) > 1
- error('There should only be one fully-k-sliceable generator');
- end
- B = polyZonotope_ROAHM(A.c, A.G(k_slc_idx), sum(abs(A.G(~k_slc_idx))) + sum(abs(A.Grest)), A.expMat(k_id_idx, k_slc_idx), k_id);
- % B = polyZonotope_ROAHM(A.c, A.G(k_slc_idx), [A.G(~k_slc_idx), A.Grest], A.expMat(k_id_idx, k_slc_idx), k_id);
-end
-
-function B = remove_dependence_and_compress_mat(A, k_id)
-import armour.pz_roahm.*
- k_id_idx = (A.id == k_id);
- k_slc_idx = (A.expMat(k_id_idx, :) ~= 0 & all(A.expMat(~k_id_idx, :) == 0, 1)); % should only be one!
-% if length(find(k_slc_idx)) > 1
-% error('There should only be one fully-k-sliceable generator');
-% end
- B = matPolyZonotope_ROAHM(A.C, A.G(:, :, k_slc_idx), cat(3, A.G(:, :, ~k_slc_idx), A.Grest), A.expMat(k_id_idx, k_slc_idx), k_id);
-end
-
diff --git a/+armour/+legacy/create_pz_bounding_boxes.m b/+armour/+legacy/create_pz_bounding_boxes.m
index a3d8aa84..9d06b975 100644
--- a/+armour/+legacy/create_pz_bounding_boxes.m
+++ b/+armour/+legacy/create_pz_bounding_boxes.m
@@ -20,6 +20,7 @@
bounds = [-0.05, 0.05;
-0.05, 0.05;
-0.05, 0.05];
+ mesh{i, 1} = [];
end
c = (bounds(:, 1) + bounds(:, 2))./2;
diff --git a/+armour/+legacy/match_deg5_bernstein_coefficients.m b/+armour/+legacy/match_deg5_bernstein_coefficients.m
index 3d0e352f..d4e70c09 100644
--- a/+armour/+legacy/match_deg5_bernstein_coefficients.m
+++ b/+armour/+legacy/match_deg5_bernstein_coefficients.m
@@ -21,10 +21,10 @@
beta{6} = q1; % beta_5;
% velocity constraints:
- beta{2} = q0 + (T*qd0)/5; % beta_1
- beta{5} = q1 - (T*qd1)/5; % beta_4
+ beta{2} = q0 + (T*qd0) * (1/5); % beta_1
+ beta{5} = q1 - (T*qd1) * (1/5); % beta_4
% acceleration constraints
- beta{3} = (qdd0*T^2)/20 + (2*qd0*T)/5 + q0; % beta_2
- beta{4} = (qdd1*T^2)/20 - (2*qd1*T)/5 + q1; % beta_3
+ beta{3} = (qdd0*T^2)*(1/20) + (2*qd0*T)*(1/5) + q0; % beta_2
+ beta{4} = (qdd1*T^2)*(1/20) - (2*qd1*T)*(1/5) + q1; % beta_3
end
diff --git a/+armour/+legacy/readme.md b/+armour/+legacy/readme.md
deleted file mode 100644
index 539fe361..00000000
--- a/+armour/+legacy/readme.md
+++ /dev/null
@@ -1 +0,0 @@
-In this package is code carried over from armour-dev.
\ No newline at end of file
diff --git a/+armour/+pz_roahm/@matPolyZonotope_ROAHM/matPolyZonotope_ROAHM.m b/+armour/+pz_roahm/@matPolyZonotope_ROAHM/matPolyZonotope_ROAHM.m
index af53cd39..9de30084 100644
--- a/+armour/+pz_roahm/@matPolyZonotope_ROAHM/matPolyZonotope_ROAHM.m
+++ b/+armour/+pz_roahm/@matPolyZonotope_ROAHM/matPolyZonotope_ROAHM.m
@@ -94,6 +94,7 @@
% extra functions
pZ = reduce(pZ,option,order,varargin);
pZ = polyZonotope_ROAHM(mpz);
+ pZ = tranpose(pZ);
end
diff --git a/+armour/+pz_roahm/@matPolyZonotope_ROAHM/transpose.m b/+armour/+pz_roahm/@matPolyZonotope_ROAHM/transpose.m
new file mode 100644
index 00000000..b756e804
--- /dev/null
+++ b/+armour/+pz_roahm/@matPolyZonotope_ROAHM/transpose.m
@@ -0,0 +1,11 @@
+function pZ_out = transpose(pZ)
+import armour.pz_roahm.*
+% Lazy matrix transpose operation so we don't have to keep saving the
+% _t versions of everything
+
+C_t = pZ.C.';
+G_t = permute(pZ.G, [2, 1, 3]);
+Grest_t = permute(pZ.Grest, [2, 1, 3]);
+pZ_out = matPolyZonotope_ROAHM(C_t, G_t, Grest_t, pZ.expMat, pZ.id);
+
+end
\ No newline at end of file
diff --git a/+armour/+pz_roahm/get_pz_rotations_from_q.m b/+armour/+pz_roahm/get_pz_rotations_from_q.m
index 5b199e45..867d257e 100644
--- a/+armour/+pz_roahm/get_pz_rotations_from_q.m
+++ b/+armour/+pz_roahm/get_pz_rotations_from_q.m
@@ -22,38 +22,40 @@
cq = cos_sin_q.c(1);
sq = cos_sin_q.c(2);
C = eye(3) + sq*U + (1-cq)*U^2;
- C_t = C';
+% C_t = C';
Grest = [];
- Grest_t = [];
+% Grest_t = [];
for j = 1:size(cos_sin_q.Grest, 2)
cq = cos_sin_q.Grest(1, j);
sq = cos_sin_q.Grest(2, j);
Grest_tmp = sq*U + -cq*U^2;
Grest = cat(3, Grest, Grest_tmp);
- Grest_t = cat(3, Grest_t, Grest_tmp');
+% Grest_t = cat(3, Grest_t, Grest_tmp');
end
G = [];
- G_t = [];
+% G_t = [];
for j = 1:size(cos_sin_q.G, 2)
cq = cos_sin_q.G(1, j);
sq = cos_sin_q.G(2, j);
G_tmp = sq*U + -cq*U^2;
G = cat(3, G, G_tmp);
- G_t = cat(3, G_t, G_tmp');
+% G_t = cat(3, G_t, G_tmp');
end
if isempty(G) && isempty(Grest)
R = matPolyZonotope_ROAHM(C);
- R_t = matPolyZonotope_ROAHM(C_t);
+% R_t = matPolyZonotope_ROAHM(C_t);
elseif isempty(G) && ~isempty(Grest)
R = matPolyZonotope_ROAHM(C, G, Grest);
- R_t = matPolyZonotope_ROAHM(C_t, G_t, Grest_t);
+% R_t = matPolyZonotope_ROAHM(C_t, G_t, Grest_t);
elseif ~isempty(G)
R = matPolyZonotope_ROAHM(C, G, Grest, cos_sin_q.expMat, cos_sin_q.id);
- R_t = matPolyZonotope_ROAHM(C_t, G_t, Grest_t, cos_sin_q.expMat, cos_sin_q.id);
+% R_t = matPolyZonotope_ROAHM(C_t, G_t, Grest_t, cos_sin_q.expMat, cos_sin_q.id);
end
+ R_t = R.';
+
end
\ No newline at end of file
diff --git a/+armour/+reachsets/+JRS/BernsteinJRSGen.m b/+armour/+reachsets/+JRS/BernsteinJRSGen.m
new file mode 100644
index 00000000..974845a5
--- /dev/null
+++ b/+armour/+reachsets/+JRS/BernsteinJRSGen.m
@@ -0,0 +1,149 @@
+classdef BernsteinJRSGen < armour.reachsets.JRS.OnlineGeneratorBase
+% Online generator for Bernstein polynomial trajectories
+%
+% This class is used to generate reachable sets for a robot using a
+% Bernstein polynomial trajectory. This is built on top of the
+% OnlineGeneratorBase class.
+%
+% The bernstein polynomial is parameterised by the relative end position
+% of the entire trajectory. It's scaled to the time horizon of the
+% reachable set.
+%
+% This incorporates methods previously found in create_jrs_online.m written
+% by Patrick Holmes for ARMOUR. This class is an extension of the
+% functionality provided by that script.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-22
+% Updated: 2023-10-04 (Adam Li)
+%
+% See also: armour.reachsets.JRS.OnlineGeneratorBase,
+% armour.reachsets.JRS.JRSInstance
+%
+% --- More Info ---
+%
+
+ methods (Access=protected)
+ function reachableSet = generateReachableSet(self, robotState)
+ % Generate the joint reachable set
+ %
+ % Arguments:
+ % robotState (rtd.entity.states.ArmRobotStateInstance): The
+ % robot state to generate the reachable set for.
+ %
+ % Returns:
+ % reachableSet (struct): A struct array containing the
+ % reachable set for the robot state provided, where
+ % reachableSet.rs is the reachable set and
+ % reachableSet.id is the id of the reachable set.
+ %
+ arguments
+ self
+ robotState(1,1) rtd.entity.states.ArmRobotStateInstance
+ end
+
+ self.vdisp("Generating joint reachable set!", "INFO")
+
+ % Get the range that the parameters should cover
+ param_range = [self.param_center - self.param_extents, self.param_center + self.param_extents];
+
+ % Create the reference trajectory
+ self.vdisp("Creating Reference Trajectory!", "DEBUG")
+ [q_ref, qd_ref, qdd_ref] = genBernsteinTrajectory( ...
+ robotState.position, ...
+ robotState.velocity, ...
+ robotState.acceleration, ...
+ self.tfinal, ...
+ self.param_zonos, ...
+ param_range, ...
+ self.time_zonos);
+
+ % Create the JRS instance from this trajectory
+ jrs = self.createInstanceFromReference(q_ref, qd_ref, qdd_ref);
+ jrs.setParamRange(param_range);
+ jrs.setTrajName('bernstein');
+
+ % Outputs
+ reachableSet.rs = jrs;
+ reachableSet.id = 1;
+ end
+ end
+end
+
+function [q_ref, qd_ref, qdd_ref] = genBernsteinTrajectory(q0, qd0, qdd0, tfinal, base_params, param_range, time_zonos)
+% Generate a bernstein polynomial trajectory
+%
+% This function generates a bernstein polynomial trajectory from the
+% provided initial and final states, and the time horizon. The trajectory
+% is parameterised by the relative end position of the entire trajectory.
+% It's scaled to the time horizon of the reachable set.
+%
+% Arguments:
+% q0 (double): The initial joint position of the trajectory.
+% qd0 (double): The initial joint velocity of the trajectory.
+% qdd0 (double): The initial joint acceleration of the trajectory.
+% tfinal (double): The time horizon of the trajectory.
+% base_params (cell): The base parameters of the trajectory.
+% param_range (double): The range of the parameters.
+% time_zonos (cell): The time zonotopes of the trajectory.
+%
+% Returns:
+% q_ref (cell): The reference position trajectory as n_t x n_q.
+% qd_ref (cell): The reference velocity trajectory as n_t x n_q.
+% qdd_ref (cell): The reference acceleration trajectory as n_t x n_q.
+%
+ arguments
+ q0(:,1) double
+ qd0(:,1) double
+ qdd0(:,1) double
+ tfinal(1,1) double
+ base_params(1,:) cell
+ param_range(:,2) double
+ time_zonos(1,:) cell
+ end
+
+ % Get the bernstein polynomial
+ num_q = length(q0);
+ for i = num_q:-1:1
+ q_end{i} = q0(i) + 0.5 * (base_params{i} + 1) * (param_range(i,2) - param_range(i,1)) + param_range(i,1);
+ beta = armour.legacy.match_deg5_bernstein_coefficients({q0(i); qd0(i); qdd0(i); q_end{i}; 0; 0}, tfinal);
+ alpha{i} = armour.legacy.bernstein_to_poly(beta, 5);
+ end
+
+ scale = 1.0 / tfinal;
+
+ % Create the trajectory
+ num_t = length(time_zonos);
+ q_ref = num2cell(zeros(num_t, num_q));
+ qd_ref = q_ref;
+ qdd_ref = q_ref;
+ for idx_t=1:num_t
+ if time_zonos{idx_t}.c <= tfinal
+ % Extract time
+ t = time_zonos{idx_t} * scale;
+ % Precompute powers of time
+ t_pow = cell(1, 6);
+ t_pow{1} = 1;
+ for i=2:6
+ t_pow{i} = t_pow{i-1}.*t;
+ end
+ % Compute states for step
+ for i=num_q:-1:1
+ for k=0:5
+ q_ref{idx_t, i} = q_ref{idx_t, i} + alpha{i}{k+1}.*t_pow{k+1};
+ if k > 0
+ qd_ref{idx_t, i} = qd_ref{idx_t, i} + k*alpha{i}{k+1}.*t_pow{k};
+ end
+ if k > 1
+ qdd_ref{idx_t, i} = qdd_ref{idx_t, i} + k*(k-1)*alpha{i}{k+1}.*t_pow{k-1};
+ end
+ end
+ qd_ref{idx_t, i} = qd_ref{idx_t, i} * scale;
+ qdd_ref{idx_t, i} = qdd_ref{idx_t, i} * scale^2;
+ end
+ else
+ q_ref{idx_t, i} = q_end{i};
+ end
+ end
+end
\ No newline at end of file
diff --git a/+armour/+reachsets/+JRS/JRSInstance.m b/+armour/+reachsets/+JRS/JRSInstance.m
new file mode 100644
index 00000000..d72472af
--- /dev/null
+++ b/+armour/+reachsets/+JRS/JRSInstance.m
@@ -0,0 +1,102 @@
+classdef JRSInstance < rtd.planner.reachsets.ReachSetInstance
+% Individual instance of a Joint Reachable Set (JRS)
+%
+% This class is a basic container holding the information for a single
+% instance of a JRS. It is meant to be used in conjunction with the
+% other generators present in this package.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2022-10-01
+% Updated: 2023-09-22 (Adam Li)
+%
+% See also: armour.reachsets.JRS.OnlineGeneratorBase,
+% rtd.planner.reachsets.ReachSetInstance
+%
+% --- Revision History ---
+% 2023-09-22: jrs_info field was removed parameter scaling was moved to a seperate function
+%
+% --- More Info ---
+%
+
+ properties
+ % The range of the optimization values
+ input_range = [-1.0, 1.0]
+ % The range that would be mapped to for use based on this reachset
+ output_range = [-1.0, 1.0]
+ % The number of parameters that are being optimized from this reachset
+ num_parameters = 0
+
+ % Reference position
+ q_des
+ % Reference velocity
+ dq_des
+ % Reference acceleration
+ ddq_des
+ % Reachability for the position
+ q
+ % Reachability for the velocity
+ dq
+ % Reachability for the auxilliary velocity
+ dq_a
+ % Reachability for the auxilliary acceleration
+ ddq_a
+ % Rotatotope corresponding to q_des
+ R_des
+ % Transpose of the rotatotope corresponding to q_des
+ R_t_des
+ % Rotatotope corresponding to q
+ R
+ % Transpose of the rotatotope corresponding to q
+ R_t
+
+ % Number of joints
+ n_q
+ % Number of time steps
+ n_t
+ % Dimension id's corresponding to the parameters
+ k_id
+ % Name of the trajectory used to generate this reachable set
+ traj_name
+ end
+
+ methods
+ function setParamRange(self, param_range)
+ % Sets the range of the parameters that are being optimized
+ % for this reachset
+ %
+ % Parameters:
+ % param_range: The output range of the parameters used
+ %
+ arguments
+ self(1,1) armour.reachsets.JRS.JRSInstance
+ param_range (:,2) double
+ end
+ self.input_range = ones(self.num_parameters, 1) .* [-1.0, 1.0];
+ self.output_range = ones(self.num_parameters, 1) .* param_range;
+ end
+
+ function setTrajName(self, traj_name)
+ % Sets the name of the trajectory used to generate this
+ % reachable set
+ %
+ % Parameters:
+ % traj_name: The name of the trajectory used
+ %
+ arguments
+ self(1,1) armour.reachsets.JRS.JRSInstance
+ traj_name {mustBeTextScalar}
+ end
+ self.traj_name = char(traj_name);
+ end
+
+ % Handles the obstacle-frs pair or similar to generate the
+ % nlconstraint.
+ % Returns a function handle for the nlconstraint generated
+ % where the function's return type is [c, ceq, gc, gceq]
+ function nlconFunction = genNLConstraint(self, worldState)
+ % This is no-op, so return empty set.
+ nlconFunction = [];
+ end
+ end
+end
\ No newline at end of file
diff --git a/+armour/+reachsets/+JRS/OnlineGeneratorBase.m b/+armour/+reachsets/+JRS/OnlineGeneratorBase.m
new file mode 100644
index 00000000..220ded3e
--- /dev/null
+++ b/+armour/+reachsets/+JRS/OnlineGeneratorBase.m
@@ -0,0 +1,378 @@
+classdef OnlineGeneratorBase < rtd.planner.reachsets.ReachSetGenerator
+% Base class for online generation of joint reachable sets used in ARMOUR.
+%
+% This class is used to generate joint reachable sets for ARMOUR. The
+% reachable sets are generated online and are based on some reference
+% trajectory. The reference trajectory is provided as a cell array of
+% polynomial zonotops. This cell array is of size n_t x n_q where n_t is
+% the number of time steps and n_q is the number of joints. Each cell
+% contains a polynomial zonotope for the desired joint position, velocity,
+% or acceleration respectively.
+%
+% The reachable sets are generated by taking the reference trajectory and
+% adding a zonotope for the tracking error (if provided).
+%
+% See BernsteinJRSGen for an example where the number of parameters is
+% equal to the number of joints, and TwoBernsteinJRSGen for an example
+% where the number of parameters is not equal to the number of joints.
+%
+% This incorporates methods previously found in create_jrs_online.m written
+% by Patrick Holmes.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2022-10-01
+% Updated: 2023-10-04 (Adam Li)
+%
+% See also: armour.reachsets.JRS.JRSInstance, armour.reachsets.JRS.BernsteinJRSGen,
+% armour.reachsets.JRS.TwoBernsteinJRSGen, armour.reachsets.JRS.PiecewiseJRSGen,
+% rtd.planner.reachsets.ReachSetGenerator
+%
+% --- Revision History ---
+% 2023-10-04 - Renamed from JRSGenerator. create_jrs_online is now merged into this.
+%
+% --- More Info ---
+%
+
+ % Required Abstract Properties
+ properties
+ % This is the maximum number of reachable sets that can be stored
+ % in the cache. If the cache is full, the oldest reachable set will
+ % be removed.
+ cache_max_size = 1
+ end
+
+ % Additional Properties
+ properties (GetAccess=public, SetAccess=protected)
+ % The number of time steps in the reference trajectory
+ num_t(1,1) double {mustBeInteger}
+ % The time duration the reference trajectory should be considered active in
+ tplan(1,1) double
+ % The final time of the reference trajectory and when the braking action should be complete
+ tfinal(1,1) double
+ % The time discretization of the reference trajectory
+ tdiscretization(1,1) double
+ % Whether to use unique id's for each time step
+ use_unique_tid(1,1) logical
+ % The time zonotopes (generated in makeBaseZonos)
+ time_zonos(1,:) cell
+
+ % The number of joints in the robot
+ num_joints(1,1) double {mustBeInteger}
+ % A map of id's to their respective dimensions
+ id_map(1,1) struct
+
+ % The number of parameters in the trajectory parameterization
+ num_params(1,1) double {mustBeInteger}
+ % The parameter zonotopes (generated in makeBaseZonos)
+ % These are always zero centered and with a radius of 1
+ param_zonos(1,:) cell
+ % The center of the parameter value
+ param_center(:,1) double
+ % The range of the parameter value
+ param_extents(:,1) double
+ % The id's/dimensions of the parameters
+ param_ids(1,:) double {mustBeInteger}
+
+ % Whether to add the ultimate bound to the reachable sets
+ add_ultimate_bound(1,1) logical
+ % The ultimate bound of the controller
+ ultimate_bound(:,1) double
+ % The K_r gain of the controller
+ k_r(:,1) double
+ % The tracking error zonotopes (generated in makeBaseZonos)
+ error_pos_zonos(1,:) cell
+ % The tracking error zonotopes (generated in makeBaseZonos)
+ error_vel_zonos(1,:) cell
+
+ % The degree of the taylor approximation used to generate the
+ % final rotatotopes
+ taylor_degree(1,1) double {mustBeInteger}
+ % The axes of the joints
+ joint_axes(3,:) double
+ end
+
+ % Public Methods
+ methods
+ function self = OnlineGeneratorBase(robot, options)
+ % Constructor for the OnlineGeneratorBase class.
+ %
+ % Arguments:
+ % robot (armour.Agent): The robot to generate reachable sets for
+ % options: Keyword Arguments. See below.
+ %
+ % Keyword Arguments:
+ % taylor_degree (int): The degree of the taylor approximation
+ % used to generate the final rotatotopes. Default: 1
+ % add_ultimate_bound (bool): Whether to add the ultimate
+ % bound to the reachable sets. Default: true
+ % verboseLevel (rtd.util.types.LogLevel): The verbose level
+ % for logging. Default: DEBUG
+ % param_center (double): The center of the parameter value.
+ % Default: 0
+ % param_range (double): The range of the parameter value.
+ % Default: pi/36
+ % tplan (double): The time duration the reference trajectory
+ % should be considered active in. Default: 0.5
+ % tfinal (double): The final time of the reference trajectory
+ % and when the braking action should be complete.
+ % Default: 1
+ % use_unique_tid (bool): Whether to use unique id's for each
+ % time step. Default: false
+ % tdiscretization (double): The time discretization of the
+ % reference trajectory. Default: 0.01
+ %
+ arguments
+ robot armour.ArmourAgent
+ options.taylor_degree(1,1) double {mustBeInteger} = 1
+ options.add_ultimate_bound(1,1) logical = true
+ options.verboseLevel(1,1) rtd.util.types.LogLevel = "DEBUG"
+ options.param_center(:,1) double = 0
+ options.param_range(:,1) double = pi/36
+ options.tplan(1,1) double = 0.5
+ options.tfinal(1,1) double = 1
+ options.use_unique_tid(1,1) logical = false
+ options.tdiscretization(1,1) double = 0.01
+ end
+
+ % Set the verbose level for logging
+ self.set_vdisplevel(options.verboseLevel);
+
+ % Pull some general parameters
+ self.joint_axes = [robot.info.joints.axes];
+ self.taylor_degree = options.taylor_degree;
+ self.num_joints = robot.info.num_q;
+
+ % Save the optimization parameter range
+ self.param_center = options.param_center;
+ self.param_extents = options.param_range;
+
+ % Save the time parameters
+ self.tplan = options.tplan;
+ self.tfinal = options.tfinal;
+ self.tdiscretization = options.tdiscretization;
+ self.num_t = ceil(self.tfinal/self.tdiscretization);
+ self.use_unique_tid = options.use_unique_tid;
+
+ % Save the ultimate bound parameters (if relevant)
+ self.add_ultimate_bound = options.add_ultimate_bound;
+ self.ultimate_bound = [];
+ self.k_r = [];
+ if self.add_ultimate_bound
+ try
+ self.ultimate_bound = ones(self.num_joints, 1).*robot.controller.ultimate_bound(:);
+ self.k_r = ones(self.num_joints, 1).*robot.controller.Kr(:);
+ catch
+ self.vdisp("Ultimate bound not found! Not adding!", "WARN")
+ self.add_ultimate_bound = false;
+ self.ultimate_bound = [];
+ self.k_r = [];
+ end
+ end
+
+ % Make the base zonotopes
+ self.makeBaseZonos();
+ end
+ end
+
+ % Key class methods
+ methods (Access=protected)
+ function jrs_instance = createInstanceFromReference(self, q_ref, qd_ref, qdd_ref, extras)
+ % Create a JRSInstance from a reference trajectory.
+ %
+ % Arguments:
+ % q_ref (cell): The reference joint positions in an n_t x n_q
+ % cell array where n_t is the number of time steps and
+ % n_q is the number of joints.
+ % qd_ref (cell): The reference joint velocities in an n_t x
+ % n_q cell array.
+ % qdd_ref (cell): The reference joint accelerations in an n_t
+ % x n_q cell array.
+ % extras: Keyword Arguments. See below.
+ %
+ % Keyword Arguments:
+ % independent_nonparam_gens (bool): Whether to make the
+ % generators that don't correspond to optimization parameters
+ % independent. Default: true
+ %
+ % Returns:
+ % jrs_instance (armour.reachsets.JRS.JRSInstance): The
+ % generated JRSInstance
+ %
+ arguments
+ self(1,1) armour.reachsets.JRS.OnlineGeneratorBase
+ q_ref cell
+ qd_ref cell
+ qdd_ref cell
+ extras.independent_nonparam_gens(1,1) logical = true
+ end
+
+ % Generate the reference rotatotopes
+ self.vdisp("Creating Reference Rotations!", "DEBUG")
+ for t_idx=self.num_t:-1:1
+ for i=self.num_joints:-1:1
+ [R_ref{t_idx, i}, ~] = armour.pz_roahm.get_pz_rotations_from_q(q_ref{t_idx, i}, self.joint_axes(:,i), self.taylor_degree);
+ end
+ end
+
+ % Add tracking error if provided
+ if ~(isempty(self.error_pos_zonos) || isempty(self.error_vel_zonos))
+ self.vdisp("Adding Tracking Error!", "DEBUG")
+ for t_idx=self.num_t:-1:1
+ for i=self.num_joints:-1:1
+ q{t_idx, i} = q_ref{t_idx, i} + self.error_pos_zonos{i};
+ qd{t_idx, i} = qd_ref{t_idx, i} + self.error_vel_zonos{i};
+ qd_aux{t_idx, i} = qd_ref{t_idx, i} + self.k_r(i)*self.error_pos_zonos{i};
+ qdd_aux{t_idx, i} = qdd_ref{t_idx, i} + self.k_r(i)*self.error_vel_zonos{i};
+ [R{t_idx, i}, ~] = armour.pz_roahm.get_pz_rotations_from_q(q{t_idx, i}, self.joint_axes(:,i), self.taylor_degree);
+ end
+ end
+ end
+
+ % Make independence if requested
+ if extras.independent_nonparam_gens
+ self.vdisp("Making extra generators independent!", "DEBUG")
+ for t_idx=self.num_t:-1:1
+ for i=self.num_joints:-1:1
+ q_ref{t_idx, i} = remove_dependence_and_compress(q_ref{t_idx, i}, self.param_ids);
+ qd_ref{t_idx, i} = remove_dependence_and_compress(qd_ref{t_idx, i}, self.param_ids);
+ qdd_ref{t_idx, i} = remove_dependence_and_compress(qdd_ref{t_idx, i}, self.param_ids);
+ R_ref{t_idx, i} = remove_dependence_and_compress_mat(R_ref{t_idx, i}, self.param_ids);
+ R_t_ref{t_idx, i} = R_ref{t_idx, i}.';
+ end
+ end
+ if ~(isempty(self.error_pos_zonos) || isempty(self.error_vel_zonos))
+ for t_idx=self.num_t:-1:1
+ for i=self.num_joints:-1:1
+ q{t_idx, i} = remove_dependence_and_compress(q{t_idx, i}, self.param_ids);
+ qd{t_idx, i} = remove_dependence_and_compress(qd{t_idx, i}, self.param_ids);
+ qd_aux{t_idx, i} = remove_dependence_and_compress(qd_aux{t_idx, i}, self.param_ids);
+ qdd_aux{t_idx, i} = remove_dependence_and_compress(qdd_aux{t_idx, i}, self.param_ids);
+ R{t_idx, i} = remove_dependence_and_compress_mat(R{t_idx, i}, self.param_ids);
+ R_t{t_idx, i} = R{t_idx, i}.';
+ end
+ end
+ end
+ else
+ self.vdisp('Treating time and tracking error as dependent generators. Make sure to remove them before creating constraints.', 'INFO')
+ end
+
+ % Filler if we don't have error
+ if (isempty(self.error_pos_zonos) || isempty(self.error_vel_zonos))
+ q = q_ref;
+ qd = qd_ref;
+ qd_aux = qd_ref;
+ qdd_aux = qdd_ref;
+ R = R_ref;
+ end
+
+ % Save all the values
+ jrs_instance = armour.reachsets.JRS.JRSInstance;
+ % Split into column cells of column cells for compat
+ % Rest of the code expects the JRS in this form.
+ jrs_instance.q_des = num2cell(q_ref.', 1).';
+ jrs_instance.dq_des = num2cell(qd_ref.', 1).';
+ jrs_instance.ddq_des = num2cell(qdd_ref.', 1).';
+ jrs_instance.q = num2cell(q.', 1).';
+ jrs_instance.dq = num2cell(qd.', 1).';
+ jrs_instance.dq_a = num2cell(qd_aux.', 1).';
+ jrs_instance.ddq_a = num2cell(qdd_aux.', 1).';
+ jrs_instance.R_des = num2cell(R_ref.', 1).';
+ jrs_instance.R_t_des = num2cell(R_t_ref.', 1).';
+ jrs_instance.R = num2cell(R.', 1).';
+ jrs_instance.R_t = num2cell(R_t.', 1).';
+
+ jrs_instance.n_q = self.num_joints;
+ jrs_instance.num_parameters = self.num_params;
+ jrs_instance.n_t = self.num_t;
+ jrs_instance.k_id = self.param_ids;
+ jrs_instance.setParamRange([self.param_center - self.param_extents, self.param_center + self.param_extents]);
+ end
+
+ function makeBaseZonos(self, options)
+ % Make the base zonotopes based on the number of params given.
+ %
+ % Arguments:
+ % options: Keyword Arguments. See below.
+ %
+ % Keyword Arguments:
+ % num_params (int): The number of parameters. Default: self.num_joints
+ %
+ arguments
+ self(1,1) armour.reachsets.JRS.OnlineGeneratorBase
+ options.num_params(1,1) uint32 = self.num_joints
+ end
+
+ % Ensure the size of the param_center and param_extents match
+ self.num_params = options.num_params;
+ self.param_center = ones(options.num_params, 1).*self.param_center;
+ self.param_extents = ones(options.num_params, 1).*self.param_extents;
+
+ % Record of what id's are associated to what values
+ self.id_map = struct;
+
+ % Create base PZ's for parameters
+ self.param_zonos = cell(1, self.num_params);
+ self.param_ids = 1:self.num_params;
+ for i = 1:self.num_params
+ % Make the base pz
+ self.param_zonos{i} = armour.pz_roahm.polyZonotope_ROAHM(0, 1, [], 1, self.param_ids(i));
+ % Add its id to the map
+ self.id_map.(sprintf('k%d', i)) = self.param_ids(i);
+ end
+
+ % Create the base PZ's for time
+ self.time_zonos = cell(1, self.num_t);
+ if self.use_unique_tid
+ t_id = length(fieldnames(self.id_map)) + 1:self.num_t;
+ else
+ t_id = length(fieldnames(self.id_map)) + 1;
+ self.id_map.('t') = t_id;
+ end
+ for i = 1:self.num_t
+ % Make the base pz
+ self.time_zonos{i} = armour.pz_roahm.polyZonotope_ROAHM(self.tdiscretization*(i-1)+self.tdiscretization/2, self.tdiscretization/2, [], 1, t_id);
+ if self.use_unique_tid
+ self.id_map.(sprintf('t%d', i)) = t_id(i);
+ end
+ end
+
+ % Create PZ's for error
+ if ~(isempty(self.ultimate_bound) && isempty(self.k_r))
+ self.error_pos_zonos = cell(1,self.num_joints);
+ self.error_vel_zonos = cell(1,self.num_joints);
+ for i = 1:self.num_joints
+ e_id = length(fieldnames(self.id_map)) + 1;
+ self.error_pos_zonos{i} = armour.pz_roahm.polyZonotope_ROAHM(0, self.ultimate_bound(i)/self.k_r(i), [], 1, e_id);
+ self.id_map.(sprintf('e_pos%d', i)) = e_id;
+ self.error_vel_zonos{i} = armour.pz_roahm.polyZonotope_ROAHM(0, 2*self.ultimate_bound(i), [], 1, e_id+1);
+ self.id_map.(sprintf('e_vel%d', i)) = e_id+1;
+ end
+ end
+ end
+ end
+end
+
+% Make the specified id's the only dependent generators and turn the rest
+% to independents
+function B = remove_dependence_and_compress(A, k_id)
+import armour.pz_roahm.*
+ k_id_idx = any(A.id == k_id(:).', 2);
+ k_slc_idx = (any(A.expMat(k_id_idx, :) ~= 0, 1) & all(A.expMat(~k_id_idx, :) == 0, 1)); % should only be one!
+ if length(find(k_slc_idx)) > sum(k_id_idx)
+ error('There should only be one fully-k-sliceable generator');
+ end
+ B = polyZonotope_ROAHM(A.c, A.G(k_slc_idx), sum(abs(A.G(~k_slc_idx))) + sum(abs(A.Grest)), A.expMat(k_id_idx, k_slc_idx), A.id(k_id_idx));
+ % B = polyZonotope_ROAHM(A.c, A.G(k_slc_idx), [A.G(~k_slc_idx), A.Grest], A.expMat(k_id_idx, k_slc_idx), k_id);
+end
+
+% Same as above but for matPolyZonotopes
+function B = remove_dependence_and_compress_mat(A, k_id)
+import armour.pz_roahm.*
+ k_id_idx = any(A.id == k_id(:).', 2);
+ k_slc_idx = (any(A.expMat(k_id_idx, :) ~= 0, 1) & all(A.expMat(~k_id_idx, :) == 0, 1)); % should only be one!
+% if length(find(k_slc_idx)) > 1
+% error('There should only be one fully-k-sliceable generator');
+% end
+ B = matPolyZonotope_ROAHM(A.C, A.G(:, :, k_slc_idx), cat(3, A.G(:, :, ~k_slc_idx), A.Grest), A.expMat(k_id_idx, k_slc_idx), A.id(k_id_idx));
+end
\ No newline at end of file
diff --git a/+armour/+reachsets/+JRS/PiecewiseJRSGen.m b/+armour/+reachsets/+JRS/PiecewiseJRSGen.m
new file mode 100644
index 00000000..8a9714f5
--- /dev/null
+++ b/+armour/+reachsets/+JRS/PiecewiseJRSGen.m
@@ -0,0 +1,157 @@
+classdef PiecewiseJRSGen < armour.reachsets.JRS.OnlineGeneratorBase
+% Online reachable set generator for the piecewise trajectory
+%
+% This class generates the reachable set for a piecewise trajectory. The
+% trajectory is generated by taking the current state of the robot and
+% planning a trajectory to a goal state. The trajectory is then split into
+% two parts, the first part is the acceleration phase and the second part
+% is the braking phase. The reachable set is then generated for the
+% trajectory.
+%
+% This is built on top of the OnlineGeneratorBase class.
+%
+% This incorporates methods previously found in create_jrs_online.m written
+% by Patrick Holmes for ARMOUR. This class is a re-write of that code and
+% and extension of the functionality provided by it.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-22
+% Updated: 2023-10-04 (Adam Li)
+%
+% See also: armour.reachsets.JRS.OnlineGeneratorBase,
+% armour.reachsets.JRS.JRSInstance
+%
+% --- More Info ---
+%
+
+ methods (Access=protected)
+ function reachableSet = generateReachableSet(self, robotState)
+ % Generate the joint reachable set
+ %
+ % Arguments:
+ % robotState (rtd.entity.states.ArmRobotStateInstance): The
+ % robot state to generate the reachable set for.
+ %
+ % Returns:
+ % reachableSet (struct): A struct array containing the
+ % reachable set for the robot state provided, where
+ % reachableSet.rs is the reachable set and
+ % reachableSet.id is the id of the reachable set.
+ %
+ arguments
+ self
+ robotState(1,1) rtd.entity.states.ArmRobotStateInstance
+ end
+
+ self.vdisp("Generating joint reachable set!", "INFO")
+
+ % Get the range that the parameters should cover
+ % Overwriting the parameter range for the piecewise trajectory
+ param_range = min(max(pi/24, abs(robotState.velocity/3)), pi/3);
+ param_range = [self.param_center - param_range, self.param_center + param_range];
+
+ % Create the reference trajectory
+ self.vdisp("Creating Reference Trajectory!", "DEBUG")
+ [q_ref, qd_ref, qdd_ref] = genPiecewiseTrajectory( ...
+ robotState.position, ...
+ robotState.velocity, ...
+ self.tplan, ...
+ self.tfinal, ...
+ self.param_zonos, ...
+ param_range, ...
+ self.time_zonos);
+
+ % Create the JRS instance from this trajectory
+ jrs = self.createInstanceFromReference(q_ref, qd_ref, qdd_ref);
+ jrs.setParamRange(param_range);
+ jrs.setTrajName('piecewise');
+
+ % Outputs
+ reachableSet.rs = jrs;
+ reachableSet.id = 1;
+ end
+ end
+end
+
+function [q_ref, qd_ref, qdd_ref] = genPiecewiseTrajectory(q0, qd0, tplan, tfinal, base_params, param_range, time_zonos)
+% Generate a piecewise trajectory
+%
+% This function generates a piecewise trajectory. The trajectory is
+% generated by taking the current state of the robot and planning a
+% trajectory to a goal state. The trajectory is then split into two parts,
+% the first part is the acceleration phase and the second part is the
+% braking phase.
+%
+% Arguments:
+% q0 (double): The initial joint positions.
+% qd0 (double): The initial joint velocities.
+% tplan (double): The time to accelerate for.
+% tfinal (double): The final time of the trajectory and when it should be stopped.
+% base_params (cell): The base parameters for the trajectory.
+% param_range (double): The range that the parameters should cover.
+% time_zonos (cell): The time zonotopes to generate the trajectory for.
+%
+% Returns:
+% q_ref (cell): The reference position trajectory as n_t x n_q.
+% qd_ref (cell): The reference velocity trajectory as n_t x n_q.
+% qdd_ref (cell): The reference acceleration trajectory as n_t x n_q.
+%
+ arguments
+ q0(:,1) double
+ qd0(:,1) double
+ tplan(1,1) double
+ tfinal(1,1) double
+ base_params(1,:) cell
+ param_range(:,2) double
+ time_zonos(1,:) cell
+ end
+
+ num_q = length(q0);
+ num_t = length(time_zonos);
+
+ for i = num_q:-1:1
+ param{i} = 0.5 * (base_params{i} + 1) * (param_range(i,2) - param_range(i,1)) + param_range(i,1);
+ end
+
+ % Compute key constants
+ stopping_time = tfinal - tplan;
+
+ % Peak values (first part of the traj)
+ for i=num_q:-1:1
+ qpeak{i} = q0(i) + qd0(i)*tplan + 0.5*param{i}*(tplan^2);
+ qdpeak{i} = qd0(i) + param{i}*tplan;
+ end
+
+ % Braking values (second part of the traj)
+ for i=num_q:-1:1
+ stopping_qdd{i} = (-1) * qdpeak{i} * (1.0/stopping_time);
+ final_q{i} = qpeak{i} + qdpeak{i}*stopping_time + 0.5*stopping_qdd{i}*(stopping_time^2);
+ end
+
+ % Create the trajectory
+ q_ref = num2cell(zeros(num_t, num_q));
+ qd_ref = q_ref;
+ qdd_ref = q_ref;
+ for idx_t=1:num_t
+ t = time_zonos{idx_t};
+ if t.c <= tplan
+ for i=num_q:-1:1
+ q_ref{idx_t, i} = q0(i) + qd0(i).*t + 0.5*param{i}.*(t.*t);
+ qd_ref{idx_t, i} = qd0(i) + param{i}.*t;
+ qdd_ref{idx_t, i} = param{i};
+ end
+ elseif t.c <= tfinal
+ for i=num_q:-1:1
+ tshift = (t-tplan);
+ q_ref{idx_t, i} = qpeak{i} + qdpeak{i}.*tshift + 0.5*stopping_qdd{i}.*(tshift.*tshift);
+ qd_ref{idx_t, i} = qdpeak{i} + stopping_qdd{i}.*(tshift);
+ qdd_ref{idx_t, i} = stopping_qdd{i};
+ end
+ else
+ for i=num_q:-1:1
+ q_ref{idx_t, i} = final_q{i};
+ end
+ end
+ end
+end
diff --git a/+armour/+reachsets/+JRS/TwoBernsteinJRSGen.m b/+armour/+reachsets/+JRS/TwoBernsteinJRSGen.m
new file mode 100644
index 00000000..f81228a3
--- /dev/null
+++ b/+armour/+reachsets/+JRS/TwoBernsteinJRSGen.m
@@ -0,0 +1,210 @@
+classdef TwoBernsteinJRSGen < armour.reachsets.JRS.OnlineGeneratorBase
+% Generate for a trajectory composed of two 5th order bernstein polynomials
+%
+% This class generates a joint reachable set for a trajectory composed of
+% two 5th order bernstein polynomials. The first polynomial is used to
+% generate a trajectory from the current state to the start of the second
+% polynomial. The second polynomial is used to generate a trajectory from
+% the middle of the first polynomial to the end of the trajectory.
+%
+% The parameters of the first polynomial are the first half of the
+% parameters, and the parameters of the second polynomial are the second
+% half of the parameters.
+%
+% The first polynomial will be referred to as the "major" polynomial, and
+% the second polynomial will be referred to as the "minor" polynomial.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-22
+% Updated: 2023-10-04 (Adam Li)
+%
+% See also: armour.reachsets.JRS.OnlineGeneratorBase,
+% armour.reachsets.JRS.JRSInstance
+%
+% --- More Info ---
+%
+
+ methods (Access=protected)
+ % Since the parameters are split into two parts, we need to
+ % override the base class to get the correct number of parameters
+ % as double the number of joints.
+ function makeBaseZonos(self)
+ makeBaseZonos@armour.reachsets.JRS.OnlineGeneratorBase(self, num_params=self.num_joints*2)
+ end
+
+ function reachableSet = generateReachableSet(self, robotState)
+ % Generate the joint reachable set
+ %
+ % Arguments:
+ % robotState (rtd.entity.states.ArmRobotStateInstance): The
+ % robot state to generate the reachable set for.
+ %
+ % Returns:
+ % reachableSet (struct): A struct array containing the
+ % reachable set for the robot state provided, where
+ % reachableSet.rs is the reachable set and
+ % reachableSet.id is the id of the reachable set.
+ %
+ arguments
+ self
+ robotState(1,1) rtd.entity.states.ArmRobotStateInstance
+ end
+
+ self.vdisp("Generating joint reachable set!", "INFO")
+
+ % Get the range that the parameters should cover
+ param_range = [self.param_center - self.param_extents, self.param_center + self.param_extents];
+
+ % Create the reference trajectory
+ self.vdisp("Creating Reference Trajectory!", "DEBUG")
+ [q_ref, qd_ref, qdd_ref] = genTwoBernsteinTrajectory( ...
+ robotState.position, ...
+ robotState.velocity, ...
+ robotState.acceleration, ...
+ self.tplan, ...
+ self.tfinal, ...
+ self.param_zonos, ...
+ param_range, ...
+ self.time_zonos);
+
+ % Create the JRS instance from this trajectory
+ jrs = self.createInstanceFromReference(q_ref, qd_ref, qdd_ref);
+ jrs.setParamRange(param_range);
+ jrs.setTrajName('bernstein');
+
+ % Outputs
+ reachableSet.rs = jrs;
+ reachableSet.id = 1;
+ end
+ end
+end
+
+function [q_ref, qd_ref, qdd_ref] = genTwoBernsteinTrajectory(q0, qd0, qdd0, tplan, tfinal, base_params, param_range, time_zonos)
+% Generate a trajectory composed of two 5th order bernstein polynomials
+%
+% This function generates a trajectory composed of two 5th order bernstein
+% polynomials. The first polynomial is used to generate a trajectory from
+% the current state to the start of the second polynomial, but is parameterized
+% by the expected end position by the overall time horizon. The second
+% polynomial is used to generate a trajectory from the middle of the first
+% polynomial to the end of the trajectory.
+%
+% The parameters of the first polynomial are the first half of the
+% parameters, and the parameters of the second polynomial are the second
+% half of the parameters.
+%
+% Arguments:
+% q0 (double): The initial joint position of the trajectory.
+% qd0 (double): The initial joint velocity of the trajectory.
+% qdd0 (double): The initial joint acceleration of the trajectory.
+% tplan (double): The time to plan for the trajectory and the start of
+% the second polynomial.
+% tfinal (double): The overall time horizon of the trajectory.
+% base_params (cell): The base parameters of the trajectory.
+% param_range (double): The range of the parameters.
+% time_zonos (cell): The time zonotopes of the trajectory.
+%
+% Returns:
+% q_ref (cell): The reference position trajectory as n_t x n_q.
+% qd_ref (cell): The reference velocity trajectory as n_t x n_q.
+% qdd_ref (cell): The reference acceleration trajectory as n_t x n_q.
+%
+ arguments
+ q0(:,1) double
+ qd0(:,1) double
+ qdd0(:,1) double
+ tplan(1,1) double
+ tfinal(1,1) double
+ base_params(1,:) cell
+ param_range(:,2) double
+ time_zonos(1,:) cell
+ end
+
+ scale1 = 1.0 / tfinal;
+ scale2 = 1.0 / (tfinal-tplan);
+
+ % Precompute powers of time for the start of the minor bernstein
+ t = tplan * scale1;
+ % powers of time
+ t_pow = cell(1, 6);
+ t_pow{1} = 1;
+ for i=2:6
+ t_pow{i} = t_pow{i-1}.*t;
+ end
+
+ % Get the bernstein polynomial parameters
+ num_q = length(q0);
+ for i = num_q:-1:1
+ q_end1{i} = q0(i) + 0.5 * (base_params{i} + 1) * (param_range(i,2) - param_range(i,1)) + param_range(i,1);
+ beta = armour.legacy.match_deg5_bernstein_coefficients({q0(i); qd0(i); qdd0(i); q_end1{i}; 0; 0}, tfinal);
+ alpha1{i} = armour.legacy.bernstein_to_poly(beta, 5);
+
+ % Compute where the second bernstein should start
+ q_plan = 0;
+ qd_plan = 0;
+ qdd_plan = 0;
+ for k=0:5
+ q_plan = q_plan + alpha1{i}{k+1}.*t_pow{k+1};
+ if k > 0
+ qd_plan = qd_plan + k*alpha1{i}{k+1}.*t_pow{k};
+ end
+ if k > 1
+ qdd_plan = qdd_plan + k*(k-1)*alpha1{i}{k+1}.*t_pow{k-1};
+ end
+ end
+ qd_plan = qd_plan * scale1;
+ qdd_plan = qdd_plan * scale1^2;
+
+ % Minor bernstein parameters (relative to the end point)
+ j = i+num_q;
+ q_end2{i} = q_plan + 0.5 * (base_params{j} + 1) * (param_range(j,2) - param_range(j,1)) + param_range(j,1);
+ beta = armour.legacy.match_deg5_bernstein_coefficients({q_plan; qd_plan; qdd_plan; q_end2{i}; 0; 0}, tfinal-tplan);
+ alpha2{i} = armour.legacy.bernstein_to_poly(beta, 5);
+ end
+
+ % Create the trajectory
+ num_t = length(time_zonos);
+ q_ref = num2cell(zeros(num_t, num_q));
+ qd_ref = q_ref;
+ qdd_ref = q_ref;
+ for idx_t=1:num_t
+ if time_zonos{idx_t}.c >= tfinal
+ q_ref{idx_t, i} = q_end2{i};
+ continue
+ end
+ % Select the right set of parameters to use
+ if time_zonos{idx_t}.c < tplan
+ scale = scale1;
+ alpha = alpha1;
+ % Extract time
+ t = time_zonos{idx_t} * scale;
+ else
+ scale = scale2;
+ alpha = alpha2;
+ % Extract time
+ t = (time_zonos{idx_t} - tplan) * scale;
+ end
+ % Compute
+ % Precompute powers of time
+ t_pow = cell(1, 6);
+ t_pow{1} = 1;
+ for i=2:6
+ t_pow{i} = t_pow{i-1}.*t;
+ end
+ % Compute states for step
+ for i=num_q:-1:1
+ for k=0:5
+ q_ref{idx_t, i} = q_ref{idx_t, i} + alpha{i}{k+1}.*t_pow{k+1};
+ if k > 0
+ qd_ref{idx_t, i} = qd_ref{idx_t, i} + k*alpha{i}{k+1}.*t_pow{k};
+ end
+ if k > 1
+ qdd_ref{idx_t, i} = qdd_ref{idx_t, i} + k*(k-1)*alpha{i}{k+1}.*t_pow{k-1};
+ end
+ end
+ qd_ref{idx_t, i} = qd_ref{idx_t, i} * scale;
+ qdd_ref{idx_t, i} = qdd_ref{idx_t, i} * scale^2;
+ end
+ end
+end
\ No newline at end of file
diff --git a/+armour/+reachsets/FOGenerator.m b/+armour/+reachsets/FOGenerator.m
index 5ca18cad..2e7c7d28 100644
--- a/+armour/+reachsets/FOGenerator.m
+++ b/+armour/+reachsets/FOGenerator.m
@@ -20,7 +20,7 @@
function self = FOGenerator(robot, jrsGenerator, options)
arguments
robot armour.ArmourAgent
- jrsGenerator armour.reachsets.JRSGenerator
+ jrsGenerator armour.reachsets.JRS.OnlineGeneratorBase
options.smooth_obs(1,1) logical = false
options.obs_frs_combs(1,1) struct = struct('maxcombs', 200, 'combs', [])
options.verboseLevel(1,1) rtd.util.types.LogLevel = "DEBUG"
@@ -47,6 +47,11 @@
% First get the JRS (allow the use of a cached value if it
% exists)
jrsInstance = self.jrsGenerator.getReachableSet(robotState, ignore_cache=false);
+ % For now, only support 1 problem
+ if length(jrsInstance) > 1
+ error('FOGenerator does not support multiple JRS yet!')
+ end
+ jrsInstance = jrsInstance.rs;
self.vdisp("Generating forward occupancy!", "INFO")
@@ -60,7 +65,10 @@
end
end
- reachableSet = armour.reachsets.FOInstance(self.robot.info, R_w, p_w, FO, jrsInstance, self.smooth_obs, self.obs_frs_combs);
+ rs = armour.reachsets.FOInstance(self.robot.info, R_w, p_w, FO, jrsInstance, self.smooth_obs, self.obs_frs_combs);
+ % Return the 1 RS that we have.
+ reachableSet.rs = rs;
+ reachableSet.id = 1;
end
end
end
diff --git a/+armour/+reachsets/FOInstance.m b/+armour/+reachsets/FOInstance.m
index 0d331d61..847683b8 100644
--- a/+armour/+reachsets/FOInstance.m
+++ b/+armour/+reachsets/FOInstance.m
@@ -26,7 +26,7 @@
self.FO = FO;
self.jrsInstance = jrsInstance;
self.smooth_obs = smooth_obs;
- self.num_parameters = jrsInstance.n_k;
+ self.num_parameters = jrsInstance.num_parameters;
self.input_range = jrsInstance.input_range;
% self.output_range = jrsInstance.output_range;
@@ -78,7 +78,7 @@
obs_constraint_pz_slice = @(k) slice(obs_constraint_pz, k);
% add gradients
- grad_obs_constraint_pz = grad(obs_constraint_pz, self.jrsInstance.n_q);
+ grad_obs_constraint_pz = grad(obs_constraint_pz, self.jrsInstance.num_parameters);
grad_obs_constraint_pz_slice = @(k) cellfun(@(C) slice(C, k), grad_obs_constraint_pz, 'UniformOutput', false);
% save
@@ -96,7 +96,7 @@
end
end
end
- % update n_k and parameter_range if smooth
+ % update num_parameters and parameter_range if smooth
if self.smooth_obs && ~isempty(smooth_obs_lambda_index)
self.num_parameters = self.jrsInstance.num_parameters;% + smooth_obs_lambda_index{end}(end);
%lambda_range = ones(5, 2) .* [0.0, 1.0];
@@ -107,8 +107,8 @@
if self.smooth_obs
nlconFunction = ...
@(k) eval_smooth_constraint( ...
- k(1:self.jrsInstance.n_k),...
- k(self.jrsInstance.n_k+1:end),...
+ k(1:self.jrsInstance.num_parameters),...
+ k(self.jrsInstance.num_parameters+1:end),...
smooth_obs_constraints_A,...
smooth_obs_constraints,...
smooth_obs_lambda_index);
@@ -124,8 +124,8 @@
[h_obs, max_idx] = max(c(k));
h_obs = -h_obs;
grad_eval = grad_c(k);
- grad_h_obs = zeros(self.jrsInstance.n_k, 1);
- for i = 1:self.jrsInstance.n_k
+ grad_h_obs = zeros(self.jrsInstance.num_parameters, 1);
+ for i = 1:self.jrsInstance.num_parameters
grad_h_obs(i, 1) = -grad_eval{i}(max_idx, :);
end
end
@@ -138,12 +138,12 @@
h_obs = -c(k)'*lambda;
% evaluate gradient w.r.t. k...
- % grad_c(k) gives n_k x 1 cell, each containing
+ % grad_c(k) gives num_parameters x 1 cell, each containing
% an N x 1 vector, where N is the number of rows of A.
% take the dot product of each cell with lambda
grad_eval = grad_c(k);
- grad_h_obs = zeros(self.jrsInstance.n_k, 1);
- for i = 1:self.jrsInstance.n_k
+ grad_h_obs = zeros(self.jrsInstance.num_parameters, 1);
+ for i = 1:self.jrsInstance.num_parameters
grad_h_obs(i, 1) = -grad_eval{i}'*lambda;
end
@@ -173,7 +173,7 @@
smooth_obs_constraints_A, smooth_obs_constraints, smooth_obs_lambda_index)
n_obs_c = length(smooth_obs_constraints);
- n_k = length(k);
+ num_parameters = length(k);
n_lambda = length(lambda);
% max_lambda_index = P.smooth_obs_lambda_index{end}(end);
@@ -184,7 +184,7 @@
% - n_lambda lambda \in {0, 1}
h = zeros(2*n_obs_c, 1);
- grad_h = zeros(n_k + n_lambda, 2*n_obs_c);
+ grad_h = zeros(num_parameters + n_lambda, 2*n_obs_c);
for i = 1:n_obs_c
lambda_idx = smooth_obs_lambda_index{i};
@@ -193,26 +193,26 @@
% obs avoidance constraints:
h(i, 1) = h_i;
- grad_h(1:n_k, i) = grad_h_i(1:n_k, 1);
- grad_h(n_k + lambda_idx, i) = grad_h_i(n_k+1:end, 1);
+ grad_h(1:num_parameters, i) = grad_h_i(1:num_parameters, 1);
+ grad_h(num_parameters + lambda_idx, i) = grad_h_i(num_parameters+1:end, 1);
% sum lambdas for this obstacle constraint >= 1
% sum lambdas for this obstacle constraint <= 1?
% h(n_obs_c + i, 1) = 1 - sum(lambda_i, 1);
-% grad_h(n_k + lambda_idx, n_obs_c + i) = -ones(length(lambda_i), 1);
+% grad_h(num_parameters + lambda_idx, n_obs_c + i) = -ones(length(lambda_i), 1);
% from Borrelli paper
h(n_obs_c + i, 1) = norm(smooth_obs_constraints_A{i}'*lambda_i, 2) - 1;
% implement gradient here!!!
A_bar = smooth_obs_constraints_A{i}*smooth_obs_constraints_A{i}';
- grad_h(n_k + lambda_idx, n_obs_c + i) = 0.5*(lambda_i'*A_bar*lambda_i)^(-0.5)*2*A_bar*lambda_i;
+ grad_h(num_parameters + lambda_idx, n_obs_c + i) = 0.5*(lambda_i'*A_bar*lambda_i)^(-0.5)*2*A_bar*lambda_i;
end
% lambda \in {0, 1} for each lambda
% heq = lambda.*(lambda - 1);
-% grad_heq = zeros(n_k + n_lambda, n_lambda);
-% grad_heq(n_k+1:end, :) = diag(2*lambda - 1);
+% grad_heq = zeros(num_parameters + n_lambda, n_lambda);
+% grad_heq(num_parameters+1:end, :) = diag(2*lambda - 1);
heq = [];
grad_heq = [];
diff --git a/+armour/+reachsets/IRSGenerator.m b/+armour/+reachsets/IRSGenerator.m
index 742a223b..915249b0 100644
--- a/+armour/+reachsets/IRSGenerator.m
+++ b/+armour/+reachsets/IRSGenerator.m
@@ -18,7 +18,7 @@
function self = IRSGenerator(robot, jrsGenerator, options)
arguments
robot armour.ArmourAgent
- jrsGenerator armour.reachsets.JRSGenerator
+ jrsGenerator armour.reachsets.JRS.OnlineGeneratorBase
options.use_robust_input(1,1) logical = true
options.verboseLevel(1,1) rtd.util.types.LogLevel = "DEBUG"
end
@@ -39,7 +39,12 @@
% First get the JRS (allow the use of a cached value if it
% exists)
jrsInstance = self.jrsGenerator.getReachableSet(robotState, ignore_cache=false);
-
+ % For now, only support 1 problem
+ if length(jrsInstance) > 1
+ error('FOGenerator does not support multiple JRS yet!')
+ end
+ jrsInstance = jrsInstance.rs;
+
self.vdisp("Generating input reachable set!", "INFO")
% set up zeros and overapproximation of r
@@ -145,7 +150,10 @@
end
% Save the generated reachable sets into the IRSInstance
- reachableSet = armour.reachsets.IRSInstance(u_ub, u_lb, jrsInstance, self.get_vdisplevel);
+ rs = armour.reachsets.IRSInstance(u_ub, u_lb, jrsInstance, self.get_vdisplevel);
+ % Return the 1 RS that we have.
+ reachableSet.rs = rs;
+ reachableSet.id = 1;
end
end
end
\ No newline at end of file
diff --git a/+armour/+reachsets/IRSInstance.m b/+armour/+reachsets/IRSInstance.m
index 0460adcd..b6a625f6 100644
--- a/+armour/+reachsets/IRSInstance.m
+++ b/+armour/+reachsets/IRSInstance.m
@@ -20,7 +20,7 @@
self.u_lb = u_lb;
self.n_q = jrsInstance.n_q;
self.n_t = jrsInstance.n_t;
- self.num_parameters = jrsInstance.n_k;
+ self.num_parameters = jrsInstance.num_parameters;
self.input_range = jrsInstance.input_range;
self.set_vdisplevel(verbosity);
@@ -43,7 +43,7 @@
msg = sprintf('ADDED UPPER BOUND INPUT CONSTRAINT ON JOINT %d AT TIME %d \n', j, i);
self.vdisp(msg, 'DEBUG');
constraints{end+1, 1} = @(k) slice(self.u_ub{i, 1}{j, 1}, k);
- grad_u_ub = grad(self.u_ub{i, 1}{j, 1}, self.n_q);
+ grad_u_ub = grad(self.u_ub{i, 1}{j, 1}, self.num_parameters);
grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_u_ub);
end
@@ -53,7 +53,7 @@
msg = sprintf('ADDED LOWER BOUND INPUT CONSTRAINT ON JOINT %d AT TIME %d \n', j, i);
self.vdisp(msg, 'DEBUG');
constraints{end+1, 1} = @(k) slice(self.u_lb{i, 1}{j, 1}, k);
- grad_u_lb = grad(self.u_lb{i, 1}{j, 1}, self.n_q);
+ grad_u_lb = grad(self.u_lb{i, 1}{j, 1}, self.num_parameters);
grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_u_lb);
end
end
diff --git a/+armour/+reachsets/JLSGenerator.m b/+armour/+reachsets/JLSGenerator.m
index c7bc44ac..e6e28e83 100644
--- a/+armour/+reachsets/JLSGenerator.m
+++ b/+armour/+reachsets/JLSGenerator.m
@@ -17,7 +17,7 @@
function self = JLSGenerator(robot, jrsGenerator, options)
arguments
robot armour.ArmourAgent
- jrsGenerator armour.reachsets.JRSGenerator
+ jrsGenerator armour.reachsets.JRS.OnlineGeneratorBase
options.verboseLevel(1,1) rtd.util.types.LogLevel = "DEBUG"
end
self.robot = robot;
@@ -36,6 +36,11 @@
% First get the JRS (allow the use of a cached value if it
% exists)
jrsInstance = self.jrsGenerator.getReachableSet(robotState, ignore_cache=false);
+ % For now, only support 1 problem
+ if length(jrsInstance) > 1
+ error('FOGenerator does not support multiple JRS yet!')
+ end
+ jrsInstance = jrsInstance.rs;
joint_state_limits = [self.robot.info.joints.position_limits];
joint_speed_limits = [self.robot.info.joints.velocity_limits];
@@ -65,7 +70,10 @@
end
% Save the generated reachable sets into the IRSInstance
- reachableSet = armour.reachsets.JLSInstance(q_ub, q_lb, dq_ub, dq_lb, jrsInstance, self.get_vdisplevel);
+ rs = armour.reachsets.JLSInstance(q_ub, q_lb, dq_ub, dq_lb, jrsInstance, self.get_vdisplevel);
+ % Return the 1 RS that we have.
+ reachableSet.rs = rs;
+ reachableSet.id = 1;
end
end
end
\ No newline at end of file
diff --git a/+armour/+reachsets/JLSInstance.m b/+armour/+reachsets/JLSInstance.m
index 03840a19..b069c5d6 100644
--- a/+armour/+reachsets/JLSInstance.m
+++ b/+armour/+reachsets/JLSInstance.m
@@ -24,7 +24,7 @@
self.dq_lb = dq_lb;
self.n_q = jrsInstance.n_q;
self.n_t = jrsInstance.n_t;
- self.num_parameters = jrsInstance.n_k;
+ self.num_parameters = jrsInstance.num_parameters;
self.input_range = jrsInstance.input_range;
self.set_vdisplevel(verbosity);
@@ -47,7 +47,7 @@
msg = sprintf('ADDED UPPER BOUND JOINT POSITION CONSTRAINT ON JOINT %d AT TIME %d \n', j, i);
self.vdisp(msg, 'DEBUG')
constraints{end+1, 1} = @(k) slice(self.q_ub{i, 1}{j, 1}, k);
- grad_q_ub = grad(self.q_ub{i, 1}{j, 1}, self.n_q);
+ grad_q_ub = grad(self.q_ub{i, 1}{j, 1}, self.num_parameters);
grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_q_ub);
end
@@ -56,7 +56,7 @@
msg = sprintf('ADDED LOWER BOUND JOINT POSITION CONSTRAINT ON JOINT %d AT TIME %d \n', j, i);
self.vdisp(msg, 'DEBUG')
constraints{end+1, 1} = @(k) slice(self.q_lb{i, 1}{j, 1}, k);
- grad_q_lb = grad(self.q_lb{i, 1}{j, 1}, self.n_q);
+ grad_q_lb = grad(self.q_lb{i, 1}{j, 1}, self.num_parameters);
grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_q_lb);
end
@@ -65,7 +65,7 @@
msg = sprintf('ADDED UPPER BOUND JOINT VELOCITY CONSTRAINT ON JOINT %d AT TIME %d \n', j, i);
self.vdisp(msg, 'DEBUG')
constraints{end+1, 1} = @(k) slice(self.dq_ub{i, 1}{j, 1}, k);
- grad_dq_ub = grad(self.dq_ub{i, 1}{j, 1}, self.n_q);
+ grad_dq_ub = grad(self.dq_ub{i, 1}{j, 1}, self.num_parameters);
grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_dq_ub);
end
@@ -74,7 +74,7 @@
msg = sprintf('ADDED LOWER BOUND JOINT VELOCITY CONSTRAINT ON JOINT %d AT TIME %d \n', j, i);
self.vdisp(msg, 'DEBUG')
constraints{end+1, 1} = @(k) slice(self.dq_lb{i, 1}{j, 1}, k);
- grad_dq_lb = grad(self.dq_lb{i, 1}{j, 1}, self.n_q);
+ grad_dq_lb = grad(self.dq_lb{i, 1}{j, 1}, self.num_parameters);
grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_dq_lb);
end
end
diff --git a/+armour/+reachsets/JRSGenerator.m b/+armour/+reachsets/JRSGenerator.m
deleted file mode 100644
index 306a2b8c..00000000
--- a/+armour/+reachsets/JRSGenerator.m
+++ /dev/null
@@ -1,80 +0,0 @@
-classdef JRSGenerator < rtd.planner.reachsets.ReachSetGenerator
- % JointReachableSetsOnline
- % This does the online computation of joint reachable sets. It then
- % generates a JRSInstance object.
-
- % Required Abstract Properties
- properties
- cache_max_size = 1
- end
-
- % Additional Properties
- properties
- taylor_degree
- add_ultimate_bound
- traj_type
- joint_axes
- controller
- end
-
- % Properties for the generator
- methods
- function self = JRSGenerator(robot, options)
- arguments
- robot armour.ArmourAgent
- options.taylor_degree(1,1) double {mustBeInteger} = 1
- options.add_ultimate_bound(1,1) logical = true
- options.traj_type {mustBeMember(options.traj_type,{'piecewise','bernstein'})} = 'piecewise'
- options.verboseLevel(1,1) rtd.util.types.LogLevel = "DEBUG"
- end
-
- self.joint_axes = [robot.info.joints.axes];
- self.controller = robot.controller;
- self.taylor_degree = options.taylor_degree;
- self.add_ultimate_bound = options.add_ultimate_bound;
- self.traj_type = options.traj_type;
- self.set_vdisplevel(options.verboseLevel);
- end
- end
-
- % Generate a new JRS
- methods (Access=protected)
- % Obtains the relevant reachable set for the robotstate provided
- % and outputs the singular instance of a reachable set.
- % Wraps create_jrs_online
- % Returns JRSInstance
- function reachableSet = generateReachableSet(self, robotState)
- % The way I choose to wrap this is not the way it has to be
- % done. I choose to have it create the object first, then
- % initialize the remaining properties from this loaded data.
- rs = armour.reachsets.JRSInstance;
-
- % compat
- traj_type_adapt = self.traj_type;
- if strcmp(traj_type_adapt, 'piecewise')
- traj_type_adapt = 'orig';
- end
-
- self.vdisp("Generating joint reachable set!", "INFO")
-
- self.vdisp("The following message is from create_jrs_online", 'INFO');
- % Generate it online as per the original implementation
- [rs.q_des, rs.dq_des, rs.ddq_des, ...
- rs.q, rs.dq, rs.dq_a, rs.ddq_a, ...
- rs.R_des, rs.R_t_des, rs.R, rs.R_t, ...
- rs.jrs_info] = armour.legacy.create_jrs_online( ...
- robotState.q, ...
- robotState.q_dot, ...
- robotState.q_ddot, ...
- self.joint_axes, ...
- self.taylor_degree, ...
- traj_type_adapt, ...
- self.add_ultimate_bound, ...
- self.controller);
-
- % Initialize this particular instance and return
- rs.initialize(self.traj_type);
- reachableSet = rs;
- end
- end
-end
\ No newline at end of file
diff --git a/+armour/+reachsets/JRSInstance.m b/+armour/+reachsets/JRSInstance.m
deleted file mode 100644
index 4566d55a..00000000
--- a/+armour/+reachsets/JRSInstance.m
+++ /dev/null
@@ -1,63 +0,0 @@
-classdef JRSInstance < rtd.planner.reachsets.ReachSetInstance
- % JRSInstance
- % This is just an individual instance of an original ARMTD JRS.
- properties
- input_range = [-1.0, 1.0]
- output_range = [-1.0, 1.0]
- num_parameters = 0
-
- % properties carried over from the original implementation
- q_des
- dq_des
- ddq_des
- q
- dq
- dq_a
- ddq_a
- R_des
- R_t_des
- R
- R_t
- jrs_info
-
- % New properties to flatten the structure
- n_q
- n_t
- k_id
- n_k
- end
- methods
- % An example constructor, but can take anything needed for the
- % respective ReachableSets class.
- %self = ReachableSets( ...
- % robotInfo ...
- % )
- function initialize(self, traj_type)
- % expand the parameter range to match the number of joints
- switch traj_type
- case 'piecewise'
- c_k = self.jrs_info.c_k_orig;
- g_k = self.jrs_info.g_k_orig;
- case 'bernstein'
- c_k = self.jrs_info.c_k_bernstein;
- g_k = self.jrs_info.g_k_bernstein;
- end
- self.input_range = ones(self.jrs_info.n_k, 1) * self.input_range;
- self.output_range = c_k + [-1.0, 1.0] .* g_k;
-
- self.n_q = self.jrs_info.n_q;
- self.num_parameters = self.jrs_info.n_k;
- self.n_k = self.jrs_info.n_k;
- self.n_t = self.jrs_info.n_t;
- self.k_id = self.jrs_info.k_id;
- end
-
- % Handles the obstacle-frs pair or similar to generate the
- % nlconstraint.
- % Returns a function handle for the nlconstraint generated
- % where the function's return type is [c, ceq, gc, gceq]
- function nlconFunction = genNLConstraint(self, worldState)
- nlconFunction = [];
- end
- end
-end
\ No newline at end of file
diff --git a/+armour/+trajectory/ArmTrajectoryFactory.m b/+armour/+trajectory/ArmTrajectoryFactory.m
index 84ad436f..ec2df19e 100644
--- a/+armour/+trajectory/ArmTrajectoryFactory.m
+++ b/+armour/+trajectory/ArmTrajectoryFactory.m
@@ -1,28 +1,71 @@
-classdef ArmTrajectoryFactory < rtd.planner.trajectory.TrajectoryFactory & handle
+classdef ArmTrajectoryFactory < rtd.trajectory.TrajectoryFactory & handle
+% Factory for creating trajectories for the arm as used in ARMOUR
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-01-27
+% Last Updated: 2023-10-04 (Adam Li)
+%
+% See also: rtd.trajectory.mTrajectoryFactory, armour.trajectory.PiecewiseArmTrajectory,
+% armour.trajectory.BernsteinArmTrajectory, armour.trajectory.ZeroHoldArmTrajectory
+% armour.trajectory.TwoBernsteinArmTrajectory
+%
+% --- Revision History ---
+% 2023-10-04 - Added support for two bernstein parameterization trajectories.
+% 2023-09-07 - removed dependency on armour.reachsets.JRSInstance for each trajectory
+%
+% --- More Info ---
+%
properties
- traj_type {mustBeMember(traj_type,{'piecewise', 'bernstein', 'zerohold'})} = 'piecewise'
+ % the default trajectory type to use
+ traj_type {mustBeMember(traj_type,{'piecewise', 'bernstein', 'twobernstein', 'zerohold'})} = 'piecewise'
end
methods
function self = ArmTrajectoryFactory(trajOptProps, traj_type)
+ % Constructor for the ArmTrajectoryFactory
+ %
+ % Arguments:
+ % trajOptProps (rtd.planner.trajopt.TrajOptProps): Trajectory optimization properties
+ % traj_type (str): The type of trajectory to use. Must be one of
+ % 'piecewise', 'bernstein', 'twobernstein', or 'zerohold'
+ %
arguments
- trajOptProps
- traj_type {mustBeMember(traj_type,{'piecewise', 'bernstein', 'zerohold'})} = 'piecewise'
+ trajOptProps rtd.planner.trajopt.TrajOptProps
+ traj_type {mustBeMember(traj_type,{'piecewise', 'bernstein', 'twobernstein', 'zerohold'})} = 'piecewise'
end
+
self.trajOptProps = trajOptProps;
self.traj_type = traj_type;
end
% Create a new trajectory object for the given state
function trajectory = createTrajectory(self, robotState, rsInstances, trajectoryParams, options)
+ % Create a new trajectory object for the given state
+ %
+ % Arguments:
+ % robotState (rtd.entity.states.ArmRobotStateInstance): The robot state
+ % rsInstances (struct, optional): The reach set instances
+ % trajectoryParams (double, optional): The trajectory parameters
+ % options: Keyword arguments. See Below.
+ %
+ % Keyword Arguments:
+ % jrsInstance (armour.reachsets.JRSInstance): The joint reach set instance
+ % traj_type (str): The type of trajectory to use. Must be one of
+ % 'piecewise', 'bernstein', 'twobernstein', or 'zerohold'. If not specified, the
+ % default trajectory type is used.
+ %
+ % Returns:
+ % trajectory (rtd.trajectory.Trajectory): The trajectory object
+ %
arguments
self armour.trajectory.ArmTrajectoryFactory
- robotState rtd.entity.states.ArmRobotState
+ robotState rtd.entity.states.ArmRobotStateInstance
rsInstances struct = struct
trajectoryParams (:,1) double = []
- options.jrsInstance armour.reachsets.JRSInstance = armour.reachsets.JRSInstance.empty()
- options.traj_type {mustBeMember(options.traj_type,{'piecewise', 'bernstein', 'zerohold'})} = self.traj_type
+ options.jrsInstance armour.reachsets.JRS.JRSInstance = armour.reachsets.JRS.JRSInstance.empty()
+ options.traj_type {mustBeMember(options.traj_type,{'piecewise', 'bernstein', 'twobernstein', 'zerohold'})} = self.traj_type
end
if ~strcmp(options.traj_type, 'zerohold') && isempty(options.jrsInstance)
@@ -35,13 +78,30 @@
switch options.traj_type
case 'piecewise'
- trajectory = armour.trajectory.PiecewiseArmTrajectory(self.trajOptProps, robotState, options.jrsInstance);
+ trajectory = armour.trajectory.PiecewiseArmTrajectory(robotState, ...
+ self.trajOptProps.planTime, ...
+ self.trajOptProps.horizonTime, ...
+ options.jrsInstance.num_parameters);
+ paramScale = rtd.util.RangeScaler(options.jrsInstance.input_range, options.jrsInstance.output_range);
+ trajectory.setParamScale(paramScale)
case 'bernstein'
- trajectory = armour.trajectory.BernsteinArmTrajectory(self.trajOptProps, robotState, options.jrsInstance);
+ trajectory = armour.trajectory.BernsteinArmTrajectory(robotState, ...
+ self.trajOptProps.horizonTime, ...
+ options.jrsInstance.num_parameters);
+ paramScale = rtd.util.RangeScaler(options.jrsInstance.input_range, options.jrsInstance.output_range);
+ trajectory.setParamScale(paramScale)
+
+ case 'twobernstein'
+ trajectory = armour.trajectory.TwoBernsteinArmTrajectory(robotState, ...
+ self.trajOptProps.planTime, ...
+ self.trajOptProps.horizonTime, ...
+ options.jrsInstance.num_parameters);
+ paramScale = rtd.util.RangeScaler(options.jrsInstance.input_range, options.jrsInstance.output_range);
+ trajectory.setParamScale(paramScale)
case 'zerohold'
- trajectory = armour.trajectory.ZeroHoldArmTrajectory(self.trajOptProps, robotState);
+ trajectory = armour.trajectory.ZeroHoldArmTrajectory(robotState);
end
if ~isempty(trajectoryParams)
diff --git a/+armour/+trajectory/BernsteinArmTrajectory.m b/+armour/+trajectory/BernsteinArmTrajectory.m
index ab3f21c3..18f8f58d 100644
--- a/+armour/+trajectory/BernsteinArmTrajectory.m
+++ b/+armour/+trajectory/BernsteinArmTrajectory.m
@@ -1,72 +1,186 @@
-classdef BernsteinArmTrajectory < rtd.planner.trajectory.Trajectory
- % BernsteinArmTrajectory
- % This encapsulates the conversion of parameters used in optimization
- % to the actual trajectory generated from those parameters. It's also
- % indirectly used to specify the OptimizationEngine's size
+classdef BernsteinArmTrajectory < rtd.trajectory.Trajectory
+% 5th Degree Bernstein Polynomial Trajectory for arm robots
+%
+% This trajectory is a 5th degree Bernstein polynomial trajectory for
+% arm robots. It is parameterized by the start state, the horizon time,
+% and the desired final position. The trajectory is generated by
+% calculating the 5th degree Bernstein polynomial coefficients for the
+% start and end positions, and then using those coefficients to generate
+% the trajectory.
+%
+% The trajectory is parameterized by the following:
+% * `startState`: The start state of the trajectory. This is used to
+% specify the initial position, velocity, and acceleration of the trajectory.
+% * `horizonTime`: The time at which the trajectory should end. This is
+% used to specify the duration of the trajectory.
+% * `params`: The desired final position of the trajectory relative to the start position.
+% This is used to specify the final position of the trajectory. The relative position
+% can also be scaled, which then paramScale is used to scale it back to the
+% desired position.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2022-10-14
+% Updated: 2023-10-04 (Adam Li)
+%
+% See also: rtd.trajectory.Trajectory, armour.trajectory.ArmTrajectoryFactory,
+% rtd.entity.states.ArmRobotStateInstance
+%
+% --- More Info ---
+%
+
% Required properties
properties (Constant)
+ % True as this trajectory supports getting commands for a time vector
vectorized = true
end
+
% Additional properties
properties
- % Initial parameters from the robot used to calculate the desired
- % trajectory
- alpha {mustBeNumeric}
- q_end {mustBeNumeric}
- % I flatten these out for simplicity, but startState is updated to
- % include these following handles too.
- % The JRS which contains the center and range to scale the
- % parameters
- jrsInstance
+ % The initial state of the trajectory. These are initial parameters
+ % from the robot used to calculate the desired bernstein coefficients
+ startState(1,1) rtd.entity.states.ArmRobotStateInstance
+
+ % The duration of the trajectory, after which the trajectory will
+ % hold the final position
+ horizonTime(1,1) double
+
+ % Extra properties to scale the parameters
+ paramScale rtd.util.RangeScaler = rtd.util.RangeScaler.empty()
+
+ % The number of parameters for the trajectory
+ numParams(1,1) uint32
end
+
+ % Internal properties
+ properties (SetAccess=private)
+ % the alpha coefficients for the bernstein polynomial
+ alpha(:,6) double
+
+ % The calculated final position of the trajectory
+ q_end(:,1) double
+ end
+
methods
- % An example constructor for the trajectory object. Should be
- % implemented with varargin
- function self = BernsteinArmTrajectory(trajOptProps, startState, jrsInstance)
+ function self = BernsteinArmTrajectory(startState, horizonTime, numParams)
+ % Constructor for the BernsteinArmTrajectory
+ %
+ % Arguments:
+ % startState: The start state of the trajectory. This is used to
+ % specify the initial position, velocity, and acceleration of the trajectory.
+ % horizonTime: The time at which the trajectory should end. This is
+ % used to specify the duration of the trajectory.
+ % numParams: The number of parameters for the trajectory. This is
+ % used to specify the number of parameters for the trajectory.
+ %
arguments
- trajOptProps(1,1) rtd.planner.trajopt.TrajOptProps
- startState(1,1) rtd.entity.states.ArmRobotState
- jrsInstance(1,1) armour.reachsets.JRSInstance
+ startState(1,1) rtd.entity.states.ArmRobotStateInstance
+ horizonTime(1,1) double
+ numParams(1,1) uint32
end
- self.trajOptProps = trajOptProps;
+
self.startState = startState;
- self.jrsInstance = jrsInstance;
+ self.horizonTime = horizonTime;
+ self.numParams = numParams;
+ end
+
+ function setParamScale(self, paramScale)
+ % Sets the parameter scaling
+ %
+ % Arguments:
+ % paramScale (rtd.util.RangeScaler): The parameter scaling to use.
+ % This is used to scale the relative position to the desired
+ % position. If not provided, then paramScale is reset to empty.
+ %
+ arguments
+ self armour.trajectory.BernsteinArmTrajectory
+ paramScale(:,1) rtd.util.RangeScaler = []
+ end
+
+ if length(paramScale) > 1
+ error('Can only set one paramScale value!');
+ end
+ self.paramScale = paramScale;
end
- % A validated method to set the parameters for the trajectory.
- % Should be implemented with a varargin.
function setParameters(self, trajectoryParams, options)
+ % Sets the parameters for the trajectory.
+ %
+ % After setting the parameters, it attempts to internally update the trajectory
+ % to reduce long term calculations.
+ %
+ % Arguments:
+ % trajectoryParams (double): The parameters for the trajectory. This is
+ % used to specify the relative position of the trajectory.
+ % The relative position can also be scaled, which then
+ % paramScale is used to scale it back to the desired position.
+ % options: Keyword arguments. See Below.
+ %
+ % Keyword Arguments:
+ % startState (rtd.entity.states.ArmRobotStateInstance): The start state of the trajectory. This is used to
+ % specify the initial position, velocity, and acceleration of the trajectory.
+ % horizonTime (double): The time at which the trajectory should end. This is
+ % used to specify the duration of the trajectory.
+ % numParams (double): The number of parameters for the trajectory. This is
+ % used to specify the number of parameters for the trajectory.
+ % paramScale (rtd.util.RangeScaler): The parameter scaling to use.
+ % This is used to scale the relative position to the desired
+ % position.
+ %
arguments
self armour.trajectory.BernsteinArmTrajectory
trajectoryParams(1,:) double
- options.startState rtd.entity.states.ArmRobotState = self.startState
- options.jrsInstance armour.reachsets.JRSInstance = self.jrsInstance
+ options.startState rtd.entity.states.ArmRobotStateInstance = self.startState
+ options.horizonTime(1,1) double = self.horizonTime
+ options.numParams(1,1) uint32 = self.numParams
+ options.paramScale(:,1) rtd.util.RangeScaler = self.paramScale
end
+
self.trajectoryParams = trajectoryParams;
- if length(self.trajectoryParams) > self.jrsInstance.n_q
- self.trajectoryParams = self.trajectoryParams(1:self.jrsInstance.n_q);
- end
self.startState = options.startState;
- self.jrsInstance = options.jrsInstance;
+ self.horizonTime = options.horizonTime;
+ self.numParams = options.numParams;
+ if length(self.trajectoryParams) > self.numParams
+ self.trajectoryParams = self.trajectoryParams(1:self.numParams);
+ end
+ self.setParamScale(options.paramScale);
% Perform an internal update (compute peak and stopping values)
self.internalUpdate();
end
- % Validate that the trajectory is fully characterized
function valid = validate(self, throwOnError)
+ % Validates the trajectory parameters.
+ %
+ % This function ensures that the trajectory parameters are valid and fully
+ % specified. If the trajectory parameters are invalid, then it either throws
+ % an error or returns false.
+ %
+ % Arguments:
+ % throwOnError (logical, optional): If true, then throws an error if the
+ % trajectory parameters are invalid. Otherwise, returns false.
+ % Defaults to false.
+ %
+ % Returns:
+ % valid (logical): Whether or not the trajectory parameters are valid.
+ %
+ % Raises:
+ % InvalidTrajectory: If the current trajectory is not valid and
+ % throwOnError is true.
+ %
arguments
self armour.trajectory.BernsteinArmTrajectory
throwOnError(1,1) logical = false
end
+
% Make sure everything is nonempty
valid = not( ...
isempty(self.trajectoryParams) || ...
- isempty(self.jrsInstance) || ...
isempty(self.startState));
% Make sure the trajectory params make sense
- valid = valid && length(self.trajectoryParams) == self.jrsInstance.n_q;
+ valid = valid && length(self.trajectoryParams) == self.numParams;
+ valid = valid && self.numParams == length(self.startState.position);
% Throw if wanted
if ~valid && throwOnError
@@ -75,51 +189,24 @@ function setParameters(self, trajectoryParams, options)
throw(errMsg)
end
end
-
- % Update internal parameters to reduce long term calculations.
- function internalUpdate(self)
- % internal update if valid. short circuit if not.
- if ~self.validate()
- return
- end
-
- % Get the desired final position
- out = self.jrsInstance.output_range;
- in = self.jrsInstance.input_range;
- q_goal = rescale(self.trajectoryParams, out(:,1), out(:,2),'InputMin',in(:,1),'InputMax',in(:,2));
- q_goal = self.startState.q + q_goal;
-
- n_q = self.jrsInstance.n_q;
- self.alpha = zeros(n_q, 6);
- for j = 1:n_q % Modified to use matrix instead of cells
- beta = armour.legacy.match_deg5_bernstein_coefficients({...
- self.startState.position(j); ...
- self.startState.velocity(j); ...
- self.startState.acceleration(j); ...
- q_goal(j); ...
- 0; 0}, self.trajOptProps.horizonTime);
- self.alpha(j,:) = cell2mat(armour.legacy.bernstein_to_poly(beta, 5));
- end
-
- % Precompute end position
- % Adapted Original
- % End position should actually just be q_goal
- %self.q_end = zeros(self.n_q, 1);
- %for j = 1:n_q
- % for coeff_idx = 0:5
- % self.q_end(j) = self.q_end(j) + ...
- % self.alpha(j,coeff_idx+1) * self.trajOptProps.horizonTime^coeff_idx;
- % end
- %end
- self.q_end = q_goal;
- % Proposed vectorized implementation
- %self.q_end = sum(self.alpha.*(trajOptProps.horizon.^(0:5)),2);
- end
- % Computes the actual input commands for the given time.
- % throws RTD:InvalidTrajectory if the trajectory isn't set
- % TODO: write in a vectorized manner
function command = getCommand(self, time)
+ % Gets the command for the given time.
+ %
+ % This function gets the command for the given time. The command is
+ % generated by calculating the position, velocity, and acceleration
+ % for the given time.
+ %
+ % Arguments:
+ % time (double): The time to get the command for.
+ %
+ % Returns:
+ % command (rtd.entity.states.ArmRobotStateInstance): The command for the given time.
+ %
+ % Raises:
+ % InvalidTrajectory: If the current trajectory is not valid.
+ % InvalidTime: If the time is invalid.
+ %
arguments
self armour.trajectory.BernsteinArmTrajectory
time(1,:) double
@@ -128,7 +215,7 @@ function internalUpdate(self)
% Do a parameter check and time check, and throw if anything is
% invalid.
self.validate(true);
- t_shifted = time - self.startState.time;
+ t_shifted = time - self.startTime;
if any(t_shifted < 0)
ME = MException('BernsteinArmTrajectory:InvalidTime', ...
'Invalid time provided to BernsteinArmTrajectory');
@@ -138,17 +225,16 @@ function internalUpdate(self)
% Get a mask for the active trajectory time and the stopped
% trajectory times.
t_size = length(t_shifted);
- horizon_mask = t_shifted < self.trajOptProps.horizonTime;
- t_masked_scaled = t_shifted(horizon_mask) / self.trajOptProps.horizonTime;
+ horizon_mask = t_shifted < self.horizonTime;
+ t_masked_scaled = t_shifted(horizon_mask) / self.horizonTime;
t_masked_size = length(t_masked_scaled);
- n_q = self.jrsInstance.n_q;
% Original implementation adapted
- q_des = zeros(n_q, t_masked_size);
- q_dot_des = zeros(n_q, t_masked_size);
- q_ddot_des = zeros(n_q, t_masked_size);
+ q_des = zeros(self.numParams, t_masked_size);
+ q_dot_des = zeros(self.numParams, t_masked_size);
+ q_ddot_des = zeros(self.numParams, t_masked_size);
- for j = 1:n_q
+ for j = 1:self.numParams
for coeff_idx = 0:5
q_des(j,:) = q_des(j,:) + ...
self.alpha(j,coeff_idx+1) * t_masked_scaled.^coeff_idx;
@@ -170,21 +256,74 @@ function internalUpdate(self)
%q_ddot_des = sum(((2:deg).*(1:deg-1)).*self.alpha(:,3:end).*(t_masked.^(0:deg-2)),2);
% Move to a combined state variable
- state = zeros(n_q*3, t_size);
- pos_idx = 1:n_q;
- vel_idx = pos_idx + n_q;
- acc_idx = vel_idx + n_q;
+ state = zeros(self.numParams*3, t_size);
+ pos_idx = 1:self.numParams;
+ vel_idx = pos_idx + self.numParams;
+ acc_idx = vel_idx + self.numParams;
state(pos_idx,horizon_mask) = q_des;
- state(vel_idx,horizon_mask) = q_dot_des / self.trajOptProps.horizonTime;
- state(acc_idx,horizon_mask) = q_ddot_des / self.trajOptProps.horizonTime^2;
+ state(vel_idx,horizon_mask) = q_dot_des / self.horizonTime;
+ state(acc_idx,horizon_mask) = q_ddot_des / self.horizonTime^2;
% Update all states times after the horizon time
state(pos_idx,~horizon_mask) = repmat(self.q_end, 1, t_size-t_masked_size);
% Generate the output.
- command = rtd.entity.states.ArmRobotState(pos_idx, vel_idx, acc_idx);
- command.time = time;
- command.state = state;
+ command(length(time)) = rtd.entity.states.ArmRobotStateInstance();
+ command.setTimes(time);
+ command.setStateSpace(state, ...
+ position_idxs=pos_idx, ...
+ velocity_idxs=vel_idx, ...
+ acceleration_idxs=acc_idx);
+ end
+ end
+
+ % Internal methods for the class
+ methods (Access=private)
+ function internalUpdate(self)
+ % Performs an internal update of the trajectory.
+ %
+ % This function performs an internal update of the trajectory. This
+ % includes calculating the bernstein coefficients and the final position.
+ % This function is called automatically when the trajectory parameters
+ % are set.
+ %
+
+ % internal update if valid. short circuit if not.
+ if ~self.validate()
+ return
+ end
+
+ % Get the desired final position
+ if isempty(self.paramScale)
+ q_goal = self.startState.position + self.trajectoryParams;
+ else
+ q_goal = self.startState.position + self.paramScale.scaleout(self.trajectoryParams);
+ end
+
+ self.alpha = zeros(self.numParams, 6);
+ for j = 1:self.numParams % Modified to use matrix instead of cells
+ beta = armour.legacy.match_deg5_bernstein_coefficients({...
+ self.startState.position(j); ...
+ self.startState.velocity(j); ...
+ self.startState.acceleration(j); ...
+ q_goal(j); ...
+ 0; 0}, self.horizonTime);
+ self.alpha(j,:) = cell2mat(armour.legacy.bernstein_to_poly(beta, 5));
+ end
+
+ % Precompute end position
+ % Adapted Original
+ % End position should actually just be q_goal
+ %self.q_end = zeros(self.n_q, 1);
+ %for j = 1:n_q
+ % for coeff_idx = 0:5
+ % self.q_end(j) = self.q_end(j) + ...
+ % self.alpha(j,coeff_idx+1) * self.trajOptProps.horizonTime^coeff_idx;
+ % end
+ %end
+
+ self.q_end = q_goal;
+ self.startTime = self.startState.time;
end
end
end
\ No newline at end of file
diff --git a/+armour/+trajectory/PiecewiseArmTrajectory.m b/+armour/+trajectory/PiecewiseArmTrajectory.m
index a003e1d8..16b4da53 100644
--- a/+armour/+trajectory/PiecewiseArmTrajectory.m
+++ b/+armour/+trajectory/PiecewiseArmTrajectory.m
@@ -1,75 +1,195 @@
-classdef PiecewiseArmTrajectory < rtd.planner.trajectory.Trajectory
- % PiecewiseArmTrajectory
- % The original ArmTD trajectory with peicewise accelerations
+classdef PiecewiseArmTrajectory < rtd.trajectory.Trajectory
+% Piecewise parameterized trajectory for the arm robot
+%
+% This trajectory is a piecewise trajectory with a plan time and a horizon
+% time. The trajectory accelerates for the plan time, and then decelerates
+% until the horizon time. The trajectory is parameterized by the acceleration
+% during the plan time, and the trajectory is computed from the initial
+% state of the robot.
+%
+% The trajectory is parameterized by the following:
+% * `startState`: the initial state of the robot
+% * `planTime`: the time to accelerate for
+% * `horizonTime`: the time after starting that the robot should be stopped by
+% * `params`: the parameters of the trajectory, which are the accelerations
+% during the plan time. This can also be scaled by a RangeScaler
+% object.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2022-09-08
+% Updated: 2023-09-12 (Adam Li)
+%
+% See also: rtd.trajectory.Trajectory, armour.trajectory.ArmTrajectoryFactory,
+% rtd.entity.states.ArmRobotStateInstance
+%
+% --- More Info ---
+%
+
% Required properties
properties (Constant)
+ % True as this trajectory supports vectorized computation
vectorized = true
end
+
% Additional properties
properties
- % Precomputed values
- q_ddot {mustBeNumeric}
- q_peak {mustBeNumeric}
- q_dot_peak {mustBeNumeric}
- q_ddot_to_stop {mustBeNumeric}
- q_end {mustBeNumeric}
- % I flatten these out for simplicity, but startState is updated to
- % include these following handles too.
- % The JRS which contains the center and range to scale the
- % parameters
- jrsInstance
+ % The initial state of the trajectory. These are initial parameters
+ % from the robot used to compute the trajectory.
+ startState(1,1) rtd.entity.states.ArmRobotStateInstance
+
+ % The plan time is the duration the trajectory accelerates at the
+ % parameterized value for before executing the braking action.
+ planTime(1,1) double
+
+ % The horizon time is the time at which the trajectory will stop
+ % after starting, after which the trajectory will hold the final
+ % position.
+ horizonTime(1,1) double
+
+ % Range scaling object for the trajectory parameters
+ paramScale rtd.util.RangeScaler = rtd.util.RangeScaler.empty()
+
+ % the number of parameters to use for the trajectory
+ numParams(1,1) uint32
+ end
+
+ % Internal properties (for precomputed values)
+ properties (SetAccess=private)
+ % the acceleration for the plan time portion of the trajectory
+ q_ddot(:,1) double
+
+ % the peak position of the trajectory
+ q_peak(:,1) double
+
+ % the peak velocity of the trajectory
+ q_dot_peak(:,1) double
+
+ % the acceleration for the stopping portion of the trajectory (horizon time - plan time)
+ q_ddot_to_stop(:,1) double
+
+ % the end position of the trajectory
+ q_end(:,1) double
end
methods
- % The PiecewiseArmTrajectory constructor, which simply sets parameters and
- % attempts to call internalUpdate, a helper function made for this
- % class to update all other internal parameters once fully
- % parameterized.
- function self = PiecewiseArmTrajectory(trajOptProps, startState, jrsInstance)
+ function self = PiecewiseArmTrajectory(startState, planTime, horizonTime, numParams)
+ % Constructor for the PiecewiseArmTrajectory class
+ %
+ % Arguments:
+ % startState (rtd.entity.states.ArmRobotStateInstance): the initial state of the robot
+ % planTime (double): the time to accelerate for
+ % horizonTime (double): the time after starting that the robot should be stopped by
+ % numParams (uint32): the number of parameters to use for the trajectory
+ %
arguments
- trajOptProps(1,1) rtd.planner.trajopt.TrajOptProps
- startState(1,1) rtd.entity.states.ArmRobotState
- jrsInstance(1,1) armour.reachsets.JRSInstance
+ startState(1,1) rtd.entity.states.ArmRobotStateInstance
+ planTime(1,1) double
+ horizonTime(1,1) double
+ numParams(1,1) uint32
end
- self.trajOptProps = trajOptProps;
+
self.startState = startState;
- self.jrsInstance = jrsInstance;
+ self.planTime = planTime;
+ self.horizonTime = horizonTime;
+ self.numParams = numParams;
+ end
+
+ function setParamScale(self, paramScale)
+ % Sets the parameter scaling
+ %
+ % Arguments:
+ % paramScale (rtd.util.RangeScaler): The parameter scaling to use.
+ % This is used to scale the relative position to the desired
+ % position. If not provided, then paramScale is reset to empty.
+ %
+ arguments
+ self armour.trajectory.PiecewiseArmTrajectory
+ paramScale(:,1) rtd.util.RangeScaler = []
+ end
+
+ if length(paramScale) > 1
+ error('Can only set one paramScale value!');
+ end
+ self.paramScale = paramScale;
end
- % Set the parameters of the trajectory, with a focus on the
- % parameters as the state should be set from the constructor.
function setParameters(self, trajectoryParams, options)
+ % Sets the parameters of the trajectory
+ %
+ % After setting the parameters, the internal parameters are
+ % updated.
+ %
+ % Arguments:
+ % trajectoryParams (double): the parameters of the trajectory, which are the
+ % accelerations during the plan time. This can also be scaled by
+ % a RangeScaler object.
+ % options: Keyword Arguments. See below.
+ %
+ % Keyword Arguments:
+ % startState (rtd.entity.states.ArmRobotStateInstance): the initial state of the robot
+ % planTime (double): the time to accelerate for
+ % horizonTime (double): the time after starting that the robot should be stopped by
+ % numParams (uint32): the number of parameters to use for the trajectory
+ % paramScale (rtd.util.RangeScaler): The parameter scaling to use.
+ % This is used to scale the relative position to the desired position.
+ %
arguments
self armour.trajectory.PiecewiseArmTrajectory
trajectoryParams(1,:) double
- options.startState rtd.entity.states.ArmRobotState = self.startState
- options.jrsInstance armour.reachsets.JRSInstance = self.jrsInstance
+ options.startState rtd.entity.states.ArmRobotStateInstance = self.startState
+ options.planTime(1,1) double = self.planTime
+ options.horizonTime(1,1) double = self.horizonTime
+ options.numParams(1,1) uint32 = self.numParams
+ options.paramScale(:,1) rtd.util.RangeScaler = self.paramScale
end
+
self.trajectoryParams = trajectoryParams;
- if length(self.trajectoryParams) > self.jrsInstance.n_q
- self.trajectoryParams = self.trajectoryParams(1:self.jrsInstance.n_q);
- end
self.startState = options.startState;
- self.jrsInstance = options.jrsInstance;
+ self.planTime = options.planTime;
+ self.horizonTime = options.horizonTime;
+ self.numParams = options.numParams;
+ if length(self.trajectoryParams) > self.numParams
+ self.trajectoryParams = self.trajectoryParams(1:self.numParams);
+ end
+ self.setParamScale(options.paramScale);
% Perform an internal update (compute peak and stopping values)
self.internalUpdate();
end
- % Validate that the trajectory is fully characterized
function valid = validate(self, throwOnError)
+ % Validates the trajectory parameters.
+ %
+ % This function ensures that the trajectory parameters are valid and fully
+ % specified. If the trajectory parameters are invalid, then it either throws
+ % an error or returns false.
+ %
+ % Arguments:
+ % throwOnError (logical, optional): If true, then throws an error if the
+ % trajectory parameters are invalid. Otherwise, returns false.
+ % Defaults to false.
+ %
+ % Returns:
+ % valid (logical): Whether or not the trajectory parameters are valid.
+ %
+ % Raises:
+ % InvalidTrajectory: If the current trajectory is not valid and
+ % throwOnError is true.
+ %
arguments
self armour.trajectory.PiecewiseArmTrajectory
throwOnError(1,1) logical = false
end
+
% Make sure everything is nonempty
valid = not( ...
isempty(self.trajectoryParams) || ...
- isempty(self.jrsInstance) || ...
isempty(self.startState));
% Make sure the trajectory params make sense
- valid = valid && length(self.trajectoryParams) == self.jrsInstance.n_q;
+ valid = valid && length(self.trajectoryParams) == self.numParams;
+ valid = valid && self.numParams == length(self.startState.position);
% Throw if wanted
if ~valid && throwOnError
@@ -79,43 +199,23 @@ function setParameters(self, trajectoryParams, options)
end
end
- % Update internal parameters to reduce long term calculations.
- function internalUpdate(self)
- % internal update if valid. short circuit if not.
- if ~self.validate()
- return
- end
-
- % Rename variables
- q_0 = self.startState.position;
- q_dot_0 = self.startState.velocity;
-
- % Scale the parameters
- out = self.jrsInstance.output_range;
- in = self.jrsInstance.input_range;
- self.q_ddot = rescale(self.trajectoryParams, out(:,1), out(:,2),'InputMin',in(:,1),'InputMax',in(:,2));
-
- % Compute the peak parameters
- self.q_peak = q_0 + ...
- q_dot_0 * self.trajOptProps.planTime + ...
- (1/2) * self.q_ddot * self.trajOptProps.planTime^2;
- self.q_dot_peak = q_dot_0 + ...
- self.q_ddot * self.trajOptProps.planTime;
-
- % Compute the stopping parameters
- self.q_ddot_to_stop = (0-self.q_dot_peak) / ...
- (self.trajOptProps.horizonTime - self.trajOptProps.planTime);
- self.q_end = self.q_peak + ...
- self.q_dot_peak * self.trajOptProps.planTime + ...
- (1/2) * self.q_ddot_to_stop * self.trajOptProps.planTime^2;
- end
-
- % Computes the actual input commands for the given time.
- % throws RTD:InvalidTrajectory if the trajectory isn't set
- % throws RTD:Trajectory:InvalidTime if the time is before the
- % trajectory exists
- % TODO: write in a vectorized manner
function command = getCommand(self, time)
+ % Gets the command for the given time.
+ %
+ % This function gets the command for the given time. The command is
+ % generated by calculating the position, velocity, and acceleration
+ % for the given time.
+ %
+ % Arguments:
+ % time (double): The time to get the command for.
+ %
+ % Returns:
+ % command (rtd.entity.states.ArmRobotStateInstance): The command for the given time.
+ %
+ % Raises:
+ % InvalidTrajectory: If the current trajectory is not valid.
+ % InvalidTime: If the time is invalid.
+ %
arguments
self armour.trajectory.PiecewiseArmTrajectory
time(1,:) double
@@ -124,7 +224,7 @@ function internalUpdate(self)
% Do a parameter check and time check, and throw if anything is
% invalid.
self.validate(true);
- t_shifted = time - self.startState.time;
+ t_shifted = time - self.startTime;
if t_shifted < 0
ME = MException('PiecewiseArmTrajectory:InvalidTrajectory', ...
'Invalid time provided to PiecewiseArmTrajectory');
@@ -132,18 +232,17 @@ function internalUpdate(self)
end
% Mask the first and second half of the trajectory
- t_plan_mask = t_shifted < self.trajOptProps.planTime;
- t_stop_mask = logical((t_shifted < self.trajOptProps.horizonTime) - t_plan_mask);
+ t_plan_mask = t_shifted <= self.planTime;
+ t_stop_mask = logical((t_shifted <= self.horizonTime) - t_plan_mask);
t_plan_vals = t_shifted(t_plan_mask);
- t_stop_vals = t_shifted(t_stop_mask) - self.trajOptProps.planTime;
+ t_stop_vals = t_shifted(t_stop_mask) - self.planTime;
% Create the combined state variable
t_size = length(time);
- n_q = self.jrsInstance.n_q;
- pos_idx = 1:n_q;
- vel_idx = pos_idx + n_q;
- acc_idx = vel_idx + n_q;
- state = zeros(n_q*3, t_size);
+ pos_idx = 1:self.numParams;
+ vel_idx = pos_idx + self.numParams;
+ acc_idx = vel_idx + self.numParams;
+ state = zeros(self.numParams*3, t_size);
% Rename variables
q_0 = self.startState.position;
@@ -173,9 +272,58 @@ function internalUpdate(self)
state(pos_idx,~(t_plan_mask+t_stop_mask)) = repmat(self.q_end, 1, t_size-sum(t_plan_mask+t_stop_mask));
% Generate the output.
- command = rtd.entity.states.ArmRobotState(pos_idx, vel_idx, acc_idx);
- command.time = time;
- command.state = state;
+ command(length(time)) = rtd.entity.states.ArmRobotStateInstance();
+ command.setTimes(time);
+ command.setStateSpace(state, ...
+ position_idxs=pos_idx, ...
+ velocity_idxs=vel_idx, ...
+ acceleration_idxs=acc_idx);
+ end
+ end
+
+ % Internal methods for the class
+ methods (Access=private)
+ function internalUpdate(self)
+ % Performs an internal update of the trajectory.
+ %
+ % This function performs an internal update of the trajectory. This
+ % includes calculating the bernstein coefficients and the final position.
+ % This function is called automatically when the trajectory parameters
+ % are set.
+ %
+
+ % internal update if valid. short circuit if not.
+ if ~self.validate()
+ return
+ end
+
+ % Rename variables
+ q_0 = self.startState.position;
+ q_dot_0 = self.startState.velocity;
+
+ % Scale the parameters if needed
+ self.q_ddot = self.trajectoryParams;
+ if isempty(self.paramScale)
+ self.q_ddot = self.trajectoryParams;
+ else
+ self.q_ddot = self.paramScale.scaleout(self.trajectoryParams);
+ end
+
+ % Compute the peak parameters
+ self.q_peak = q_0 + ...
+ q_dot_0 * self.planTime + ...
+ (1/2) * self.q_ddot * self.planTime^2;
+ self.q_dot_peak = q_dot_0 + ...
+ self.q_ddot * self.planTime;
+
+ % Compute the stopping parameters
+ stopping_time = (self.horizonTime - self.planTime);
+ self.q_ddot_to_stop = (0-self.q_dot_peak) / stopping_time;
+ self.q_end = self.q_peak + ...
+ self.q_dot_peak * stopping_time + ...
+ (1/2) * self.q_ddot_to_stop * stopping_time^2;
+
+ self.startTime = self.startState.time;
end
end
end
\ No newline at end of file
diff --git a/+armour/+trajectory/TwoBernsteinArmTrajectory.m b/+armour/+trajectory/TwoBernsteinArmTrajectory.m
new file mode 100644
index 00000000..aab8c28e
--- /dev/null
+++ b/+armour/+trajectory/TwoBernsteinArmTrajectory.m
@@ -0,0 +1,399 @@
+classdef TwoBernsteinArmTrajectory < rtd.trajectory.Trajectory
+% Trajectory utilizing two 5th Degree Bernstein Polynomials for arm robots
+%
+% This trajectory uses two 5th degree Bernstein polynomials to generate
+% a trajectory for arm robots. It is parameterized by the start state,
+% the plan time, the horizon time, and the desired final position. The
+% major bernstein polynomial is used to generate the trajectory from the
+% start state to the plan time, but is characterized by the horizon time.
+% The minor bernstein polynomial is used to generate the trajectory from
+% the plan time to the horizon time and takes the place of the final
+% position from the major bernstein polynomial.
+%
+% The trajectory is parameterized by the following:
+% * `startState`: The start state of the trajectory. This is used to
+% specify the initial position, velocity, and acceleration of the trajectory.
+% * `planTime`: The time at which the major trajectory should stop being executed, and
+% the minor trajectory should start being executed. This is used to
+% specify the active duration of the major trajectory.
+% * `horizonTime`: The time at which the overall trajectory should end. This is
+% used to specify the duration of the trajectory.
+% * `params`: The desired final position of each polynomial relative to (1) the start
+% position for the major polynomial and (2) the position at planTime for the
+% minor polynomial. The relative position can also be scaled, which then
+% paramScale is used to scale it back to the desired position.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-05-17
+% Updated: 2023-10-04 (Adam Li)
+%
+% See also: rtd.trajectory.Trajectory, armour.trajectory.ArmTrajectoryFactory,
+% rtd.entity.states.ArmRobotStateInstance, armour.trajectory.BernsteinArmTrajectory
+%
+% --- More Info ---
+%
+
+ % Required properties
+ properties (Constant)
+ % This does not support vectorized calls
+ vectorized = false
+ end
+
+ % Additional properties
+ properties
+ % The initial state of the trajectory. These are initial parameters
+ % from the robot used to calculate the desired bernstein coefficients
+ startState(1,1) rtd.entity.states.ArmRobotStateInstance
+
+ % The duration of the trajectory, after which the trajectory will
+ % hold the final position
+ planTime(1,1) double
+
+ % The duration of the trajectory, after which the trajectory will
+ % hold the final position
+ horizonTime(1,1) double
+
+ % Extra properties to scale the parameters
+ paramScale rtd.util.RangeScaler = rtd.util.RangeScaler.empty()
+
+ % The number of parameters for the trajectory
+ numParams(1,1) uint32
+ end
+
+ % Internal properties
+ properties (SetAccess=private)
+ % the alpha coefficients for the major bernstein polynomial
+ alpha(:,6) double
+
+ % the alpha coefficients for the minor bernstein polynomial
+ alpha2(:,6) double
+
+ % The calculated final position of the trajectory
+ q_end(:,1) double
+
+ % The position where the minor polynomial takes over
+ q_plan(:,1) double
+
+ % The velocity where the minor polynomial takes over
+ qd_plan(:,1) double
+
+ % The acceleration where the minor polynomial takes over
+ qdd_plan(:,1) double
+ end
+
+ methods
+ function self = TwoBernsteinArmTrajectory(startState, planTime, horizonTime, numParams)
+ % Constructor for the BernsteinArmTrajectory
+ %
+ % Arguments:
+ % startState: The start state of the trajectory. This is used to
+ % specify the initial position, velocity, and acceleration of the trajectory.
+ % planTime: The time at which the major trajectory should stop being executed, and
+ % the minor trajectory should start being executed. This is used to
+ % specify the active duration of the major polynomial.
+ % horizonTime: The time at which the overall trajectory should end. This is
+ % used to specify the duration of the overall trajectory.
+ % numParams: The number of parameters for the trajectory. This is
+ % used to specify the number of parameters for the trajectory.
+ %
+ arguments
+ startState(1,1) rtd.entity.states.ArmRobotStateInstance
+ planTime(1,1) double
+ horizonTime(1,1) double
+ numParams(1,1) uint32
+ end
+
+ self.startState = startState;
+ self.planTime = planTime;
+ self.horizonTime = horizonTime;
+ self.numParams = numParams;
+ end
+
+ function setParamScale(self, paramScale)
+ % Sets the parameter scaling
+ %
+ % Arguments:
+ % paramScale (rtd.util.RangeScaler): The parameter scaling to use.
+ % This is used to scale the relative position to the desired
+ % position. If not provided, then paramScale is reset to empty.
+ %
+ arguments
+ self armour.trajectory.TwoBernsteinArmTrajectory
+ paramScale(:,1) rtd.util.RangeScaler = []
+ end
+
+ if length(paramScale) > 1
+ error('Can only set one paramScale value!');
+ end
+ self.paramScale = paramScale;
+ end
+
+ function setParameters(self, trajectoryParams, options)
+ % Sets the parameters for the trajectory.
+ %
+ % After setting the parameters, it attempts to internally update the trajectory
+ % to reduce long term calculations.
+ %
+ % Arguments:
+ % trajectoryParams (double): The parameters for the trajectory. This is
+ % used to specify the relative position of the trajectory.
+ % The relative position can also be scaled, which then
+ % paramScale is used to scale it back to the desired position.
+ % options: Keyword arguments. See Below.
+ %
+ % Keyword Arguments:
+ % startState (rtd.entity.states.ArmRobotStateInstance): The start state of the trajectory. This is used to
+ % specify the initial position, velocity, and acceleration of the trajectory.
+ % planTime (double): The time at which the major trajectory should stop being executed, and
+ % the minor trajectory should start being executed. This is used to
+ % specify the active duration of the major polynomial.
+ % horizonTime (double): The time at which the overall trajectory should end. This is
+ % used to specify the duration of the overall trajectory.
+ % numParams (double): The number of parameters for the trajectory. This is
+ % used to specify the number of parameters for the trajectory.
+ % paramScale (rtd.util.RangeScaler): The parameter scaling to use.
+ % This is used to scale the relative position to the desired
+ % position.
+ %
+ arguments
+ self armour.trajectory.TwoBernsteinArmTrajectory
+ trajectoryParams(1,:) double
+ options.startState rtd.entity.states.ArmRobotStateInstance = self.startState
+ options.planTime(1,1) double = self.planTime
+ options.horizonTime(1,1) double = self.horizonTime
+ options.numParams(1,1) uint32 = self.numParams
+ options.paramScale(:,1) rtd.util.RangeScaler = self.paramScale
+ end
+
+ self.trajectoryParams = trajectoryParams;
+ self.startState = options.startState;
+ self.planTime = options.planTime;
+ self.horizonTime = options.horizonTime;
+ self.numParams = options.numParams;
+ if length(self.trajectoryParams) > self.numParams
+ self.trajectoryParams = self.trajectoryParams(1:self.numParams);
+ end
+ self.setParamScale(options.paramScale);
+
+ % Perform an internal update (compute peak and stopping values)
+ self.internalUpdate();
+ end
+
+ function valid = validate(self, throwOnError)
+ % Validates the trajectory parameters.
+ %
+ % This function ensures that the trajectory parameters are valid and fully
+ % specified. If the trajectory parameters are invalid, then it either throws
+ % an error or returns false.
+ %
+ % Arguments:
+ % throwOnError (logical, optional): If true, then throws an error if the
+ % trajectory parameters are invalid. Otherwise, returns false.
+ % Defaults to false.
+ %
+ % Returns:
+ % valid (logical): Whether or not the trajectory parameters are valid.
+ %
+ % Raises:
+ % InvalidTrajectory: If the current trajectory is not valid and
+ % throwOnError is true.
+ %
+ arguments
+ self armour.trajectory.TwoBernsteinArmTrajectory
+ throwOnError(1,1) logical = false
+ end
+
+ % Make sure everything is nonempty
+ valid = not( ...
+ isempty(self.trajectoryParams) || ...
+ isempty(self.startState));
+
+ % Make sure the trajectory params make sense
+ valid = valid && length(self.trajectoryParams) == self.numParams;
+ valid = valid && self.numParams/2 == length(self.startState.position);
+ valid = valid && self.planTime < self.horizonTime;
+
+ % Throw if wanted
+ if ~valid && throwOnError
+ errMsg = MException('TwoBernsteinArmTrajectory:InvalidTrajectory', ...
+ 'Called trajectory object does not have complete parameterization!');
+ throw(errMsg)
+ end
+ end
+
+ function command = getCommand(self, time)
+ % Gets the command for the given time.
+ %
+ % This function gets the command for the given time. The command is
+ % generated by calculating the position, velocity, and acceleration
+ % for the given time.
+ %
+ % Arguments:
+ % time (double): The time to get the command for.
+ %
+ % Returns:
+ % command (rtd.entity.states.ArmRobotStateInstance): The command for the given time.
+ %
+ % Raises:
+ % InvalidTrajectory: If the current trajectory is not valid.
+ % InvalidTime: If the time is invalid.
+ %
+ arguments
+ self armour.trajectory.TwoBernsteinArmTrajectory
+ time(1,:) double
+ end
+
+ % Do a parameter check and time check, and throw if anything is
+ % invalid.
+ self.validate(true);
+ t_shifted = time - self.startTime;
+ if any(t_shifted < 0)
+ ME = MException('TwoBernsteinArmTrajectory:InvalidTime', ...
+ 'Invalid time provided to TwoBernsteinArmTrajectory');
+ throw(ME)
+ end
+
+ % Get a mask for the active trajectory time and the stopped
+ % trajectory times.
+ % t_size = length(t_shifted);
+ % horizon_mask = t_shifted < self.horizonTime;
+ % t_masked_scaled = t_shifted(horizon_mask) / self.horizonTime;
+ % t_masked_size = length(t_masked_scaled);
+
+ % Original implementation adapted
+ q_des = zeros(self.numParams/2, 1);
+ q_dot_des = zeros(self.numParams/2, 1);
+ q_ddot_des = zeros(self.numParams/2, 1);
+
+ if t_shifted < self.planTime
+ scale_coeff = 1.0 / self.horizonTime;
+ t_scaled = t_shifted * scale_coeff;
+ alpha = self.alpha;
+ elseif t_shifted < self.horizonTime
+ scale_coeff = 1.0 / (self.horizonTime - self.planTime);
+ t_scaled = (t_shifted - self.planTime) * scale_coeff;
+ alpha = self.alpha2;
+ end
+
+ if t_shifted < self.horizonTime
+ for j = 1:self.numParams/2
+ for coeff_idx = 0:5
+ q_des(j,:) = q_des(j,:) + ...
+ alpha(j,coeff_idx+1) * t_scaled.^coeff_idx;
+ if coeff_idx > 0
+ q_dot_des(j,:) = q_dot_des(j,:) + ...
+ coeff_idx * alpha(j,coeff_idx+1) * t_scaled.^(coeff_idx-1);
+ end
+ if coeff_idx > 1
+ q_ddot_des(j,:) = q_ddot_des(j,:) + ...
+ (coeff_idx) * (coeff_idx-1) * alpha(j,coeff_idx+1) * t_scaled.^(coeff_idx-2);
+ end
+ end
+ end
+ q_dot_des = q_dot_des * scale_coeff;
+ q_ddot_des = q_ddot_des * scale_coeff^2;
+ else
+ q_des = self.q_end;
+ end
+
+ % Generate the output.
+ command = rtd.entity.states.ArmRobotStateInstance();
+ command.setTimes(time);
+ command.position = q_des;
+ command.velocity = q_dot_des;
+ command.acceleration = q_ddot_des;
+ end
+ end
+
+ % Internal methods for the class
+ methods (Access=private)
+ function internalUpdate(self)
+ % Performs an internal update of the trajectory.
+ %
+ % This function performs an internal update of the trajectory. This
+ % includes calculating the bernstein coefficients and the final position.
+ % This function is called automatically when the trajectory parameters
+ % are set.
+ %
+
+ % internal update if valid. short circuit if not.
+ if ~self.validate()
+ return
+ end
+
+ % Get the desired final position & scale parameters as needed
+ n_q = length(self.startState.position);
+ if isempty(self.paramScale)
+ scaled_params = self.trajectoryParams;
+ else
+ scaled_params = self.paramScale.scaleout(self.trajectoryParams);
+ end
+ q_goal = self.startState.position + scaled_params(1:self.numParams/2);
+
+ % Get major parameters
+ self.alpha = zeros(self.numParams/2, 6);
+ for j = 1:self.numParams/2 % Modified to use matrix instead of cells
+ beta = armour.legacy.match_deg5_bernstein_coefficients({...
+ self.startState.position(j); ...
+ self.startState.velocity(j); ...
+ self.startState.acceleration(j); ...
+ q_goal(j); ...
+ 0; 0}, self.horizonTime);
+ self.alpha(j,:) = cell2mat(armour.legacy.bernstein_to_poly(beta, 5));
+ end
+
+ % Compute middle position
+ q_plan = zeros(n_q, 1);
+ qd_plan = zeros(n_q, 1);
+ qdd_plan = zeros(n_q, 1);
+ for j = 1:n_q
+ for coeff_idx = 0:5
+ q_plan(j,:) = q_plan(j,:) + ...
+ self.alpha(j,coeff_idx+1) * (self.planTime).^coeff_idx;
+ if coeff_idx > 0
+ qd_plan(j,:) = qd_plan(j,:) + ...
+ coeff_idx * self.alpha(j,coeff_idx+1) * (self.planTime).^(coeff_idx-1);
+ end
+ if coeff_idx > 1
+ qdd_plan(j,:) = qdd_plan(j,:) + ...
+ (coeff_idx) * (coeff_idx-1) * self.alpha(j,coeff_idx+1) * (self.planTime).^(coeff_idx-2);
+ end
+ end
+ end
+ self.q_plan = q_plan;
+ self.qd_plan = qd_plan/(self.horizonTime);
+ self.qdd_plan = qdd_plan/(self.horizonTime)^2;
+
+
+ if ~isempty(self.paramScale)
+ q_goal2 = q_plan + scaled_params((self.numParams/2+1):end);
+ end
+
+ % Get minor parameters
+ self.alpha2 = zeros(self.numParams/2, 6);
+ for j = 1:self.numParams/2 % Modified to use matrix instead of cells
+ beta = armour.legacy.match_deg5_bernstein_coefficients({...
+ self.q_plan(j); ...
+ self.qd_plan(j); ...
+ self.qdd_plan(j); ...
+ q_goal2(j); ...
+ 0; 0}, self.horizonTime - self.planTime);
+ self.alpha2(j,:) = cell2mat(armour.legacy.bernstein_to_poly(beta, 5));
+ end
+ % Precompute end position
+ % Adapted Original
+ % End position should actually just be q_goal
+ %self.q_end = zeros(self.n_q, 1);
+ %for j = 1:n_q
+ % for coeff_idx = 0:5
+ % self.q_end(j) = self.q_end(j) + ...
+ % self.alpha(j,coeff_idx+1) * self.trajOptProps.horizonTime^coeff_idx;
+ % end
+ %end
+
+ self.q_end = q_goal2;
+ self.startTime = self.startState.time;
+ end
+ end
+end
\ No newline at end of file
diff --git a/+armour/+trajectory/ZeroHoldArmTrajectory.m b/+armour/+trajectory/ZeroHoldArmTrajectory.m
index 13f83a5e..13df544d 100644
--- a/+armour/+trajectory/ZeroHoldArmTrajectory.m
+++ b/+armour/+trajectory/ZeroHoldArmTrajectory.m
@@ -1,42 +1,102 @@
-classdef ZeroHoldArmTrajectory < rtd.planner.trajectory.Trajectory
- % ZeroHoldArmTrajectory
+classdef ZeroHoldArmTrajectory < rtd.trajectory.Trajectory
+% An arm trajectory that holds the arm at a fixed position and zero velocity/acceleration.
+%
+% This acts as the initial trajectory for the arm, and is used to hold the
+% arm in place while the robot is waiting for the user to start the
+% trajectory.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-01-05
+% Updated: 2023-09-12 (Adam Li)
+%
+% See also: rtd.trajectory.Trajectory, armour.trajectory.ArmTrajectoryFactory,
+% rtd.entity.states.ArmRobotStateInstance
+%
+% --- More Info ---
+%
+
% Required properties
properties (Constant)
+ % True as this trajectory supports vectorized computation
vectorized = true
end
+
% Additional properties
+ properties
+ % The initial state of the trajectory. This is the state that the
+ % trajectory will hold the arm position at.
+ startState(1,1) rtd.entity.states.ArmRobotStateInstance
+ end
+
methods
- % The ZeroHoldArmTrajectory constructor, which simply sets parameters and
- % attempts to call internalUpdate, a helper function made for this
- % class to update all other internal parameters once fully
- % parameterized.
function self = ZeroHoldArmTrajectory(startState)
+ % The constructor for the ZeroHoldArmTrajectory class.
+ %
+ % Arguments:
+ % startState: The initial state of the trajectory. This is the state that the
+ % trajectory will hold the arm position at.
+ %
arguments
- startState(1,1) rtd.entity.states.ArmRobotState
+ startState(1,1) rtd.entity.states.ArmRobotStateInstance
end
+
self.startState = startState;
+ self.startTime = self.startState.time;
end
% Set the parameters of the trajectory, with a focus on the
% parameters as the state should be set from the constructor.
function setParameters(self, trajectoryParams, options)
+ % Set the parameters of the trajectory.
+ %
+ % Arguments:
+ % trajectoryParams: The parameters of the trajectory, which is ignored
+ % for this trajectory.
+ % options: Keyword arguments. See below.
+ %
+ % Keyword Arguments:
+ % startState: The initial state of the trajectory. This is the state that the
+ % trajectory will hold the arm position at.
+ %
arguments
self armour.trajectory.ZeroHoldArmTrajectory
trajectoryParams(1,:) double
- options.startState rtd.entity.states.ArmRobotState = self.startState
+ options.startState rtd.entity.states.ArmRobotStateInstance = self.startState
end
+
self.trajectoryParams = trajectoryParams;
self.startState = options.startState;
+ self.startTime = self.startState.time;
end
% Validate that the trajectory is fully characterized
function valid = validate(self, throwOnError)
+ % Validates the trajectory parameters.
+ %
+ % This function ensures that the trajectory parameters are valid and fully
+ % specified. If the trajectory parameters are invalid, then it either throws
+ % an error or returns false.
+ %
+ % Arguments:
+ % throwOnError (logical, optional): If true, then throws an error if the
+ % trajectory parameters are invalid. Otherwise, returns false.
+ % Defaults to false.
+ %
+ % Returns:
+ % valid (logical): Whether or not the trajectory parameters are valid.
+ %
+ % Raises:
+ % InvalidTrajectory: If the current trajectory is not valid and
+ % throwOnError is true.
+ %
arguments
self armour.trajectory.ZeroHoldArmTrajectory
throwOnError(1,1) logical = false
end
+
% Make sure we actually have a robot state to work with.
- valid = ~isempty(self.startState);
+ valid = ~isempty(self.startState.position);
% Throw if wanted
if ~valid && throwOnError
@@ -46,12 +106,23 @@ function setParameters(self, trajectoryParams, options)
end
end
- % Computes the actual input commands for the given time.
- % throws RTD:InvalidTrajectory if the trajectory isn't set
- % throws RTD:Trajectory:InvalidTime if the time is before the
- % trajectory exists
- % TODO: write in a vectorized manner
function command = getCommand(self, time)
+ % Gets the command for the given time.
+ %
+ % This function gets the command for the given time. The command is
+ % generated by calculating the position, velocity, and acceleration
+ % for the given time.
+ %
+ % Arguments:
+ % time (double): The time to get the command for.
+ %
+ % Returns:
+ % command (rtd.entity.states.ArmRobotStateInstance): The command for the given time.
+ %
+ % Raises:
+ % InvalidTrajectory: If the current trajectory is not valid.
+ % InvalidTime: If the time is invalid.
+ %
arguments
self armour.trajectory.ZeroHoldArmTrajectory
time(1,:) double
@@ -60,22 +131,25 @@ function setParameters(self, trajectoryParams, options)
% Do a parameter check and time check, and throw if anything is
% invalid.
self.validate(true);
- if any(time < self.startState.time)
+ if any(time < self.startTime)
ME = MException('ZeroHoldArmTrajectory:InvalidTime', ...
'Invalid time provided to ZeroHoldArmTrajectory');
throw(ME)
end
% Make the state
- n_q = length(self.startState.q);
- state = repmat([self.startState.q;0], 1, length(time));
+ n_q = length(self.startState.position);
+ state = repmat([self.startState.position;0], 1, length(time));
pos_idx = 1:n_q;
acc_vel_idx = ones(1,n_q)+n_q;
% Generate the output.
- command = rtd.entity.states.ArmRobotState(pos_idx, acc_vel_idx, acc_vel_idx);
- command.time = time;
- command.state = state;
+ command(length(time)) = rtd.entity.states.ArmRobotStateInstance();
+ command.setTimes(time);
+ command.setStateSpace(state, ...
+ position_idxs=pos_idx, ...
+ velocity_idxs=acc_vel_idx, ...
+ acceleration_idxs=acc_vel_idx);
end
end
end
\ No newline at end of file
diff --git a/+armour/ArmourAgent.m b/+armour/ArmourAgent.m
index b70c81aa..5d95c5f5 100644
--- a/+armour/ArmourAgent.m
+++ b/+armour/ArmourAgent.m
@@ -83,7 +83,7 @@
function self = from_options(robot, params, options)
a = armour.agent.ArmourAgentInfo(robot, params, options.component_options.info);
- self = armour.ArmourAgent(a, optionsStruct=options);
+ self = armour.ArmourAgent(a, options=options);
end
end
@@ -98,7 +98,7 @@
components.dynamics = []
components.collision = []
components.visual = []
- optionsStruct.optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.components
options.component_options
options.component_logLevelOverride
@@ -110,7 +110,7 @@
override_options = rtd.sim.world.WorldEntity.get_componentOverrideOptions(components);
% Merge all options
- self.mergeoptions(optionsStruct.optionsStruct, options, override_options);
+ self.mergeoptions(optionsStruct.options, options, override_options);
% Set components and (Re)construct dependent components for
% consistency
@@ -130,7 +130,7 @@
function reset(self, optionsStruct, options)
arguments
self
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.component_options
options.component_logLevelOverride
options.verboseLevel
@@ -138,7 +138,7 @@ function reset(self, optionsStruct, options)
end
% Perform an internal update, then merge in options
self.getoptions();
- options = self.mergeoptions(optionsStruct, options);
+ options = self.mergeoptions(optionsStruct.options, options);
% reset all components
self.reset_components()
diff --git a/+armour/ArmourCudaPlanner.m b/+armour/ArmourCudaPlanner.m
index 2f9b0a76..50239b5c 100644
--- a/+armour/ArmourCudaPlanner.m
+++ b/+armour/ArmourCudaPlanner.m
@@ -101,7 +101,7 @@ function setTimeoutState(self, state)
function [trajectory, info] = planTrajectory(self, robotState, worldState, waypoint)
arguments
self armour.ArmourCudaPlanner
- robotState rtd.entity.states.ArmRobotState
+ robotState rtd.entity.states.ArmRobotStateInstance
worldState
waypoint
end
diff --git a/+armour/ArmourPlanner.m b/+armour/ArmourPlanner.m
index 366cfbdb..46843503 100644
--- a/+armour/ArmourPlanner.m
+++ b/+armour/ArmourPlanner.m
@@ -36,19 +36,26 @@
options.input_constraints_flag (1,1) logical
options.use_robust_input (1,1) logical
options.smooth_obs (1,1) logical
- options.traj_type {mustBeMember(options.traj_type,{'piecewise','bernstein'})}
+ options.traj_type {mustBeMember(options.traj_type,{'piecewise','bernstein','twobernstein'})}
options.verboseLevel (1,1) rtd.util.types.LogLevel
end
options = self.mergeoptions(options);
% Create our reachable sets
self.rsGenerators = struct;
- self.rsGenerators.jrs = armour.reachsets.JRSGenerator(robot, traj_type=options.traj_type, verboseLevel=options.verboseLevel);
+ switch options.traj_type
+ case 'piecewise'
+ self.rsGenerators.jrs = armour.reachsets.JRS.PiecewiseJRSGen(robot, verboseLevel=options.verboseLevel);
+ case 'bernstein'
+ self.rsGenerators.jrs = armour.reachsets.JRS.BernsteinJRSGen(robot, verboseLevel=options.verboseLevel);
+ case 'twobernstein'
+ self.rsGenerators.jrs = armour.reachsets.JRS.TwoBernsteinJRSGen(robot, verboseLevel=options.verboseLevel);
+ end
self.rsGenerators.fo = armour.reachsets.FOGenerator(robot, self.rsGenerators.jrs, smooth_obs=options.smooth_obs, verboseLevel=options.verboseLevel);
if options.input_constraints_flag
self.rsGenerators.irs = armour.reachsets.IRSGenerator(robot, self.rsGenerators.jrs, use_robust_input=options.use_robust_input, verboseLevel=options.verboseLevel);
- self.rsGenerators.jls = armour.reachsets.JLSGenerator(robot, self.rsGenerators.jrs, verboseLevel=options.verboseLevel);
end
+ self.rsGenerators.jls = armour.reachsets.JLSGenerator(robot, self.rsGenerators.jrs, verboseLevel=options.verboseLevel);
% Create the trajectoryFactory
self.trajectoryFactory = armour.trajectory.ArmTrajectoryFactory(trajOptProps, options.traj_type);
@@ -62,7 +69,6 @@
% Create the trajopt object.
self.trajopt = {rtd.planner.trajopt.RtdTrajOpt( ...
trajOptProps, ...
- robot, ...
self.rsGenerators, ...
self.objective, ...
self.optimizationEngine, ...
@@ -75,7 +81,7 @@
function [trajectory, info] = planTrajectory(self, robotState, worldState, waypoint)
arguments
self armour.ArmourPlanner
- robotState rtd.entity.states.ArmRobotState
+ robotState rtd.entity.states.ArmRobotStateInstance
worldState
waypoint
end
diff --git a/+armour/ArmourSimulation.m b/+armour/ArmourSimulation.m
index ec25d40d..22eceec2 100644
--- a/+armour/ArmourSimulation.m
+++ b/+armour/ArmourSimulation.m
@@ -1,157 +1,142 @@
classdef ArmourSimulation < rtd.sim.BaseSimulation & handle
+ % Inherited properties we want to define
properties
+ % We plan and run ARMOUR at half second intervals
simulation_timestep = 0.5
- world = struct
- world_by_uuid = struct
- entities
- %systems
- simulation_log rtd.util.containers.VarLogger = rtd.util.containers.VarLogger.empty()
end
+
+ % These are extra references to entities and systems stored in the world model just for convenience
properties
+ % Reference to the primary arm agent entity for convenience
agent
- obstacles
+
+ % Reference to the collision system for convenience
collision_system
+
+ % Reference to the visual system for convenience
visual_system
+
+ % Reference to the goal system for convenience
goal_system
end
- events
- PreStep
- Step
- PostStep
- NewObjectAdded
- end
+
+ % Simulation Methods
methods
% Important stuff to get started
+ % TODO breakout options
function self = ArmourSimulation(optionsStruct, options)
arguments
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.simulation_timestep
end
self.simulation_state = 'CONSTRUCTED';
end
-
- % Add object
- function add_object(self, object, options)
- arguments
- self armour.ArmourSimulation
- object
- options.isentity = false
- options.update_name = false
- options.collision = []
- options.visual = []
- end
-
- % Add the object to the world
- % Create a name for the object based on its classname if it
- % doesn't have a given name.
- name = object.name;
- if isempty(name)
- % Base classname
- name = object.classname;
- % Get number to append by summing a regexp. It returns the
- % index of the occurance in each string, but since it's
- % limited to the first word anyway, it'll be 1 or empty.
- search_string = ['^', char(name), '\d+$'];
- all_names = fieldnames(self.world);
- id = sum(cell2mat(regexp(all_names, search_string)));
- name = [char(name), num2str(id)];
- end
- if options.update_name
- object.update_name(name);
- end
- self.world.(name) = object;
-
- % Add to the entity list if it's an entity
- if options.isentity
- self.entities = [self.entities, {object}];
- % Add the collision component provided to the collision system
- if ~isempty(options.collision)
- self.collision_system.addObjects(dynamic=options.collision);
- end
- % Add the visualization component provided to the visual
- % system
- if ~isempty(options.visual)
- self.visual_system.addObjects(dynamic=options.visual);
- end
- % if it's not, check for and add to collision or visual
- else
- if ~isempty(options.collision)
- self.collision_system.addObjects(static=options.collision);
- end
- % Add the visualization component provided to the visual
- % system
- if ~isempty(options.visual)
- self.visual_system.addObjects(static=options.visual);
- end
- end
-
- % TODO setup custom event data to return the object added
- notify(self, 'NewObjectAdded')
- end
-
+
function setup(self, agent)
+ % Setup the entity and systems for the simulation
+ %
+ % Resets the simulation to a blank state and sets up the
+ % simulation with the given ArmourAgent. For this simulation,
+ % this must be called before initialize or import as the
+ % ArmourAgent class is currently not fully serializable.
+ %
+ % Arguments:
+ % agent: The ArmourAgent to use for the simulation
+ %
arguments
- self armour.ArmourSimulation
- agent armour.ArmourAgent
- end
- if self.simulation_state > "SETTING_UP"
- self.world = struct;
- self.entities = [];
+ self(1,1) armour.ArmourSimulation
+ agent(1,1) armour.ArmourAgent
end
+
self.simulation_state = 'SETTING_UP';
+
+ self.world = rtd.sim.world.WorldModel;
self.agent = agent;
+ self.world.addEntity(agent, type='dynamic');
+
% Initialize the visual and collision systems
self.visual_system = rtd.sim.systems.patch_visual.PatchVisualSystem;
self.collision_system = rtd.sim.systems.patch3d_collision.Patch3dCollisionSystem(time_discretization=0.01);
- %self.systems = {self.visual_system, self.collision_system};
-
- % add the agent
- self.add_object(agent, isentity=true, collision=agent.collision, visual=agent.visual);
+ self.world.addSystem(self.visual_system, 'visual_system');
+ self.world.addSystem(self.collision_system, 'collision_system');
+
+ self.collision_system.addObjects(dynamic=self.agent.collision)
+ self.visual_system.addObjects(dynamic=self.agent.visual)
-% % Create the base obstacles
-% base_creation_buffer = 0.025;
-% face_color = [0.5 0.5 0.5];
-% edge_color = [0 0 0];
-%
-% base_options.info.is_base_obstacle = true;
-% base_options.info.creation_buffer = base_creation_buffer;
-% base_options.visual.face_color = face_color;
-% base_options.visual.edge_color = edge_color;
-% optionsStruct.component_options = base_options;
-% base = rtd.entity.BoxObstacle.makeBox( [-0.0580; 0; 0.1778], ...
-% 2*[0.2794, 0.2794, 0.1778], ...
-% optionsStruct);
-% tower = rtd.entity.BoxObstacle.makeBox([-0.2359; 0; 0.6868], ...
-% 2*[0.1016, 0.1651, 0.3312], ...
-% optionsStruct);
-% head = rtd.entity.BoxObstacle.makeBox( [-0.0580; 0; 1.0816], ...
-% 2*[0.1651, 0.1397, 0.0635], ...
-% optionsStruct);
-% % Floor
-% floor_color = [0.9, 0.9, 0.9];
-% optionsStruct.component_options.visual.face_color = floor_color;
-% floor = rtd.entity.BoxObstacle.makeBox([-0.0331;0;0.005], ...
-% 2*[1.3598, 1.3598, 0.0025], ...
-% optionsStruct);
-%
-% % Add them to the world
-% for obs = [base, tower, head, floor]
-% self.add_object(obs, collision=obs.collision.getCollisionObject, visual=obs.visual);
-% end
+ self.goal_system = armour.deprecation.RandomArmConfigurationGoal(self.collision_system, self.agent);
+ self.world.addSystem(self.goal_system, 'goal_system');
% reset the log
% For other simulations, you might want to validate keys and
% manually add them to ensure the summary section works as
% expected.
self.simulation_log = rtd.util.containers.VarLogger(validate_keys=false);
-
+ self.simulation_time = 0;
self.simulation_state = 'SETUP_READY';
end
+ function import(self, filename)
+ % Import all configurations for the simulation world from an XML file
+ %
+ % This function will import all entities and systems from an
+ % XML file. This needs to be called after setup and will
+ % overwrite any existing entities and systems.
+ %
+ % Arguments:
+ % filename: The filename of the XML file to import
+ %
+ arguments
+ self(1,1) armour.ArmourSimulation
+ filename {mustBeFile}
+ end
+
+ if self.simulation_state > "INITIALIZING"
+ self.visual_system.close();
+ self.setup(self.agent);
+ end
+ self.simulation_state = 'INITIALIZING';
+ self.world.mergeFromXML(filename)
+
+ % Add collision objects to the collision system
+ static_collisions = self.world.getEntityComponent('collision', type='static');
+ static_collision_objects = cellfun(@(x)(static_collisions.(x).getCollisionObject()),fieldnames(static_collisions));
+
+ dynamic_collisions = self.world.getEntityComponent('collision', type='dynamic');
+ dynamic_collision_objects = cellfun(@(x)(dynamic_collisions.(x)),fieldnames(dynamic_collisions));
+
+ self.collision_system.addObjects(static=static_collision_objects, dynamic=dynamic_collision_objects);
+
+ % add visual objects to the visual system
+ static_visuals = self.world.getEntityComponent('visual', type='static');
+ static_visual_objects = cellfun(@(x)(static_visuals.(x)),fieldnames(static_visuals));
+
+ dynamic_visuals = self.world.getEntityComponent('visual', type='dynamic');
+ dynamic_visual_objects = cellfun(@(x)(dynamic_visuals.(x)),fieldnames(dynamic_visuals));
+
+ self.visual_system.addObjects(static=static_visual_objects, dynamic=dynamic_visual_objects);
+
+ % Add the collision system
+ self.visual_system.addObjects(static=self.goal_system);
+
+ % Reset the visuals and ready the sim
+ self.visual_system.redraw();
+ self.simulation_state = 'READY';
+ end
+
function initialize(self)
+ % Initialize the simulation
+ %
+ % This function will initialize the simulation. This needs to
+ % be called after setup and will ready the simulation for
+ % stepping.
+ %
+ arguments
+ self(1,1) armour.ArmourSimulation
+ end
+
if self.simulation_state > "INITIALIZING"
- %error("This simulation currently does not support reinitialization without resetup");
+ error("This simulation currently does not support reinitialization without resetup");
end
self.simulation_state = 'INITIALIZING';
@@ -162,70 +147,43 @@ function initialize(self)
start_tic = tic;
t_cur = toc(start_tic);
while randomizing && t_cur <= timeout
- self.agent.state.random_init();
+ self.agent.state.random_init(save_to_options=true);
proposal_obj = self.agent.collision.getCollisionObject();
% test it in the collision system
- [randomizing, pairs] = self.collision_system.checkCollisionObject(proposal_obj);
+ [randomizing, ~] = self.collision_system.checkCollisionObject(proposal_obj);
t_cur = toc(start_tic);
end
-% self.agent.state.reset(initial_position = [0,-pi/2,0,0,0,0,0]);
- % This is captured by the goal generator if we don't set anything as the
- % start.
% Create the random obstacles
n_obstacles = 3;
obstacle_size_range = [0.01 0.5] ; % [min, max] side length
creation_buffer = 0.05;
-% centers = [-0.0584, 0.1813, 0.4391;
-% 0.5333, -0.2291, 0.2884;
-% 0.2826, 0.5121, 0.2953];
-% side_lengthss = [0.3915, 0.0572, 0.1350;
-% 0.1760, 0.3089, 0.1013;
-% 0.1545, 0.2983, 0.0352];
world_bounds = [self.agent.info.reach_limits(1:2:6); self.agent.info.reach_limits(2:2:6)];
- for obs_num = 1:n_obstacles
- randomizing = true;
- start_tic = tic;
- t_cur = toc(start_tic);
- while randomizing && t_cur <= timeout
- % create center, side lengths
- center = ...
- rtd.random.deprecation.rand_range( world_bounds(1,:) + obstacle_size_range(2)/2,...
- world_bounds(2,:) - obstacle_size_range(2)/2 );
- side_lengths = ...
- rtd.random.deprecation.rand_range(obstacle_size_range(1),...
- obstacle_size_range(2),...
- [],[],...
- 1, 3); % 3 is the dim of the world in this case
- % Create obstacle
-% center = centers(:,obs_num);
-% side_lengths = side_lengthss(obs_num,:);
- optionsStruct = struct;
- optionsStruct.component_options.info.creation_buffer = creation_buffer;
- prop_obs = rtd.entity.BoxObstacle.makeBox(center, side_lengths, optionsStruct);
-
- % test it
- proposal_obj = prop_obs.collision.getCollisionObject(buffered=true);
-
- % test it in the collision system
- [randomizing, pairs] = self.collision_system.checkCollisionObject(proposal_obj);
- t_cur = toc(start_tic);
+ for i=1:n_obstacles
+ % Randomize an obstacle
+ [box, timed_out] = rtd.entity.BoxObstacle.randomizeBox(world_bounds, ...
+ obstacle_size_range, creation_buffer=creation_buffer, ...
+ collision_system=self.collision_system, timeout=timeout);
+ if timed_out
+ warning("Timeout occured when creating obstacle, obstacle may be overlapping with robot configuration!")
end
- % if it's good, we save the proposal_obj
- self.add_object(prop_obs, collision=prop_obs.collision.getCollisionObject, visual=prop_obs.visual);
+
+ % Add it
+ self.world.addEntity(box);
+ self.collision_system.addObjects(static=box.collision.getCollisionObject)
+ self.visual_system.addObjects(static=box.visual)
end
- % Create and add the goal
- self.goal_system = armour.deprecation.RandomArmConfigurationGoal(self.collision_system, self.agent);
-% goal_position = [2.19112372555967;0.393795848789382;-2.08886547149797;-1.94078143810946;-1.82357815033695;-1.80997964933365;2.12483409695310];
-% self.goal_system.reset();
-% self.goal_system.createGoal(goal_position);
+ % Create the goal
self.goal_system.reset();
self.goal_system.createGoal();
+ % Save the goal position (workaround for the deprecated goal
+ % system)
+ self.goal_system.reset(goal_position=self.goal_system.goal_position);
self.visual_system.addObjects(static=self.goal_system);
- % reset the agent
+ % reset the agent (We saved the position to options earlier)
self.agent.reset
% redraw
@@ -233,18 +191,14 @@ function initialize(self)
self.simulation_state = 'READY';
end
- function info = pre_step(self)
- self.simulation_state = 'PRE_STEP';
-
- % CALL PLANNER
- info = struct;
+ end
- % TODO setup custom event data to return the sim
- notify(self, 'PreStep')
- end
- function info = step(self)
- self.simulation_state = 'STEP';
-
+ % Protected methods for stepping
+ methods(Access=protected)
+ % Pre-step doesn't exist for this simulation
+
+ % Step the simulation
+ function info = step_impl(self)
% Update entities
agent_results = self.agent.update(self.simulation_timestep);
@@ -253,73 +207,21 @@ function initialize(self)
if collision
disp("Collision Detected, Breakpoint!")
- pause
+ keyboard
disp("Continuing")
end
info.agent_results = agent_results;
info.collision = collision;
info.contactPairs = contactPairs;
-
- % TODO setup custom event data to return the sim
- notify(self, 'Step')
end
- function info = post_step(self)
- self.simulation_state = 'POST_STEP';
+
+ % Post-step goal checks and visual updates
+ function info = post_step_impl(self)
% Check if goal was achieved
goal = self.goal_system.updateGoal(self.simulation_timestep);
pause_requested = self.visual_system.updateVisual(self.simulation_timestep);
info.goal = goal;
info.pause_requested = pause_requested;
-
- % TODO setup custom event data to return the sim
- notify(self, 'PostStep')
- end
- function summary(self, options)
- end
- function run(self, options)
- arguments
- self armour.ArmourSimulation
- options.max_steps = 1e8
- options.max_time = Inf
- options.pre_step_callback cell = {}
- options.step_callback cell = {}
- options.post_step_callback cell = {}
- options.stop_on_goal = true
- end
-
- % Build the execution order
- execution_queue = [ {@(self)self.pre_step()}, ...
- options.pre_step_callback, ...
- {@(self)self.step()}, ...
- options.step_callback, ...
- {@(self)self.post_step()}, ...
- options.post_step_callback ];
-
- steps = 0;
- start_tic = tic;
- t_cur = toc(start_tic);
- pause_time = 0;
- stop = false;
- while steps < options.max_steps && t_cur < options.max_time && ~stop
- % Iterate through all functions in the execution queue
- for fcn = execution_queue
- info = fcn{1}(self);
- % Automate logging here if wanted
- stop = stop || (isfield(info, 'stop') && info.stop);
- if options.stop_on_goal && isfield(info, 'goal') && info.goal
- stop = true;
- disp("Goal acheived!")
- end
- % Pause if requested
- if isfield(info, 'pause_requested') && info.pause_requested
- start_pause = tic;
- keyboard
- pause_time = pause_time + toc(start_pause);
- end
- end
- steps = steps + 1;
- t_cur = toc(start_tic) - pause_time;
- end
end
end
end
diff --git a/+demos/+box2d/BoxAgent.m b/+demos/+box2d/BoxAgent.m
new file mode 100644
index 00000000..643c1a4c
--- /dev/null
+++ b/+demos/+box2d/BoxAgent.m
@@ -0,0 +1,59 @@
+classdef BoxAgent < rtd.sim.world.WorldEntity & handle
+ properties
+ info = demos.box2d.BoxAgentInfo.empty()
+ state = rtd.entity.components.GenericEntityState.empty()
+ visual
+ end
+
+
+ methods(Static)
+ function options = defaultoptions()
+ options = rtd.sim.world.WorldEntity.baseoptions();
+ components.info = 'demos.box2d.BoxAgentInfo';
+ components.state = 'rtd.entity.components.GenericEntityState';
+ components.visual = 'demos.box2d.BoxAgentVisual';
+ options.components = components;
+ end
+ end
+
+
+ methods
+ % constructor
+ function self = BoxAgent(components, optionsStruct, options)
+ arguments
+ components.info = []
+ components.state = []
+ components.visual = []
+ optionsStruct.optionsStruct struct = struct()
+ options.components
+ options.component_options
+ end
+ % Retrieve options from provided components
+ override_options = rtd.sim.world.WorldEntity.get_componentOverrideOptions(components);
+
+ % Merge all options
+ self.mergeoptions(optionsStruct.optionsStruct, options, override_options);
+
+ % (Re)construct all components for consistency
+ self.construct_component('info');
+ self.construct_component('state', self.info);
+ self.construct_component('visual', self.info, self.state);
+
+ self.reset()
+ end
+
+
+ % reset parameters to what was set when first instantiated
+ function reset(self, optionsStruct, options)
+ arguments
+ self
+ optionsStruct.optionsStruct struct = struct()
+ options.components
+ options.component_options
+ end
+ self.mergeoptions(optionsStruct.optionsStruct, options);
+
+ self.reset_components()
+ end
+ end
+end
\ No newline at end of file
diff --git a/+demos/+box2d/BoxAgentInfo.m b/+demos/+box2d/BoxAgentInfo.m
new file mode 100644
index 00000000..b15625ce
--- /dev/null
+++ b/+demos/+box2d/BoxAgentInfo.m
@@ -0,0 +1,53 @@
+classdef BoxAgentInfo < rtd.entity.components.BaseInfoComponent & rtd.util.mixins.Options & handle
+ properties
+ dimension = 2
+ width {mustBePositive}
+ height {mustBePositive}
+ color(1,3) {mustBeInRange(color,0,1,"inclusive")}
+ end
+
+
+ methods (Static)
+ function options = defaultoptions()
+ % Configurable options for this component
+ options.width = 1;
+ options.height = 1;
+ options.color = [1 0 1]; % magenta
+ end
+ end
+
+
+ methods
+ % constructor
+ function self = BoxAgentInfo(optionsStruct, options)
+ arguments
+ optionsStruct.options struct = struct()
+ options.width {mustBePositive}
+ options.height {mustBePositive}
+ options.color(1,3) {mustBeInRange(options.color,0,1,"inclusive")}
+ end
+ self.mergeoptions(optionsStruct.options, options);
+
+ % initialize
+ self.reset();
+ end
+
+
+ % reset parameters to what was set when first instantiated
+ function reset(self, optionsStruct, options)
+ arguments
+ self
+ optionsStruct.options struct = struct()
+ options.width {mustBePositive}
+ options.height {mustBePositive}
+ options.color(1,3) {mustBeInRange(options.color,0,1,"inclusive")}
+ end
+ options = self.mergeoptions(optionsStruct.options, options);
+
+ % Save
+ self.width = options.width;
+ self.height = options.height;
+ self.color = options.color;
+ end
+ end
+end
\ No newline at end of file
diff --git a/+demos/+box2d/BoxAgentVisual.m b/+demos/+box2d/BoxAgentVisual.m
new file mode 100644
index 00000000..5205845d
--- /dev/null
+++ b/+demos/+box2d/BoxAgentVisual.m
@@ -0,0 +1,134 @@
+classdef BoxAgentVisual < rtd.sim.systems.patch_visual.PatchVisualObject & rtd.util.mixins.NamedClass & rtd.util.mixins.Options & handle
+ properties
+ box_info demos.box2d.BoxAgentInfo = demos.box2d.BoxAgentInfo.empty()
+ box_state rtd.entity.components.GenericEntityState = rtd.entity.components.GenericEntityState.empty()
+
+ patch_style(1,1) struct
+ plot_patch_data(1,1) struct
+
+ set_view_when_animating(1,1) logical
+ animation_view(1,1) int8
+
+ plot_data
+ end
+
+ methods (Static)
+ function options = defaultoptions()
+ options.face_color = [1 0 1];
+ options.face_opacity = 0.2;
+ options.edge_color = [0 0 0];
+ options.edge_opacity = 0.75;
+ options.edge_width = 1;
+ options.set_view_when_animating = false;
+ options.animation_view = 3;
+ options.verboseLevel = 'INFO';
+ options.name = '';
+ end
+ end
+
+ methods
+ function self = BoxAgentVisual(box_info,box_state_component,optionsStruct,options)
+ arguments
+ box_info demos.box2d.BoxAgentInfo
+ box_state_component rtd.entity.components.GenericEntityState
+ optionsStruct.options struct = struct()
+ options.face_opacity
+ options.face_color
+ options.edge_color
+ options.edge_opacity
+ options.edge_width
+ options.set_view_when_animating
+ options.animation_view
+ options.verboseLevel
+ options.name
+ end
+ options.face_color = box_info.color;
+ self.mergeoptions(optionsStruct.options, options);
+
+ self.box_info = box_info;
+ self.box_state = box_state_component;
+
+ self.reset();
+ end
+
+ function reset(self, optionsStruct, options)
+ arguments
+ self
+ optionsStruct.options struct = struct()
+ options.face_opacity
+ options.face_color
+ options.edge_color
+ options.edge_opacity
+ options.edge_width
+ options.set_view_when_animating
+ options.animation_view
+ options.verboseLevel
+ options.name
+ end
+ options = self.mergeoptions(optionsStruct.options, options);
+
+ % Set up verbose output
+ self.name = options.name;
+ self.set_vdisplevel(options.verboseLevel);
+
+ self.patch_style.FaceColor = options.face_color;
+ self.patch_style.FaceAlpha = options.face_opacity;
+ self.patch_style.EdgeColor = options.edge_color;
+ self.patch_style.EdgeAlpha = options.edge_opacity;
+ self.patch_style.LineWidth = options.edge_width;
+ self.set_view_when_animating = options.set_view_when_animating;
+ self.animation_view = options.animation_view;
+
+ self.create_plot_patch_data();
+
+ self.plot_data.body = [];
+ end
+ end
+
+ % Geometry Generation
+ methods
+ function create_plot_patch_data(self)
+ dim = self.box_info.dimension;
+ side_len = [self.box_info.width, self.box_info.height];
+ center = zeros(dim,1) ;
+
+ [F,V] = rtd.functional.geometry.make_box(side_len,center) ;
+
+ self.plot_patch_data.faces = F ;
+ self.plot_patch_data.vertices = V ;
+ end
+ end
+
+ % Plotting
+ methods
+ function plot(self, options)
+ arguments
+ self
+ options.time(1,1) double = self.box_state.time(end)
+ end
+
+ % Get the position
+ state = self.box_state.get_state(options.time);
+
+ % Shift the patch points
+ V = self.plot_patch_data.vertices + state.state_data.';
+ F = self.plot_patch_data.faces;
+
+ % Plot/update them
+ if self.isPlotDataValid('body')
+ self.plot_data.body.Faces = F ;
+ self.plot_data.body.Vertices = V ;
+ else
+ patch_data = patch('Faces',F,...
+ 'Vertices',V,...
+ self.patch_style) ;
+ self.plot_data.body = patch_data ;
+ end
+ end
+
+ % Compat
+ function plot_at_time(self,t)
+ self.plot(time=t);
+ end
+ end
+end
\ No newline at end of file
diff --git a/+demos/+box2d/README.md b/+demos/+box2d/README.md
new file mode 100644
index 00000000..33a83004
--- /dev/null
+++ b/+demos/+box2d/README.md
@@ -0,0 +1,45 @@
+# Box2D Agent example
+Can be found under scripts/armour/tasks
+
+## Relevant Scripts
+> ### BoxAgent
+> Class for creating a simple 2D Box Agent.
+>
+> **Input:** `BoxAgentInfo, GenericEntityState, BoxAgentVisual`
+>
+> **Methods:**
+> - `reset(options)`: - resets components (or override with option)
+> - `plot((time), (xlim), (ylim))`: - plot the graph in range x=xlim, y=ylim, at time=t
+> - `animate((start_time), (end_time), (speed), (fps))`: - animates from start_time to end_time at 1 second = speed*t
+
+> ### BoxAgentInfo
+> Class for storing relevant information related to the BoxAgent
+>
+> **Input:** `(width), (height), (color)`
+>
+> **Methods:**
+> - `reset(options)`: - resets info (or override with option)
+
+> ### BoxAgentVisual
+> Class to handle visualization of the BoxAgent. Uses stored info to change looks of the agent.
+>
+> **Input:** `BoxAgentInfo, GenericEntityState, (options deriving from PatchVisualObject)`
+>
+> **Methods:**
+> - `reset(options)`: - resets info (or override with option)
+> - `plot((time))`: - plots agent at time=time
+> - `plot_at_time(time)`: same as above
+
+## Usage
+*Note: make sure that you add "rtd-code-architecture-base" to path in the workspace and rehash*
+
+To use the BoxAgent, first create the BoxAgentInfo, GenericEntityState, and BoxAgentVisual objects.
+Initialize GenericEntityState with ntates=2 if not so already by default.
+Create a new BoxAgent using the created info, state, and visual components.
+Feel free to define the movements of the agent by using `boxagent.state.commit_state_data(time, [x y])`
+
+Finally, you can animate the defined movement of the agent using the `animate()` method under BoxAgent.
+By default it will animate at 30fps, 1second per 1t.
+You can also plot at a single moment in time using the `plot()` method under BoxAgent.
+
+There is a demo file `box2d_agent.m` for an example of the BoxAgent.
\ No newline at end of file
diff --git a/+rtd/+entity/+box_obstacle/BoxObstacleInfo.m b/+rtd/+entity/+box_obstacle/BoxObstacleInfo.m
index d38595df..797c48aa 100644
--- a/+rtd/+entity/+box_obstacle/BoxObstacleInfo.m
+++ b/+rtd/+entity/+box_obstacle/BoxObstacleInfo.m
@@ -35,13 +35,13 @@
methods
function self = BoxObstacleInfo(optionsStruct, options)
arguments
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.dimension
options.side_lengths
options.creation_buffer
options.is_base_obstacle
end
- self.mergeoptions(optionsStruct, options);
+ self.mergeoptions(optionsStruct.options, options);
% initialize
self.reset();
@@ -51,13 +51,13 @@
function reset(self, optionsStruct, options)
arguments
self
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.dimension
options.side_lengths
options.creation_buffer
options.is_base_obstacle
end
- options = self.mergeoptions(optionsStruct, options);
+ options = self.mergeoptions(optionsStruct.options, options);
% Validate
if isempty(options.dimension)
diff --git a/+rtd/+entity/+box_obstacle/BoxObstacleZonotope.m b/+rtd/+entity/+box_obstacle/BoxObstacleZonotope.m
index 20730e85..2debac95 100644
--- a/+rtd/+entity/+box_obstacle/BoxObstacleZonotope.m
+++ b/+rtd/+entity/+box_obstacle/BoxObstacleZonotope.m
@@ -36,7 +36,7 @@ function reset(self)
state = self.box_state.get_state(options.time);
end
- zono = self.base_zonotope + state.state;
+ zono = self.base_zonotope + state.state_data;
end
end
end
\ No newline at end of file
diff --git a/+rtd/+entity/+box_obstacle/BoxPatchCollision.m b/+rtd/+entity/+box_obstacle/BoxPatchCollision.m
index 8e9572db..0a811f91 100644
--- a/+rtd/+entity/+box_obstacle/BoxPatchCollision.m
+++ b/+rtd/+entity/+box_obstacle/BoxPatchCollision.m
@@ -77,7 +77,7 @@ function create_collision_check_patch_data(self)
end
% Compat for Patch3d
- shift = state.state.';
+ shift = state.state_data.';
if self.box_info.dimension == 2
shift = [shift, 0];
end
diff --git a/+rtd/+entity/+box_obstacle/BoxPatchVisual.m b/+rtd/+entity/+box_obstacle/BoxPatchVisual.m
index fdafc9b8..839e290a 100644
--- a/+rtd/+entity/+box_obstacle/BoxPatchVisual.m
+++ b/+rtd/+entity/+box_obstacle/BoxPatchVisual.m
@@ -37,7 +37,7 @@
arguments
box_info rtd.entity.box_obstacle.BoxObstacleInfo
box_state_component rtd.entity.components.GenericEntityState
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.face_color
options.face_opacity
options.edge_color
@@ -48,7 +48,7 @@
options.verboseLevel
options.name
end
- self.mergeoptions(optionsStruct, options);
+ self.mergeoptions(optionsStruct.options, options);
self.box_info = box_info;
self.box_state = box_state_component;
@@ -59,7 +59,7 @@
function reset(self, optionsStruct, options)
arguments
self
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.face_color
options.face_opacity
options.edge_color
@@ -70,7 +70,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;
@@ -121,7 +121,7 @@ function plot(self, options)
state = self.box_state.get_state(options.time);
% Shift the patch points
- V = self.plot_patch_data.vertices + state.state.';
+ V = self.plot_patch_data.vertices + state.state_data.';
F = self.plot_patch_data.faces;
% Plot/update them
diff --git a/+rtd/+entity/+components/BaseControllerComponent.m b/+rtd/+entity/+components/BaseControllerComponent.m
index 7ca5d330..7419681b 100644
--- a/+rtd/+entity/+components/BaseControllerComponent.m
+++ b/+rtd/+entity/+components/BaseControllerComponent.m
@@ -3,7 +3,7 @@
properties (Abstract)
robot_info rtd.entity.components.BaseInfoComponent
robot_state rtd.entity.components.BaseStateComponent
- trajectories
+ trajectories(1,1) rtd.trajectory.TrajectoryContainer
n_inputs uint32
end
methods (Abstract)
diff --git a/+rtd/+entity/+components/GenericEntityState.m b/+rtd/+entity/+components/GenericEntityState.m
index 6547888e..80fa9459 100644
--- a/+rtd/+entity/+components/GenericEntityState.m
+++ b/+rtd/+entity/+components/GenericEntityState.m
@@ -27,13 +27,13 @@
function self = GenericEntityState(entity_info, optionsStruct, options)
arguments
entity_info rtd.entity.components.BaseInfoComponent
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.initial_state
options.n_states
options.verboseLevel
options.name
end
- self.mergeoptions(optionsStruct, options);
+ self.mergeoptions(optionsStruct.options, options);
% Setup
self.entity_info = entity_info;
@@ -45,13 +45,13 @@
function reset(self, optionsStruct, options)
arguments
self rtd.entity.components.GenericEntityState
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.initial_state
options.n_states
options.verboseLevel
options.name
end
- options = self.mergeoptions(optionsStruct, options);
+ options = self.mergeoptions(optionsStruct.options, options);
% Set up verbose output
self.name = options.name;
@@ -101,21 +101,23 @@ function random_init(self, state_range, save_to_options)
end
end
- function state = get_state(self, time)
+ function state_out = get_state(self, time)
arguments
self rtd.entity.components.GenericEntityState
- time = self.time(end)
+ time(1,:) double = self.time(end)
end
- state = rtd.entity.states.EntityState();
-
% Default to the last time and state
- state.time = time;
- state.state = self.state(:,end);
+ state_data = repmat(self.state(:,end),1,length(time));
% If we can and need to interpolate the state, do it
- if ~(length(self.time) == 1 || time > self.time(end))
- state.state = interp1(self.time, self.state.', time).';
+ mask = time <= self.time(end);
+ if length(self.time) > 1 && any(mask)
+ state_data(:,mask) = interp1(self.time, self.state.', time).';
end
+
+ state_out(length(time)) = rtd.entity.states.GenericEntityStateInstance();
+ state_out.setTimes(time);
+ state_out.setStateSpace(state_data);
end
function commit_state_data(self,T_state,Z_state)
diff --git a/+rtd/+entity/+states/ArmRobotState.m b/+rtd/+entity/+states/ArmRobotState.m
deleted file mode 100644
index 88888719..00000000
--- a/+rtd/+entity/+states/ArmRobotState.m
+++ /dev/null
@@ -1,73 +0,0 @@
-classdef ArmRobotState < rtd.entity.states.EntityState
- % ArmRobotState
- % Information on the atomic state of the robot at a given point of
- % time. Each hard instance of this (unique object, not seperate
- % handles to the same underlying object) will have a unique uuid.
- properties
- position_indices (1,:) uint32
- velocity_indices (1,:) uint32
- acceleration_indices (1,:) uint32
- end
- properties (Dependent)
- q
- q_dot
- q_ddot
- q_des
- q_dot_des
- q_ddot_des
- position
- velocity
- acceleration
- end
-
- methods
- function self = ArmRobotState(position_indices, velocity_indices, acceleration_indices)
- arguments
- position_indices (1,:) uint32 = []
- velocity_indices (1,:) uint32 = []
- acceleration_indices (1,:) uint32 = []
- end
- self.position_indices = position_indices;
- self.velocity_indices = velocity_indices;
- self.acceleration_indices = acceleration_indices;
- end
-
-
- function position = get.position(self)
- position = self.state(self.position_indices,:);
- end
-
- function velocity = get.velocity(self)
- velocity = self.state(self.velocity_indices,:);
- end
-
- function acceleration = get.acceleration(self)
- acceleration = self.state(self.acceleration_indices,:);
- end
-
- % Compat
- function q = get.q_des(self)
- q = self.position;
- end
-
- function q_dot = get.q_dot_des(self)
- q_dot = self.velocity;
- end
-
- function q_ddot = get.q_ddot_des(self)
- q_ddot = self.acceleration;
- end
-
- function q = get.q(self)
- q = self.position;
- end
-
- function q_dot = get.q_dot(self)
- q_dot = self.velocity;
- end
-
- function q_ddot = get.q_ddot(self)
- q_ddot = self.acceleration;
- end
- end
-end
\ No newline at end of file
diff --git a/+rtd/+entity/+states/ArmRobotStateInstance.m b/+rtd/+entity/+states/ArmRobotStateInstance.m
new file mode 100644
index 00000000..9c7be777
--- /dev/null
+++ b/+rtd/+entity/+states/ArmRobotStateInstance.m
@@ -0,0 +1,172 @@
+classdef ArmRobotStateInstance < rtd.entity.states.BaseEntityStateInstance
+% Single atomic state instance for an generic arm robot with position, velocity, and acceleration.
+%
+% This is a concrete implementation of the BaseEntityStateInstance class.
+% It is used to represent the state of a generic arm robot with position,
+% velocity, and acceleration.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-12
+%
+% See also: rtd.entity.states.BaseEntityStateInstance
+%
+% --- More Info ---
+%
+
+ properties
+ % Position of each joint in an arm robot in configuration space
+ position(:,1) double
+
+ % Velocity of each joint in an arm robot in configuration space
+ velocity(:,1) double
+
+ % Acceleration of each joint in an arm robot in configuration space
+ acceleration(:,1) double
+ end
+
+ properties (Dependent)
+ % number of joints in the arm robot, based on the size of the position vector
+ num_joints
+
+ % expected size of a state space which captures this state instance
+ num_states
+ end
+
+ % methods that must be implemented
+ methods
+ function state = getStateSpace(self, options)
+ % Get the state space of the robot state instance
+ %
+ % Returns a matrix of the state space of the robot. The rows
+ % correspond to the state space indices provided in the options
+ % struct. The columns correspond to the individual robot state
+ % instances.
+ %
+ % Arguments:
+ % options.position_idxs (uint32) - desired indices of the position states
+ % options.velocity_idxs (uint32) - desired indices of the velocity states
+ % options.acceleration_idxs (uint32) - desired indices of the acceleration states
+ %
+ % Returns:
+ % state (double) - matrix of the state space of the robot for the state instances
+ %
+ arguments
+ self(1,:) rtd.entity.states.ArmRobotStateInstance
+ options.position_idxs(1,:) uint32 = []
+ options.velocity_idxs(1,:) uint32 = []
+ options.acceleration_idxs(1,:) uint32 = []
+ end
+
+ [pos_idx, vel_idx, acc_idx] = self(1).resolveStateSpaceIndices( ...
+ options.position_idxs, options.velocity_idxs, options.acceleration_idxs);
+ state = zeros(self(1).num_states, length(self));
+ state(pos_idx, :) = [self.position];
+ state(vel_idx, :) = [self.velocity];
+ state(acc_idx, :) = [self.acceleration];
+ end
+
+ function setStateSpace(self, state_data, options)
+ % Set the state space of the robot state instance
+ %
+ % Sets the state space of the robot. The rows correspond to the
+ % state space indices provided in the options struct. The columns
+ % correspond to the individual robot state instances.
+ %
+ % Arguments:
+ % state_data (double) - matrix of the state space of the robot for the state instances
+ % options.position_idxs (uint32) - indices of the position states in state_data
+ % options.velocity_idxs (uint32) - indices of the velocity states in state_data
+ % options.acceleration_idxs (uint32) - indices of the acceleration states in state_data
+ %
+ arguments
+ self(1,:) rtd.entity.states.ArmRobotStateInstance
+ state_data(:,:) double
+ options.position_idxs(1,:) uint32 = []
+ options.velocity_idxs(1,:) uint32 = []
+ options.acceleration_idxs(1,:) uint32 = []
+ end
+
+ pos_idx = options.position_idxs;
+ vel_idx = options.velocity_idxs;
+ acc_idx = options.acceleration_idxs;
+
+ pos_data_to_deal = num2cell(state_data(pos_idx, :), 1);
+ vel_data_to_deal = num2cell(state_data(vel_idx, :), 1);
+ acc_data_to_deal = num2cell(state_data(acc_idx, :), 1);
+ [self.position] = deal(pos_data_to_deal{:});
+ [self.velocity] = deal(vel_data_to_deal{:});
+ [self.acceleration] = deal(acc_data_to_deal{:});
+ end
+ end
+
+ % helper methods internal to the class
+ methods (Access=private)
+ function [pos_idx, vel_idx, acc_idx] = resolveStateSpaceIndices(self, pos_idx, vel_idx, acc_idx)
+ % resolveStateSpaceIndices - Resolve the state space indices
+ % Resolves the state space indices for the position, velocity,
+ % and acceleration. If the indices are not provided, then they
+ % are automatically resolved. If they are provided, then they
+ % must be valid and match the number of states in the state
+ % space.
+ bad_pos_idx = length(pos_idx) ~= length(self.position);
+ bad_vel_idx = length(vel_idx) ~= length(self.velocity);
+ bad_acc_idx = length(acc_idx) ~= length(self.acceleration);
+
+ need_pos_idx = isempty(pos_idx) && ~isempty(self.position);
+ need_vel_idx = isempty(vel_idx) && ~isempty(self.velocity);
+ need_acc_idx = isempty(acc_idx) && ~isempty(self.acceleration);
+
+ bad_count = bad_pos_idx + bad_vel_idx + bad_acc_idx;
+ need_count = need_pos_idx + need_vel_idx + need_acc_idx;
+
+ % Short circuit if all is good
+ if (bad_count + need_count) == 0
+ return
+ end
+
+ % Otherwise resolve some indices
+ if isempty(pos_idx) && isempty(vel_idx) && isempty(acc_idx)
+ pos_idx = 1:length(self.position);
+ vel_idx = (1:length(self.velocity)) + pos_idx(end);
+ acc_idx = (1:length(self.acceleration)) + vel_idx(end);
+
+ elseif bad_count ~= need_count
+ errMsg = MException('ArmRobotStateInstance:BadStateIndices', ...
+ 'Invalid / Unmatching state indices provided for state space');
+ throw(errMsg)
+
+ elseif need_count > 1
+ errMsg = MException('ArmRobotStateInstance:BadStateIndices', ...
+ 'Must provide enough indices for valid state space');
+ throw(errMsg)
+
+ elseif need_count
+ idx_arr = 1:self.num_states;
+ % Delete already known indices
+ idx_arr(pos_idx) = [];
+ idx_arr(vel_idx) = [];
+ idx_arr(acc_idx) = [];
+
+ if need_pos_idx
+ pos_idx = idx_arr;
+ elseif need_vel_idx
+ vel_idx = idx_arr;
+ else
+ acc_idx = idx_arr;
+ end
+ end
+ end
+ end
+
+ % methods for the dependent properties
+ methods
+ function n_joints = get.num_joints(self)
+ n_joints = length(self.position);
+ end
+
+ function n_states = get.num_states(self)
+ n_states = length(self.position) + length(self.velocity) + length(self.acceleration);
+ end
+ end
+end
\ No newline at end of file
diff --git a/+rtd/+entity/+states/BaseEntityStateInstance.m b/+rtd/+entity/+states/BaseEntityStateInstance.m
new file mode 100644
index 00000000..1e9161c6
--- /dev/null
+++ b/+rtd/+entity/+states/BaseEntityStateInstance.m
@@ -0,0 +1,89 @@
+classdef BaseEntityStateInstance < rtd.util.mixins.UUID & handle
+% BaseEntityStateInstance is the base class for all state instances
+%
+% This represents the state of some entity at a point in time. Each hard
+% instance of this (unique object, not seperate handles to the same
+% underlying object) will have a unique uuid.
+%
+% State in this case refers to configuration states for the entity, and
+% may not correspond to the physical position of the entity. For example,
+% this may store information about the extension of a robotic planar platform,
+% which would be used in combination with a kinematic model to determine
+% the actual position of the platform.
+%
+% This class is abstract, and should not be instantiated directly. Instead,
+% use one of the subclasses.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2022-08-24
+% Last Revised: 2023-09-12 (Adam Li)
+%
+% --- Revision History ---
+% 2023-09-12 - Added getTimes and setTimes. Time is forcefully set to a single value
+% 2023-09-08 - Removed state as a required property, and added getStateSpace / setStateSpace interface
+%
+% --- More Info ---
+%
+
+ properties
+ % time is a scalar double, and is the time at which this state
+ % instance was recorded
+ time(1,1) double
+ end
+
+ properties (Abstract, Dependent)
+ % num_states is the number of states in the state-space form
+ num_states
+ end
+
+ % Interface methods that must be implemented by subclasses
+ methods (Abstract)
+ % getStateSpace returns the state-space form of a vector of state
+ % instance, in the form of a matrix of size num_states x num_time
+ % where num_time is the number of time points in the state instance
+ state = getStateSpace(self, options)
+
+ % setStateSpace sets the state-space form of a vector of state instance,
+ % in the form of a matrix of size num_states x num_time
+ % where num_time is the number of time points in the state instance
+ setStateSpace(self, state, options)
+ end
+
+ % Base methods for time from an array of BaseEntityStateInstances
+ methods
+ function times = getTimes(self)
+ % get times for an array of BaseEntityStateInstances
+ %
+ % getTimes returns the times at which the vector elements of state
+ % instance was recorded, as a vector of size 1 x num_time
+ %
+ % Returns:
+ % times (double): times at which the state instance was recorded
+ %
+ arguments
+ self(1,:) rtd.entity.states.BaseEntityStateInstance
+ end
+
+ times = [self.time];
+ end
+
+ function setTimes(self, time_data)
+ % set times for an array of BaseEntityStateInstances
+ %
+ % setTimes sets the times at which the vector elements of state
+ % instance was recorded, as a vector of size 1 x num_time
+ %
+ % Arguments:
+ % time_data (double): times at which the state instance was recorded
+ %
+ arguments
+ self(1,:) rtd.entity.states.BaseEntityStateInstance
+ time_data(1,:) double
+ end
+
+ time_data_to_deal = num2cell(time_data,1);
+ [self.time] = deal(time_data_to_deal{:});
+ end
+ end
+end
\ No newline at end of file
diff --git a/+rtd/+entity/+states/EntityState.m b/+rtd/+entity/+states/EntityState.m
deleted file mode 100644
index f55154d2..00000000
--- a/+rtd/+entity/+states/EntityState.m
+++ /dev/null
@@ -1,11 +0,0 @@
-classdef EntityState < rtd.util.mixins.UUID & handle
- % RobotState
- % The state of a generic robot at a point in time, which is saved.
- % Each hard instance of this (unique object, not seperate handles to
- % the same underlying object) will have a unique uuid.
- properties
- time
- % An relevant robot properties that evolve over time
- state
- end
-end
\ No newline at end of file
diff --git a/+rtd/+entity/+states/GenericEntityStateInstance.m b/+rtd/+entity/+states/GenericEntityStateInstance.m
new file mode 100644
index 00000000..31d558c4
--- /dev/null
+++ b/+rtd/+entity/+states/GenericEntityStateInstance.m
@@ -0,0 +1,71 @@
+classdef GenericEntityStateInstance < rtd.entity.states.BaseEntityStateInstance
+% Single atomic state instance for an generic entity with some abitrary state
+%
+% This is a concrete implementation of the BaseEntityStateInstance class.
+% It is used to represent a single atomic state instance for an generic
+% entity with some abitrary state.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-12
+%
+% See also: rtd.entity.states.BaseEntityStateInstance
+%
+% --- More Info ---
+%
+
+ properties
+ % State data for the entity.
+ % This could be xyz position, quaternion, etc.
+ state_data(:,1) double
+ end
+
+ properties (Dependent)
+ % Number of states in the entity
+ num_states
+ end
+
+ % methods that must be implemented
+ methods
+ function state = getStateSpace(self)
+ % Get the state space of the entity
+ %
+ % This method returns the state space of the entity as a matrix.
+ % Each column of the matrix represents a single state instance.
+ %
+ % Returns:
+ % state (double): state space of the entity
+ %
+ arguments
+ self(1,:) rtd.entity.states.GenericEntityStateInstance
+ end
+
+ state = [self.state_data];
+ end
+
+ function setStateSpace(self, state_data)
+ % Set the state space of the entity
+ %
+ % This method sets the state space of the entity from a matrix.
+ % Each column of the matrix represents a single state instance.
+ %
+ % Parameters:
+ % state_data (double): state space of the entity
+ %
+ arguments
+ self(1,:) rtd.entity.states.GenericEntityStateInstance
+ state_data(:,:) double
+ end
+
+ state_data_to_deal = num2cell(state_data,1);
+ [self.state_data] = deal(state_data_to_deal{:});
+ end
+ end
+
+ % methods for dependent properties
+ methods
+ function n_states = get.num_states(self)
+ n_states = length(self.state_data);
+ end
+ end
+end
\ No newline at end of file
diff --git a/+rtd/+entity/BoxObstacle.m b/+rtd/+entity/BoxObstacle.m
index 286ff951..5f0e1c08 100644
--- a/+rtd/+entity/BoxObstacle.m
+++ b/+rtd/+entity/BoxObstacle.m
@@ -59,13 +59,54 @@
arguments
center
side_lengths
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
end
component_options.info.side_lengths = side_lengths;
component_options.state.initial_state = center;
- box = rtd.entity.BoxObstacle(optionsStruct=optionsStruct, ...
+ box = rtd.entity.BoxObstacle(options=optionsStruct.options, ...
component_options=component_options);
end
+
+ function [box, timed_out] = randomizeBox(world_bounds, size_range, options)
+ arguments
+ world_bounds(2,3) double
+ size_range(1,2) double
+ options.creation_buffer(1,1) double = 0
+ options.collision_system(:,1) = []
+ options.timeout(1,1) double = 10
+ options.extra_options(1,1) struct = struct()
+ end
+
+ randomizing = true;
+ start_tic = tic;
+ t_cur = toc(start_tic);
+ while randomizing && t_cur <= options.timeout
+ % create center, side lengths
+ center = ...
+ rtd.random.deprecation.rand_range( world_bounds(1,:) + size_range(2)/2,...
+ world_bounds(2,:) - size_range(2)/2 );
+ side_lengths = ...
+ rtd.random.deprecation.rand_range(size_range(1),...
+ size_range(2),...
+ [],[],...
+ 1, 3); % 3 is the dim of the world in this case
+ % Create obstacle
+ optionsStruct = options.extra_options;
+ optionsStruct.component_options.info.creation_buffer = options.creation_buffer;
+ prop_box = rtd.entity.BoxObstacle.makeBox(center, side_lengths, options=optionsStruct);
+
+ % test it if we have a collision system
+ if ~isempty(options.collision_system)
+ proposal_collision = prop_box.collision.getCollisionObject(buffered=true);
+ [randomizing, ~] = options.collision_system.checkCollisionObject(proposal_collision);
+ else
+ randomizing = false;
+ end
+ t_cur = toc(start_tic);
+ end
+ box = prop_box;
+ timed_out = t_cur > options.timeout;
+ end
end
methods
@@ -77,7 +118,7 @@
components.collision = []
components.visual = []
components.representation = []
- optionsStruct.optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.components
options.component_options
options.component_logLevelOverride
@@ -88,7 +129,7 @@
override_options = rtd.sim.world.WorldEntity.get_componentOverrideOptions(components);
% Merge all options
- self.mergeoptions(optionsStruct.optionsStruct, options, override_options);
+ self.mergeoptions(optionsStruct.options, options, override_options);
% (Re)construct all components for consistency
self.construct_component('info');
@@ -105,7 +146,7 @@
function reset(self, optionsStruct, options)
arguments
self
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.component_options
options.component_logLevelOverride
options.verboseLevel
@@ -113,7 +154,7 @@ function reset(self, optionsStruct, options)
end
% Perform an internal update, then merge in options
self.getoptions();
- options = self.mergeoptions(optionsStruct, options);
+ options = self.mergeoptions(optionsStruct.options, options);
% reset all components
self.reset_components()
diff --git a/+rtd/+functional/htmldecode.m b/+rtd/+functional/htmldecode.m
new file mode 100644
index 00000000..92cf69d2
--- /dev/null
+++ b/+rtd/+functional/htmldecode.m
@@ -0,0 +1,20 @@
+function decodedout = htmldecode(in_string)
+%HTMLDECODE Decode HTML entities
+%
+% DECODEDOUT = HTMLDECODE(IN_STRING) decodes the HTML entities in
+% IN_STRING and returns the decoded text in DECODEDOUT.
+%
+% Example:
+% htmldecode('AT&T')
+%
+% See also rtd.functional.htmlencode
+%
+ arguments
+ in_string {mustBeTextScalar}
+ end
+
+ parser = matlab.io.xml.dom.Parser;
+ encoded = ['',char(in_string),''];
+ out = parser.parseString(encoded);
+ decodedout = out.Children.TextContent;
+end
diff --git a/+rtd/+functional/htmlencode.m b/+rtd/+functional/htmlencode.m
new file mode 100644
index 00000000..1a1370fa
--- /dev/null
+++ b/+rtd/+functional/htmlencode.m
@@ -0,0 +1,21 @@
+function encodedout = htmlencode(in_string)
+% HTMLENCODE Encode string for HTML display
+%
+% ENCODEDOUT = HTMLENCODE(IN_STRING) encodes the string IN_STRING for
+% HTML display. The output ENCODEDOUT is a string that can be displayed
+% in a browser.
+%
+% Example:
+% htmlencode('MATLAB')
+%
+% See also rtd.functional.htmldecode
+%
+ arguments
+ in_string {mustBeTextScalar}
+ end
+
+ dummydoc = matlab.io.xml.dom.Document('dummydoc');
+ textNode = createTextNode(dummydoc, in_string);
+ writer = matlab.io.xml.dom.DOMWriter;
+ encodedout = writer.writeToString(textNode);
+end
diff --git a/+rtd/+functional/node2struct.m b/+rtd/+functional/node2struct.m
new file mode 100644
index 00000000..9a1ef889
--- /dev/null
+++ b/+rtd/+functional/node2struct.m
@@ -0,0 +1,165 @@
+function [struct_out, root_name, attributes_struct] = node2struct(rootNode)
+% Converts a DOM object to a struct
+%
+% This is a function that converts a DOM object to a struct. The struct
+% will have the same structure as the XML file. This is useful for
+% converting XML files to a struct for use in MATLAB, and is the
+% counterpart to struct2node.
+%
+% Usage:
+% parser = matlab.io.xml.dom.Parser;
+% dom = parser.parseFile('example.xml');
+% rootNode = dom.getDocumentElement();
+% [struct_out, root_name, attributes_struct] = rtd.functional.node2struct(rootNode)
+%
+% Arguments
+% node - A DOM node object
+%
+% Returns:
+% struct_out - A struct with the same structure as the XML file's node
+% root_name - The name of the root element
+% attributes_struct - Attributes associated with the root element
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-14
+%
+% See Also: rtd.functional.struct2node
+%
+% --- More Info ---
+%
+ arguments
+ rootNode(1,1) matlab.io.xml.dom.Node
+ end
+
+% rootNode = dom.getDocumentElement();
+ root_name = rootNode.TagName;
+ attributes_struct = struct;
+ for i=0:rootNode.getAttributes().getLength()-1
+ attr_node = rootNode.getAttributes().item(i);
+ attributes_struct.(attr_node.Name) = attr_node.TextContent;
+ end
+ struct_out = parseStruct(rootNode);
+end
+
+% --- Helper Functions ---
+
+function struct_out = parseStruct(element)
+% Parses a struct from an element
+%
+% This is a helper function that parses a struct from an element. It
+% recursively calls itself to parse sub-structs.
+%
+% Arguments:
+% element - A DOM element object
+%
+% Returns:
+% struct_out - A struct with the same structure as the XML file's node
+%
+ arguments
+ element(1,1) matlab.io.xml.dom.Node
+ end
+
+ struct_out = struct;
+ for child=element.Children
+ if isa(child, 'matlab.io.xml.dom.Text')
+ % We don't expect any plain text nodes here.
+ % AKA ignore whitespace
+ continue
+ end
+ [key, value] = parseElement(child);
+ struct_out.(key) = value;
+ end
+end
+
+function cells_out = parseCells(element, size_vec)
+% Parses a cell array from an element
+%
+% This is a helper function that parses a cell array from an element. It
+% recursively calls itself to parse sub-cells.
+%
+% Arguments:
+% element - A DOM element object
+% size_vec - The size of the cell array
+%
+% Returns:
+% cells_out - A cell array with the same structure as the XML file's node
+%
+ arguments
+ element(1,1) matlab.io.xml.dom.Node
+ size_vec(1,:) double
+ end
+
+ cells_out = cell(prod(size_vec),1);
+ idx = 1;
+ for child=element.Children
+ if isa(child, 'matlab.io.xml.dom.Text')
+ % We don't expect any plain text nodes here.
+ % AKA ignore whitespace
+ continue
+ end
+ [~, subvalue] = parseElement(child);
+ cells_out{idx} = subvalue;
+ idx = idx + 1;
+ end
+ cells_out = reshape(cells_out, size_vec);
+end
+
+function [key, value] = parseElement(element)
+% Parses an element
+%
+% This is a helper function that parses an element. It determines the type
+% of the element and calls the appropriate function to parse it.
+%
+% Arguments:
+% element - A DOM element object
+%
+% Returns:
+% key - The name of the element
+% value - The value of the element
+%
+ arguments
+ element(1,1) matlab.io.xml.dom.Node
+ end
+
+ % Get the type of the element
+ classtype = element.TagName;
+ key = element.getAttribute('name');
+ size_vec = element.getAttribute('size');
+ % If the size is empty, then it's a scalar
+ if isempty(size_vec)
+ size_vec = [1,1];
+ else
+ size_vec = jsondecode(size_vec).';
+ end
+
+ % Get the empty type for dispatching, also get the type handle
+ emptytype = feval([classtype, '.empty']);
+ typehandle = str2func(classtype);
+
+ % Parse the element
+ if isstruct(emptytype)
+ value = parseStruct(element);
+
+ elseif iscell(emptytype)
+ value = parseCells(element, size_vec);
+
+ elseif ischar(emptytype)
+ value = element.TextContent;
+
+ elseif isstring(emptytype)
+ value = jsondecode(element.TextContent);
+ value = string(value);
+ value = reshape(value, size_vec);
+
+ elseif isnumeric(emptytype) || islogical(emptytype)
+ % Decode and reshape the value
+ value = jsondecode(element.TextContent);
+ value = reshape(value, size_vec);
+ % Convert the type
+ value = typehandle(value);
+
+ else
+ error(['Encountered Unsupported Type ', classtype, '!'])
+ end
+end
\ No newline at end of file
diff --git a/+rtd/+functional/struct2node.m b/+rtd/+functional/struct2node.m
new file mode 100644
index 00000000..50cc988c
--- /dev/null
+++ b/+rtd/+functional/struct2node.m
@@ -0,0 +1,196 @@
+function node = struct2node(struct_in, document, root_name, attributes_struct)
+% Convert a struct to a DOM object
+%
+% This is a function that converts a struct to a DOM object. This is
+% useful for writing out a struct to an XML file. The resulting DOM object
+% can be conversely converted back to a struct using the node2struct
+% function.
+%
+% Usage:
+% dom = matlab.io.xml.dom.Document('example_dom');
+% old_rootNode = dom.getDocumentElement();
+% new_rootNode = rtd.functional.struct2node(struct_in, dom, root_name, attributes_struct);
+% dom.replaceChild(new_rootNode, old_rootNode)
+% writer = matlab.io.xml.dom.DOMWriter;
+% writer.writeToFile(dom, 'example.xml');
+%
+% Arguments:
+% struct_in: The struct to convert to a DOM object
+% document: The document that DOM object is associated with
+% root_name: The name of the root node to store in the DOM object
+% attributes_struct: Attributes to associate with the root element
+%
+% Returns:
+% node: An element node created for the given document
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-14
+%
+% See also: rtd.functional.node2struct
+%
+% --- More Info ---
+%
+ arguments
+ struct_in(1,1) struct
+ document(1,1) matlab.io.xml.dom.Document
+ root_name {mustBeTextScalar} = 'root_node'
+ attributes_struct(1,1) struct = struct('version', '0.1')
+ end
+
+% doc = matlab.io.xml.dom.Document(root_name);
+% rootNode = doc.getDocumentElement();
+ node = document.createElement(root_name);
+ for name_cell=fieldnames(attributes_struct).'
+ name = name_cell{1};
+ node.setAttribute(name, attributes_struct.(name));
+ end
+ processStruct(document, node, struct_in);
+% dom = doc;
+end
+
+% --- Helper Functions ---
+
+function rootNode = processStruct(document, rootNode, struct_to_parse)
+% Process a struct into a DOM object node
+%
+% This is a function that converts a struct to a DOM object. This is
+% useful for writing out a struct to an XML file. The resulting DOM object
+% can be conversely converted back to a struct using the node2struct
+% function.
+%
+% Arguments:
+% document: The document that DOM object is associated with
+% rootNode: The root node to append to
+% struct_to_parse: The struct to convert to a DOM object
+%
+% Returns:
+% rootNode: The root node with the struct appended
+%
+ arguments
+ document(1,1) matlab.io.xml.dom.Document
+ rootNode(1,1) matlab.io.xml.dom.Node
+ struct_to_parse(1,1) struct
+ end
+
+ % Parse through the struct
+ keys = fieldnames(struct_to_parse).';
+ for key_cell=keys
+ key = key_cell{1};
+ val = struct_to_parse.(key);
+ % Create our node
+ node = createNode(document, key, val);
+ if ~isempty(node)
+ rootNode.appendChild(node);
+ end
+ end
+end
+
+function processDispatcher(document, node, val)
+% Dispatches the processing of a value to the appropriate function
+%
+% This is a function that dispatches the processing of a value to the
+% appropriate function. It serializes the value to a string and then
+% assigns it to the node.
+%
+% Arguments:
+% document: The document that DOM object is associated with
+% node: The node to append to
+% val: The value to process
+%
+ arguments
+ document(1,1) matlab.io.xml.dom.Document
+ node(1,1) matlab.io.xml.dom.Node
+ val
+ end
+
+ if isstruct(val)
+ processStruct(document, node, val);
+
+ elseif iscell(val)
+ processCell(document, node, val);
+
+ elseif ischar(val)
+ node.setTextContent(val)
+
+ elseif isnumeric(val) || islogical(val) || isstring(val)
+ val = val(:);
+ node.setTextContent(jsonencode(val, ConvertInfAndNaN=false));
+
+ else
+ error(['Encountered Unsupported Type ', class(val), '!'])
+ end
+end
+
+function rootNode = processCell(document, rootNode, cells_to_parse)
+% Process a cell into a DOM object node
+%
+% This is a function that converts a cell to a DOM object by converting
+% each cell entry to a DOM object.
+%
+% Arguments:
+% document: The document that DOM object is associated with
+% rootNode: The root node to append to
+% cells_to_parse: The cell to convert to a DOM object
+%
+% Returns:
+% rootNode: The root node with the cell appended
+%
+ arguments
+ document(1,1) matlab.io.xml.dom.Document
+ rootNode(1,1) matlab.io.xml.dom.Node
+ cells_to_parse cell
+ end
+
+ % Flatten the cells
+ cells_to_parse = cells_to_parse(:).';
+ for cell_entry=cells_to_parse
+ val = cell_entry{1};
+ % Create our node with an empty name
+ node = createNode(document, '', val);
+ if ~isempty(node)
+ rootNode.appendChild(node);
+ end
+ end
+end
+
+function node = createNode(document, name, val)
+% Create a node for a given value
+%
+% This is a function that creates a node for a given value. It adds the
+% appropriate attributes to the node and then dispatches the processing of
+% the value to the appropriate function.
+%
+% Arguments:
+% document: The document that DOM object is associated with
+% name: The name of the node to create
+% val: The value to process
+%
+% Returns:
+% node: The node created for the given value
+%
+ arguments
+ document(1,1) matlab.io.xml.dom.Document
+ name {mustBeTextScalar}
+ val
+ end
+
+ % Create our node if we have a value
+ if numel(val) == 0
+ node = [];
+ return
+ end
+ node = document.createElement(class(val));
+
+ % Add a name attribute if we have one
+ if ~isempty(name)
+ node.setAttribute('name', name);
+ end
+
+ % Add the size attribute if we have a non-scalar value
+ if numel(val) > 1
+ node.setAttribute('size', jsonencode(size(val)));
+ end
+
+ processDispatcher(document, node, val);
+end
\ No newline at end of file
diff --git a/+rtd/+planner/+reachsets/ReachSetGenerator.m b/+rtd/+planner/+reachsets/ReachSetGenerator.m
index d2434213..5efcfd72 100644
--- a/+rtd/+planner/+reachsets/ReachSetGenerator.m
+++ b/+rtd/+planner/+reachsets/ReachSetGenerator.m
@@ -43,7 +43,9 @@
% varargin: Additional optional arguments if desired. Will always be Name-Value arguments.
%
% Returns:
- % rtd.planner.reachsets.ReachSetInstance: A new instance of some derived version of `ReachSetInstance`
+ % struct: A struct array containing the reachable set for the robot state
+ % provided, where reachableSet.rs is an instance of some derived version
+ % of `ReachSetInstance` and reachableSet.id is the id of the reachable set.
%
reachableSet = generateReachableSet(self, robotState, varargin)
end
@@ -132,6 +134,11 @@
% generate and store.
reachableSet = generateReachableSet(self, robotState, passthrough_args{:});
+ % verify the output
+ names = fieldnames(reachableSet);
+ if ~any([contains(names, 'id'), contains(names, 'rs')])
+ error('Expected return with id and rs fields!')
+ end
self.cache{self.cache_index, 1} = hash;
self.cache{self.cache_index, 2} = reachableSet;
end
diff --git a/+rtd/+planner/+trajopt/GenericArmObjective.m b/+rtd/+planner/+trajopt/GenericArmObjective.m
index e310c86e..0d29ac5a 100644
--- a/+rtd/+planner/+trajopt/GenericArmObjective.m
+++ b/+rtd/+planner/+trajopt/GenericArmObjective.m
@@ -34,11 +34,11 @@
%
% Arguments:
% trajOptProps (rtd.planner.trajopt.TrajOptProps)
- % trajectoryFactory (rtd.planner.trajectory.TrajectoryFactory)
+ % trajectoryFactory (rtd.trajectory.TrajectoryFactory)
%
arguments
trajOptProps rtd.planner.trajopt.TrajOptProps
- trajectoryFactory rtd.planner.trajectory.TrajectoryFactory
+ trajectoryFactory rtd.trajectory.TrajectoryFactory
end
self.trajectoryFactory = trajectoryFactory;
self.t_cost = trajOptProps.timeForCost;
@@ -79,5 +79,5 @@
function [cost] = evalTrajectory(trajectoryParams, trajectoryObj, q_des, t_cost)
trajectoryObj.setParameters(trajectoryParams);
plan = trajectoryObj.getCommand(t_cost);
- cost = sum((plan.q_des - q_des).^2);
+ cost = sum((plan.position - q_des).^2);
end
\ No newline at end of file
diff --git a/+rtd/+planner/+trajopt/RtdTrajOpt.m b/+rtd/+planner/+trajopt/RtdTrajOpt.m
index 06c40c08..ddeb76e4 100644
--- a/+rtd/+planner/+trajopt/RtdTrajOpt.m
+++ b/+rtd/+planner/+trajopt/RtdTrajOpt.m
@@ -22,7 +22,6 @@
%
properties
trajOptProps rtd.planner.trajopt.TrajOptProps %
- robot %
reachableSets %
objective %
optimizationEngine %
@@ -31,7 +30,6 @@
methods
function self = RtdTrajOpt( ...
trajOptProps, ...
- robot, ...
reachableSets, ...
objective, ...
optimizationEngine, ...
@@ -51,7 +49,7 @@
% reachableSets (struct)
% objective (rtd.planner.trajopt.Objective)
% optimizationEngine (rtd.planner.trajopt.OptimizationEngine)
- % trajectoryFactory (rtd.planner.trajectory.TrajectoryFactory)
+ % trajectoryFactory (rtd.trajectory.TrajectoryFactory)
% options: Keyword arguments. See below.
%
% Keyword Arguments
@@ -59,15 +57,13 @@
%
arguments
trajOptProps (1,1) rtd.planner.trajopt.TrajOptProps
- robot (1,1) rtd.sim.world.WorldEntity
reachableSets (1,1) struct
objective (1,1) rtd.planner.trajopt.Objective
optimizationEngine (1,1) rtd.planner.trajopt.OptimizationEngine
- trajectoryFactory (1,1) rtd.planner.trajectory.TrajectoryFactory
+ trajectoryFactory (1,1) rtd.trajectory.TrajectoryFactory
options.verboseLevel (1,1) rtd.util.types.LogLevel = 'DEBUG'
end
self.trajOptProps = trajOptProps;
- self.robot = robot; % this might go
self.reachableSets = reachableSets;
self.objective = objective;
self.optimizationEngine = optimizationEngine;
@@ -100,11 +96,11 @@
% robotState: State of the robot.
% worldState: Observed state of the world for the reachable sets
% waypoint: Waypoint we want to optimize to
- % initialGuess (rtd.planner.trajectory.Trajectory): Past trajectory to use as an initial guess
+ % initialGuess (rtd.trajectory.Trajectory): Past trajectory to use as an initial guess
% rsAdditionalArgs (Optional struct): additional arguments to pass to the reachable sets, by set name
%
% Returns:
- % [trajectory(rtd.planner.trajectory.Trajectory), cost(double), info(struct)]:
+ % [trajectory(rtd.trajectory.Trajectory), cost(double), info(struct)]:
% `trajectory` is an instance of the trajectory object that
% corresponds to the solved trajectory optimization. If it
% wasn't successful, this will either be an invalid
@@ -119,7 +115,7 @@
% Generate reachable sets
self.vdisp("Generating reachable sets and nonlinear constraints", 'INFO')
- rsInstances = struct;
+ rsInstances_arr = struct('id', double.empty(), 'rs', struct, 'num_instances', 0);
for rs_name=fieldnames(self.reachableSets).'
self.vdisp(['Generating ', rs_name{1}], 'DEBUG')
rs_args = {};
@@ -127,100 +123,148 @@
self.vdisp(['Passing additional arguments to generate ', rs_name{1}], 'GENERAL')
rs_args = namedargs2cell(rsAdditionalArgs.(rs_name{1}));
end
- rs = self.reachableSets.(rs_name{1}).getReachableSet(robotState, rs_args{:}, ignore_cache=false);
- rsInstances.(rs_name{1}) = rs;
- end
-
- % Generate nonlinear constraints
- self.vdisp("Generating nonlinear constraints", 'DEBUG')
- rsInstances_cell = struct2cell(rsInstances).';
- nlconCallbacks = cellfun(@(rs)rs.genNLConstraint(worldState), ...
- rsInstances_cell, ...
- UniformOutput = false);
-
- % Validate rs sizes
- self.vdisp("Validating sizes", 'DEBUG')
- num_parameters = cellfun(@(rs)rs.num_parameters, rsInstances_cell);
- if ~all(num_parameters == num_parameters(1))
- error("Reachable set parameter sizes don't match!")
- end
- num_parameters = num_parameters(1);
-
- % Compute slack sizes
- num_slack = cellfun(@(rs)size(rs.input_range,1), rsInstances_cell);
- mask_ones = num_slack == 1;
- num_slack(mask_ones) = num_parameters;
- num_slack = num_slack - num_parameters;
- if any(num_slack < 0)
- error("Reachable sets have invalid `num_parameters` or invalid size on `input_range`.")
+ rs_struct = self.reachableSets.(rs_name{1}).getReachableSet(robotState, rs_args{:}, ignore_cache=false);
+ % expand struct into id's
+ for idx=1:length(rs_struct)
+ id = rs_struct(idx).id;
+ rsInstances_arr(id).id = id;
+ rsInstances_arr(id).rs.(rs_name{1}) = rs_struct(idx).rs;
+ if isempty(rsInstances_arr(id).num_instances)
+ rsInstances_arr(id).num_instances = 0;
+ end
+ rsInstances_arr(id).num_instances = rsInstances_arr(id).num_instances + 1;
+ end
end
- slack_idxs = [0, cumsum(num_slack)];
- % Compute bounds
- self.vdisp("Computing bounds", 'DEBUG')
- param_bounds = [ones(num_parameters,1)*-Inf, ones(num_parameters,1)*Inf];
- slack_bounds = zeros(slack_idxs(end), 2);
- for i=1:length(num_slack)
- new_bounds = rsInstances_cell{i}.input_range;
- if mask_ones(i)
- new_bounds = repmat(new_bonds, num_parameters, 1);
- elseif num_slack(i) > 0
- % Isolate and add the slack
- slack_bounds(slack_idxs(i)+1:slack_idxs(i+1),:) = new_bounds(num_parameters+1:end,:);
- new_bounds = new_bounds(1:num_parameters,:);
- end
+ % Discard empties
+ mask = [rsInstances_arr.num_instances] > 0;
+ rsInstances_arr = rsInstances_arr(mask);
- % Ensure bounds are the intersect of the intervals for the
- % parameters
- param_bounds(:, 1) = max(param_bounds(:, 1), new_bounds(:, 1));
- param_bounds(:, 2) = min(param_bounds(:, 2), new_bounds(:, 2));
- end
-
- % Combine nlconCallbacks
- % TODO update to allow combination with any other constraints
- % needed if wanted
- constraintCallback = @(k) merge_constraints(k, num_parameters, num_slack, nlconCallbacks, fieldnames(self.reachableSets).');
-
- % create bounds (robotInfo and worldInfo come into play here?)
- bounds.param_limits = [param_bounds; slack_bounds];
- bounds.ouput_limits = [];
-
- % Create the objective
- objectiveCallback = self.objective.genObjective(robotState, ...
- waypoint, rsInstances);
-
% If initialGuess is none, or invalid, make a zero
try
guess = initialGuess.trajectoryParams;
catch
guess = [];
end
+
+ % If there are no problems, at least let us know
+ if isempty(rsInstances_arr)
+ self.vdisp('No problems to optimize! Continuing...','INFO');
+ end
+
+ % Copy and expand the instances for the output
+ problem_infos = rsInstances_arr;
+ [problem_infos.success] = deal(false);
+ [problem_infos.cost] = deal(Inf);
+ % Generate nonlinear constraints
+ for rsInstances_idx=1:length(rsInstances_arr)
+ rsInstances = rsInstances_arr(rsInstances_idx).rs;
+ id = rsInstances_arr(rsInstances_idx).id;
+ self.vdisp(['Solving problem ', num2str(id)], 'INFO')
+ self.vdisp("Generating nonlinear constraints", 'DEBUG')
+ rsInstances_cell = struct2cell(rsInstances).';
+ nlconCallbacks = cellfun(@(rs)rs.genNLConstraint(worldState), ...
+ rsInstances_cell, ...
+ UniformOutput = false);
+
+ % Validate rs sizes
+ self.vdisp("Validating sizes", 'DEBUG')
+ num_parameters = cellfun(@(rs)rs.num_parameters, rsInstances_cell);
+ if ~all(num_parameters == num_parameters(1))
+ error("Reachable set parameter sizes don't match!")
+ end
+ num_parameters = num_parameters(1);
+
+ % Compute slack sizes
+ num_slack = cellfun(@(rs)size(rs.input_range,1), rsInstances_cell);
+ mask_ones = num_slack == 1;
+ num_slack(mask_ones) = num_parameters;
+ num_slack = num_slack - num_parameters;
+ if any(num_slack < 0)
+ error("Reachable sets have invalid `num_parameters` or invalid size on `input_range`.")
+ end
+ slack_idxs = [0, cumsum(num_slack)];
+
+ % Compute bounds
+ self.vdisp("Computing bounds", 'DEBUG')
+ param_bounds = [ones(num_parameters,1)*-Inf, ones(num_parameters,1)*Inf];
+ slack_bounds = zeros(slack_idxs(end), 2);
+ for i=1:length(num_slack)
+ new_bounds = rsInstances_cell{i}.input_range;
+ if mask_ones(i)
+ new_bounds = repmat(new_bounds, num_parameters, 1);
+ elseif num_slack(i) > 0
+ % Isolate and add the slack
+ slack_bounds(slack_idxs(i)+1:slack_idxs(i+1),:) = new_bounds(num_parameters+1:end,:);
+ new_bounds = new_bounds(1:num_parameters,:);
+ end
+
+ % Ensure bounds are the intersect of the intervals for the
+ % parameters
+ param_bounds(:, 1) = max(param_bounds(:, 1), new_bounds(:, 1));
+ param_bounds(:, 2) = min(param_bounds(:, 2), new_bounds(:, 2));
+ end
+
+ % Combine nlconCallbacks
+ % TODO update to allow combination with any other constraints
+ % needed if wanted
+ constraintCallback = @(k) merge_constraints(k, num_parameters, num_slack, nlconCallbacks, fieldnames(self.reachableSets).');
+
+ % create bounds (robotInfo and worldInfo come into play here?)
+ bounds.param_limits = [param_bounds; slack_bounds];
+ bounds.ouput_limits = [];
+
+ % Create the objective
+ objectiveCallback = self.objective.genObjective(robotState, ...
+ waypoint, rsInstances);
+
+ % Optimize
+ self.vdisp("Optimizing!",'INFO')
+ [success, parameter, cost] = self.optimizationEngine.performOptimization(guess, ...
+ objectiveCallback, constraintCallback, bounds);
+
+ % save outputs
+ if success
+ problem_infos(rsInstances_idx).success = true;
+ problem_infos(rsInstances_idx).parameters = parameter;
+ problem_infos(rsInstances_idx).cost = cost;
+ end
+ problem_infos(rsInstances_idx).nlconCallbacks = nlconCallbacks;
+ problem_infos(rsInstances_idx).objectiveCallback = objectiveCallback;
+ problem_infos(rsInstances_idx).bounds = bounds;
+ problem_infos(rsInstances_idx).num_parameters = num_parameters;
+ end
-
- % Optimize
- self.vdisp("Optimizing!",'INFO')
- [success, parameters, cost] = self.optimizationEngine.performOptimization(guess, ...
- objectiveCallback, constraintCallback, bounds);
-
- % if success
- if success
- trajectory = self.trajectoryFactory.createTrajectory(robotState, rsInstances, parameters);
+ % Select the best cost
+ num_success = sum([problem_infos.success]);
+ if num_success > 0
+ [min_cost, min_idx] = min([problem_infos.cost]);
+ id = rsInstances_arr(min_idx).id;
+ self.vdisp(['Optimal solution found in problem ', num2str(id)],'INFO');
+ rsInstances = rsInstances_arr(min_idx).rs;
+ parameter = problem_infos(min_idx).parameters;
+ trajectory = self.trajectoryFactory.createTrajectory(robotState, rsInstances, parameter);
+ cost = min_cost;
else
+ id = [];
+ min_idx = -1;
trajectory = [];
+ parameter = [];
+ cost = Inf;
end
% Create an info struct for return
info.worldState = worldState;
info.robotState = robotState;
- info.rsInstances = rsInstances;
- info.nlconCallbacks = nlconCallbacks;
- info.objectiveCallback = objectiveCallback;
info.waypoint = waypoint;
- info.bounds = bounds;
- info.num_parameters = num_parameters;
info.guess = guess;
- info.trajectory = trajectory;
- info.cost = cost;
+ info.problems = problem_infos;
+ info.num_success = num_success;
+ info.best_cost = cost;
+ info.best_trajectory = trajectory;
+ info.best_solution_idx = min_idx;
+ info.best_solution_id = id;
+ info.best_parameter = parameter;
end
end
end
@@ -258,4 +302,4 @@
grad_h = [grad_h, temp_grad_h];
grad_heq = [grad_heq, temp_grad_heq];
end
-end
\ No newline at end of file
+end
diff --git a/+rtd/+sim/+systems/+patch3d_collision/Patch3dCollisionSystem.m b/+rtd/+sim/+systems/+patch3d_collision/Patch3dCollisionSystem.m
index 21d8c0be..fa89904e 100644
--- a/+rtd/+sim/+systems/+patch3d_collision/Patch3dCollisionSystem.m
+++ b/+rtd/+sim/+systems/+patch3d_collision/Patch3dCollisionSystem.m
@@ -45,13 +45,13 @@
function reset(self, optionsStruct, options)
arguments
self
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.time_discretization
options.log_collisions
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_collisions
diff --git a/+rtd/+sim/+systems/+patch_visual/PatchVisualSystem.m b/+rtd/+sim/+systems/+patch_visual/PatchVisualSystem.m
index 8346d233..6f11acaa 100644
--- a/+rtd/+sim/+systems/+patch_visual/PatchVisualSystem.m
+++ b/+rtd/+sim/+systems/+patch_visual/PatchVisualSystem.m
@@ -24,6 +24,10 @@
options.time_discretization = 0.1;
%options.log_collisions = false;
options.enable_camlight = false;
+ options.xlim = [];
+ options.ylim = [];
+ options.zlim = [];
+ options.view = 3;
options.verboseLevel = 'INFO';
options.name = '';
end
@@ -34,31 +38,42 @@
arguments
objects.static_objects (1,:) rtd.sim.systems.patch_visual.PatchVisualObject = rtd.sim.systems.patch_visual.PatchVisualObject.empty()
objects.dynamic_objects (1,:) rtd.sim.systems.patch_visual.PatchVisualObject = rtd.sim.systems.patch_visual.PatchVisualObject.empty()
- optionsStruct.optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.time_discretization
options.enable_camlight
+ options.xlim
+ options.ylim
+ options.zlim
+ options.view
options.verboseLevel
options.name
end
- self.mergeoptions(optionsStruct.optionsStruct, options);
+ self.mergeoptions(optionsStruct.options, options);
% Reset first
self.reset()
% add static or dynamic objects if provided
self.addObjects(static=objects.static_objects, dynamic=objects.dynamic_objects);
+
+ % Trigger a redraw
+ self.redraw()
end
function reset(self, optionsStruct, options)
arguments
self
- optionsStruct struct = struct()
+ optionsStruct.options struct = struct()
options.time_discretization
options.enable_camlight
+ options.xlim
+ options.ylim
+ options.zlim
+ options.view
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_collisions
@@ -91,10 +106,17 @@ function validateOrCreateFigure(self)
if isempty(self.figure_handle) || ...
~isvalid(self.figure_handle) || ...
~isgraphics(self.figure_handle)
+ % Save the prior figure
+ prev_fig = get(groot,'CurrentFigure');
+ % Setup the new figure
self.figure_handle = figure('Name',[self.name, ' - ', self.classname]);
- view(3);
- set(self.figure_handle,'KeyPressFcn',@self.set_pause);
- set(self.figure_handle,'GraphicsSmoothing','off')
+ set(self.figure_handle,'KeyPressFcn',@self.keypress);
+ set(self.figure_handle,'GraphicsSmoothing','off');
+ % Restore the prior figure
+ try
+ set(groot, 'CurrentFigure', prev_fig);
+ catch
+ end
end
end
@@ -155,9 +177,12 @@ function remove(self, object)
self.vdisp('Running visualization!', 'DEBUG');
+ % Save the prior figure
+ prev_fig = get(groot,'CurrentFigure');
+
% set the active figure
try
- set(0, 'CurrentFigure', self.figure_handle)
+ set(groot, 'CurrentFigure', self.figure_handle)
% plot each of the times requested
for t_plot = t_vec
@@ -169,6 +194,12 @@ function remove(self, object)
catch
% If reopen on close is enabled
end
+
+ % Restore the prior figure
+ try
+ set(groot, 'CurrentFigure', prev_fig);
+ catch
+ end
% Save the time change
self.time = [self.time, t_vec];
@@ -178,25 +209,94 @@ function remove(self, object)
self.pause_requested = false;
end
- function redraw(self, time)
+ function redraw(self, time, options)
arguments
self
time = self.time(end)
+ options.xlim = []
+ options.ylim = []
+ options.zlim = []
+ options.view = []
+ options.reset_view = false
end
+
% if the figure handle is deleted, recreate it
self.validateOrCreateFigure()
+ % Save the prior figure
+ prev_fig = get(groot,'CurrentFigure');
% set the active figure
- set(0, 'CurrentFigure', self.figure_handle)
- % Save the camera view
+ set(groot, 'CurrentFigure', self.figure_handle);
+
+ % Try to save the limits if possible
try
ax = get(self.figure_handle, 'CurrentAxes');
- [az, el] = view(ax);
- % clear and reset view
- clf;view(az, el)
+ if strcmp(ax.XLimMode, 'manual') ...
+ && isempty(options.xlim)
+ options.xlim = ax.XLim;
+ end
+ if strcmp(ax.YLimMode, 'manual') ...
+ && isempty(options.ylim)
+ options.ylim = ax.YLim;
+ end
+ if strcmp(ax.ZLimMode, 'manual') ...
+ && isempty(options.zlim)
+ options.zlim = ax.ZLim;
+ end
catch
- clf;view(3)
end
+
+ % If provided, use the view
+ if options.reset_view
+ clf
+ if iscell(self.getoptions.view)
+ view(self.getoptions.view{:})
+ else
+ view(self.getoptions.view)
+ end
+ elseif ~isempty(options.view)
+ clf
+ if iscell(options.view)
+ view(options.view{:})
+ else
+ view(options.view)
+ end
+ else
+ % Save the camera view if possible
+ try
+ ax = get(self.figure_handle, 'CurrentAxes');
+ [az, el] = view(ax);
+ % clear and reset view
+ clf;view(az, el)
+ catch
+ clf
+ if iscell(self.getoptions.view)
+ view(self.getoptions.view{:})
+ else
+ view(self.getoptions.view)
+ end
+ end
+ end
+
axis equal;grid on
+ % Set limits
+ if ~options.reset_view
+ if ~isempty(options.xlim)
+ xlim(options.xlim)
+ elseif ~isempty(self.getoptions.xlim)
+ xlim(self.getoptions.xlim)
+ end
+ if ~isempty(options.ylim)
+ ylim(options.ylim)
+ elseif ~isempty(self.getoptions.ylim)
+ ylim(self.getoptions.ylim)
+ end
+ if ~isempty(options.zlim)
+ zlim(options.zlim)
+ elseif ~isempty(self.getoptions.zlim)
+ zlim(self.getoptions.zlim)
+ end
+ end
+
if self.enable_camlight
camlight
end
@@ -209,6 +309,11 @@ function redraw(self, time)
end
drawnow limitrate
%pause(self.draw_time)
+ % Restore the prior figure
+ try
+ set(groot, 'CurrentFigure', prev_fig);
+ catch
+ end
end
function animate(self, options)
@@ -217,6 +322,11 @@ function animate(self, options)
options.t_span = [0, self.time(end)]
options.time_discretization = self.time_discretization
options.pause_time = []
+ options.xlim = []
+ options.ylim = []
+ options.zlim = []
+ options.view = []
+ options.reset_view = false
end
if isempty(options.pause_time)
@@ -229,7 +339,17 @@ function animate(self, options)
proc_time_start = tic;
% Redraw everything
- self.redraw(0);
+ self.redraw(0, ...
+ xlim=options.xlim, ...
+ ylim=options.ylim, ...
+ zlim=options.zlim, ...
+ view=options.view, ...
+ reset_view=options.reset_view);
+
+ % Save the prior figure
+ prev_fig = get(groot,'CurrentFigure');
+ % set the active figure
+ set(groot, 'CurrentFigure', self.figure_handle);
% Animate the dynamic stuff
for t_plot = t_vec
@@ -251,13 +371,42 @@ function animate(self, options)
self.pause_requested = false;
end
end
+ % Restore the prior figure
+ try
+ set(groot, 'CurrentFigure', prev_fig);
+ catch
+ end
end
- function set_pause(self, src, event)
+ function keypress(self, src, event)
if event.Character == "p" && ~self.pause_requested
self.vdisp("Pause requested", 'INFO');
self.pause_requested = true;
end
+ if event.Character == "r"
+ self.vdisp("Redraw requested", 'INFO');
+ self.redraw();
+ end
+ end
+
+ function close(self)
+ % try to close
+ try
+ close(self.figure_handle)
+ catch
+ end
+ end
+
+ end
+
+ methods (Static)
+ function [time_discretization, pause_time] = get_discretization_and_pause(framerate, speed)
+ arguments
+ framerate (1,1) double = 30
+ speed (1,1) double = 1
+ end
+ pause_time = 1 / framerate;
+ time_discretization = speed / framerate;
end
end
end
\ No newline at end of file
diff --git a/+rtd/+sim/+world/WorldEntity.m b/+rtd/+sim/+world/WorldEntity.m
index c34d83d1..825acd81 100644
--- a/+rtd/+sim/+world/WorldEntity.m
+++ b/+rtd/+sim/+world/WorldEntity.m
@@ -1,20 +1,47 @@
classdef WorldEntity < matlab.mixin.Heterogeneous & rtd.util.mixins.Options & rtd.util.mixins.NamedClass & handle
+% WorldEntity is the base class for all entities in the world. It is
+% responsible for managing the state and info of the entity, as well as
+% providing a common interface for interacting with the entity.
+%
+% It is also responsible for managing the options of the entity, and
+% providing a common interface for interacting with the options.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-01-11
+%
+% See also: rtd.sim.world.WorldModel, rtd.sim.BaseSimulation,
+% rtd.util.mixins.Options, rtd.util.mixins.NamedClass
+%
+% --- More Info ---
+%
properties (Abstract)
+ % Intrinsic properties of the entity which generally do not change
info rtd.entity.components.BaseInfoComponent
+
+ % Internal state of the entity which changes over time
state rtd.entity.components.BaseStateComponent
end
properties (Dependent)
+ % UUID of the entity from the info
uuid
end
methods
- function uuid = get.uuid(self)
- uuid = self.info.uuid;
- end
-
function update_name(self, name)
+ % Update the name of the entity and all of its components
+ %
+ % Arguments:
+ % name: Name of the entity
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldEntity
+ name {mustBeTextScalar}
+ end
+
+ % update the name
self.name = name;
% update options
@@ -32,18 +59,44 @@ function update_name(self, name)
end
function component = construct_component(self, name, varargin)
+ % Construct a component with the given name and arguments
+ %
+ % Arguments:
+ % name: Name of the component
+ % varargin: Arguments to pass to the component
+ %
+ % Returns:
+ % component: Constructed component
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldEntity
+ name {mustBeTextScalar}
+ end
+ arguments (Repeating)
+ varargin
+ end
+
+ % Get the options for the component if they exist
option_struct = self.instanceOptions;
if isfield(option_struct, 'component_options') ...
&& isfield(option_struct.component_options, name)
- component = feval(option_struct.components.(name), varargin{:}, option_struct.component_options.(name));
+ % If we have options, pass them in
+ nvargs = namedargs2cell(option_struct.component_options.(name));
+ component = feval(option_struct.components.(name), varargin{:}, nvargs{:});
else
+ % Otherwise, just pass in the arguments
component = feval(option_struct.components.(name), varargin{:});
end
+
self.(name) = component;
end
- % TODO add inclusion, exclusion options
function reset_components(self)
+ % Reset all components of the entity
+ %
+ % TODO add inclusion, exclusion options
+ %
+
% Get the latest options
options = self.getoptions();
@@ -72,19 +125,25 @@ function reset_components(self)
options.component_options.(component_name{1}).verboseLog = options.component_logLevelOverride;
end
- self.(component_name{1}).reset(options.component_options.(component_name{1}));
+ self.(component_name{1}).reset(options=options.component_options.(component_name{1}));
end
end
- % Get all the options used for initialization of the robot
function options = getoptions(self)
+ % Get the options for the entity and all of its components
+ % Also performs an internal update of the options
+ %
+ % Returns:
+ % options (struct): Options for the entity and its components
+ %
+
% Update all the component options before returning.
% Get a proposal set of options
options = getoptions@rtd.util.mixins.Options(self);
% Get all the component options
- options.component_options = rtd.sim.world.WorldEntity.get_componentoptions(options.components, self);
+ options.component_options = get_componentoptions(options.components, self);
% Merge it back into the stored options
options = self.mergeoptions(options);
@@ -92,9 +151,21 @@ function reset_components(self)
end
- methods (Static)
+ % Methods for dependent properties
+ methods
+ function uuid = get.uuid(self)
+ uuid = self.info.uuid;
+ end
+ end
+ methods (Static)
function options = baseoptions()
+ % Base options for any world entity
+ %
+ % Returns:
+ % options (struct): Options for the entity and its components
+ %
+
options.components = struct;
options.component_options = struct;%get_componentoptions(components);
options.component_logLevelOverride = [];
@@ -103,6 +174,19 @@ function reset_components(self)
end
function options = get_componentOverrideOptions(components)
+ % Gets options for provided components if they are already constructed or the default options if they aren't
+ % and returns them in a partial options struct for the entity to merge in as overrides
+ %
+ % Arguments:
+ % components (struct): Struct of components to generate override options for
+ %
+ % Returns:
+ % options (struct): Partial struct of WorldEntity options for the just the components provided
+ %
+ arguments
+ components (1,1) struct
+ end
+
% mark each of our preconstructed components and save them
names = fieldnames(components).';
components = struct2cell(components).';
@@ -121,28 +205,36 @@ function reset_components(self)
% Preprocess the instances for options
component_handles = [provided_component_names(~string_mask); provided_components(~string_mask)];
- options.component_options = rtd.sim.world.WorldEntity.get_componentoptions(struct(component_handles{:}));
+ options.component_options = get_componentoptions(struct(component_handles{:}));
end
+ end
+end
- function componentoptions = get_componentoptions(component_classnames, components)
- arguments
- component_classnames
- components = component_classnames
- end
- % Generate an entry for each field which is of type rtd.util.mixins.Options
- fields = fieldnames(component_classnames).';
- componentoptions = struct;
- for fieldname = fields
- if ismember('rtd.util.mixins.Options', superclasses(components.(fieldname{1})))
- try
- componentoptions.(fieldname{1}) ...
- = components.(fieldname{1}).getoptions();
- catch
- componentoptions.(fieldname{1}) ...
- = eval([components.(fieldname{1}) '.defaultoptions()']);
- end
- end
+% Helper functions
+function componentoptions = get_componentoptions(component_classnames, components)
+ % Get the options for the provided components or call their default options
+ % if they aren't actually constructed
+ %
+ % Arguments:
+ % component_classnames (struct): Struct of component classnames
+ % components (struct): Struct of components or their classnames
+ arguments
+ component_classnames(1,1) struct
+ components = component_classnames
+ end
+
+ % Generate an entry for each field which is of type rtd.util.mixins.Options
+ fields = fieldnames(component_classnames).';
+ componentoptions = struct;
+ for fieldname = fields
+ if ismember('rtd.util.mixins.Options', superclasses(components.(fieldname{1})))
+ try
+ componentoptions.(fieldname{1}) ...
+ = components.(fieldname{1}).getoptions();
+ catch
+ componentoptions.(fieldname{1}) ...
+ = feval([components.(fieldname{1}) '.defaultoptions']);
end
end
end
-end
+end
\ No newline at end of file
diff --git a/+rtd/+sim/+world/WorldModel.m b/+rtd/+sim/+world/WorldModel.m
index 18c4cd77..78d4a37b 100644
--- a/+rtd/+sim/+world/WorldModel.m
+++ b/+rtd/+sim/+world/WorldModel.m
@@ -1,73 +1,502 @@
classdef WorldModel < handle
- %WORLDMODEL A model of the world used by RTD
- % Similar to RobotModel, in that it contains a single initialization
- % of WorldInfo, and then will create atomic instances of WorldState
- % as needed as well as evolve world dynamics/predictions.
-
+% Contains the entities and systems for a simulation.
+%
+% A WorldModel is a container for entities and systems. In any given
+% simulation, there should be one WorldModel. The WorldModel is responsible
+% for describing the entities and systems in the simulation, and the
+% simulation itself is responsible for running the simulation.
+%
+% This class contains methods for importing and exporting the world to and
+% from XML files.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-18
+%
+% See also: rtd.sim.world.WorldEntity, rtd.sim.systems.SimulationSystem
+%
+% --- More Info ---
+%
+
properties
- agent
- obstacle
- end
- methods (Static)
- function world = import(filename)
- end
+ % all_entities is a struct of all entities in the world
+ all_entities(1,1) struct
+
+ % dynamic_entities is a struct of all dynamic entities in the world.
+ % it is a subset of all_entities
+ dynamic_entities(1,1) struct
+
+ % static_entities is a struct of all static entities in the world.
+ % it is a subset of all_entities
+ static_entities(1,1) struct
+
+ % systems is a struct of all systems in the world
+ systems(1,1) struct
end
+
+ % Entity and system management methods
methods
- % Add object
- function addObject(self, object, options)
+ function name = addEntity(self, entity, options)
+ % Add an entity to the world
+ %
+ % Arguments:
+ % entity(rtd.sim.world.WorldEntity): the entity to add
+ % options: Keyword arguments. See below.
+ %
+ % Keyword Arguments:
+ % type: the type of entity to add. Must be 'static' or
+ % 'dynamic'. Default: 'static'
+ % update_name: whether to update the name of the entity to
+ % match the name given in the options. Default: false
+ % name: the name of the entity. If not given, the name will
+ % be the classname of the entity with a number appended
+ % to it. Default: ''
+ %
+ % Returns:
+ % name: the name of the entity added
+ %
arguments
- self rtd.sim.world.WorldModel
- object
- options.type {mustBeMember(options.type,{'obstacle','agent'})} = 'obstacle'
- options.collision = []
- options.visual = []
+ self(1,1) rtd.sim.world.WorldModel
+ entity(1,1) rtd.sim.world.WorldEntity
+ options.type {mustBeMember(options.type,{'static','dynamic'})} = 'static'
+ options.update_name(1,1) logical = false
+ options.name {mustBeTextScalar} = ''
end
% Add the object to the world
% Create a name for the object based on its classname if it
- % doesn't have a given name.
- name = object.name;
+ % doesn't have a given name either passed in or as the entity itself.
+ if ~isempty(options.name)
+ name = options.name;
+ else
+ name = entity.name;
+ end
if isempty(name)
% Base classname
- name = object.classname;
+ name = entity.classname;
% Get number to append by summing a regexp. It returns the
% index of the occurance in each string, but since it's
% limited to the first word anyway, it'll be 1 or empty.
search_string = ['^', char(name), '\d+$'];
- all_names = [fieldnames(self.agent); fieldnames(self.obstacle)];
+ all_names = fieldnames(self.all_entities);
id = sum(cell2mat(regexp(all_names, search_string)));
name = [char(name), num2str(id)];
end
- object.name = name;
- self.world.(type).(name) = object;
+ if options.update_name
+ entity.update_name(name);
+ end
+ self.all_entities.(name) = entity;
- % Add to the entity list if it's an entity
- if options.isentity
- self.entities = [self.entities, {object}];
- % Add the collision component provided to the collision system
- if ~isempty(options.collision)
- self.collision_system.addObjects(dynamic=options.collision);
+ % Save to the appropriate entity struct
+ if strcmp(options.type, 'dynamic')
+ self.dynamic_entities.(name) = entity;
+ else
+ self.static_entities.(name) = entity;
+ end
+ end
+
+ function name = addSystem(self, system, name, options)
+ % Add a system to the world
+ %
+ % Arguments:
+ % system(rtd.sim.systems.SimulationSystem): the system to add
+ % name: the name of the system
+ % options: Keyword arguments. See below.
+ %
+ % Keyword Arguments:
+ % update_name: whether to update the name of the system to
+ % match the name given in the options. Default: false
+ % replace_system: whether to replace the system if it already
+ % exists. If false, an error will be thrown. Default: true
+ %
+ % Raises:
+ % SystemExists: if the system already exists and replace_system
+ % is false
+ %
+ % Returns:
+ % name: the name of the system added
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ system(1,1) rtd.sim.systems.SimulationSystem
+ name {mustBeTextScalar}
+ options.update_name(1,1) logical = false
+ options.replace_system(1,1) logical = true
+ end
+
+ % Check if the system already exists
+ if isfield(self.systems, name)
+ if ~options.replace_system
+ errMsg = MException('WorldModel:SystemExists', ...
+ 'System name already exists!');
+ throw(errMsg)
end
- % Add the visualization component provided to the visual
- % system
- if ~isempty(options.visual)
- self.visual_system.addObjects(dynamic=options.visual);
+ end
+
+ % Update the name if necessary and add to the world
+ if options.update_name
+ system.update_name(name);
+ end
+ self.systems.(name) = system;
+ end
+
+ function struct_out = getEntityComponent(self, component_name, options)
+ % Get a component from all entities in the world
+ %
+ % Arguments:
+ % component_name: the name of the component to get
+ % options: Keyword arguments. See below.
+ %
+ % Keyword Arguments:
+ % type: the type of entity to get the component from. Must be
+ % 'all', 'static', or 'dynamic'. Default: 'all'
+ % MissingComponentAction: what to do if the component is
+ % missing. Must be 'skip', 'empty', or 'throw'. If 'skip',
+ % the entity will be skipped. If 'empty', an empty
+ % component will be returned for that entity. If 'throw',
+ % an error will be thrown. Default: 'skip'
+ %
+ % Returns:
+ % struct_out: a struct of the components, with the same
+ % fields as the entities
+ %
+ % Raises:
+ % MissingComponent: if the component is missing and
+ % MissingComponentAction is 'throw'
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ component_name {mustBeTextScalar}
+ options.type {mustBeMember(options.type,{'all','static','dynamic'})} = 'all'
+ options.MissingComponentAction {mustBeMember(options.MissingComponentAction,{'skip','empty','throw'})} = 'skip'
+ end
+
+ % Determine which struct to use
+ switch options.type
+ case 'all'
+ struct_to_use = self.all_entities;
+ case 'static'
+ struct_to_use = self.static_entities;
+ case 'dynamic'
+ struct_to_use = self.dynamic_entities;
+ end
+
+ % Get the component from each entity and put it in a struct
+ names = fieldnames(struct_to_use).';
+ struct_out = struct;
+ for name_cell=names
+ name = name_cell{1};
+ entity = struct_to_use.(name);
+ % If the component isn't there, or isempty, execute the
+ % missing component action
+ if ~isprop(entity, component_name) || isempty(entity.(component_name))
+ switch options.MissingComponentAction
+ case 'skip'
+ continue
+ case 'empty'
+ component = [];
+ case 'throw'
+ errMsg = MException('WorldModel:MissingComponent', ...
+ 'Component requests is either missing or empty!');
+ throw(errMsg)
+ end
+ else
+ component = entity.(component_name);
end
- % if it's not, check for and add to collision or visual
+ struct_out.(name) = component;
+ end
+ end
+
+ function [in_sync, out_entities, out_systems, time] = checkTimeSync(self, time)
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ time(1,1) double = -1
+ end
+ % Get the latest times & names
+ % Don't consider static entities.
+ dyn_ent_names = fieldnames(self.dynamic_entities).';
+ entity_times = cellfun(@(name)self.dynamic_entities.(name).state.time(end), dyn_ent_names);
+
+ system_names = fieldnames(self.systems).';
+ system_times = cellfun(@(name)self.systems.(name).time(end), system_names);
+
+ % If provided time is negative, we are just finding synchrony
+ % to the latest time
+ if time < 0
+ time = max([entity_times, system_times]);
+ end
+
+ % Check time synchrony
+ in_sync_entities = entity_times == time;
+ in_sync_systems = system_times == time;
+ in_sync = all([in_sync_entities, in_sync_systems]);
+
+ % Get out of sync systems and entities
+ out_entities = dyn_ent_names(~in_sync_entities);
+ out_systems = system_names(~in_sync_systems);
+ end
+ end
+
+ % XML import/export static methods
+ methods (Static)
+ function world = import(filename)
+ % Import a world from an XML file
+ %
+ % Arguments:
+ % filename: the filename to import from
+ %
+ % Returns:
+ % world: the world model
+ %
+ arguments
+ filename {mustBeTextScalar}
+ end
+
+ % create a base worldmodel
+ world = rtd.sim.world.WorldModel();
+ world.mergeFromXML(filename)
+ end
+ end
+
+ % XML import/export methods
+ methods
+ function mergeFromXML(self, filename)
+ % Merge a world from an XML file
+ %
+ % Arguments:
+ % filename: the filename to import from
+ %
+ % Raises:
+ % EntityUnexpectedType: if an entity already exists as the
+ % opposite type (dynamic/static)
+ % InvalidVersion: if the version is not 0.1
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ filename {mustBeTextScalar}
+ end
+
+ parser = matlab.io.xml.dom.Parser;
+ dom = parser.parseFile(filename);
+ rootNode = dom.getDocumentElement();
+ self.mergeFromNode(rootNode);
+ end
+
+ function mergeFromNode(self, rootNode)
+ % Merge a world from an XML node
+ %
+ % Arguments:
+ % rootNode: the node to import from
+ %
+ % Raises:
+ % EntityUnexpectedType: if an entity already exists as the
+ % opposite type (dynamic/static)
+ % InvalidVersion: if the version is not 0.1
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ rootNode matlab.io.xml.dom.Node
+ end
+
+ % Verify that this is a version we can work with
+ version = rootNode.getAttribute('format_version');
+ if ~strcmp(version, '0.1')
+ errMsg = MException('WorldModel:InvalidVersion', ...
+ 'Invalid version!');
+ throw(errMsg)
+ end
+
+ % Get the nodes and add them to the world
+ static_nodelist = rootNode.getElementsByTagName('static_entities');
+ dynamic_nodelist = rootNode.getElementsByTagName('dynamic_entities');
+ system_nodelist = rootNode.getElementsByTagName('systems');
+ for i=0:static_nodelist.getLength()-1
+ element = static_nodelist.item(i);
+ config_nodelist = element.getElementsByTagName('configuration_object');
+ self.addEntityFromConfiguration(config_nodelist, 'static');
+ end
+ for i=0:dynamic_nodelist.getLength()-1
+ element = dynamic_nodelist.item(i);
+ config_nodelist = element.getElementsByTagName('configuration_object');
+ self.addEntityFromConfiguration(config_nodelist, 'dynamic');
+ end
+ for i=0:system_nodelist.getLength()-1
+ element = system_nodelist.item(i);
+ config_nodelist = element.getElementsByTagName('configuration_object');
+ self.addSystemFromConfiguration(config_nodelist);
+ end
+ end
+
+ function outstring = export(self, filename)
+ % Export a world to an XML file
+ %
+ % Arguments:
+ % filename: the filename to export to. If empty, the XML
+ % string will be returned instead of written to a file.
+ % Default: ''
+ %
+ % Returns:
+ % outstring: the XML string if filename is empty
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ filename {mustBeTextScalar} = ''
+ end
+
+ dom = matlab.io.xml.dom.Document('world');
+ old_rootNode = dom.getDocumentElement();
+ new_rootNode = self.to_nodes(dom);
+ dom.replaceChild(new_rootNode, old_rootNode);
+ writer = matlab.io.xml.dom.DOMWriter;
+ writer.Configuration.FormatPrettyPrint = true;
+ if ~isempty(filename)
+ writer.writeToFile(dom, filename);
else
- self.obstacles = [self.obstacles, box_obstacle_zonotope('center', object.state.state, 'side_lengths', object.info.side_lengths)];
- if ~isempty(options.collision)
- self.collision_system.addObjects(static=options.collision);
+ outstring = writer.writeToString(dom);
+ end
+ end
+ end
+
+ % Private helper methods for XML import/export
+ methods (Access=private)
+ function addSystemFromConfiguration(self, config_nodelist)
+ % Add or update systems from a configuration node list
+ %
+ % Arguments:
+ % config_nodelist(matlab.io.xml.dom.NodeList): the node list
+ % to import systems from
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ config_nodelist(1,1) matlab.io.xml.dom.NodeList
+ end
+
+ for i=0:config_nodelist.getLength()-1
+ config_element = config_nodelist.item(i);
+ [options_struct, ~, attributes] = rtd.functional.node2struct(config_element);
+ % if we already have this system, just update it
+ if isfield(self.systems, attributes.name) ...
+ && isa(self.systems.(attributes.name), attributes.class)
+ if ~isempty(fieldnames(options_struct))
+ self.systems.(attributes.name).reset(options=options_struct)
+ else
+ self.all_entities.(attributes.name).reset();
+ end
+ % otherwise add it
+ else
+ nvargs = namedargs2cell(options_struct);
+ system = feval(attributes.class, nvargs{:});
+ self.addSystem(system, attributes.name);
end
- % Add the visualization component provided to the visual
- % system
- if ~isempty(options.visual)
- self.visual_system.addObjects(static=options.visual);
+ end
+ end
+
+ function addEntityFromConfiguration(self, config_nodelist, type)
+ % Add or update entities from a configuration node list
+ %
+ % Arguments:
+ % config_nodelist(matlab.io.xml.dom.NodeList): the node list
+ % to import entities from
+ % type: the type of entity to add. Must be 'static' or
+ % 'dynamic'
+ %
+ % Raises:
+ % EntityUnexpectedType: if an entity already exists as the
+ % opposite type (dynamic/static)
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ config_nodelist(1,1) matlab.io.xml.dom.NodeList
+ type {mustBeMember(type,{'static','dynamic'})}
+ end
+
+ for i=0:config_nodelist.getLength()-1
+ config_element = config_nodelist.item(i);
+ [options_struct, ~, attributes] = rtd.functional.node2struct(config_element);
+ % If we have it as the wrong type, error
+ if isfield(self.all_entities, attributes.name) ...
+ && ~isfield(self.([type, '_entities']), attributes.name)
+ errMsg = MException('WorldModel:EntityUnexpectedType', ...
+ 'Entity already exists as opposite type (dynamic/static)!');
+ throw(errMsg)
+ end
+ % if we already have this configuration, just update it
+ if isfield(self.all_entities, attributes.name) ...
+ && isa(self.all_entities.(attributes.name), attributes.class) ...
+ && isequal(self.all_entities.(attributes.name).getoptions().components, options_struct.components)
+ self.all_entities.(attributes.name).reset(options=options_struct);
+ % otherwise add/replace it
+ else
+ nvargs = namedargs2cell(options_struct);
+ entity = feval(attributes.class, nvargs{:});
+ self.addEntity(entity, type=type, name=attributes.name);
end
end
end
- function export(self, filename)
+
+ function rootNode = to_nodes(self, dom)
+ % Convert the world to XML nodes
+ %
+ % Arguments:
+ % dom(matlab.io.xml.dom.Document): the document to create the
+ % nodes in
+ %
+ % Returns:
+ % rootNode(matlab.io.xml.dom.Element): the root node of the
+ % world model
+ %
+ arguments
+ self(1,1) rtd.sim.world.WorldModel
+ dom(1,1) matlab.io.xml.dom.Document
+ end
+
+ % Create the root node, then add the entities and systems
+ % if they exist
+ rootNode = dom.createElement('world');
+ if ~isempty(fieldnames(self.dynamic_entities))
+ dynamic_node = dom.createElement('dynamic_entities');
+ configuration_struct_to_dom_helper(dom, dynamic_node, self.dynamic_entities);
+ rootNode.appendChild(dynamic_node);
+ end
+ if ~isempty(fieldnames(self.static_entities))
+ static_node = dom.createElement('static_entities');
+ configuration_struct_to_dom_helper(dom, static_node, self.static_entities);
+ rootNode.appendChild(static_node);
+ end
+ if ~isempty(fieldnames(self.systems))
+ system_node = dom.createElement('systems');
+ configuration_struct_to_dom_helper(dom, system_node, self.systems);
+ rootNode.appendChild(system_node);
+ end
+ % Add a version attribute for future later validation
+ rootNode.setAttribute('format_version', '0.1');
end
end
end
+function configuration_struct_to_dom_helper(dom, rootNode, struct_to_use)
+ % Helper function to convert a struct to XML nodes
+ %
+ % Arguments:
+ % dom(matlab.io.xml.dom.Document): the document to create the
+ % nodes in
+ % rootNode(matlab.io.xml.dom.Node): the root node to add the
+ % nodes to
+ % struct_to_use(struct): the struct to
+ %
+ names = fieldnames(struct_to_use).';
+ for name_cell=names
+ name = name_cell{1};
+ struct_item = struct_to_use.(name);
+ entity_classname = class(struct_item);
+ attributes = struct;
+ attributes.class = entity_classname;
+ attributes.name = name;
+ options_struct = struct;
+ if ismember('rtd.util.mixins.Options', superclasses(struct_item))
+ options_struct = struct_item.getoptions();
+ end
+ node = rtd.functional.struct2node(options_struct, dom, 'configuration_object', attributes);
+ rootNode.appendChild(node);
+ end
+end
diff --git a/+rtd/+sim/+world/readme.md b/+rtd/+sim/+world/readme.md
deleted file mode 100644
index 3576c84c..00000000
--- a/+rtd/+sim/+world/readme.md
+++ /dev/null
@@ -1,58 +0,0 @@
-# World
-
-In this folder are files that interface with the world for RTD.
-At the moment, WorldInfo, WorldModel, and RobotModel are unused, but they're here for future feature reasons.
-As such, they'll aslo be discussed below.
-
----
-
-## WorldModel
-
-This class should be extended as needed to create a model of the world used by RTD.
-It should contains a single initialization of WorldInfo, and then will create atomic instances of WorldState as needed as well as evolve world dynamics/predictions.
-If other agents start to be more of a concern, it is through the WorldModel that you learn more about these agents.
-
----
-
-## WorldInfo
-
-This stores common static info about the world.
-For example, this might be the bounds on the configuration space for an arm robot.
-OR it could be things like the location of a target platform.
-If you find any parts of this changing during runtime, it should either be a part of the WorldModel or WorldState!
-
----
-
-## WorldState
-
-This should hold atomic state instances of the world.
-Parts of the world that change over time should be included in this.
-For example, obstacles are something that are always an evolving part of the state of the world, so that's going to be in here.
-Other possible inclusions would be other agents and their states.
-
----
-
-## RobotModel
-
-Like the world model, this class encapsulates everything needed for a robot that exists in some world to function.
-Dynamics and other such evolving functions based on the model itself would be fit to extend from here.
-Eventually, this will act as an interface to the underlying robot in simulation or world.
-If it includes dynamics it could also generate state predictions.
-An example of something that might go in here is RNEA for a manipulator.
-This should include a single instance of RobotInfo, and generate atomic instances of RobotState.
-
----
-
-## RobotInfo
-
-Parameters and static unchanging aspects of a robot.
-Again, if you find any part of this changing during runtime, it should be a part of the RobotModel or RobotState!
-
----
-
-## RobotState
-
-State of a generic robot at a given moment in time.
-Time is always relevant, so it is included in the base object.
-For utility, this extends rtd.util.mixins.UUID, so that every initialization of this class will have a different id, even if they share the same state values.
-That also means if you intialize it once, but copy it (so it's a copy of the same state realizaton), it will share the same id.
diff --git a/+rtd/+sim/BaseSimulation.m b/+rtd/+sim/BaseSimulation.m
index 15935501..ddc230b7 100644
--- a/+rtd/+sim/BaseSimulation.m
+++ b/+rtd/+sim/BaseSimulation.m
@@ -1,39 +1,332 @@
classdef BaseSimulation < rtd.util.mixins.NamedClass & handle
- % properties
- % world_def
- % end
+% BaseSimulation: Base class for all simulations
+%
+% Whenever creating a new simulation, it is recommended to inherit from
+% this class. This class provides the basic functionality for running a
+% simulation. It also provides a few helper functions for exporting and
+% importing the world model. The setup and initialize functions are expected
+% to be defined. The simulation must be put into the state of "READY" before
+% any of the step functions can be called.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-20
+%
+% See also: rtd.sim.world.WorldModel, rtd.sim.types.SimulationState
+%
+% --- More Info ---
+%
+
+ % Some default properties that are useful for all simulations
properties
+ % Internal history of time for the overall simulation
+ % This is used to verify synchrony of all entities and components
+ simulation_time(:,1) double = 0;
+
+ % Configuration state / step state of the simulation
simulation_state rtd.sim.types.SimulationState = 'INVALID'
+
+ % Historical log of the simulation
+ simulation_log rtd.util.containers.VarLogger = rtd.util.containers.VarLogger.empty()
+
+ % Container which holds the entities and systems relevant for the simulation
+ world rtd.sim.world.WorldModel = rtd.sim.world.WorldModel.empty()
end
+
+ % Properties that must be defined by the user
properties (Abstract)
- simulation_timestep
+ % Define this variable to specify what a complete step of the simulation should progress
+ % forward in time by
+ simulation_timestep(1,1) double
end
+
+ % Events that are triggered during the lifecycle of the simulation
+ events
+ % Triggered after pre-step operations, before step operations
+ PreStep
+
+ % Triggered after step operations, before post-step operations
+ Step
+
+ % Triggered after post-step operations
+ PostStep
+ end
+
+ % Methods that must be defined by the user
methods (Abstract)
- % add some object to the simulation
- add_object(self, object)
- % setup all the world
+ % This function should setup key entities and systems
setup(self)
- % initialize everything to start
+
+ % This function should run after setup to initialize everything to start
initialize(self)
- % Execute before the overall step
- pre_step(self)
- % Execute all the updates needed for each step
- step(self)
- % Execute after each step
- post_step(self)
- % generate the summary
- summary(self, options)
-
- % Run the lifecycle
- % Max iterations or max length is embedded in this.
- run(self, options)
- % Exports the world_def struct to a
- %export_world(self, filename)
- %import_world(self, filename)
+ % This function should imports the world model from an xml file
+ import(self, filename)
% saves and loads the simulation
%save_checkpoint(self, filename)
%load_checkpoint(self, filename)
end
+
+ % Optionally overridable function
+ methods
+ % Generate a default summary based on the simulation log
+ function struct_out = summary(self)
+ % Default implementation returns the consolidated log
+ %
+ % This function should be overridden by the user to
+ % provide a summary with desired information as a struct
+ %
+ % Returns:
+ % struct_out: Struct of what happened during the simulation run
+ %
+
+ if ~isempty(self.simulation_log)
+ struct_out = self.simulation_log.get;
+ end
+ end
+ end
+
+ % Internal methods for the lifecycle of the simulation
+ % These methods are meant to be overridden by the user
+ methods (Access = protected)
+ % Execute before the overall step
+ function info = pre_step_impl(self)
+ % Default implementation does nothing
+ %
+ % This function is meant to be overridden by the user
+ % to perform any pre-step operations
+ %
+ % Returns:
+ % info: Struct of information of what happened during the pre-step
+ %
+
+ info = struct;
+ end
+
+ % Execute all the updates needed for each step
+ function info = step_impl(self)
+ % Default implementation does nothing
+ %
+ % This function is meant to be overridden by the user
+ % to perform any step operations
+ %
+ % Returns:
+ % info: Struct of information of what happened during the step
+ %
+
+ info = struct;
+ end
+
+ % Execute after each step
+ function info = post_step_impl(self)
+ % Default implementation does nothing
+ %
+ % This function is meant to be overridden by the user
+ % to perform any post-step operations
+ %
+ % Returns:
+ % info: Struct of information of what happened during the post-step
+ %
+
+ info = struct;
+ end
+ end
+
+ % Sealed methods for the lifecycle of the simulation
+ methods (Sealed)
+ function info = pre_step(self)
+ % Execute before the overall step
+ %
+ % This is the function that the user calls to execute the
+ % pre-step operations
+ %
+ % Returns:
+ % info: Struct of information of what happened during the pre-step
+ %
+ % Notifies:
+ % PreStep: Notifies all listeners that the pre-step has been executed
+ %
+
+ if self.simulation_state < "READY"
+ error("Simulation not ready! Make sure to set the simulation_state to ready when ready!")
+ end
+ self.simulation_state = 'PRE_STEP';
+ info = self.pre_step_impl();
+ notify(self, 'PreStep')
+ end
+
+ function info = step(self)
+ % Execute all the updates needed for each step
+ %
+ % This is the function that the user calls to execute the
+ % step operations
+ %
+ % Returns:
+ % info: Struct of information of what happened during the step
+ %
+ % Notifies:
+ % Step: Notifies all listeners that the step has been executed
+ %
+
+ if self.simulation_state < "READY"
+ error("Simulation not ready! Make sure to set the simulation_state to ready when ready!")
+ end
+ self.simulation_state = 'STEP';
+ info = self.step_impl();
+ notify(self, 'Step')
+ end
+
+ function info = post_step(self)
+ % Execute after each step
+ %
+ % This is the function that the user calls to execute the
+ % post-step operations. This function also updates the
+ % simulation time and checks for synchronization of all entities
+ % and systems.
+ %
+ % Returns:
+ % info: Struct of information of what happened during the post-step
+ %
+ % Notifies:
+ % PostStep: Notifies all listeners that the post-step has been executed
+ %
+
+ if self.simulation_state < "READY"
+ error("Simulation not ready! Make sure to set the simulation_state to ready when ready!")
+ end
+ self.simulation_state = 'POST_STEP';
+ info = self.post_step_impl();
+
+ % Notify before validation
+ notify(self, 'PostStep')
+
+ % Update simulation time and make sure everything is in sync
+ self.simulation_time(end+1) = self.simulation_time(end) + self.simulation_timestep;
+ in_sync = false(length(self.world), 1);
+ for idx=1:length(self.world)
+ in_sync(idx) = self.world(idx).checkTimeSync(self.simulation_time(end));
+ end
+ if ~isempty(in_sync) && ~all(in_sync)
+ warning("Simulation synchronization has been lost!")
+ end
+ info.simulation_poststep_time = self.simulation_time(end);
+ info.simulation_in_sync = in_sync;
+ end
+
+ function run(self, options)
+ % Run the lifecycle of the simulation all at once
+ %
+ % This is the function that the user calls to execute the
+ % entire lifecycle of the simulation if they don't want to
+ % manually call each step. This function also provides the
+ % ability to automatically log the simulation. It will run
+ % until the simulation is stopped or the max number of steps
+ % or max time is reached.
+ %
+ % Arguments:
+ % options: Keyword arguments. See below
+ %
+ % Keyword Arguments:
+ % max_steps: Maximum number of steps to run the simulation for. Default: 1e8
+ % max_time: Maximum amount of time to run the simulation for. Default: Inf
+ % pre_step_callback: Function handle to execute before each step. Default: {}
+ % step_callback: Function handle to execute during each step. Default: {}
+ % post_step_callback: Function handle to execute after each step. Default: {}
+ % stop_on_goal: Stop the simulation if the goal is reached. Default: true
+ % autolog: Automatically log the simulation. Default: false
+ %
+
+ arguments
+ self rtd.sim.BaseSimulation
+ options.max_steps = 1e8
+ options.max_time = Inf
+ options.pre_step_callback cell = {}
+ options.step_callback cell = {}
+ options.post_step_callback cell = {}
+ options.stop_on_goal = true
+ options.autolog = false
+ end
+
+ % Build the execution order
+ execution_queue = [ {@(self)self.pre_step()}, ...
+ options.pre_step_callback, ...
+ {@(self)self.step()}, ...
+ options.step_callback, ...
+ {@(self)self.post_step()}, ...
+ options.post_step_callback ];
+
+ steps = 0;
+ start_tic = tic;
+ t_cur = toc(start_tic);
+ pause_time = 0;
+ stop = false;
+ while steps < options.max_steps && t_cur < options.max_time && ~stop
+ % Iterate through all functions in the execution queue
+ for fcn = execution_queue
+ info = fcn{1}(self);
+ % Automate logging here if wanted
+ stop = stop || (isfield(info, 'stop') && info.stop);
+ if options.stop_on_goal && isfield(info, 'goal') && info.goal
+ stop = true;
+ disp("Goal acheived!")
+ end
+ % Pause if requested
+ if isfield(info, 'pause_requested') && info.pause_requested
+ start_pause = tic;
+ keyboard
+ pause_time = pause_time + toc(start_pause);
+ end
+ if options.autolog && ~isempty(self.simulation_log)
+ self.simulation_log.addStruct(info);
+ end
+ end
+ steps = steps + 1;
+ t_cur = toc(start_tic) - pause_time;
+ end
+ end
+ end
+
+ methods
+ function export(self, filename)
+ % Export the world model to an xml
+ %
+ % This function exports the world model to an xml file. If
+ % multiple world models are present, then each world model
+ % will be exported to a separate xml file with the index
+ % appended to the filename. If no filename is provided, then
+ % the default filename will be the name of the simulation
+ % class and the current date and time.
+ %
+ % Arguments:
+ % filename: Name of the file to export to. Default: ''
+ %
+ arguments
+ self(1,1) rtd.sim.BaseSimulation
+ filename {mustBeTextScalar} = ''
+ end
+
+ if self.simulation_state < "READY"
+ error("Unable to export simulation. Simulation hasn't been initialized!");
+ end
+ if isempty(filename)
+ filename = [self.classname, '-', ...
+ char(datetime("now","Format",'yyyyMMdd''T''HHmmss'))];
+ else
+ % Strip the xml extension if provided.
+ [path, name, extension] = fileparts(filename);
+ if ~strcmpi(extension, '.xml')
+ name = [name, extension];
+ end
+ filename = fullfile(path, name);
+ end
+ if length(self.world) == 1
+ self.world.export([filename, '.xml']);
+ else
+ for idx=1:length(self.world)
+ self.world.export([filename, '-', num2str(idx), '.xml']);
+ end
+ end
+ end
+ end
end
\ No newline at end of file
diff --git a/+rtd/+planner/+trajectory/Trajectory.m b/+rtd/+trajectory/Trajectory.m
similarity index 80%
rename from +rtd/+planner/+trajectory/Trajectory.m
rename to +rtd/+trajectory/Trajectory.m
index 9b853617..46cc9480 100644
--- a/+rtd/+planner/+trajectory/Trajectory.m
+++ b/+rtd/+trajectory/Trajectory.m
@@ -5,7 +5,7 @@
% the actual trajectory generated from those parameters. This can also be
% used by an :mat:class:`+rtd.+planner.+trajopt.Objective` object as part
% of the objective function call. It should be generated with some
-% :mat:class:`+rtd.+planner.+trajectory.TrajectoryFactory`.
+% :mat:class:`+rtd.+trajectory.TrajectoryFactory`.
%
% Note:
% The functions `validate(self, throwOnError)`, `setParameters(self,
@@ -23,28 +23,32 @@
% Last Revised: 2023-02-02
%
% See also validate, setParameters, getCommand,
-% rtd.planner.trajectory.TrajectoryFactory, arguments
+% rtd.trajectory.TrajectoryFactory, arguments,
+% rtd.trajectory.TrajectoryContainer
%
-% ---More Info ---
+% --- Revision History ---
+% 2023-09-07 Removed the start state and trajoptprops properties and
+% added the startTime property to reduce class interdependencies.
+% 2023-02-02 Added vectorized property
+%
+% --- More Info ---
%
properties
- % Properties from the trajectory optimization, which also describe
- % properties for the trajectory.
- trajOptProps rtd.planner.trajopt.TrajOptProps %
-
% The parameters used for this trajectory
trajectoryParams(:,1) double %
- % The initial state for this trajectory
- startState(1,1) rtd.entity.states.EntityState %
+ % The time at which this trajectory is valid
+ startTime(1,1) double
end
+
properties (Abstract, Constant)
% Set to true if this trajectory supports getting commands for a
% time vector instead of just a single moment in time.
vectorized(1,1) logical
end
+ % Expected interfaces for subclasses
methods (Abstract)
% Validates if the trajectory is parameterized right.
%
@@ -68,7 +72,7 @@
%
% Arguments:
% trajectoryParams: the parameters of the trajectory to set.
- % options: Any additiona keyword arguments or choice.
+ % options: Any additional keyword arguments or choice.
%
setParameters(self, trajectoryParams, options)
@@ -80,7 +84,7 @@
% time: Time to use to calculate the desired state for this trajectory
%
% Returns:
- % rtd.entity.states.EntityState: Desired state at the given time
+ % rtd.entity.states.BaseEntityStateInstance: Desired state at the given time
%
command = getCommand(self, time)
end
diff --git a/+rtd/+trajectory/TrajectoryContainer.m b/+rtd/+trajectory/TrajectoryContainer.m
new file mode 100644
index 00000000..9f6fd5f7
--- /dev/null
+++ b/+rtd/+trajectory/TrajectoryContainer.m
@@ -0,0 +1,186 @@
+classdef TrajectoryContainer < handle
+% Container class for holding a set of trajectories and consolidating their commands
+%
+% TrajectoryContainer is a class that holds a set of trajectories and
+% provides a method to generate a command based on the time. This class is
+% ensure that the commands are generated based on the correct trajectory.
+%
+% Note:
+% `setInitialTrajectory` must be called before any other methods are
+% called. The return type of the initial trajectory must be the same as
+% the return type of the other trajectories.
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-12
+%
+% See also: rtd.trajectory.Trajectory
+%
+% --- More Info ---
+%
+
+ properties (SetAccess=private)
+ % startTimes is a vector of start times for each trajectory
+ % The last element is always inf to ensure that the last trajectory is always used
+ startTimes(1,:) double = [inf]
+
+ % trajectories is a cell array of trajectories that correspond to the start times
+ trajectories(1,:) cell = {}
+ end
+
+ methods
+ function setInitialTrajectory(self, initialTrajectory)
+ % sets the initial trajectory for the container
+ %
+ % setInitialTrajectory sets the initial trajectory for the container.
+ % This method must be called before any other methods are called.
+ %
+ % Arguments:
+ % initialTrajectory (rtd.trajectory.Trajectory): The initial trajectory
+ %
+ % Raises:
+ % BadInitialTrajectory: If the initial trajectory is invalid or doesn't start at 0
+ %
+ arguments
+ self(1,1) rtd.trajectory.TrajectoryContainer
+ initialTrajectory(1,1) rtd.trajectory.Trajectory
+ end
+
+ if initialTrajectory.startTime ~= 0
+ errMsg = MException('TrajectoryContainer:BadInitialTrajectory', ...
+ 'Provided initial trajectory does not start at 0!');
+ throw(errMsg)
+ elseif ~initialTrajectory.validate()
+ errMsg = MException('TrajectoryContainer:BadInitialTrajectory', ...
+ 'Provided initial trajectory is invalid!');
+ throw(errMsg)
+ end
+ if ~self.isValid()
+ self.startTimes = [initialTrajectory.startTime, inf];
+ self.trajectories = {initialTrajectory};
+ else
+ self.startTimes(1) = initialTrajectory.startTime;
+ self.trajectories{1} = initialTrajectory;
+ end
+ end
+
+ function clear(self)
+ % clears the container
+ %
+ % `clear` clears the container and resets it to the initial state.
+ % It's expected that there is already some initial trajectory set.
+ % If not, a warning is shown.
+ %
+ if self.isValid()
+ self.startTimes = [0, inf];
+ self.trajectories = self.trajectories(1);
+ else
+ warning('clear() for rtd.trajectory.TrajectoryContainer called before valid initial trajectory was set!')
+ end
+ end
+
+ function valid = isValid(self, throwIfInvalid)
+ % Checks if the container is valid
+ %
+ % `isValid` checks if the container is valid. If `throwIfInvalid` is
+ % true, then an error is thrown if the container is invalid. The container
+ % is considered invalid if the initial trajectory hasn't been set.
+ %
+ % Arguments:
+ % throwIfInvalid (logical, optional): If true, an error is thrown if the container is invalid
+ %
+ % Returns:
+ % valid (logical): True if the container is valid, false otherwise
+ %
+ % Raises:
+ % InvalidContainer: If the container is invalid and `throwIfInvalid` is true
+ %
+ arguments
+ self(1,1) rtd.trajectory.TrajectoryContainer
+ throwIfInvalid(1,1) logical = false
+ end
+
+ valid = length(self.startTimes) >= 2 && length(self.trajectories) >= 1;
+ if ~valid && throwIfInvalid
+ errMsg = MException('TrajectoryContainer:InvalidContainer', ...
+ 'Initial trajectory for the container has not been set!');
+ throw(errMsg)
+ end
+ end
+
+ function setTrajectory(self, trajectory, throwIfInvalid)
+ % Sets a new trajectory for the container to the end
+ %
+ % `setTrajectory` sets a new trajectory for the container to the end.
+ % The new trajectory must start at the same time or after the end of the
+ % last trajectory. If the new trajectory is invalid, a warning is shown.
+ %
+ % Arguments:
+ % trajectory (rtd.trajectory.Trajectory): The new trajectory to add
+ % throwIfInvalid (logical, optional): If true, an error is thrown if the trajectory is invalid
+ %
+ % Raises:
+ % InvalidTrajectory: If the new trajectory is invalid or starts before the end of the last trajectory and `throwIfInvalid` is true
+ %
+ arguments
+ self(1,1) rtd.trajectory.TrajectoryContainer
+ trajectory(1,1) rtd.trajectory.Trajectory
+ throwIfInvalid(1,1) logical = false
+ end
+
+ self.isValid(true);
+ % Add the trajectory if it is valid
+ if trajectory.validate() && trajectory.startTime >= self.startTimes(end-1)
+ self.startTimes(end) = trajectory.startTime;
+ self.startTimes = [self.startTimes, inf];
+ self.trajectories = [self.trajectories, {trajectory}];
+ elseif throwIfInvalid
+ errMsg = MException('TrajectoryContainer:InvalidTrajectory', ...
+ 'Provided trajectory starts before the end of the last trajectory!');
+ throw(errMsg)
+ else
+ warning('Invalid trajectory provided to rtd.trajectory.TrajectoryContainer')
+ end
+ end
+
+ function output = getCommand(self, time)
+ % Generates a command based on the time
+ %
+ % `getCommand` generates a command based on the time. The command is
+ % generated based on the trajectory that is active at the time. If the
+ % time is before the start of the first trajectory, then the command is
+ % generated based on the initial trajectory. If the time is after the end
+ % of the last trajectory, then the command is generated based on the last
+ % trajectory.
+ %
+ % Arguments:
+ % time (double): The times to generate the commands for
+ %
+ % Returns:
+ % output: The generated commands
+ %
+ arguments
+ self(1,1) rtd.trajectory.TrajectoryContainer
+ time(1,:) double
+ end
+
+ self.isValid(true);
+ % Generate an output trajectory based on the provided
+ output(length(time)) = self.trajectories{1}.getCommand(0);
+ idx_vec = 1:length(time);
+ for i=1:(length(self.startTimes)-1)
+ mask = (time >= self.startTimes(i) & time < self.startTimes(i+1));
+ if sum(mask) == 0
+ continue
+ elseif self.trajectories{i}.vectorized
+ output(mask) = self.trajectories{i}.getCommand(time(mask));
+ else
+ % Iterate through the relevant indices
+ for j=idx_vec(mask)
+ output(j) = self.trajectories{i}.getCommand(time(j));
+ end
+ end
+ end
+ end
+ end
+end
\ No newline at end of file
diff --git a/+rtd/+planner/+trajectory/TrajectoryFactory.m b/+rtd/+trajectory/TrajectoryFactory.m
similarity index 85%
rename from +rtd/+planner/+trajectory/TrajectoryFactory.m
rename to +rtd/+trajectory/TrajectoryFactory.m
index 523e9be9..ae6af014 100644
--- a/+rtd/+planner/+trajectory/TrajectoryFactory.m
+++ b/+rtd/+trajectory/TrajectoryFactory.m
@@ -2,7 +2,7 @@
% Base class for the Trajectory factory object
%
% This should be used to initialize desired instaces of
-% :mat:class:`+rtd.+planner.+trajectory.Trajectory`.
+% :mat:class:`+rtd.+trajectory.Trajectory`.
%
% Note:
% `createTrajectory(self, robotState, rsInstances, trajectoryParams,
@@ -13,8 +13,8 @@
% Written: 2023-01-25
% Last Revised: 2023-02-02
%
-% See also createTrajectory, rtd.planner.trajectory.Trajectory,
-% rtd.planner.trajopt.TrajOptProps
+% See also createTrajectory, rtd.trajectory.Trajectory,
+% rtd.planner.trajopt.TrajOptProps, rtd.trajectory.TrajectoryContainer
%
% --- More Info ---
%
@@ -39,7 +39,7 @@
% options: Additional optional keyword arguments if desired
%
% Returns:
- % rtd.planner.trajectory.Trajectory: Desired Trajectory Object
+ % rtd.trajectory.Trajectory: Desired Trajectory Object
%
trajectory = createTrajectory(self, robotState, rsInstances, trajectoryParams, options)
end
diff --git a/+rtd/+util/+containers/VarLogger.m b/+rtd/+util/+containers/VarLogger.m
index fb3388dc..516d60ad 100644
--- a/+rtd/+util/+containers/VarLogger.m
+++ b/+rtd/+util/+containers/VarLogger.m
@@ -5,11 +5,6 @@
% validation and return flattened or stepwise data for analysis later or
% for summarizing.
%
-% Warning:
-% The current documentation engine is unable to document the `get`
-% method of this class. Please check the source for how to use it!
-% https://github.com/sphinx-contrib/matlabdomain/issues/151
-%
% --- More Info ---
% Author: Adam Li (adamli@umich.edu)
% Written: 2022-12-10
@@ -185,7 +180,11 @@ function addStruct(self, struct_data)
for i=1:num_res
entry = self.log_entries(key_name{i});
if options.flatten && ~isempty(entry)
- values{i} = [entry{:}];
+ try
+ values{i} = [entry{:}];
+ catch
+ values{i} = {entry};
+ end
else
values{i} = {entry};
end
diff --git a/+rtd/+util/+mixins/UUID.m b/+rtd/+util/+mixins/UUID.m
index 2744aaec..01ee9b74 100644
--- a/+rtd/+util/+mixins/UUID.m
+++ b/+rtd/+util/+mixins/UUID.m
@@ -5,24 +5,26 @@
% object is also stored in a table or similar, this acts as a clearly
% unique identifier for lookups and vice versa.
%
-% Warning:
-% If you generate an array of a class derived from this through last
-% object placement in MATLAB, all prior objects in that array will
-% share the same UUID due to how MATLAB initializes arrays of handle
-% objects.
-% https://www.mathworks.com/help/matlab/matlab_oop/initializing-arrays-of-handle-objects.html
-%
% --- More Info ---
% Author: Adam Li (adamli@umich.edu)
% Written: 2022-10-05
-% Last Revised: 2023-01-18 (Adam Li)
+% Last Revised: 2023-09-12 (Adam Li)
+%
+% See also: rtd.functional.uuid
+%
+% --- Revision History ---
+% 2023-09-12 (Adam Li): Created an internal noncopyable property to store the UUID to ensure each hard instance can generate a unique UUID.
%
% --- More Info ---
%
- properties
+ properties (NonCopyable, Access=private)
% A character-array string representing the UUID generated.
- uuid
+ rtd_internal_uuid(1,:) char
+ end
+ properties (Dependent)
+ % A character-array string representing the UUID generated for this object.
+ uuid(1,:) char
end
methods
function self = UUID()
@@ -32,7 +34,16 @@
% from this mixin. It will generate and associate a UUID to
% the `uuid` property of this class.
%
- self.uuid = rtd.functional.uuid();
+ self.rtd_internal_uuid = rtd.functional.uuid();
+ end
+
+ function uuid = get.uuid(self)
+ % If the UUID has not been generated, generate it.
+ if isempty(self.rtd_internal_uuid)
+ self.rtd_internal_uuid = rtd.functional.uuid();
+ end
+ % return the UUID
+ uuid = self.rtd_internal_uuid;
end
end
end
\ No newline at end of file
diff --git a/+rtd/+util/MexWrapper.m b/+rtd/+util/MexWrapper.m
index 91714ef1..871a1dfc 100644
--- a/+rtd/+util/MexWrapper.m
+++ b/+rtd/+util/MexWrapper.m
@@ -59,6 +59,7 @@
% Check if we need to do a compilation update
function files_updated = checkFileDateTimes(self)
+ %
% Get the list of functions
fcns = self.mex_functionsToBuild();
fcn_list = string(fieldnames(fcns).');
diff --git a/+rtd/+util/RangeScaler.m b/+rtd/+util/RangeScaler.m
new file mode 100644
index 00000000..165b24d3
--- /dev/null
+++ b/+rtd/+util/RangeScaler.m
@@ -0,0 +1,105 @@
+classdef RangeScaler
+% Scales parameters from one range to another.
+%
+% This is a utility class for scaling parameters from one range to another.
+% It stores the input and output ranges and provides methods for scaling
+% parameters from the input to the output range (scaleout) and from the
+% output to the input range (scalein).
+%
+% --- More Info ---
+% Author: Adam Li (adamli@umich.edu)
+% Written: 2023-09-07
+%
+% See also: rescale
+%
+% --- More Info ---
+%
+
+ properties
+ % The input range of the parameters. The first column is the lower
+ % bound and the second column is the upper bound.
+ in_range(:,2) double
+
+ % The output range of the parameters. The first column is the lower
+ % bound and the second column is the upper bound.
+ out_range(:,2) double
+ end
+
+ properties (Dependent)
+ % The number of parameters to be scaled.
+ num_parameters(1,1) double
+ end
+
+ methods
+ function self = RangeScaler(in_range, out_range)
+ % Constructor for the RangeScaler class.
+ %
+ % Arguments:
+ % in_range (double): The input range of the parameters. The
+ % first column is the lower bound and the second column
+ % is the upper bound.
+ % out_range (double): The output range of the parameters.
+ % The first column is the lower bound and the second
+ % column is the upper bound.
+ %
+ arguments
+ in_range(:,2) double
+ out_range(:,2) double
+ end
+
+ self.in_range = in_range;
+ self.out_range = out_range;
+ end
+
+ function scaled_out = scaleout(self, parameters)
+ % Scales parameters from the input range to the output range.
+ %
+ % Arguments:
+ % parameters (double): The parameters to be scaled. The
+ % length of the vector must match the number of parameters.
+ %
+ % Returns:
+ % scaled_out (double): The scaled parameters.
+ %
+ arguments
+ self(1,1) rtd.util.RangeScaler
+ parameters(:,1) double
+ end
+
+ scaled_out = rescale(parameters, ...
+ self.out_range(:,1), ...
+ self.out_range(:,2), ...
+ 'InputMin', self.in_range(:,1), ...
+ 'InputMax', self.in_range(:,2));
+ end
+ function scaled_in = scalein(self, parameters)
+ % Scales parameters from the output range to the input range.
+ %
+ % Arguments:
+ % parameters (double): The parameters to be scaled. The
+ % length of the vector must match the number of parameters.
+ %
+ % Returns:
+ % scaled_in (double): The scaled parameters.
+ %
+ arguments
+ self(1,1) rtd.util.RangeScaler
+ parameters(:,1) double
+ end
+
+ scaled_in = rescale(parameters, ...
+ self.in_range(:,1), ...
+ self.in_range(:,2), ...
+ 'InputMin', self.out_range(:,1), ...
+ 'InputMax', self.out_range(:,2));
+ end
+ end
+
+ % methods for dependent properties
+ methods
+ function num_parameters = get.num_parameters(self)
+ num_parameters = size(self.in_range);
+ num_parameters = num_parameters(1);
+ end
+ end
+end
diff --git a/+waitr/+agent/WaitrAgentState.m b/+waitr/+agent/WaitrAgentState.m
new file mode 100644
index 00000000..a480ff94
--- /dev/null
+++ b/+waitr/+agent/WaitrAgentState.m
@@ -0,0 +1,78 @@
+classdef WaitrAgentState < armour.agent.ArmourAgentState
+ % Wrapper for adding force checks
+
+ methods
+ function random_init(self, options)
+ arguments
+ self
+ options.x_pos_range = []
+ options.y_pos_range = []
+ options.z_pos_range = []
+ options.vel_range = []
+ options.random_position = true
+ options.random_velocity = false
+ options.save_to_options = false
+ end
+
+ x_pos_range = options.x_pos_range;
+ y_pos_range = options.y_pos_range;
+ z_pos_range = options.z_pos_range;
+ vel_range = options.vel_range;
+
+ if isempty(x_pos_range)
+ x_pos_range = [-0.4,0.6];
+ end
+ if isempty(y_pos_range)
+ y_pos_range = [-0.5,0.5];
+ end
+ if isempty(z_pos_range)
+ z_pos_range = [0.2,0.6];
+ end
+ if isempty(vel_range)
+ vel_range = [self.entity_info.joints(1:self.entity_info.num_q).velocity_limits];
+ end
+
+ ik = robotics.InverseKinematics('RigidBodyTree',self.entity_info.robot);
+
+ weights = [0.25 0.25 0.25 1 1 1];
+ initialguess = homeConfiguration(self.entity_info.robot);
+
+ % reset
+ self.state = zeros(self.n_states,1);
+ self.time = 0;
+ self.step_start_idxs = 0;
+
+ % Make the random configuration
+ if options.random_position
+ pitch = 1;
+ roll = 1;
+ while abs(pitch) > 0.001 || abs(roll) > 0.001
+ % choose random x,y,z components for location of 10th joint
+ % in taskspace
+ rand_x = rtd.random.deprecation.rand_range(x_pos_range(1),x_pos_range(2));
+ rand_y = rtd.random.deprecation.rand_range(y_pos_range(1),y_pos_range(2));
+ rand_z = rtd.random.deprecation.rand_range(z_pos_range(1),z_pos_range(2));
+
+ % solving for a config other than home config
+ new_config = ik('cube_link',[1 0 0 rand_x; 0 1 0 rand_y; 0 0 1 rand_z; 0 0 0 1],weights,initialguess);
+ self.state(self.position_indices) = new_config;
+
+ T = self.entity_info.robot.getTransform(new_config,'cube_link');
+% yaw=atan2(T(2,1),T(1,1));
+ pitch=atan2(-T(3,1),sqrt(T(3,2)^2+T(3,3)^2));
+ roll=atan2(T(3,2),T(3,3));
+ end
+ end
+ if options.random_velocity
+ self.state(self.velocity_indices) = rtd.random.deprecation.rand_range(vel_range(1,:),vel_range(2,:))';
+ end
+
+ % Take these initials and merge them in again.
+ if options.save_to_options
+ merge.initial_position = self.position;
+ merge.initial_velocity = self.velocity;
+ self.mergeoptions(merge);
+ end
+ end
+ end
+end
\ No newline at end of file
diff --git a/+waitr/+agent/WaitrDynamics.m b/+waitr/+agent/WaitrDynamics.m
new file mode 100644
index 00000000..4db6ad0d
--- /dev/null
+++ b/+waitr/+agent/WaitrDynamics.m
@@ -0,0 +1,168 @@
+classdef WaitrDynamics < armour.agent.ArmourDynamics
+ % Wrapper for adding force checks
+
+ % Extra properties we define
+ properties
+ u_s = 0.609382421;
+ surf_rad = 0.029;
+ end
+
+ methods (Static)
+ function options = defaultoptions()
+ options = armour.agent.ArmourDynamics.defaultoptions();
+ options.u_s = 0.609382421;
+ options.surf_rad = 0.029;
+ end
+ end
+
+ methods
+ function self = WaitrDynamics(arm_info, arm_state_component, controller_component, optionsStruct, options)
+ arguments
+ arm_info armour.agent.ArmourAgentInfo
+ arm_state_component armour.agent.ArmourAgentState
+ controller_component armour.agent.ArmourController
+ optionsStruct struct = struct()
+ options.time_discretization
+ options.measurement_noise_points
+ options.measurement_noise_pos_sigma
+ options.measurement_noise_vel_sigma
+ options.log_controller
+ options.verboseLevel
+ options.name
+ options.u_s
+ options.surf_rad
+ end
+ self@armour.agent.ArmourDynamics(arm_info, arm_state_component, controller_component);
+ self.mergeoptions(optionsStruct, options);
+
+ self.reset()
+ end
+
+ function reset(self, optionsStruct, options)
+ arguments
+ self
+ optionsStruct.options struct = struct()
+ options.time_discretization
+ options.measurement_noise_points
+ options.measurement_noise_pos_sigma
+ options.measurement_noise_vel_sigma
+ options.log_controller
+ options.verboseLevel
+ options.name
+ options.u_s
+ options.surf_rad
+ end
+ options = self.mergeoptions(optionsStruct.options, options);
+ reset@armour.agent.ArmourDynamics(self);
+
+ % Grasp constraint parameters
+ self.u_s = options.u_s;
+ self.surf_rad = options.surf_rad;
+ end
+
+ % Check that the grasp constraints aren't violated
+ function out = grasp_constraint_check(self, t_check_step)
+ % if not logging, skip
+ if isempty(self.controller_log)
+ self.vdisp('Not logging controller inputs, skipping grasp checks!', 'INFO');
+ out = false;
+ return
+ end
+
+ % retrieve the last log entry
+ entries = self.controller_log.get('input_time', flatten=false);
+ t_input = entries.input_time{end};
+ % interpolate for the t_check_step and get agent input
+ % trajectory interpolated to time
+ t_check = t_input(1):t_check_step:t_input(end);
+ states = self.robot_state.get_state(t_check);
+ z_check = states.getStateSpace( ...
+ position_idxs=self.robot_state.position_indices, ...
+ velocity_idxs=self.robot_state.velocity_indices);
+
+ % run grasp check
+ self.vdisp('Running grasp checks!', 'INFO');
+ out = false;
+ sep_val = false; % optimism!
+ slip_val = false; % more optimism!!
+ tip_val = false; % even more optimism!!!
+ t_idx = 1;
+
+ while ~sep_val && ~slip_val && ~tip_val && t_idx <= length(t_check)
+ out = false;
+ sep_val = false;
+ slip_val = false;
+ tip_val = false;
+
+ t = t_check(t_idx);
+ z = z_check(:,t_idx);
+ q = z(self.robot_state.position_indices);
+ qd = z(self.robot_state.velocity_indices);
+
+ % True dynamics
+ [M, C, g] = self.calculate_dynamics(q, qd, self.robot_info.params.true);
+
+ for i = 1:length(q)
+ M(i,i) = M(i,i) + self.robot_info.transmission_inertia(i);
+ end
+
+ u = self.controller.getControlInputs(t, z);
+
+ % calculate acceleration
+ qdd = M\(u-C*qd-g);
+
+ %% Calculating forces
+ % call RNEA on current configuration (assuming at rest? cannot)
+ % (also assuming use_gravity is true)
+ [tau, f, n] = armour.legacy.dynamics.rnea(q, qd, qd, qdd, true, self.robot_info.params.true);
+
+ fx = f(1,10);
+ fy = f(2,10);
+ fz = f(3,10);
+
+ %% Calculating Constraints (written as <0)
+ sep = -1*fz; %fz; %
+% slip = sqrt(fx^2+fy^2) - self.u_s*abs(fz);
+ slip2 = fx^2+fy^2 - self.u_s^2*fz^2;
+% ZMP = cross([0;0;1],n(:,10))./dot([0;0;1],f(:,10));
+% tip = sqrt(ZMP(1)^2 + ZMP(2)^2) - self.surf_rad; % + tip_threshold;
+ ZMP_top = cross([0;0;1],n(:,10));
+ ZMP_bottom = dot([0;0;1],f(:,10));
+ tip2 = ZMP_top(1)^2 + ZMP_top(2)^2 - self.surf_rad^2*ZMP_bottom^2;
+
+ if (sep > 1e-6) || (slip2 > 1e-6) || (tip2 > 1e-6)
+ out = true;
+ end
+
+ if sep > 1e-6
+ sep_val = true;
+ end
+
+ if slip2 > 1e-6
+ slip_val = true;
+ end
+
+ if tip2 > 1e-6
+ tip_val = true;
+ end
+
+ t_idx = t_idx + 1;
+
+ end
+
+ if out
+ if sep_val
+ self.vdisp(['Grasp separation violation detected at t = ',num2str(t_check(t_idx))], 'ERROR');
+ end
+ if slip_val
+ self.vdisp(['Grasp slipping violation detected at t = ',num2str(t_check(t_idx))], 'ERROR');
+ end
+ if tip_val
+ self.vdisp(['Grasp tipping violation detected at t = ',num2str(t_check(t_idx))], 'ERROR');
+ end
+ else
+ self.vdisp('No grasp violations detected', 'INFO');
+ end
+ end
+ end
+end
\ No newline at end of file
diff --git a/+waitr/+agent/WaitrMexController.m b/+waitr/+agent/WaitrMexController.m
new file mode 100644
index 00000000..79003d9c
--- /dev/null
+++ b/+waitr/+agent/WaitrMexController.m
@@ -0,0 +1,46 @@
+classdef WaitrMexController < armour.agent.ArmourController
+ properties(Access=private)
+ mex_controller
+ end
+ methods
+ function self = WaitrMexController(varargin)
+ self = self@armour.agent.ArmourController(varargin{:});
+ self.mex_controller = armour.agent.ArmourMexController(varargin{:});
+ end
+
+ function reset(self, varargin)
+ robot_file = fullfile(basepath(self), "kinova_with_gripper.txt");
+ self.mex_controller.reset("robot_file", robot_file, varargin{:})
+
+ % Set all variables to match
+ self.n_inputs = self.mex_controller.n_inputs;
+ self.name = self.mex_controller.name;
+ self.Kr = self.mex_controller.Kr;
+ self.alpha_constant = self.mex_controller.alpha_constant;
+ self.V_max = self.mex_controller.V_max;
+ self.r_norm_threshold = self.mex_controller.r_norm_threshold;
+ self.ultimate_bound = self.mex_controller.ultimate_bound;
+ self.ultimate_bound_position = self.mex_controller.ultimate_bound_position;
+ self.ultimate_bound_velocity = self.mex_controller.ultimate_bound_velocity;
+ self.trajectories = self.mex_controller.trajectories;
+ end
+
+ function setTrajectory(self, trajectory)
+ self.mex_controller.setTrajectory(trajectory);
+ end
+
+ function [u, info] = getControlInputs(self, varargin)
+ [u, info] = self.mex_controller.getControlInputs(varargin{:});
+ end
+
+ function out = ultimate_bound_check(self, varargin)
+ out = self.mex_controller.ultimate_bound_check(varargin{:});
+ end
+ end
+end
+
+% Utility to retrieve the basepath of the current class file.
+function currpath = basepath(obj)
+ currfile = which(class(obj));
+ [currpath, ~, ~] = fileparts(currfile);
+end
diff --git a/+waitr/+agent/kinova_with_gripper.txt b/+waitr/+agent/kinova_with_gripper.txt
new file mode 100644
index 00000000..7a2fbc00
--- /dev/null
+++ b/+waitr/+agent/kinova_with_gripper.txt
@@ -0,0 +1,34 @@
+numJoints <7>
+gravity <0.000000000000000000000000000000 0.000000000000000000000000000000 -9.810000000000000497379915032070>
+twist 0 <0 0 1 0 0 0>
+twist 1 <0 0 1 0 0 0>
+twist 2 <0 0 1 0 0 0>
+twist 3 <0 0 1 0 0 0>
+twist 4 <0 0 1 0 0 0>
+twist 5 <0 0 1 0 0 0>
+twist 6 <0 0 1 0 0 0>
+inertia 0 <1.377299999999999968736119626556 0.012130140316820799822883358843 0.000000671690244399999913829484 -0.000000323890743999999863780154 0.000000671690244399999913829484 0.012243201814671700322634251279 -0.000599165376991999927905330203 -0.000000323890743999999863780154 -0.000599165376991999711064895706 0.001556939959332499963384766062 0.000000000000000000000000000000 0.101038727999999994322521956747 -0.014274337199999998990196381499 -0.101038727999999994322521956747 0.000000000000000000000000000000 0.000031677899999999997011045083 0.014274337199999998990196381499 -0.000031677899999999997011045083 0.000000000000000000000000000000>
+inertia 1 <1.163599999999999967670305522915 0.022831611687502400415139547363 -0.000000098336671999999593395472 -0.000000679812355199999901043029 -0.000000098336671999998746362525 0.001277151081191999978936557625 -0.002229538962064000172380584530 -0.000000679812355199999901043029 -0.002229538962064000172380584530 0.022793465111769599384938800313 0.000000000000000000000000000000 0.015450280799999998801452782971 -0.115871288000000002837985846327 -0.015450280799999998801452782971 0.000000000000000000000000000000 0.000051198399999999994174765261 0.115871288000000002837985846327 -0.000051198399999999994174765261 0.000000000000000000000000000000>
+inertia 2 <1.163599999999999967670305522915 0.027155640247762000138997962040 -0.000000340008574399999980911570 -0.000013035881772799999263161286 -0.000000340008574399999980911570 0.027299324388159997012426316587 -0.000305006610299199984286111498 -0.000013035881772799999263161286 -0.000305006610299199984286111498 0.001094320365061199991016382960 0.000000000000000000000000000000 0.137179131199999998491634300990 -0.007727467600000000221005524992 -0.137179131199999998491634300990 0.000000000000000000000000000000 0.000051198399999999994174765261 0.007727467600000000221005524992 -0.000051198399999999994174765261 0.000000000000000000000000000000>
+inertia 3 <0.930200000000000026822988274944 0.013655745345303999319086507569 -0.000002263773440800000211062856 -0.000000251254461600000035755335 -0.000002263773440800000211062856 0.000840462770872000067279294910 -0.001553565791813600171361597901 -0.000000251254461599999982815776 -0.001553565791813599954521163404 0.013615283177201599473793436346 0.000000000000000000000000000000 0.013958581200000000355210261205 -0.070209635600000000477116657294 -0.013958581200000000355210261205 0.000000000000000000000000000000 0.000016743600000000000084694751 0.070209635600000000477116657294 -0.000016743600000000000084694751 0.000000000000000000000000000000>
+inertia 4 <0.678100000000000036060043839825 0.004423677212245300319204410755 0.000000006395839199999999032985 0.000000043319062299999997344341 0.000000006395839199999999860166 0.004374351657589000026693515366 -0.000152585395613599988495073800 0.000000043319062299999997344341 -0.000152585395613599934284965176 0.000459325556012499995692938137 0.000000000000000000000000000000 0.043319062300000002463384873863 -0.006395839199999999585832810567 -0.043319062300000002463384873863 0.000000000000000000000000000000 -0.000000678100000000000017066177 0.006395839199999999585832810567 0.000000678100000000000017066177 0.000000000000000000000000000000>
+inertia 5 <0.678100000000000036060043839825 0.003106934067520900327047383982 0.000000030842022300000004621856 0.000000006543665000000000481002 0.000000030842022300000004621856 0.000473146367928100014359216186 -0.000575625515195000016059612147 0.000000006543665000000000481002 -0.000575625515195000016059612147 0.003043787700949000286920753666 0.000000000000000000000000000000 0.006543665000000000592228488472 -0.030842022300000004192410685278 -0.006543665000000000592228488472 0.000000000000000000000000000000 -0.000000678100000000000017066177 0.030842022300000004192410685278 0.000000678100000000000017066177 0.000000000000000000000000000000>
+inertia 6 <1.323749999999999982236431605997 0.021801918982366282062645979067 0.000001515296016412691680627189 0.001492185945646744109349679519 0.000001515296016412692104143663 0.020931317522264913238982941834 -0.001134167016442727982800109388 0.001492185945646744109349679519 -0.001134167016442727549119240393 0.002068872081646421206418962058 0.000000000000000000000000000000 0.113154200867412818576340782784 -0.013560744160345391054467434344 -0.113154200867412818576340782784 0.000000000000000000000000051699 -0.005336008398015326149865789063 0.013560744160345387585020482391 0.005336008398015325282504051074 0.000000000000000000000092321080>
+Xtree 0 <1.000000000000000000000000000000 -0.000000000000000000000000000000 -0.000000000000000002762900000000 -0.000000000000000000000020297397 -0.999999999973015141208065870160 -0.000007346410206643587117926898 -0.000000000000000002762899999925 0.000007346410206643587117926898 -0.999999999973015141208065870160 -0.000000000000000000000000000000 -0.000000000000000000000000000000 0.156430000000000041238124026677>
+Xtree 1 <1.000000000000000000000000000000 -0.000000000000000111020000000000 -0.000000000000000021343000000000 0.000000000000000021342592200625 -0.000003673205103346573931351847 0.999999999993253729790865236282 -0.000000000000000111020078396468 -0.999999999993253729790865236282 -0.000003673205103346573931351847 -0.000000000000000000000000000000 0.005374999999999998702426839969 -0.128379999999999994120258861585>
+Xtree 2 <1.000000000000000000000000000000 -0.000000000000000291220000000000 -0.000000000000000000000000000000 -0.000000000000000000001069710790 -0.000003673205103346573931351847 -0.999999999993253729790865236282 0.000000000000000291219999998035 0.999999999993253729790865236282 -0.000003673205103346573931351847 -0.000000000000000000000000000000 -0.210379999999999983684162430109 -0.006374999999999998723243521681>
+Xtree 3 <1.000000000000000000000000000000 -0.000000000000000166530000000000 0.000000000000000066954000000000 -0.000000000000000066954611698394 -0.000003673205103346573931351847 0.999999999993253729790865236282 -0.000000000000000166529754063102 -0.999999999993253729790865236282 -0.000003673205103346573931351847 0.000000000000000000000000000000 0.006374999999999999590605259669 -0.210379999999999983684162430109>
+Xtree 4 <1.000000000000000000000000000000 -0.000000000000000063730000000000 -0.000000000000000222040000000000 -0.000000000000000222040234091863 -0.000003673205103346573931351847 -0.999999999993253729790865236282 0.000000000000000063729184401109 0.999999999993253729790865236282 -0.000003673205103346573931351847 -0.000000000000000000000000000000 -0.208429999999999976401099388568 -0.006374999999999998723243521681>
+Xtree 5 <1.000000000000000000000000000000 -0.000000000000008215700000000000 -0.000000000000000000000000000921 -0.000000000000000000030177950247 -0.000003673205103346573931351847 0.999999999993253729790865236282 -0.000000000000008215699999944574 -0.999999999993253729790865236282 -0.000003673205103346573931351847 0.000000000000000000000000000000 0.000175049999999999972332201392 -0.105929999999999968629538216192>
+Xtree 6 <1.000000000000000000000000000000 0.000000000000000096396000000000 0.000000000000000055511000000000 0.000000000000000055511354081905 -0.000003673205103346573931351847 -0.999999999993253729790865236282 -0.000000000000000096395796096061 0.999999999993253729790865236282 -0.000003673205103346573931351847 0.000000000000000000000000000000 -0.105929999999999982507326024006 -0.000175049999999999972332201392>
+parent <-1 0 1 2 3 4 5>
+CoM 0 <-0.000023000000000 -0.010364000000000 -0.073360000000000>
+CoM 1 <-0.000044000000000 -0.099580000000000 -0.013278000000000>
+CoM 2 <-0.000044000000000 -0.006641000000000 -0.117892000000000>
+CoM 3 <-0.000018000000000 -0.075478000000000 -0.015006000000000>
+CoM 4 <0.000001000000000 -0.009432000000000 -0.063883000000000>
+CoM 5 <0.000001000000000 -0.045483000000000 -0.009650000000000>
+CoM 6 <0.004030978959785 -0.010244188223113 -0.085480038426752>
+transI <8.02999999999999936 11.99620246153036440 9.00254278617515169 11.58064393167063599 8.46650409179141228 8.85370693737424297 8.85873036646853151>
+friction <0.0 0.0 0.0 0.0 0.0 0.0 0.0>
+damping <0.0 0.0 0.0 0.0 0.0 0.0 0.0>
\ No newline at end of file
diff --git a/+waitr/+reachsets/EEGenerator.m b/+waitr/+reachsets/EEGenerator.m
new file mode 100644
index 00000000..792543d4
--- /dev/null
+++ b/+waitr/+reachsets/EEGenerator.m
@@ -0,0 +1,207 @@
+classdef EEGenerator < rtd.planner.reachsets.ReachSetGenerator
+ % ArmTdForwardOccupancy
+ % This either encapsulates the reachable sets in memory, or enables the
+ % online computation of reachable sets. It acts as a generator for a
+ % single instance of ReachableSet
+ properties
+ cache_max_size = 1
+ end
+
+ % Additional Properties
+ properties
+ jrsGenerator
+ robot
+ u_s
+ surf_rad
+ end
+ methods
+ % An example constructor, but can take anything needed
+ function self = EEGenerator(robot, jrsGenerator, options)
+ arguments
+ robot armour.ArmourAgent
+ jrsGenerator armour.reachsets.JRS.OnlineGeneratorBase
+ options.u_s(1,1) double = 0.609382421
+ options.surf_rad(1,1) double = 0.029
+ end
+ self.robot = robot;
+ self.jrsGenerator = jrsGenerator;
+ self.u_s = options.u_s;
+ self.surf_rad = options.surf_rad;
+ end
+ end
+ methods (Access=protected)
+
+ % Obtains the relevant reachable set for the robotstate provided
+ % and outputs the singular instance of a reachable set.
+ % Returns ReachbleSet
+ function reachableSet = generateReachableSet(self, robotState)
+ % Computes the forward kinematics and occupancy
+
+ % First get the JRS (allow the use of a cached value if it
+ % exists)
+ jrsInstance = self.jrsGenerator.getReachableSet(robotState, ignore_cache=false);
+ jrsInstance = jrsInstance.rs;
+
+ % set up zeros and overapproximation of r
+ self.vdisp("Set up zeros for overapproximation", 'TRACE')
+ for j = 1:jrsInstance.n_q
+ zero_cell{j, 1} = armour.pz_roahm.polyZonotope_ROAHM(0);
+ r{j, 1} = armour.pz_roahm.polyZonotope_ROAHM(0, [], self.robot.controller.ultimate_bound);
+ end
+
+ self.vdisp("start RNEA")
+ for i = 1:jrsInstance.n_t
+ self.vdisp("RNEA interval for robust input", 'TRACE')
+ [tau_int{i, 1}, f_int{i, 1}, n_int{i, 1}] = ...
+ armour.legacy.dynamics.poly_zonotope_rnea( ...
+ jrsInstance.R{i}, ...
+ jrsInstance.R_t{i}, ...
+ jrsInstance.dq{i}, ...
+ jrsInstance.dq_a{i}, ...
+ jrsInstance.ddq_a{i}, ...
+ true, ...
+ self.robot.info.params.pz_interval);
+ end
+
+ % need to add a check somewhere to see if constraint is
+ % trivially satisfied
+
+ % iterate through all of the time steps
+ for i = 1:jrsInstance.n_t
+
+ % only checking one contact joint (for now)
+ % ASSUMING SURFACE NORMAL IS POSITIVE Z-DIRECTION
+
+ % ! this depends on the robot urdf being used!
+ % for fetch_waiter_Zac.urdf, f_int{i,1}(10),
+ % n_int{i,1}(10) are the forces/moments
+ % polyzonotopes at the contact point between
+ % tray and cup.
+
+ % extract relevant polyzonotope from f_int{i,1}
+ % ! depends on robot urdf!
+ contact_poly = f_int{i,1}{10};
+
+
+ % create individual force polyzonotopes
+ % 1. collapse grest at end of forming constraints
+ % where <0 so can always add and it doesn't affect
+ % calculations.
+ % 2. the reduce operation that is performed in the
+ % poly_zonotope_rnea() call means that the PZs output
+ % might not have G (and therefore expMat and id) or
+ % Grest so that is why there is if statements below
+ % handling empty components of the output PZs.
+
+ % centers
+ Fx_poly_c = contact_poly.c(1);
+ Fy_poly_c = contact_poly.c(2);
+ Fz_poly_c = contact_poly.c(3);
+ % generators
+ if isempty(contact_poly.G)
+ Fx_poly_G = [];
+ Fy_poly_G = [];
+ Fz_poly_G = [];
+ else
+ Fx_poly_G = contact_poly.G(1,:);
+ Fy_poly_G = contact_poly.G(2,:);
+ Fz_poly_G = contact_poly.G(3,:);
+ end
+ % Grest generators
+ if isempty(contact_poly.Grest)
+ Fx_poly_Grest = [];
+ Fy_poly_Grest = [];
+ Fz_poly_Grest = [];
+ else
+ Fx_poly_Grest = contact_poly.Grest(1,:);
+ Fy_poly_Grest = contact_poly.Grest(2,:);
+ Fz_poly_Grest = contact_poly.Grest(3,:);
+ end
+ % exponent matrices
+ if isempty(contact_poly.expMat)
+ Fx_poly_expMat = [];
+ Fy_poly_expMat = [];
+ Fz_poly_expMat = [];
+ else
+ Fx_poly_expMat = contact_poly.expMat;
+ Fy_poly_expMat = contact_poly.expMat;
+ Fz_poly_expMat = contact_poly.expMat;
+ end
+ % id matrix
+ if isempty(contact_poly.id)
+ Fx_poly_id = [];
+ Fy_poly_id = [];
+ Fz_poly_id = [];
+ else
+ Fx_poly_id = contact_poly.id;
+ Fy_poly_id = contact_poly.id;
+ Fz_poly_id = contact_poly.id;
+ end
+ % creating individual force polyzonotopes
+ Fx_poly = armour.pz_roahm.polyZonotope_ROAHM(Fx_poly_c,Fx_poly_G,Fx_poly_Grest,Fx_poly_expMat,Fx_poly_id);
+ Fy_poly = armour.pz_roahm.polyZonotope_ROAHM(Fy_poly_c,Fy_poly_G,Fy_poly_Grest,Fy_poly_expMat,Fy_poly_id);
+ Fz_poly = armour.pz_roahm.polyZonotope_ROAHM(Fz_poly_c,Fz_poly_G,Fz_poly_Grest,Fz_poly_expMat,Fz_poly_id);
+
+ % which way is the Fz/normal returned by rnea
+ % pointing? upwards out of the tray
+
+ % Need to create the correct moment vectors for the ZMP
+ % calculation. The moments need to have the
+ % cross(distance,force) added to them.
+% ZMP_Moment{i,1} = n_int{i,1}{10} + cross([0;0;cup_height],f_int{i,1}{10});
+
+ % separation constraint: Fnormal < 0
+ % ? does this need to be multiplied by -1? yes
+ % looking at the testing in
+ % Waiter_urdf_RNEA_Testing.m, the cup upright on
+ % the tray has RNEA output a positive normal force
+ % in the z-axis. This means that Fz>0 is the
+ % constraint so to rewrite as -1.*Fz<0 it
+ % needs to be multiplied by a -1.
+ sep_poly_temp = -1.*Fz_poly; % verified (matches regular rnea and constraint, but slightly more negative (safer) than regular value. should subtract Grest instead?)
+ sep_poly{i,1} = armour.pz_roahm.polyZonotope_ROAHM(sep_poly_temp.c + sum(abs(sep_poly_temp.Grest)),sep_poly_temp.G,[],sep_poly_temp.expMat,sep_poly_temp.id);
+ % create new pz with grest collapsed
+
+ % slipping constraint: Ftanx^2+Ftany^2 < u_s^2*Fnorm^2
+ % this is rewritten as:
+ % Ftanx^2+Ftany^2 - u_s^2*Fnorm^2 < 0
+
+ slip_poly_temp = Fx_poly.*Fx_poly + Fy_poly.*Fy_poly - self.u_s^2*Fz_poly.*Fz_poly;
+ slip_poly{i,1} = armour.pz_roahm.polyZonotope_ROAHM(slip_poly_temp.c + sum(abs(slip_poly_temp.Grest)),slip_poly_temp.G,[],slip_poly_temp.expMat,slip_poly_temp.id);
+ % create new pz with grest collapsed
+
+ % tipping constraint version 1
+% ZMP_top = cross(n_int{i,1}{10},[0;0;1]); % verified (same center as normal rnea)
+ ZMP_top = cross([0;0;1],n_int{i,1}{10});
+ % for the bottom component:
+ ZMP_bottom = f_int{i,1}{10}*[0,0,1]; % verified (same center as normal rnea)
+ ZMP_temp = (ZMP_bottom.*ZMP_bottom).*(self.surf_rad)^2;
+
+ % there should either be only G, only Grest or both.
+ % this should handle those three cases.
+ if isempty(ZMP_top.G)
+ ZMP_topx = armour.pz_roahm.polyZonotope_ROAHM(ZMP_top.c(1),[],ZMP_top.Grest(1,:),[],[]);
+ ZMP_topy = armour.pz_roahm.polyZonotope_ROAHM(ZMP_top.c(2),[],ZMP_top.Grest(2,:),[],[]);
+ elseif isempty(ZMP_top.Grest)
+ ZMP_topx = armour.pz_roahm.polyZonotope_ROAHM(ZMP_top.c(1),ZMP_top.G(1,:),[],ZMP_top.expMat,ZMP_top.id);
+ ZMP_topy = armour.pz_roahm.polyZonotope_ROAHM(ZMP_top.c(2),ZMP_top.G(2,:),[],ZMP_top.expMat,ZMP_top.id);
+ else
+ ZMP_topx = armour.pz_roahm.polyZonotope_ROAHM(ZMP_top.c(1),ZMP_top.G(1,:),ZMP_top.Grest(1,:),ZMP_top.expMat,ZMP_top.id);
+ ZMP_topy = armour.pz_roahm.polyZonotope_ROAHM(ZMP_top.c(2),ZMP_top.G(2,:),ZMP_top.Grest(2,:),ZMP_top.expMat,ZMP_top.id);
+ end
+
+ tip_poly_full = ZMP_topx.*ZMP_topx + ZMP_topy.*ZMP_topy - ZMP_temp;
+ tip_poly{i,1} = armour.pz_roahm.polyZonotope_ROAHM(tip_poly_full.c + sum(abs(tip_poly_full.Grest)),tip_poly_full.G,[],tip_poly_full.expMat,tip_poly_full.id);
+
+ % remove dependence of grasp constraints
+ sep_poly{i,1} = remove_dependence(sep_poly{i,1},jrsInstance.k_id(end));
+ tip_poly{i,1} = remove_dependence(tip_poly{i,1},jrsInstance.k_id(end));
+ slip_poly{i,1} = remove_dependence(slip_poly{i,1},jrsInstance.k_id(end));
+ end
+
+ reachableSet = struct;
+ reachableSet.id = 1;
+ reachableSet.rs = waitr.reachsets.EEInstance(sep_poly, slip_poly, tip_poly, jrsInstance);
+ end
+ end
+end
\ No newline at end of file
diff --git a/+waitr/+reachsets/EEInstance.m b/+waitr/+reachsets/EEInstance.m
new file mode 100644
index 00000000..11cd2dca
--- /dev/null
+++ b/+waitr/+reachsets/EEInstance.m
@@ -0,0 +1,83 @@
+classdef EEInstance < rtd.planner.reachsets.ReachSetInstance
+ % EEInstance
+ % This is just an individual instance of an original ARMTD EE.
+ properties
+ input_range = [-1.0, 1.0]
+ output_range = []
+ num_parameters = 0
+
+ % properties carried over from the original implementation
+ sep_poly
+ slip_poly
+ tip_poly
+ n_q
+ n_t
+ end
+ methods
+ % An example constructor, but can take anything needed for the
+ % respective ReachableSets class.
+ function self = EEInstance( ...
+ sep_poly, slip_poly, tip_poly, jrsInstance ...
+ )
+ self.sep_poly = sep_poly;
+ self.slip_poly = slip_poly;
+ self.tip_poly = tip_poly;
+ self.n_q = jrsInstance.n_q;
+ self.n_t = jrsInstance.n_t;
+ self.num_parameters = jrsInstance.num_parameters;
+
+ self.input_range = jrsInstance.input_range;
+ self.output_range = [];
+ end
+
+ % Handles the obstacle-frs pair or similar to generate the
+ % nlconstraint.
+ % Returns a function handle for the nlconstraint generated
+ % where the function's return type is [c, ceq, gc, gceq]
+ function nlconFunction = genNLConstraint(self, worldState)
+ constraints = {}; % cell will contain functions of $k$ for evaluating constraints
+ grad_constraints = {}; % cell will contain functions of $k$ for evaluating gradients
+ sep_poly = self.sep_poly;
+ slip_poly = self.slip_poly;
+ tip_poly = self.tip_poly;
+ % add the grasp constraints here
+ for i = 1:self.n_t
+ % adding separation constraints
+ sep_int = interval(sep_poly{i,1});
+% fprintf('ADDED GRASP SEPARATION CONSTRAINT \n')
+ constraints{end+1,1} = @(k) slice(sep_poly{i,1},k);
+ grad_sep_poly = grad(sep_poly{i,1},self.num_parameters);
+ grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_sep_poly);
+
+ % adding slipping constraints
+ slip_int = interval(slip_poly{i,1});
+% fprintf('ADDED GRASP SLIPPING CONSTRAINT \n')
+ constraints{end+1,1} = @(k) slice(slip_poly{i,1},k);
+ grad_slip_poly = grad(slip_poly{i,1},self.num_parameters);
+ grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_slip_poly);
+
+ % adding tipping constraints
+ tip_int = interval(tip_poly{i,1});
+% fprintf('ADDED GRASP TIPPING CONSTRAINT \n')
+ constraints{end+1,1} = @(k) slice(tip_poly{i,1},k);
+ grad_tip_poly = grad(tip_poly{i,1},self.num_parameters);
+ grad_constraints{end+1, 1} = @(k) cellfun(@(C) slice(C, k), grad_tip_poly);
+ end
+ nlconFunction = @(k) eval_constraints(k, length(constraints), constraints, grad_constraints);
+ end
+ end
+end
+
+% NOTE: remember that smooth constraints still need to be considered.
+function [h, heq, grad_h, grad_heq] = eval_constraints(k, n_c, constraints, grad_constraints)
+ h = zeros(n_c, 1);
+ grad_h = zeros(length(k), n_c);
+
+ for i = 1:n_c
+ h(i) = constraints{i}(k);
+ grad_h(:, i) = grad_constraints{i}(k);
+ end
+
+ grad_heq = [];
+ heq = [];
+end
diff --git a/+waitr/WaitrAgent.m b/+waitr/WaitrAgent.m
new file mode 100644
index 00000000..daa0c384
--- /dev/null
+++ b/+waitr/WaitrAgent.m
@@ -0,0 +1,32 @@
+classdef WaitrAgent < armour.ArmourAgent
+% The Agent with the robust controller for ARMOUR
+% Left to do are the helper safety check functions like check input limits
+% and glueing this together
+ methods (Static)
+ function options = defaultoptions()
+ options = armour.ArmourAgent.defaultoptions();
+
+ % These are the names for the default components
+ components = options.components;
+ components.state = 'waitr.agent.WaitrAgentState';
+ components.dynamics = 'waitr.agent.WaitrDynamics';
+ options.components = components;
+ end
+
+ function self = from_options(robot, params, options)
+ a = armour.agent.ArmourAgentInfo(robot, params, options.component_options.info);
+ self = waitr.WaitrAgent(a, optionsStruct=options);
+ end
+ end
+
+ methods
+ % Update this agent's state by t_move
+ function results = update(self, t_move)
+ update@armour.ArmourAgent(self, t_move);
+
+ % Added post-movement check
+ t_check_step = 0.01;
+ results.checks.grasp_constraints = self.dynamics.grasp_constraint_check(t_check_step);
+ end
+ end
+end
\ No newline at end of file
diff --git a/+waitr/WaitrPlanner.m b/+waitr/WaitrPlanner.m
new file mode 100644
index 00000000..4bbc6b8e
--- /dev/null
+++ b/+waitr/WaitrPlanner.m
@@ -0,0 +1,120 @@
+classdef WaitrPlanner < rtd.planner.RtdPlanner & rtd.util.mixins.Options
+ properties
+ rsGenerators
+ objective
+ optimizationEngine
+ trajectoryFactory
+
+ trajopt
+ end
+
+ methods (Static)
+ function options = defaultoptions()
+ options.input_constraints_flag = false;
+ options.joint_limits_flag = false;
+ options.use_robust_input = false;
+ options.grasp_constraints_flag = false;
+ options.smooth_obs = false;
+ options.traj_type = 'piecewise';
+ options.verboseLevel = 'DEBUG';
+ options.u_s = 0.609382421;
+ options.surf_rad = 0.029;
+ end
+ end
+
+ % This class specifies the overall planner, so it sets up everything
+ % for any special type of rtd.planner.RtdPlanner.
+ methods
+ % The constructor would do the following
+ % - determine RTD_TrajOpt parameters
+ % - Construct OptimizationEngine(s) with parameters
+ % - Construct ReachableSets for each reachable set and trajectory type
+ % - Construct some Objective object for problem at hand
+
+ % - Create RTD_TrajOpt for each trajectory type
+ % - Create initial trajectories for each trajectory type
+ function self = WaitrPlanner(trajOptProps, robot, options)
+ arguments
+ trajOptProps (1,1) rtd.planner.trajopt.TrajOptProps
+ robot (1,1) armour.ArmourAgent
+ options.input_constraints_flag (1,1) logical
+ options.joint_limits_flag (1,1) logical
+ options.grasp_constraints_flag (1,1) logical
+ options.use_robust_input (1,1) logical
+ options.smooth_obs (1,1) logical
+ options.traj_type {mustBeMember(options.traj_type,{'piecewise','bernstein','twobernstein'})}
+ options.verboseLevel (1,1) rtd.util.types.LogLevel
+ options.u_s (1,1) double
+ options.surf_rad (1,1) double
+ end
+ options = self.mergeoptions(options);
+
+ % Create our reachable sets
+ self.rsGenerators = struct;
+ switch options.traj_type
+ case 'piecewise'
+ self.rsGenerators.jrs = armour.reachsets.JRS.PiecewiseJRSGen(robot, verboseLevel=options.verboseLevel);
+ case 'bernstein'
+ self.rsGenerators.jrs = armour.reachsets.JRS.BernsteinJRSGen(robot, verboseLevel=options.verboseLevel);
+ case 'twobernstein'
+ self.rsGenerators.jrs = armour.reachsets.JRS.TwoBernsteinJRSGen(robot, verboseLevel=options.verboseLevel);
+ end
+ self.rsGenerators.fo = armour.reachsets.FOGenerator(robot, self.rsGenerators.jrs, smooth_obs=options.smooth_obs, verboseLevel=options.verboseLevel);
+ if options.input_constraints_flag
+ self.rsGenerators.irs = armour.reachsets.IRSGenerator(robot, self.rsGenerators.jrs, use_robust_input=options.use_robust_input, verboseLevel=options.verboseLevel);
+ end
+ if options.joint_limits_flag
+ self.rsGenerators.jls = armour.reachsets.JLSGenerator(robot, self.rsGenerators.jrs, verboseLevel=options.verboseLevel);
+ end
+ if options.grasp_constraints_flag
+ self.rsGenerators.ee = waitr.reachsets.EEGenerator(robot, self.rsGenerators.jrs, u_s=options.u_s, surf_rad=options.surf_rad);
+ end
+
+ % Create the trajectoryFactory
+ self.trajectoryFactory = armour.trajectory.ArmTrajectoryFactory(trajOptProps, options.traj_type);
+
+ % Create the objective
+ self.objective = rtd.planner.trajopt.GenericArmObjective(trajOptProps, self.trajectoryFactory);
+
+ % Selection the optimization engine
+ self.optimizationEngine = rtd.planner.trajopt.FminconOptimizationEngine(trajOptProps);
+
+ % Create the trajopt object.
+ self.trajopt = {rtd.planner.trajopt.RtdTrajOpt( ...
+ trajOptProps, ...
+ self.rsGenerators, ...
+ self.objective, ...
+ self.optimizationEngine, ...
+ self.trajectoryFactory)};
+ end
+
+ % Then on each waypoint, we call for a trajectory plan:
+ % Use RTD to solve for a trajectory and return either
+ % the parameters or invalid signal (continue)
+ function [trajectory, info] = planTrajectory(self, robotState, worldState, waypoint)
+ arguments
+ self waitr.WaitrPlanner
+ robotState rtd.entity.states.ArmRobotStateInstance
+ worldState
+ waypoint
+ end
+ % Loops over each RTD_TrajOpt instance (thus, each trajectory
+ % type) with the given RobotState, rtd.sim.world.WorldState, Waypoint, and
+ % initial guess
+
+ % From the results, selects the best valid Trajectory,
+ % otherwise return an invalid trajectory which will throw when
+ % attempting to set the new trajectory, ensuring the old one
+ % continues.
+
+ % Later
+ %f = parfeval(backgroundPool, ...
+ % @self.trajopt.solveTrajOpt,...
+ % 3, ...
+ % robotState, worldState, waypoint);
+ %wait(f);
+ %[trajectory, ~] = fetchOutputs(f);
+ [trajectory, ~, info] = self.trajopt{1}.solveTrajOpt(robotState, worldState, waypoint);
+ end
+ end
+end
\ No newline at end of file
diff --git a/+waitr/WaitrSimulation.m b/+waitr/WaitrSimulation.m
new file mode 100644
index 00000000..12683e1e
--- /dev/null
+++ b/+waitr/WaitrSimulation.m
@@ -0,0 +1,75 @@
+classdef WaitrSimulation < armour.ArmourSimulation & handle
+
+ % Simulation Methods
+ methods
+ function initialize(self)
+ % Initialize the simulation
+ %
+ % This function will initialize the simulation. This needs to
+ % be called after setup and will ready the simulation for
+ % stepping.
+ %
+ arguments
+ self(1,1) waitr.WaitrSimulation
+ end
+
+ if self.simulation_state > "INITIALIZING"
+ error("This simulation currently does not support reinitialization without resetup");
+ end
+ self.simulation_state = 'INITIALIZING';
+
+ % A lot of temporaries
+ timeout = 10;
+ % Create a random start (assuming no obstacles)
+ % randomizing = true;
+ % start_tic = tic;
+ % t_cur = toc(start_tic);
+ % while randomizing && t_cur <= timeout
+ % self.agent.state.random_init(save_to_options=true);
+ % proposal_obj = self.agent.collision.getCollisionObject();
+
+ % % test it in the collision system
+ % [randomizing, ~] = self.collision_system.checkCollisionObject(proposal_obj);
+ % t_cur = toc(start_tic);
+ % end
+ self.agent.state.reset(initial_position=[0, -pi/2, 0, 0, 0, 0, 0]);
+
+ % Create the random obstacles
+ n_obstacles = 3;
+ obstacle_size_range = [0.01 0.5] ; % [min, max] side length
+ creation_buffer = 0.05;
+ world_bounds = [self.agent.info.reach_limits(1:2:6); self.agent.info.reach_limits(2:2:6)];
+ for i=1:n_obstacles
+ % Randomize an obstacle
+ [box, timed_out] = rtd.entity.BoxObstacle.randomizeBox(world_bounds, ...
+ obstacle_size_range, creation_buffer=creation_buffer, ...
+ collision_system=self.collision_system, timeout=timeout);
+ if timed_out
+ warning("Timeout occured when creating obstacle, obstacle may be overlapping with robot configuration!")
+ end
+
+ % Add it
+ self.world.addEntity(box);
+ self.collision_system.addObjects(static=box.collision.getCollisionObject)
+ self.visual_system.addObjects(static=box.visual)
+ end
+
+ % Create the goal
+ self.goal_system.reset();
+% self.goal_system.createGoal([2.19112372555967;0.393795848789382;-2.08886547149797;-1.94078143810946;-1.82357815033695;-1.80997964933365;2.12483409695310]);
+ self.goal_system.createGoal();
+ % Save the goal position (workaround for the deprecated goal
+ % system)
+ self.goal_system.reset(goal_position=self.goal_system.goal_position);
+ self.visual_system.addObjects(static=self.goal_system);
+
+ % reset the agent (We saved the position to options earlier)
+ self.agent.reset
+
+ % redraw
+ self.visual_system.redraw();
+
+ self.simulation_state = 'READY';
+ end
+ end
+end
\ No newline at end of file
diff --git a/docs/examples/examples.rst b/docs/examples/examples.rst
new file mode 100644
index 00000000..30e9b4a6
--- /dev/null
+++ b/docs/examples/examples.rst
@@ -0,0 +1,13 @@
+RTD Code Examples
+=================
+
+.. toctree::
+ :maxdepth: 1
+ :caption: Simulator Examples:
+
+ sim/box2d_agent
+
+
+* :ref:`genindex`
+* :ref:`mat-modindex`
+* :ref:`search`
diff --git a/docs/examples/sim/box2d_agent.rst b/docs/examples/sim/box2d_agent.rst
new file mode 100644
index 00000000..3319ff39
--- /dev/null
+++ b/docs/examples/sim/box2d_agent.rst
@@ -0,0 +1,54 @@
+Box2D Agent example
+===================
+
+Can be found under scripts/armour/tasks
+
+Usage
+-----
+*Note: make sure that you add "rtd-code-architecture-base" to path in the workspace and rehash*
+
+To use the BoxAgent, first create the BoxAgentInfo, GenericEntityState, and BoxAgentVisual objects.
+Initialize GenericEntityState with ntates=2 if not so already by default.
+Create a new BoxAgent using the created info, state, and visual components.
+Feel free to define the movements of the agent by using `boxagent.state.commit_state_data(time, [x y])`
+
+Finally, you can animate the defined movement of the agent using the `animate()` method under BoxAgent.
+By default it will animate at 30fps, 1second per 1t.
+You can also plot at a single moment in time using the `plot()` method under BoxAgent.
+
+There is a demo file `/scripts/demos/box2d_agent.m` for an example of the BoxAgent.
+
+BoxAgent
+--------
+Class for creating a simple 2D Box Agent.
+
+**Input:** `BoxAgentInfo, GenericEntityState, BoxAgentVisual`
+
+.. mat:autoclass:: +demos.+box2d.BoxAgent
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
+BoxAgentInfo
+------------
+Class for storing relevant information related to the BoxAgent
+
+**Input:** `(width), (height), (color)`
+
+.. mat:autoclass:: +demos.+box2d.BoxAgentInfo
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
+BoxAgentVisual
+--------------
+Class to handle visualization of the BoxAgent. Uses stored info to change looks of the agent.
+
+**Input:** `BoxAgentInfo, GenericEntityState, (options deriving from PatchVisualObject)`
+
+.. mat:autoclass:: +demos.+box2d.BoxAgentVisual
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
+
diff --git a/docs/index.rst b/docs/index.rst
index a1ca9bba..d812aa73 100644
--- a/docs/index.rst
+++ b/docs/index.rst
@@ -11,6 +11,7 @@ Welcome to RTD-code's documentation!
:caption: Contents:
introduction
+ examples/examples
planner/planner
simulation/simulation
utilities/utilities
diff --git a/docs/introduction.rst b/docs/introduction.rst
index 5ddb7a55..d38c7ce9 100644
--- a/docs/introduction.rst
+++ b/docs/introduction.rst
@@ -1,12 +1,12 @@
Introduction
============
-The current version of the rewritten codebase you are looking at is v0.1.0. It is subject to
+The current version of the rewritten codebase you are looking at is v0.2.0. It is subject to
significant changes in the near futures which may need code rewrites. This MATLAB implementation
supports r2021b and later.
-In here, the code is split up into many different, with a flattened out structure. Changing out
-smaller behaviors of an agent or simulator now involves looking at files that pertain to that
+In here, the code is split up into many different packages, with a somewhat flattened out structure.
+Changing out smaller behaviors of an agent or simulator now involves looking at files that pertain to that
specific behavior instead of the whole simulator or agent.
Additionally, the code is written with the intention of making it more explicit. For example,
diff --git a/docs/planner/armour.rst b/docs/planner/armour.rst
index c24d3de9..1ca31113 100644
--- a/docs/planner/armour.rst
+++ b/docs/planner/armour.rst
@@ -1,10 +1,8 @@
Example RTD Planner Implementation of ARMOUR
============================================
-ArmourPlanner and these related components define the ARMOUR planner
-as described by Michaux et. al. in a to be published paper. This planner
-requires the use of the matching robust controller.
-
+ArmourPlanner and these related components define the ARMOUR planner as described by Michaux et. al. in https://arxiv.org/abs/2301.13308.
+This planner requires the use of the matching robust controller.
The Base ARMOUR Planner
-----------------------
@@ -13,15 +11,15 @@ The Base ARMOUR Planner
:members:
:undoc-members:
-Reachable Sets Generation
--------------------------
+Reachable Set Generation
+------------------------
.. mat:automodule:: +armour.+reachsets
:show-inheritance:
:members:
:undoc-members:
-Trajectory Types
-----------------
+Trajectory Types and Factory
+----------------------------
.. mat:automodule:: +armour.+trajectory
:show-inheritance:
:members:
diff --git a/docs/planner/base.rst b/docs/planner/base.rst
index 561771cb..bada634f 100644
--- a/docs/planner/base.rst
+++ b/docs/planner/base.rst
@@ -5,6 +5,10 @@ The core components to any mid-level RTD Planner.
The RTD Planner
---------------
+
+This is an interface to specify the midlevel planner.
+It's written to be relatively isolated from the rest of the simulation package.
+
.. mat:autoclass:: +rtd.+planner.RtdPlanner
:show-inheritance:
:members:
@@ -19,7 +23,7 @@ Reachable Sets Generation Interfaces
Trajectory Interfaces
---------------------
-.. mat:automodule:: +rtd.+planner.+trajectory
+.. mat:automodule:: +rtd.+trajectory
:show-inheritance:
:members:
:undoc-members:
diff --git a/docs/planner/planner.rst b/docs/planner/planner.rst
index 01e9a12b..2bb6ba6c 100644
--- a/docs/planner/planner.rst
+++ b/docs/planner/planner.rst
@@ -7,6 +7,7 @@ RTD Base Planner and Implementations
base
armour
+ waitr
* :ref:`genindex`
* :ref:`mat-modindex`
diff --git a/docs/planner/waitr.rst b/docs/planner/waitr.rst
new file mode 100644
index 00000000..b46bad94
--- /dev/null
+++ b/docs/planner/waitr.rst
@@ -0,0 +1,20 @@
+Example RTD Planner Implementation of WAITR
+===========================================
+
+WaitrPlanner and these related components define the WAITR planner as described by Brei et. al. in https://arxiv.org/abs/2309.03111.
+This planner extends ArmourPlanner and requires the use of the matching robust controller.
+It still uses the same sets of trajectories that ARMOUR has.
+
+The Base WAITR Planner
+----------------------
+.. mat:autoclass:: +waitr.WaitrPlanner
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
+Extra Reachable Sets not in ARMOUR
+----------------------------------
+.. mat:automodule:: +waitr.+reachsets
+ :show-inheritance:
+ :members:
+ :undoc-members:
diff --git a/docs/simulation/components.rst b/docs/simulation/components.rst
index 74b727e0..6c4b60a3 100644
--- a/docs/simulation/components.rst
+++ b/docs/simulation/components.rst
@@ -18,18 +18,23 @@ Base Components
:members:
:undoc-members:
+Box Obstacle Components
+-----------------------
+.. mat:automodule:: +rtd.+entity.+box_obstacle
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
Armour Components
-----------------
.. mat:automodule:: +armour.+agent
:show-inheritance:
:members:
:undoc-members:
- :exclude-members: ArmourAgent
-Box Obstacle Components
------------------------
-.. mat:automodule:: +rtd.+entity.+box_obstacle
+Waitr Components
+-----------------
+.. mat:automodule:: +waitr.+agent
:show-inheritance:
:members:
:undoc-members:
- :exclude-members: BoxObstacle
diff --git a/docs/simulation/entities.rst b/docs/simulation/entities.rst
index a8fbb0ba..f006a9b3 100644
--- a/docs/simulation/entities.rst
+++ b/docs/simulation/entities.rst
@@ -7,11 +7,15 @@ architecture. Each entity aggregates a few core components with special cases ag
any additional necessary components. Entity data purely exists in the components. Components
can be categorized as Data Components, Data + Behavior Components, and Behavior Components.
-Because of this, there is no base entity class (as of yet). The following are implemented.
+To see a demo for making your own basic entity, refer to the Box2D Agent example in the :doc:`/examples/sim/box2d_agent`.
-Armour Agent
-------------
-.. mat:autoclass:: +armour.ArmourAgent
+Base Entity Class (WorldEntity)
+-------------------------------
+All entities derive from the WorldEntity, which prescribes the requirement for some info component and state component.
+WorldEntity also provides a handful of utility functions to assist with the composition of each entity.
+Additional components can be added as desired, and this is demonstrated in the following concrete entities.
+
+.. mat:autoclass:: +rtd.+sim.+world.WorldEntity
:show-inheritance:
:members:
:undoc-members:
@@ -23,3 +27,16 @@ Box Obstacle Entity
:members:
:undoc-members:
+Armour Agent
+------------
+.. mat:autoclass:: +armour.ArmourAgent
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
+Waitr Agent
+------------
+.. mat:autoclass:: +waitr.WaitrAgent
+ :show-inheritance:
+ :members:
+ :undoc-members:
diff --git a/docs/simulation/simulation.rst b/docs/simulation/simulation.rst
index d7a814e6..72fadc76 100644
--- a/docs/simulation/simulation.rst
+++ b/docs/simulation/simulation.rst
@@ -8,14 +8,7 @@ RTD Simulation Reference
entities
components
systems
-
-
-Simulation
-----------
-.. mat:autoclass:: +rtd.+sim.Simulation
- :show-inheritance:
- :members:
- :undoc-members:
+ simulations
* :ref:`genindex`
diff --git a/docs/simulation/simulations.rst b/docs/simulation/simulations.rst
new file mode 100644
index 00000000..234fb358
--- /dev/null
+++ b/docs/simulation/simulations.rst
@@ -0,0 +1,34 @@
+Simulation Files and World Container
+====================================
+
+In this version of the simulator, we explicitly isolate world as just a container.
+It holds what should exist in a single simulation instance and can be exported to/from a file.
+At the moment, only the initial configuration of the world can be exported and it relies on the use of the :mat:class:`+rtd.+util.+mixin.Options` mixin.
+
+World Model Container
+---------------------
+.. mat:autoclass:: +rtd.+sim.+world.WorldModel
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
+Base Simulation
+---------------
+.. mat:autoclass:: +rtd.+sim.BaseSimulation
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
+Armour Simulation
+-----------------
+.. mat:autoclass:: +armour.ArmourSimulation
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
+Waitr Simulation
+----------------
+.. mat:autoclass:: +waitr.WaitrSimulation
+ :show-inheritance:
+ :members:
+ :undoc-members:
diff --git a/docs/utilities/utilities.rst b/docs/utilities/utilities.rst
index 8f22ef97..252a85e4 100644
--- a/docs/utilities/utilities.rst
+++ b/docs/utilities/utilities.rst
@@ -3,12 +3,19 @@ RTD Code Utilities Reference
.. toctree::
:maxdepth: 2
- :caption: Contents:
+ :caption: Subtopics:
mixins
containers
types
+General Utilities
+-----------------
+.. mat:automodule:: +rtd.+util
+ :show-inheritance:
+ :members:
+ :undoc-members:
+
* :ref:`genindex`
* :ref:`mat-modindex`
* :ref:`search`
diff --git a/scripts/armour/new_agent_demo.m b/scripts/armour/new_agent_demo.m
index d861a0a0..a8ad95b7 100644
--- a/scripts/armour/new_agent_demo.m
+++ b/scripts/armour/new_agent_demo.m
@@ -49,8 +49,8 @@
camlight
end
-%controller = [];
-controller = 'armour.agent.ArmourMexController';
+controller = [];
+% controller = 'armour.agent.ArmourMexController';
% Create
agent = armour.ArmourAgent(agent_info, visual=visual, ...
@@ -102,8 +102,8 @@
trajOptProps.randomInit = true;
trajOptProps.timeForCost = 1.0;
-input_constraints_flag = true;
-use_robust_input = true;
+input_constraints_flag = false;
+use_robust_input = false;
smooth_obs = false;
planner = armour.ArmourPlanner( ...
@@ -111,12 +111,12 @@
input_constraints_flag=input_constraints_flag,...
use_robust_input=use_robust_input,...
smooth_obs=smooth_obs, ...
- traj_type="bernstein", ...
+ traj_type="piecewise", ...
verboseLevel='DEBUG');
-planner = armour.ArmourCudaPlanner( ...
- trajOptProps, sim.agent, ...
- verboseLevel='DEBUG');
+% planner = armour.ArmourCudaPlanner( ...
+% trajOptProps, sim.agent, ...
+% verboseLevel='DEBUG');
%% HLP stuff to migrate
HLP = robot_arm_straight_line_HLP();
@@ -159,13 +159,19 @@
% sim.run(max_steps=1);
% pause(0.1)
% end
+% Using callbacks to attach to the sim
cb = @(sim) planner_callback(sim, planner, agent_info, world_info, lookahead, HLP);
-sim.run(max_steps=100, pre_step_callback={cb});
+sim.run(max_steps=100, pre_step_callback={cb}, autolog=true);
+
+% % How to use the listeners to attach to the sim instead of callbacks
+% cb = @(sim, event) planner_callback(sim, planner, agent_info, world_info, lookahead, HLP);
+% addlistener(sim, 'Step', cb);
+% sim.run(max_steps=100, autolog=true);
function info = planner_callback(sim, planner, agent_info, world_info, lookahead, HLP)
% Get the end state
time = sim.agent.state.time(end);
- ref_state = sim.agent.controller.trajectories{end}.getCommand(time);
+ ref_state = sim.agent.controller.trajectories.getCommand(time);
agent_info.state = sim.agent.state.state(:,end);
q_des = HLP.get_waypoint(agent_info,world_info,lookahead);
@@ -175,11 +181,13 @@
end
% get the sensor readings at the time
- worldState.obstacles = rtd.sim.sensors.zonotope_sensor(sim.world, sim.agent, time);
+ worldState.obstacles = rtd.sim.sensors.zonotope_sensor(sim.world.all_entities, sim.agent, time);
+ a = tic;
[trajectory, plan_info] = planner.planTrajectory(ref_state, worldState, q_des);
+ toc(a)
%FO = plan_info.rsInstances{2}.FO;
%jrsinfo = plan_info.rsInstances{1}.jrs_info;
-
+
if ~isempty(trajectory)
sim.agent.controller.setTrajectory(trajectory)
end
diff --git a/scripts/demos/box2d_agent.m b/scripts/demos/box2d_agent.m
new file mode 100644
index 00000000..0a449c6f
--- /dev/null
+++ b/scripts/demos/box2d_agent.m
@@ -0,0 +1,31 @@
+%% reset
+
+clear ; clc; close all;
+
+%% create agent
+
+agentinfo = demos.box2d.BoxAgentInfo(width=2, height=1, color=[170/255 107/255 220/255]);
+agentstate = rtd.entity.components.GenericEntityState(agentinfo, n_states=2); % give it two states (x_pos, y_pos)
+agentvisual = demos.box2d.BoxAgentVisual(agentinfo, agentstate, face_opacity=0.8);
+
+boxagent = demos.box2d.BoxAgent(info=agentinfo, state=agentstate, visual=agentvisual);
+%disp(boxagent);
+
+%% add states (i.e., move agent around)
+
+% default position at time=0 is (0, 0)
+boxagent.state.commit_state_data(2, [0; -2]); % (0, -2) at t=2
+boxagent.state.commit_state_data(5, [-2; 1]); % (-2, 1) at t=7
+boxagent.state.commit_state_data(2, [-4; -1]); % (-4,-1) at t=9
+boxagent.state.commit_state_data(3, [4; 3]); % (4, 3) at t=12
+boxagent.state.commit_state_data(2, [2; 0]); % (2, 0) at t=14
+for i = 0:pi/20:2*pi
+ boxagent.state.commit_state_data(0.25, [3*cos(i); 3*sin(i)]);
+end
+boxagent.state.get_state()
+
+%% set up visual system and animate
+vs = rtd.sim.systems.patch_visual.PatchVisualSystem(...
+ dynamic_objects=boxagent.visual, ...
+ view=2, xlim=[-5 5], ylim=[-5 5]);
+vs.updateVisual(24.25); % aniamte the first 24.25 t
\ No newline at end of file
diff --git a/scripts/waitr/waitr_demo.m b/scripts/waitr/waitr_demo.m
new file mode 100644
index 00000000..55280764
--- /dev/null
+++ b/scripts/waitr/waitr_demo.m
@@ -0,0 +1,436 @@
+
+clear ; clc; close all;
+
+%% Agent parameters
+verbosity = 'DEBUG';
+
+%%% for agent
+agent_urdf = 'Kinova_Grasp_URDF.urdf';
+
+add_uncertainty_to = 'link'; % choose 'all', 'link', or 'none'
+links_with_uncertainty = {'cube_link'}; % if add_uncertainty_to = 'link', specify links here.
+uncertain_mass_range = [0.97, 1.03];
+u_s = 0.609382421;
+surf_rad = 0.029;
+
+% If this flag is true, we use the ArmourCadPatchVisual component instead
+visualize_cad = false;
+
+% Add noise to the dynamics
+component_options.dynamics.measurement_noise_points = 0;
+component_options.dynamics.log_controller = true;
+component_options.dynamics.u_s = u_s;
+component_options.dynamics.surf_rad = surf_rad;
+
+% for controller
+component_options.controller.use_true_params_for_robust = false;
+
+%% Setup the info
+robot = importrobot(agent_urdf);
+robot.DataFormat = 'col';
+robot.Gravity = [0 0 -9.81];
+params = armour.legacy.dynamics.load_robot_params(robot, ...
+ 'add_uncertainty_to', add_uncertainty_to, ...
+ 'links_with_uncertainty', links_with_uncertainty,...
+ 'uncertain_mass_range', uncertain_mass_range);
+vel_limits = [-1.3963, -1.3963, -1.3963, -1.3963, -1.2218, -1.2218, -1.2218;
+ 1.3963, 1.3963, 1.3963, 1.3963, 1.2218, 1.2218, 1.2218]; % matlab doesn't import these from urdf so hard code into class
+input_limits = [-56.7, -56.7, -56.7, -56.7, -29.4, -29.4, -29.4;
+ 56.7, 56.7, 56.7, 56.7, 29.4, 29.4, 29.4]; % matlab doesn't import these from urdf so hard code into class
+transmision_inertia = [8.02999999999999936 11.99620246153036440 9.00254278617515169 11.58064393167063599 8.46650409179141228 8.85370693737424297 8.85873036646853151]; % matlab doesn't import these from urdf so hard code into class
+M_min_eigenvalue = 5.095620491878957; % matlab doesn't import these from urdf so hard code into class
+
+agent_info = armour.agent.ArmourAgentInfo(robot, params, ...
+ joint_velocity_limits=vel_limits, ...
+ joint_torque_limits=input_limits, ...
+ transmission_inertia=transmision_inertia, ...
+ M_min_eigenvalue=M_min_eigenvalue);
+
+% Visualization?
+visual = [];
+if visualize_cad
+ % this could be improved
+ visual = armour.agent.ArmourCadPatchVisual(armour.agent.ArmourAgentInfo.empty(),armour.agent.ArmourAgentState.empty(),armour.agent.ArmKinematics.empty());
+ camlight
+end
+
+controller = [];
+% controller = 'waitr.agent.WaitrMexController';
+
+% Create
+agent = waitr.WaitrAgent(agent_info, visual=visual, ...
+ component_options=component_options, ...
+ component_logLevelOverride=verbosity, ...
+ controller=controller);
+
+close all;
+
+%% Demo section of copy-ability
+% A.visual.plot()
+% axis equal
+%
+% % Demo the ability to copy paste
+% opts = A.getoptions
+% A_2 = ArmourAgent.from_options(robot, params, opts)
+%
+% % Change out the visual for the third copy paste
+% opts.components.visual = 'ArmourCadPatchVisual';
+% opts.component_options.visual = ArmourCadPatchVisual.defaultoptions();
+% % Also name it
+% opts.name = 'CAD'
+%
+% % Demo
+% A_3 = ArmourAgent.from_options(robot, params, opts)
+% figure; view(3); grid on
+% plot(A_3.visual)
+% axis equal; camlight
+% disp("press enter to continue")
+% pause
+
+%% Create the simulation
+sim = waitr.WaitrSimulation;
+sim.setup(agent)
+sim.initialize()
+% not the proper way to do it, but proper way isn't implemented yet
+sim.visual_system.enable_camlight = visualize_cad;
+sim.visual_system.redraw();
+%sim.run(max_steps=1);
+
+disp("press enter to continue")
+pause
+
+%% Interface of Planner should improve
+trajOptProps = rtd.planner.trajopt.TrajOptProps;
+trajOptProps.planTime = 0.5;
+trajOptProps.horizonTime = 1.0;
+trajOptProps.doTimeout = false;
+trajOptProps.timeoutTime = 0.5;
+trajOptProps.randomInit = true;
+trajOptProps.timeForCost = 0.5;
+
+input_constraints_flag = true;
+grasp_constraints_flag = true;
+use_robust_input = true;
+smooth_obs = false;
+joint_limits_flag = true;
+
+planner = waitr.WaitrPlanner( ...
+ trajOptProps, sim.agent, ...
+ input_constraints_flag=input_constraints_flag,...
+ grasp_constraints_flag=grasp_constraints_flag,...
+ use_robust_input=use_robust_input,...
+ smooth_obs=smooth_obs, ...
+ joint_limits_flag=joint_limits_flag, ...
+ traj_type="bernstein", ...
+ verboseLevel='DEBUG');
+
+%% HLP stuff to migrate
+HLP = robot_arm_straight_line_HLP();
+world_info.goal = sim.goal_system.goal_position;
+agent_info = struct;
+agent_info.n_states = sim.agent.state.n_states;
+agent_info.n_inputs = sim.agent.controller.n_inputs;
+agent_info.n_links_and_joints = sim.agent.info.n_links_and_joints;
+agent_info.joint_state_limits = [sim.agent.info.joints.position_limits];
+agent_info.joint_speed_limits = [sim.agent.info.joints.velocity_limits];
+agent_info.joint_input_limits = [sim.agent.info.joints.torque_limits];
+agent_info.joint_state_indices = sim.agent.state.position_indices;
+agent_info.joint_speed_indices = sim.agent.state.velocity_indices;
+% if bounds are +- inf, set to small/large number
+joint_limit_infs = isinf(agent_info.joint_state_limits) ;
+speed_limit_infs = isinf(agent_info.joint_speed_limits) ;
+input_limit_infs = isinf(agent_info.joint_input_limits) ;
+
+agent_info.joint_state_limits(1,joint_limit_infs(1,:)) = -200*pi ;
+agent_info.joint_state_limits(2,joint_limit_infs(2,:)) = +200*pi ;
+agent_info.joint_speed_limits(1,speed_limit_infs(1,:)) = -200*pi ;
+agent_info.joint_speed_limits(2,speed_limit_infs(2,:)) = +200*pi ;
+agent_info.joint_input_limits(1,input_limit_infs(1,:)) = -200*pi ;
+agent_info.joint_input_limits(2,input_limit_infs(2,:)) = +200*pi ;
+
+HLP.setup(agent_info,world_info);
+HLP.make_new_graph_every_iteration_flag = 1;
+HLP.sampling_timeout = 0.5;
+
+%% Run planning step by step
+lookahead = 0.4;
+iter = 0;
+pausing = false;
+% while true
+% if pausing
+% pause
+% end
+% iter = iter + 1;
+% sim.run(max_steps=1);
+% pause(0.1)
+% end
+
+% Using callbacks to attach to the sim
+cb = @(sim) planner_callback(sim, planner, agent_info, world_info, lookahead, HLP);
+sim.run(max_steps=100, pre_step_callback={cb}, autolog=true);
+
+% % How to use the listeners to attach to the sim instead of callbacks
+% cb = @(sim, event) planner_callback(sim, planner, agent_info, world_info, lookahead, HLP);
+% addlistener(sim, 'Step', cb);
+% sim.run(max_steps=100, autolog=true);
+
+%% Evaluate simulation
+% sim.visual_system.animate();
+enable_compare = false;
+if enable_compare
+ load("saved_data_case1.mat");
+end
+
+% Plot joint positions, velocities, and torques
+entries = agent.dynamics.controller_log.get('input_time', 'input', flatten=false);
+inputs = [];
+for i = 1:size(entries.input,2)
+ temp_input = entries.input{i};
+ if i ~= size(entries.input,2)
+ temp_input = temp_input(:,1:(end-1));
+ end
+ inputs = [inputs, temp_input];
+end
+
+figure(2);
+for joint_id = 1:7
+ subplot(2,4,joint_id);
+ plot(agent.state.time,agent.state.position(joint_id,:),'b'); hold on;
+ if enable_compare
+ plot(agent.state.time,saved_data.pos(joint_id,:),'g');
+ end
+ xlabel('time (s)');
+ ylabel('state (rad)');
+ title(['Joint #',num2str(joint_id)]);
+ if enable_compare && joint_id == 1
+ legend("Use code platform","ARMOUR's original code");
+ end
+end
+sgtitle('Joint Positions');
+
+figure(3);
+for joint_id = 1:7
+ subplot(2,4,joint_id);
+ plot(agent.state.time,agent.state.velocity(joint_id,:), 'b'); hold on;
+ if enable_compare
+ plot(agent.state.time,saved_data.vel(joint_id,:),'g');
+ end
+ xlabel('time (s)');
+ ylabel('velocity (rad/s)');
+ title(['Joint #',num2str(joint_id)]);
+ if enable_compare && joint_id == 1
+ legend("Use code platform","ARMOUR's original code");
+ end
+end
+sgtitle('Joint Velocities');
+
+figure(4);
+for joint_id = 1:7
+ subplot(2,4,joint_id);
+ plot(agent.state.time,inputs(joint_id,:), 'b'); hold on;
+ if enable_compare
+ plot(agent.state.time,saved_data.torque(joint_id,:),'g');
+ end
+ xlabel('time (s)');
+ ylabel('torque (Nm)');
+ title(['Joint #',num2str(joint_id)]);
+ if enable_compare && joint_id == 1
+ legend("Use code platform","ARMOUR's original code");
+ end
+end
+sgtitle('Joint Torques');
+
+% Calculating the Acceleration
+qdd_post = zeros(7,length(agent.state.time));
+% calculating the acceleration in post to compare with what is stored
+for i = 1:length(agent.state.time)
+ t = agent.state.time(i);
+ z = agent.state.state(:,i);
+ [M, C, g] = agent.dynamics.calculate_dynamics(agent.state.position(:,i), agent.state.velocity(:,i), agent.dynamics.robot_info.params.true);
+ for j = 1:agent_info.n_inputs
+ M(j,j) = M(j,j) + agent.dynamics.robot_info.transmission_inertia(j);
+ end
+ qdd_post(:,i) = M\(inputs(:,i)-C*agent.state.velocity(:,i)-g);
+end
+
+figure(5);
+for joint_id = 1:7
+ subplot(2,4,joint_id);
+ plot(agent.state.time,qdd_post(joint_id,:), 'b'); hold on;
+ if enable_compare
+ plot(agent.state.time,saved_data.acc(joint_id,:),'g');
+ end
+ xlabel('time (s)');
+ ylabel('acceleration (rad/s^2)');
+ title(['Joint #',num2str(joint_id)]);
+ if enable_compare && joint_id == 1
+ legend("Use code platform","ARMOUR's original code");
+ end
+end
+sgtitle('Joint Accelerations');
+
+% Calling RNEA
+for i = 1:length(agent.state.time)
+ % clear relevant variables
+ clear tau f n
+ % call rnea
+ [tau, f, n] = armour.legacy.dynamics.rnea(agent.state.position(:,i)',agent.state.velocity(:,i)',agent.state.velocity(:,i)',qdd_post(:,i)',true,agent.dynamics.robot_info.params.true);
+ % store rnea results
+ Tau{i} = tau;
+ F{i} = f;
+ N{i} = n;
+ % store the contact forces
+ force(:,i) = f(:,10);
+ % store the contact moments
+ moment(:,i) = n(:,10);
+end
+
+% Plotting the forces
+figure(6);
+% plot the x-axis force (in the tray frame)
+subplot(1,3,1);
+hold on;
+plot(agent.state.time(1:end), force(1,:), 'b');
+if enable_compare
+ plot(agent.state.time(1:end), saved_data.force(1,:), 'g');
+end
+xlabel('time (s)');
+ylabel('x-axis Force (N)');
+axis('square');
+grid on;
+if enable_compare
+ legend("Use code platform","ARMOUR's original code");
+end
+% plot the y-axis force (in the tray frame)
+subplot(1,3,2);
+hold on;
+plot(agent.state.time(1:end), force(2,:), 'b');
+if enable_compare
+ plot(agent.state.time(1:end), saved_data.force(2,:), 'g');
+end
+xlabel('time (s)');
+ylabel('y-axis Force (N)');
+axis('square');
+grid on;
+% plot the z-axis force (in the tray frame)
+subplot(1,3,3);
+hold on;
+plot(agent.state.time(1:end), force(3,:), 'b');
+if enable_compare
+ plot(agent.state.time(1:end), saved_data.force(3,:), 'g');
+end
+xlabel('time (s)');
+ylabel('z-axis Force (N)');
+axis('square');
+grid on;
+sgtitle('Contact Force');
+
+% Calculating Constraints
+% separation constraint
+sep = -1*force(3,:);
+% slipping constraint
+% slip = sqrt(force(1,:).^2+force(2,:).^2) - u_s.*abs(force(3,:));
+slip2 = force(1,:).^2+force(2,:).^2 - u_s^2.*force(3,:).^2;
+
+for i = 1:length(agent.state.time)
+ % tipping constraint the normal way
+ ZMP_top = cross([0;0;1],moment(:,i)); % normal vector should come first
+ ZMP_bottom = dot([0;0;1],force(:,i));
+ ZMP(:,i) = ZMP_top/ZMP_bottom;
+ ZMP_rad(i) = sqrt(ZMP(1,i)^2+ZMP(2,i)^2);
+% tip(i) = ZMP_rad(i) - surf_rad;
+ % tipping constraint the PZ way
+ ZMP_top2 = cross([0;0;1],moment(:,i));
+ ZMP_bottom2 = dot([0;0;1],force(:,i));
+ tip2(i) = ZMP_top(1)^2 + ZMP_top(2)^2 - ZMP_bottom^2*(surf_rad)^2;
+end
+
+% plotting grasp constraints
+figure(7);
+% plot the separation constraint
+subplot(1,3,1);
+hold on;
+plot(agent.state.time(1:end),sep, 'b');
+if enable_compare
+ plot(agent.state.time(1:end), saved_data.sep, 'g');
+end
+plot(agent.state.time(1:end), zeros(1,length(agent.state.time)), 'r');
+xlabel('time (s)');
+ylabel('Separation Constraint');
+axis('square');
+grid on;
+if enable_compare
+ legend("Use code platform","ARMOUR's original code","Constraint limit");
+else
+ legend("Use code platform","Constraint limit");
+end
+% plot the slipping constraint
+subplot(1,3,2);
+hold on
+% plot(agent.state.time(1:end),slip, 'k')
+plot(agent.state.time(1:end),slip2, 'b');
+if enable_compare
+ plot(agent.state.time(1:end), saved_data.slip2, 'g');
+end
+plot(agent.state.time(1:end), zeros(1,length(agent.state.time)), 'r');
+xlabel('time (s)');
+ylabel('Slipping Constraint');
+axis('square');
+grid on;
+% plot the tipping constraint
+subplot(1,3,3);
+hold on;
+% plot(agent.state.time(1:end),tip, 'k')
+plot(agent.state.time(1:end),tip2, 'b');
+if enable_compare
+ plot(agent.state.time(1:end), saved_data.tip2, 'g');
+end
+plot(agent.state.time(1:end), zeros(1,length(agent.state.time)), 'r');
+xlabel('time (s)');
+ylabel('Tipping Constraint');
+axis('square');
+grid on;
+sgtitle('Grasp Constraints');
+
+%% Save simulation data
+saved_data = struct();
+saved_data.pos = agent.state.position;
+saved_data.vel = agent.state.velocity;
+saved_data.torque = inputs;
+saved_data.acc = qdd_post;
+saved_data.force = force;
+saved_data.sep = sep;
+saved_data.slip2 = slip2;
+saved_data.tip2 = tip2;
+saved_data.goal_pos = sim.goal_system.goal_position;
+% saved_data.obstacles = sim.obstacles;
+savename = sprintf('comparison_result/saved_data_arch_%s.mat', datestr(now,'mm_dd_yyyy_HH_MM_SS'));
+save(savename,"saved_data");
+
+
+
+%% Helper functions
+function info = planner_callback(sim, planner, agent_info, world_info, lookahead, HLP)
+ % Get the end state
+ time = sim.agent.state.time(end);
+ ref_state = sim.agent.controller.trajectories.getCommand(time);
+ agent_info.state = sim.agent.state.state(:,end);
+
+ q_des = HLP.get_waypoint(agent_info,world_info,lookahead);
+ if isempty(q_des)
+ disp('Waypoint creation failed! Using global goal instead.')
+ q_des = HLP.goal ;
+ end
+
+ % get the sensor readings at the time
+ worldState.obstacles = rtd.sim.sensors.zonotope_sensor(sim.world.all_entities, sim.agent, time);
+ [trajectory, plan_info] = planner.planTrajectory(ref_state, worldState, q_des);
+ %FO = plan_info.rsInstances{2}.FO;
+ %jrsinfo = plan_info.rsInstances{1}.jrs_info;
+
+ if ~isempty(trajectory)
+ sim.agent.controller.setTrajectory(trajectory)
+ end
+
+ info = plan_info;
+end
\ No newline at end of file