R/fitMechanism.R
In aaronolsen/linkR: 3D Lever and Linkage Mechanism Modeling
fitMechanism <- function(mechanism, fit.points, body.assoc, input.param,
coor.optim = rep(TRUE, nrow(joint.coor)), pose.optim = TRUE, fit.wts = NULL,
planar = FALSE, coor.vectors = NULL, use.ref.as.prev = FALSE,
direct.input = FALSE, print.progress = FALSE, ref.iter = 1, control = NULL, print.level = 0){
# Start run time
time1 <- proc.time()
# Set indent
indent <- '\t'
if(print.progress) cat(paste0(paste0(rep(indent, print.level), collapse=''), 'fitMechanism()\n'))
control_default <- list(
'joint.coor.bounds'=0.1,
'fit.points.bounds.factor'=0.1,
'optim.frame.max'=21,
'optim.to.percent'=0.001,
'optim.iter.max'=30,
'optim.error.stop'=1e-5
)
# Set nlminb controls
nlminb_control <- list('abs.tol'=1e-20, 'rel.tol'=1e-10, 'x.tol'=1.5e-8, 'xf.tol'=2.2e-14) # Defaults
nlminb_control[['abs.tol']] <- 1e-12
nlminb_control[['rel.tol']] <- 1e-6
# Overwrite default controls with inputs, if applicable
if(is.null(control)){
control_new <- control_default
}else{
control_new <- control_default
for(i in 1:length(control))
if(!is.null(control[[names(control)[i]]])) control_new[[names(control)[i]]] <- control[[names(control)[i]]]
}
control <- control_new
# Set parameters from mechanism
joint.types <- mechanism$joint.types
joint.coor <- mechanism$joint.coor
joint.names <- rownames(mechanism$joint.coor)
input.body <- mechanism$input.body
input.joint <- mechanism$input.joint
input_joint_name <- joint.names[input.joint]
input.dof <- mechanism$input.dof
joint.cons <- mechanism$joint.cons
body.conn <- mechanism$body.conn
fixed.body <- mechanism$fixed.body
body_names <- mechanism$body.names
# If there's an N joint attached to fixed body, check if there are fit points associated with fixed body and add if there are not
if('N' %in% joint.types){
if(fixed.body %in% body.conn[joint.types == 'N',]){
if(!fixed.body %in% body.assoc){
new_fixed_pts <- paste0(fixed.body, '_fit_pt', 1:3)
fit_points_new <- array(NA, dim=dim(fit.points)+c(3,0,0), dimnames=list(c(new_fixed_pts, dimnames(fit.points)[[1]]), dimnames(fit.points)[[2]], dimnames(fit.points)[[3]]))
fit_points_new[new_fixed_pts,,] <- diag(3)
fit_points_new[dimnames(fit.points)[[1]],,] <- fit.points
fit.points <- fit_points_new
body.assoc <- c(rep(fixed.body, 3), body.assoc)
if(is.matrix(ref.iter)){
ref.iter <- rbind(matrix(diag(3), 3, 3, dimnames=list(new_fixed_pts, NULL)), ref.iter)
}
}
}
}
# Use motion between two bodies connected by each joint to generate initial joint
# constraint estimate
# Make sure that row names are unique
if(!is.null(rownames(fit.points))) if(length(unique(rownames(fit.points))) != nrow(fit.points)) stop("Row names in 'fit.points' must be unique.")
# Get number of iterations
n_iter <- dim(fit.points)[3]
# Standardize joint types
stand_joint_type <- list('Our'='O', 'Ou'='O', 'Ur'='U')
for(i in 1:length(stand_joint_type)){
joint.types[joint.types == names(stand_joint_type)[i]] <- stand_joint_type[[i]]
if(!is.null(input_joint_name)){
input_joint_name[input_joint_name == names(stand_joint_type)[i]] <- stand_joint_type[[i]]
}
}
# Get number of joints
n_joints <- length(joint.types)
# Set input parameters if single joint and NULL
if(is.null(input.param)){
if(n_joints > 1) stop("'input.param' can only be NULL for single joint models (i.e. joint.types of length 1).")
# Set input.param based on model DoFs
input.param <- matrix(NA, nrow=n_iter, ncol=jointDoF(joint.types))
}
# Set NA joint coordinates
#if(is.null(joint.coor)) joint.coor <- matrix(NA, nrow=n_joints, ncol=3, dimnames=list(joint.names, NULL))
#if(is.vector(joint.coor)) joint.coor <- matrix(joint.coor, nrow=1, ncol=3, dimnames=list(joint.names, NULL))
# Set joint names from coordinates if NULL
#if(is.null(joint.names)) joint.names <- rownames(joint.coor)
# If single coor.optim value and multiple joints, repeat for same number of joints
if(length(coor.optim) == 1 && n_joints > 1) coor.optim <- rep(coor.optim, n_joints)
# Set joint coordinates to optimize
joints_optim <- which(coor.optim)
# Set logical whether joint coordinates will be optimized
if(length(joints_optim) > 0){ optim_joint_coor <- TRUE }else{ optim_joint_coor <- FALSE }
# Set NA joint constraints if no set values specified
#if(is.null(joint.cons)){
# joint.cons <- list()
# for(joint_num in 1:n_joints){
# if(joint.types[joint_num] %in% c('R', 'L', 'T')) joint.cons[[joint_num]] <- matrix(NA, 1, 3)
# if(joint.types[joint_num] %in% c('X', 'U', 'P', 'PR')) joint.cons[[joint_num]] <- matrix(NA, 2, 3)
# if(joint.types[joint_num] %in% c('S', 'O')) joint.cons[[joint_num]] <- matrix(NA, 3, 3)
# if(joint.types[joint_num] %in% c('N')) joint.cons[[joint_num]] <- matrix(NA, 6, 3)
# }
#}
# Use define linkage just to get body.conn.num and other mechanism properties (skip path finding)
#mechanism <- defineMechanism(joint.coor=joint.coor, joint.types=joint.types,
# input.body=input.body, input.joint=input.joint, input.dof=input.dof, joint.cons=joint.cons, body.conn=body.conn,
# fixed.body=fixed.body, print.progress=FALSE)
# Save input formatted joint constraints
#joint.cons <- mechanism$joint.cons
joint_cons_input <- mechanism$joint.cons
# Create numeric body association
body_assoc_num <- rep(NA, length(body.assoc))
for(i in 1:length(body_assoc_num)) body_assoc_num[i] <- which(body.assoc[i] == mechanism$body.names)
# Find most dissimilar shapes for optimization
if(n_iter > control$optim.frame.max){
optim_use <- sort(whichMaxShapeDist(fit.points, max.index=min(n_iter, control$optim.frame.max)))
}else{
optim_use <- 1:n_iter
}
if(FALSE){
cols <- rainbow(n=length(dist_index), start=0, end=5/6)
svg.new(file='plot.html')
for(i in 1:length(dist_index)) svg.lines(fit.points[c(1:3,1), , dist_index[i]], col=cols[i])
for(i in 1:dim(fit.points)[3]) svg.lines(fit.points[c(1:3,1), , i], opacity=0.05)
svg.frame(fit.points[, , optim_use])
svg.close()
}
# Set number of optimization iterations
n_optim <- length(optim_use)
if(print.progress){
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Total time points: ', n_iter, '\n'))
if(is.vector(ref.iter)){
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Reference iteration: ', ref.iter, '\n'))
}else{
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Reference coordinates:\n'))
for(i in 1:nrow(ref.iter)){
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), rownames(ref.iter)[i], ': {', paste0(signif(ref.iter[i,], 3), collapse=','), '}\n'))
}
}
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Number of time points used in optimization: ', n_optim, '\n'))
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Time points (max. dispersed): ', paste0(optim_use, collapse=','), '\n'))
}
## Estimate initial joint constraints
if(print.progress) cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Estimating joint constraints...\n'))
for(joint_num in 1:n_joints){
if(joint.types[joint_num] == 'N'){
joint.coor[joint_num, ] <- c(0,0,0)
joint.cons[[joint_num]] <- rbind(diag(3), diag(3))
next
}
if(!is.na(joint.coor[joint_num, 1]) && joint.types[joint_num] == 'S'){
if(is.na(joint.cons[[joint_num]][3, 1])) joint.cons[[joint_num]] <- diag(3)
next
}
# Get two bodies connected by joint
bodies <- mechanism$body.conn.num[joint_num, ]
# Get points associated with each body
body_points <- list(which(body_assoc_num == bodies[1]), which(body_assoc_num == bodies[2]))
# Skip certain joints
if(joint.types[joint_num] %in% c('P')) next
# If fixed link among bodies
if(1 %in% bodies){
# Choose other body as mobile
mobile_body <- which(bodies != 1)
# Get fit points
fit_points <- fit.points[body_points[[mobile_body]], , ]
# If no fixed link
}else{
if(is.vector(ref.iter)){
# Fix body2 points relative to body1 (need to input body1+body2 points)
fit_points_rel <- immobilize(fit.points[unlist(body_points), , ], fit.points[body_points[[1]], , 1])
# Save body2 points only
fit_points <- fit_points_rel[dimnames(fit.points)[[1]][body_points[[2]]], , ]
}else{
# Fix body2 points relative to body1 (need to input body1+body2 points)
fit_points_rel <- immobilize(fit.points[unlist(body_points), , ], ref.iter[dimnames(fit.points)[[1]][body_points[[1]]], ])
# Save body2 points only
fit_points <- fit_points_rel[dimnames(fit.points)[[1]][body_points[[2]]], , ]
}
}
# Set initial fit type
init_fit_type <- joint.types[joint_num]
# **** Temp fix for initial guess - seems to work OK
if(joint.types[joint_num] %in% c('U', 'X', 'O')) init_fit_type <- 'R'
# Get initial joint constraint estimate
fit_joint_cons <- fitJointConstraint(coor=fit_points, type=init_fit_type, smooth=TRUE, print.progress=FALSE)
# Set center of rotation
if(joint.types[joint_num] %in% c('R', 'S', 'X', 'U', 'O'))
if(is.na(joint.coor[joint_num, 1])) joint.coor[joint_num, ] <- fit_joint_cons$cons[1:3]
# Set axes of rotation, filling in where initial joint constraints are not already specified
if(joint.types[joint_num] == 'R'){
if(is.na(joint.cons[[joint_num]][1, 1])) joint.cons[[joint_num]] <- matrix(fit_joint_cons$cons[4:6], 1, 3)
}
if(joint.types[joint_num] %in% c('X', 'U')){
if(is.na(joint.cons[[joint_num]][1, 1])) joint.cons[[joint_num]] <- rbind(fit_joint_cons$cons[4:6], rep(NA, 3))
if(is.na(joint.cons[[joint_num]][2, 1])) joint.cons[[joint_num]][2, ] <- vorthogonal(joint.cons[[joint_num]][1, ])
}
if(joint.types[joint_num] %in% c('O')){
if(is.na(joint.cons[[joint_num]][1, 1])) joint.cons[[joint_num]] <- rbind(fit_joint_cons$cons[4:6], rep(NA, 3), rep(NA, 3))
if(is.na(joint.cons[[joint_num]][2, 1])) joint.cons[[joint_num]][2, ] <- vorthogonal(joint.cons[[joint_num]][1, ])
if(is.na(joint.cons[[joint_num]][3, 1])) joint.cons[[joint_num]][3, ] <- vorthogonal(joint.cons[[joint_num]][2, ])
}
if(joint.types[joint_num] %in% c('S')){
if(is.na(joint.cons[[joint_num]][1, 1])){
# No input joint constraint vectors - use standard
joint.cons[[joint_num]] <- diag(3)
joint_cons_input[[joint_num]] <- diag(3)
}else{
if(is.na(joint.cons[[joint_num]][2, 1])){
joint.cons[[joint_num]][2, ] <- vorthogonal(joint.cons[[joint_num]][1, ])
joint.cons[[joint_num]][3, ] <- cprod(joint.cons[[joint_num]][1, ], joint.cons[[joint_num]][2, ])
}
}
}
}
#joint.cons[[1]][1,] <- c(0.1,0.3,1)
#joint.cons[[1]][2,] <- vorthogonal(joint_cons_true[1,])
#joint.cons[[1]][1,] <- joint_cons_true[1,]
#joint.cons[[1]][2,] <- joint_cons_true[2,]
#joint.cons[[1]][2,] <- vorthogonal(joint_cons_true[1,])
# If planar is TRUE, set joint coordinates into same plane and R-joint axes perpendicular to plane
if(planar){
if(sum(!coor.optim) <= 1){
# Find normal vector to plane
v123 <- cprod(joint.coor[3, ]-joint.coor[1, ], joint.coor[2, ]-joint.coor[1, ])
v124 <- cprod(joint.coor[4, ]-joint.coor[1, ], joint.coor[3, ]-joint.coor[2, ])
plane_nvector <- uvector(colMeans(rbind(v123, v124)))
}else{
# ****** Temp fix
joints_plane <- which(!coor.optim)
if(length(joints_plane) == 3){
v123 <- joint.coor[joints_plane[3], ] - joint.coor[joints_plane[1], ]
v124 <- joint.coor[joints_plane[2], ] - joint.coor[joints_plane[1], ]
}else{
v123 <- joint.coor[joints_plane[4], ] - joint.coor[joints_plane[1], ]
v124 <- joint.coor[joints_plane[3], ] - joint.coor[joints_plane[1], ]
}
plane_nvector <- uvector(cprod(v123, v124))
}
# Check if a joint is fixed
if(sum(!coor.optim) > 0){
# Set joint center as point in plane
plane_point <- joint.coor[which(!coor.optim)[1], ]
}else{
# Set joint center as point in plane
plane_point <- colMeans(joint.coor)
}
# Set all R-joint axes to normal vector
for(joint_num in 1:n_joints){
# Project joint coordinates into plane
joint.coor[joint_num, ] <- pointPlaneProj(joint.coor[joint_num, ], plane_point, plane_nvector)
if(joint.types[joint_num] == 'R') joint.cons[[joint_num]] <- plane_nvector
}
}
# Re-define linkage with initial joint coordinate and constraints
mechanism <- defineMechanism(joint.coor=joint.coor, joint.types=joint.types,
input.body=input.body, input.joint=input.joint, input.dof=input.dof, joint.cons=joint.cons,
body.conn=body.conn, fixed.body=fixed.body)
# If input.joint is NULL and a single joint, set sole joint as input
if(is.null(input.joint)){
if(n_joints > 1) stop("If the mechanism has more than one joint 'input.joint' must be specified.")
input.joint <- 1
}
# If input.joint is non-numeric, convert to numeric equivalent
if(!is.numeric(input.joint[1])){
if(sum(!input.joint %in% mechanism$joint.names) > 0) stop("'input.joint' names do not match joint names.")
input_joint_num <- rep(NA, length(input.joint))
for(i in 1:length(input.joint)) input_joint_num[i] <- which(mechanism$joint.names == input.joint[i])
input.joint <- input_joint_num
}
# Set joint compare to choose toggle positions (not necessary for single joint)
if(n_joints == 1){
joint_compare <- NULL
}else{
# Create joint coordinates animated with fit points for toggle comparisons
joint_compare <- array(NA, dim=c(dim(mechanism$joint.coor), n_iter),
dimnames=list(rownames(mechanism$joint.coor), NULL, NULL))
#
for(i in 1:dim(joint_compare)[1]){
# Find associated bodies
assoc_bodies <- mechanism$body.conn.num[i, ]
# Find associated fit points
body_names_a <- mechanism$body.names[assoc_bodies]
# Get fit point indices associated with each body
which_body1 <- which(body.assoc == body_names_a[1])
which_body2 <- which(body.assoc == body_names_a[2])
# Skip if less than 3 fit points
if(length(which_body1) < 3 && length(which_body2) < 3) next
# Choose one body
which_body <- which_body1
if(length(which_body1) < 3) which_body <- which_body2
if(length(which_body2) < 3) which_body <- which_body1
# Combine fit points and joint
if(is.vector(ref.iter)){
fit_point_joint <- rbind(fit.points[which_body, , ref.iter], 'joint.coor'=mechanism$joint.coor[i, ])
}else{
fit_point_joint <- rbind(ref.iter[dimnames(fit.points[which_body,,])[[1]],], 'joint.coor'=mechanism$joint.coor[i, ])
}
# Fill array
for(iter in 1:n_iter){
# Move joint based on fit point motion
joint_compare[i, , iter] <- bestAlign(fit.points[which_body, , iter], fit_point_joint)$mat['joint.coor', ]
}
}
}
#print(mechanism$joint.coor['SH_cra',])
#print(t(joint_compare['SH_cra',,]))
#return(1)
## Prepare optimization vectors
if(print.progress) cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Preparing optimization parameters...\n'))
## Prepare fit point pose optimization vectors
# Get range of joint coordinate positions along each axis
fit_points_range <- apply(apply(fit.points[, , 1], 2, 'range'), 2, 'diff')
# If one dimension is 0, set to minimum of other dimensions
if(sum(abs(fit_points_range) < 1e-3) > 0) fit_points_range[abs(fit_points_range) < 1e-3] <- min(fit_points_range[fit_points_range !=0])
# Set bound for translation
pose_optim_bounds_add <- fit_points_range*control$fit.points.bounds.factor
# Set amount to shift from boundary
bound_shift <- control$joint.coor.bounds*0.01
# Create vector of input parameters (start with first iteration)
input_optim_vec <- c()
input_optim_names <- c()
# Create matrix of bounds for input parameters
input_optim_bounds <- matrix(NA, nrow=2, ncol=0, dimnames=list(c('lower', 'upper'), NULL))
# Convert input.param into list of matrices for consistency
if(class(input.param) == 'numeric') input.param <- list(matrix(input.param, nrow=n_inputs, ncol=1))
if(class(input.param) == 'matrix') input.param <- list(input.param)
if(class(input.param) == 'list'){
for(i in 1:length(input.param)) if(is.vector(input.param[[i]])) input.param[[i]] <- matrix(input.param[[i]], nrow=length(input.param[[i]]), ncol=1)
}
# Create modifiable copy of input parameters
input_param <- input.param
# Create vector for number of input parameters per input
n_input <- rep(NA, length(input_param))
# Create optimization input parameter
input_param_optim <- list()
# Create list to specify which values from input_optim will be placed in which columns of input_param
input_param_fill <- list()
# Set joints for which input parameters will be calculated directly (for now only use in single joint case)
direct.input <- rep(FALSE, length(input.joint))
if(direct.input && n_joints == 1) direct.input[joint.types == 'U'] <- TRUE
# Logical for whether there are any fixed inputs
any_fixed_input <- FALSE
# Fill vector with initial values
col_idx_start <- 1
for(i in 1:length(input.joint)){
# Create list element
input_param_fill[[i]] <- list()
# Create optimization input parameters
input_param_optim[[i]] <- matrix(input_param[[i]][optim_use, ], nrow=n_optim, ncol=ncol(input_param[[i]]))
if(sum(!is.na(input_param[[i]][1,])) > 0) any_fixed_input <- TRUE
# Set column indices to pull from input_optim matrix
input_param_fill[[i]][['col.idx']] <- col_idx_start:(col_idx_start + sum(is.na(input_param[[i]][1,]))-1)
col_idx_start <- max(input_param_fill[[i]][['col.idx']]) + 1
# Set column indices where to add in input_param_fill and input_param lists
input_param_fill[[i]][['list.idx']] <- (1:ncol(input_param[[i]]))[is.na(input_param[[i]][1,])]
# Set number of columns
n_cols <- length(input_param_fill[[i]][['list.idx']])
# Add names for input_optim column names
if(is.null(joint.names)){
input_optim_names <- c(input_optim_names, paste0(joint.types[input.joint[i]], input.joint[i],
'-', which(is.na(input_param[[i]][1,]))))
}else{
input_optim_names <- c(input_optim_names, paste0(joint.names[input.joint[i]],
'-', which(is.na(input_param[[i]][1,]))))
}
# Fill with default values (for now, same for all joint types)
input_optim_vec <- c(input_optim_vec, rep(-0.1, n_cols))
input_bounds_r <- c(-6*pi, 6*pi)
#input_bounds_r <- c(0*(pi/180), 45*(pi/180))
input_bounds_t <- c(-max(fit_points_range), max(fit_points_range))
# Set input value
if(joint.types[input.joint[i]] %in% c('R', 'S', 'U', 'X', 'O')){
add_bounds <- rbind(rep(input_bounds_r[1], n_cols), rep(input_bounds_r[2], n_cols))
}else if(joint.types[input.joint[i]] %in% c('P')){
add_bounds <- rbind(rep(input_bounds_t[1], n_cols), rep(input_bounds_t[2], n_cols))
}else if(joint.types[input.joint[i]] %in% c('PR')){
add_bounds <- rbind(c(rep(input_bounds_t[1], 2), input_bounds_r[1]), c(rep(input_bounds_t[2], 2), input_bounds_r[2]))
}else if(joint.types[input.joint[i]] %in% c('N')){
add_bounds <- rbind(c(rep(input_bounds_t[1], 3), rep(input_bounds_r[1], 3)), c(rep(input_bounds_t[2], 3), rep(input_bounds_r[2], 3)))
}else{
stop("Unrecognized joint type for setting initial input parameter values.")
}
# Fill vectors and matrices
input_optim_bounds <- cbind(input_optim_bounds, add_bounds)
}
# For now, turn off direct input when any fixed input parameters are input
if(any_fixed_input) direct.input <- rep(FALSE, length(direct.input))
# Convert to matrix with row for each iteration
input_optim <- matrix(NA, nrow=n_iter, ncol=length(input_optim_vec), byrow=TRUE, dimnames=list(NULL, input_optim_names))
# Set default values for rows that will be used in optimization
input_optim[optim_use, ] <- input_optim_vec
# Add column names to bounds matrix
colnames(input_optim_bounds) <- input_optim_names
# Set initial value for reference iteration to 0 - this will also be optimized
if(is.vector(ref.iter)) input_optim[ref.iter, ] <- 0
if(print.progress){
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Joint parameter optimization:\n'))
cat(paste0(paste0(rep(indent, print.level+3), collapse=''), 'Joints for which input parameters will be calculated directly : '))
if(any(direct.input)){
cat(paste0(joint.names[direct.input], collapse=','))
}else{
cat('none')
}
cat(paste0(paste0(rep(indent, print.level+3), collapse=''), 'Input parameter columns that will be optimized: '))
cat(paste0(colnames(input_optim), collapse=', '))
cat('\n')
}
## Prepare joint coordinate optimization vectors
# Create boundary matrix for each joint in joints_optim
joints_optim_initial <- matrix(NA, nrow=n_joints, ncol=3, dimnames=list(joint.names, NULL))
joints_optim_initial[joints_optim, ] <- mechanism$joint.coor[joints_optim, ]
# Set coor vectors input
coor_vectors_input <- rep(FALSE, n_joints)
if(!is.null(coor.vectors)){
for(i in 1:length(coor.vectors)){
if(length(coor.vectors[[i]]) == 1) next
coor_vectors_input[i] <- TRUE
if(is.vector(coor.vectors[[i]])) coor.vectors[[i]] <- matrix(coor.vectors[[i]], 1, 3)
}
}
# These are values that are *added* to joint.coor
coor_optim <- c()
for(i in joints_optim){
# Use input coor vectors length if available
if(coor_vectors_input[i]){
coor_optim <- c(coor_optim, rep(0,nrow(coor.vectors[[i]])))
next
}
# Create sequence of values to add to joint coordinates
if(planar){
if(joint.types[i] == 'R') coor_optim <- c(coor_optim, rep(0,3))
}else{
if(joint.types[i] %in% c('R', 'P')){
coor_optim <- c(coor_optim, rep(0,2))
}else{
coor_optim <- c(coor_optim, rep(0,3))
}
}
}
# Print joint coordinate optimization parameters
if(print.progress){
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Joint coordinate optimization:\n'))
for(j in 1:n_joints){
cat(paste0(paste0(rep(indent, print.level+3), collapse=''), joint.names[j], ': '))
if(j %in% joints_optim){
cat(paste0('Adding values to {', paste0(signif(mechanism$joint.coor[j, ], 3), collapse=','), '}'))
if(coor_vectors_input[j]){
cat(paste0(' along input vectors {', paste0(signif(coor.vectors[[j]], 3), collapse=','), '}'))
}else if(joint.types[j] == 'R'){
cat(paste0(' along vectors in plane perpendicular to ', joint.names[j], ' axis'))
}else if(joint.types[j] == 'P'){
cat(paste0(' along vector perpendicular to ', joint.names[j], ' axis'))
}else{
cat(paste0(' along x, y, and z-axes'))
}
cat(paste0(' with bounds of +/- ', signif(control$joint.coor.bounds, 3), ' (per optim iter)'))
}else{
cat(paste0('Using {', paste0(signif(mechanism$joint.coor[j, ], 3), collapse=','), '}'))
}
cat('\n')
}
}
## Prepare joint constraint optimization vectors
# Create vector to specify which parameters will be optimized
cons_fill <- c()
cons_optim <- matrix(NA, nrow=0, ncol=3)
for(i in 1:n_joints){
for(j in 1:dim(joint_cons_input[[i]])[1]){
if(is.na(joint_cons_input[[i]][j, 1])){
if(joint.types[i] == 'R'){
cons_fill <- c(cons_fill, 'v')
cons_optim <- rbind(cons_optim, joint.cons[[i]][j, ])
}
if(joint.types[i] %in% c('X', 'U', 'O') && j == 1){
cons_fill <- c(cons_fill, 'v')
cons_optim <- rbind(cons_optim, joint.cons[[i]][j, ])
}
if(joint.types[i] %in% c('U', 'X', 'S', 'O') && j == 2){
cons_fill <- c(cons_fill, 'vo')
cons_optim <- rbind(cons_optim, c(0, NA, NA))
}
if(joint.types[i] %in% c('O') && j == 3){
cons_fill <- c(cons_fill, 'vo')
cons_optim <- rbind(cons_optim, c(0, NA, NA))
}
if(joint.types[i] %in% c('S') && j == 3){
cons_fill <- c(cons_fill, 'cprod')
cons_optim <- rbind(cons_optim, c(NA, NA, NA))
}
if(joint.types[i] %in% c('N')){
cons_fill <- c(cons_fill, 'n')
cons_optim <- rbind(cons_optim, rep(NA, 3))
}
}else{
cons_fill <- c(cons_fill, 'n')
cons_optim <- rbind(cons_optim, rep(NA, 3))
}
}
}
# Create vector of upper and lower bounds
cons_optim_bounds_low <- matrix(NA, nrow=length(cons_fill), ncol=3)
cons_optim_bounds_upp <- matrix(NA, nrow=length(cons_fill), ncol=3)
for(i in 1:length(cons_fill)){
if(cons_fill[i] == 'v'){
cons_optim_bounds_low[i, ] <- -1
cons_optim_bounds_upp[i, ] <- 1
}
if(cons_fill[i] == 'vo'){
cons_optim_bounds_low[i, 1] <- -6*pi
cons_optim_bounds_upp[i, 1] <- 6*pi
}
}
# Determine whether joint constraints will be optimized
if(sum(!cons_fill %in% c('n', 'cprod')) > 0){ optim_joint_cons <- TRUE }else { optim_joint_cons <- FALSE }
# Print joint constraint optimization parameters
if(print.progress){
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Joint constraint optimization:\n'))
n <- 1
for(i in 1:n_joints){
for(j in 1:dim(joint_cons_input[[i]])[1]){
cat(paste0(paste0(rep(indent, print.level+3), collapse=''), joint.names[i], '-', j, ': '))
if(cons_fill[n] %in% c('n', 'cprod')){
if(cons_fill[n] == 'n') cat(paste0('Using input vectors {', paste0(signif(joint.cons[[i]][j, ], 3), collapse=','), '}'))
if(cons_fill[n] == 'cprod') cat(paste0('Will be filled with cross product of ', joint.names[i], '-', j-2, ' and ', joint.names[i], '-', j-1))
}else{
if(cons_fill[n] == 'v') cat('Optimizing 3-unit vector')
if(cons_fill[n] == 'vo') cat(paste0('Optimizing vector orthogonal to ', joint.names[i], '-', j-1))
cat(paste0(' with initial value(s) {', paste0(signif(na.omit(cons_optim[n, ]), 3), collapse=','), '}'))
cat(paste0(', lower bounds {', paste0(signif(na.omit(cons_optim_bounds_low[n, ]), 3), collapse=','), '}'))
cat(paste0(', and upper bounds {', paste0(signif(na.omit(cons_optim_bounds_upp[n, ]), 3), collapse=','), '}'))
}
cat('\n')
n <- n + 1
}
}
}
# Add fit points to mechanism as body points
for(body_name in mechanism$body.names){
# Get fit points associated with body
fit_assoc <- which(body.assoc == body_name)
if(is.vector(ref.iter)){
if(length(fit_assoc) == 0) next
if(length(fit_assoc) == 1){
# Single fit point, set to position at reference
pose_ref <- matrix(fit.points[fit_assoc, , ref.iter], 1,3, dimnames=list(dimnames(fit.points)[[1]][fit_assoc], NULL))
points_connect <- list(c(1,1))
}else{
## Align coordinates across all time points using generalized procrustes analysis to
# find mean (consensus) shape scaled to mean centroid size
proc_align <- procrustes(fit.points[fit_assoc, , optim_use], scale=FALSE, rotate=TRUE, scale.to.mean.Csize=FALSE)
consensus <- proc_align$consensus
# Set initial reference pose by aligning consensus with reference time point
pose_ref <- bestAlign(fit.points[fit_assoc, , ref.iter], consensus)$mat
# Set points connect
if(length(fit_assoc) == 2){
points_connect <- list(c(1,2))
}else if(length(fit_assoc) == 3){
points_connect <- list(c(1,2), c(2,3), c(1,3))
}else{
points_connect <- list(c(1,2), c(2,3), c(1,3))
}
}
}else{
# Use input ref.iter matrix to set reference pose
pose_ref <- ref.iter[dimnames(fit.points)[[1]][fit_assoc],]
points_connect <- list(c(1,2), c(2,3), c(1,3))
}
# Associate fit points with body
mechanism <- associatePoints(mechanism, points=pose_ref, body=body_name,
points.connect=points_connect)
}
# Sort fit point weights to match mechanism$body.points
if(!is.null(fit.wts)){
names(fit.wts) <- dimnames(fit.points)[[1]]
fit.wts <- fit.wts[rownames(mechanism$body.points)]
}else{
fit.wts <- setNames(rep(1, nrow(mechanism$body.points)), rownames(mechanism$body.points))
}
# Sort fit points to match mechanism$body.points
fit.points <- fit.points[rownames(mechanism$body.points), , ]
# Create transformation array for fit points (resulting from pose optimization)
body_points_tmarr <- array(diag(4), dim=c(4,4,mechanism$num.bodies), dimnames=list(NULL, NULL, mechanism$body.names))
# Draw animation
#drawMechanism(mechanism, file=paste0('Fit mechanism.html'),
# window.title='Fit mechanism', animate.reverse=FALSE)
if(print.progress){
optimizing_which <- c(TRUE, optim_joint_coor, optim_joint_cons, pose.optim)
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Optimizing ',
paste0(c('input parameters', 'joint coordinates', 'constraint parameters', 'whole joint poses')[optimizing_which], collapse=', '),
'...\n'))
}
# Set difference threshold at which optimization will stop, as proportion of mean fit point range
#opt_to_diff <- control$optim.to.percent*mean(fit_points_range)
# Initial setting of optimization sequence parameters
optim_iter <- 0
#optim_errors <- c(mean(fit_points_range)*1000, mean(fit_points_range)*1000-opt_to_diff*2)
optim_errors <- c(10000, 1000)
# Create change types vector
change_types <- rep(NA, control$optim.iter.max)
if(TRUE){
# Cycle optimizing the input parameters, joint constraints and coordinates, and body
# pose until error changes less than difference threshold between consecutive
# optimization steps
optim_iter_max <- control$optim.iter.max
while(optim_iter < optim_iter_max){
if(print.progress) cat(paste0(paste0(rep(indent, print.level+2), collapse=''), optim_iter+1, ') Errors: '))
## Optimize input parameters
# Create vectors for initial and final error
input_fit_errors_i <- rep(NA, n_optim)
input_fit_errors_f <- rep(NA, n_optim)
# Switch back and forth between direct and optimized input (seems to improve fit)
if(any(direct.input) && optim_iter %in% seq(1,control$optim.iter.max,by=2)){direct_input <- TRUE}else{direct_input <- FALSE}
#direct_input <- FALSE
if(direct_input){ # && optim_iter == 0
# Get input parameters
for(i in which(direct_input)){
input_optim[optim_use, ] <- findJointInputParam(type=mechanism$joint.type[i],
coor=mechanism$joint.coor[i,], cons=mechanism$joint.cons[[i]], ref=mechanism$body.points,
poses=fit.points[, , optim_use])
}
for(i in 1:n_optim){
# Set iteration
iter <- optim_use[i]
# Find initial error
if(print.progress) input_fit_errors_i[i] <- animate_mechanism_error(input_optim[iter, ],
replace='input.param', joint.compare=joint_compare[, , iter], fit.points=fit.points[, , iter],
mechanism=mechanism, input.param=input_param_optim, fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev,
input.param.fill=input_param_fill, iter=i)
}
input_fit_errors_f <- input_fit_errors_i
input_error_outliers <- rep(FALSE, length(input_fit_errors_f))
}else{
# Run optimization
for(i in 1:n_optim){
# Set iteration
iter <- optim_use[i]
# Find initial error
if(print.progress) input_fit_errors_i[i] <- animate_mechanism_error(input_optim[iter, ],
replace='input.param', joint.compare=joint_compare[, , iter], fit.points=fit.points[, , iter],
mechanism=mechanism, input.param=input_param_optim,
fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev, input.param.fill=input_param_fill, iter=i)
# Run optimization
input_fit <- tryCatch(
expr={
nlminb(start=input_optim[iter, ], objective=animate_mechanism_error,
lower=input_optim_bounds['lower', ], upper=input_optim_bounds['upper', ],
replace='input.param', joint.compare=joint_compare[, , iter], fit.points=fit.points[, , iter],
mechanism=mechanism, input.param=input_param_optim,
fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev, input.param.fill=input_param_fill,
iter=i, control=nlminb_control)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Save error
input_fit_errors_f[i] <- input_fit$objective
# Save optimized parameters
input_optim[optim_use[i], ] <- input_fit$par
# If first cycle, set next input parameters to current values
if(optim_iter == 0 && i < n_optim) input_optim[optim_use[i+1], ] <- input_fit$par
}
# Get median fit error
med_fit_error <- median(input_fit_errors_f, na.rm=TRUE)
#cat(paste0('med{', med_fit_error, '}'))
# Correct for very small error in noiseless fit (otherwise many outliers are erroneously replaced)
if(med_fit_error < 1e-5) med_fit_error <- 0.1
#cat(paste0('med{', med_fit_error, '}'))
# Check for outlier input parameter fit errors (2x greater than median), likely poor convergence
#input_error_outliers <- input_fit_errors_f > med_fit_error*20
input_error_outliers <- rep(FALSE, length(input_fit_errors_f))
#print(input_fit_errors_f[input_error_outliers])
# If any found, replace with closest non-outlier
if(any(input_error_outliers)){
for(i in which(input_error_outliers)){
# Find closest non-outlier
i_replace <- which(!input_error_outliers)[which.min(abs(which(!input_error_outliers) - i))]
# Replace input parameters
input_optim[optim_use[i], ] <- input_optim[optim_use[i_replace], ]
}
}
#print(input_optim[!is.na(input_optim[, 1]), ])
#print(input_optim_bounds['lower', ])
#print(input_optim_bounds['upper', ])
# Set final fit error
final_fit_error <- mean(input_fit_errors_f, na.rm=TRUE)
}
# Print error
if(print.progress){
cat(paste0('', signif(mean(input_fit_errors_i, na.rm=TRUE), 3), '->', signif(mean(input_fit_errors_f, na.rm=TRUE), 3)))
cat(paste0(' {', sum(input_error_outliers), ' out}'))
}
# Add input parameters into list
#n <- 1
#for(j in 1:length(input_param_optim)){
# input_param_optim[[j]] <- matrix(input_optim[optim_use, n:(n + n_input[j] - 1)], nrow=n_optim, ncol=n_input[j])
# n <- n + n_input[j]
#}
# Add optimized input parameters into main optimization input list
for(i in 1:length(input_param_optim)){
input_param_optim[[i]][, input_param_fill[[i]][['list.idx']]] <-
matrix(input_optim[optim_use, input_param_fill[[i]][['col.idx']]], nrow=n_optim, ncol=length(input_param_fill[[i]][['col.idx']]))
}
## Optimize mechanism orientation
if(planar && FALSE){
# Find initial error
if(print.progress) mtfm_fit_error_i <- mechanism_transform_error(rep(0, 1),
fit.points=fit.points[, , optim_use], mechanism=mechanism, input.param=input_param_optim,
fit.wts=fit.wts, center=mechanism$joint.coor[!coor.optim, ])
# Run optimization
mtfm_fit <- tryCatch(
expr={
nlminb(start=rep(0, 1), objective=mechanism_transform_error,
lower=c(rep(-2*pi, 1)),
upper=c(rep(2*pi, 1)),
fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, center=mechanism$joint.coor[!coor.optim, ])
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Print error
if(print.progress) cat(paste0(', ', signif(mtfm_fit_error_i, 3), '->', signif(mtfm_fit$objective, 3)))
# Apply optimized parameters
tmat1 <- tmat2 <- tmat3 <- diag(4)
tmat1[1:3, 4] <- mechanism$joint.coor[which(!coor.optim)[1], ]
tmat2[1:3, 1:3] <- tMatrixEP(mechanism$joint.coor[which(!coor.optim)[4], ]-mechanism$joint.coor[which(!coor.optim)[1], ], mtfm_fit$par)
tmat3[1:3, 4] <- -mechanism$joint.coor[which(!coor.optim)[1], ]
tmat <- tmat1 %*% tmat2 %*% tmat3
# Apply transformation to joint constraints
for(i in 1:mechanism$num.joints){
if(mechanism$joint.types[i] == 'R'){
cons_vec <- rbind(mechanism$joint.coor[i,], mechanism$joint.coor[i,]+mechanism$joint.cons[[i]])
cons_vec_t <- applyTransform(cons_vec, tmat)
mechanism$joint.cons[[i]] <- cons_vec_t[2, ] - cons_vec_t[1, ]
}
}
# Apply transformation to joint coordinates
mechanism$joint.coor <- applyTransform(mechanism$joint.coor, tmat)
}
## Optimize joint coordinates
if(optim_joint_coor){
# Create list for vector components
bound_hit <- 0
coor_vectors <- as.list(rep(NA, n_joints))
coor_optim_bounds_low <- c()
coor_optim_bounds_upp <- c()
for(j in joints_optim){
if(planar){
#vo <- vorthogonal(mechanism$joint.cons[[j]])
#coor_vectors[[j]] <- rbind(uvector(vo), uvector(cprod(vo, mechanism$joint.cons[[j]])))
}else{
if(coor_vectors_input[j]){
coor_vectors[[j]] <- coor.vectors[[j]]
}else if(joint.types[j] == 'R'){
vo <- vorthogonal(mechanism$joint.cons[[j]])
coor_vectors[[j]] <- rbind(uvector(vo), uvector(cprod(vo, mechanism$joint.cons[[j]])))
}else if(joint.types[j] == 'P'){
coor_vectors[[j]] <- mechanism$joint.cons[[j]]
}else{
#coor_vectors[[j]] <- diag(3)[1:2, ]
coor_vectors[[j]] <- diag(3)
}
}
# Set upper and lower bound for parameter to add to vector
for(k in 1:nrow(coor_vectors[[j]])){
# Project initial position onto vector
pnl <- pointNormalOnLine(joints_optim_initial[j, ], mechanism$joint.coor[j, ], mechanism$joint.coor[j, ]+coor_vectors[[j]][k, ])
# Find bounds on vector from projection
int_sl <- rbind(
pnl - control$joint.coor.bounds*coor_vectors[[j]][k, ],
pnl + control$joint.coor.bounds*coor_vectors[[j]][k, ]
)
# Find distance from current position and bounds
dpp <- distPointToPoint(mechanism$joint.coor[j, ], int_sl)
# If point is at boundary
if(dpp[1] < 1e-4){
# Record that bound was hit
bound_hit <- bound_hit + 1
# Move joint in positive direction away from boundary
mechanism$joint.coor[j, ] <- mechanism$joint.coor[j, ] + bound_shift*coor_vectors[[j]][k, ]
# Update bounds
coor_optim_bounds_low <- c(coor_optim_bounds_low, -bound_shift)
coor_optim_bounds_upp <- c(coor_optim_bounds_upp, dpp[2]-bound_shift)
}else if(dpp[2] < 1e-4){
# Record that bound was hit
bound_hit <- bound_hit + 1
# Move joint in negative direction away from boundary
mechanism$joint.coor[j, ] <- mechanism$joint.coor[j, ] - bound_shift*coor_vectors[[j]][k, ]
coor_optim_bounds_low <- c(coor_optim_bounds_low, -dpp[1])
coor_optim_bounds_upp <- c(coor_optim_bounds_upp, 0)
}else{
# Try scaling vector toward one intersection
int_scale <- mechanism$joint.coor[j, ] + dpp[1]*coor_vectors[[j]][k, ]
# Set upper and lower bounds in each direction, depending on which direction the positive vector points
if(distPointToPoint(int_scale, int_sl[1, ]) < 1e-10){
coor_optim_bounds_low <- c(coor_optim_bounds_low, -dpp[2])
coor_optim_bounds_upp <- c(coor_optim_bounds_upp, dpp[1])
}else{
coor_optim_bounds_low <- c(coor_optim_bounds_low, -dpp[1])
coor_optim_bounds_upp <- c(coor_optim_bounds_upp, dpp[2])
}
}
}
}
# Get initial error
if(print.progress) coor_fit_error_i <- animate_mechanism_error(coor_optim,
replace='joint.coor', joint.compare=joint_compare[, , optim_use], planar=planar,
fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, coor.vectors=coor_vectors,
joint.optim=joints_optim, use.ref.as.prev=use.ref.as.prev)
# Run optimization
coor_fit <- tryCatch(
expr={
nlminb(start=coor_optim, objective=animate_mechanism_error,
lower=coor_optim_bounds_low, upper=coor_optim_bounds_upp,
replace='joint.coor', joint.compare=joint_compare[, , optim_use], planar=planar,
fit.points=fit.points[, , optim_use],
mechanism=mechanism, input.param=input_param_optim,
fit.wts=fit.wts, coor.vectors=coor_vectors,
joint.optim=joints_optim, use.ref.as.prev=use.ref.as.prev, control=nlminb_control)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
#cat('\n')
#print(coor_fit)
# Print error
if(print.progress){
cat(paste0(', ', signif(coor_fit_error_i, 3), '->', signif(coor_fit$objective, 3)))
if(bound_hit > 0){
if(bound_hit == 1){ cat(' {1 bound hit}') }else{ cat(paste0(' {', bound_hit, ' bounds hit}')) }
}
}
#print(coor_optim_bounds_low)
#print(coor_optim_bounds_upp)
#print(coor_fit$par)
# Replace previous with optimized joint coordinates
j <- 1
for(i in joints_optim){
if(length(coor_vectors[[i]]) == 1){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + coor_fit$par[j:(j+2)]
j <- j + 3
}else{
if(nrow(coor_vectors[[i]]) == 1){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(coor_fit$par[j]*coor_vectors[[i]])
j <- j + 1
}else if(nrow(coor_vectors[[i]]) == 2){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(coor_fit$par[j:(j+1)]*coor_vectors[[i]])
j <- j + 2
}else if(nrow(coor_vectors[[i]]) == 3){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(coor_fit$par[j:(j+2)]*coor_vectors[[i]])
j <- j + 3
}
}
if(planar){
mechanism$joint.coor[i, ] <- pointPlaneProj(mechanism$joint.coor[i, ],
p=mechanism$joint.coor[1, ], n=mechanism$joint.cons[[i]])
}
#print(joints_optim_initial[i, ])
#print(mechanism$joint.coor[i, ])
#print(distPointToPoint(joints_optim_initial[i, ], mechanism$joint.coor[i, ]))
}
# Set final error (in case last optim block)
final_fit_error <- coor_fit$objective
# No need to update coor_optim because joint coordinates are updated and coor_optim restarts at 0
}
#cat('\n');print(mechanism$joint.coor[joints_optim, ])
## Optimize joint constraints
if(!planar && optim_joint_cons){
# Convert optimization parameters to vector
if(optim_iter == 0) cons_optim_v <- na.omit(c(t(cons_optim)))
cons_optim_bounds_low_v <- na.omit(c(cons_optim_bounds_low))
cons_optim_bounds_upp_v <- na.omit(c(cons_optim_bounds_upp))
# if(optim_iter > 0) direct_input <- rep(FALSE, length(direct_input))
# Get initial error
if(print.progress) cons_fit_error_i <- animate_mechanism_error(cons_optim_v,
replace='joint.cons', direct.input=direct_input, joint.compare=joint_compare[, , optim_use],
fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, cons.fill=cons_fill,
use.ref.as.prev=use.ref.as.prev, print.progress=FALSE)
# Run optimization
cons_fit <- tryCatch(
expr={
nlminb(start=cons_optim_v, objective=animate_mechanism_error,
lower=cons_optim_bounds_low_v, upper=cons_optim_bounds_upp_v,
replace='joint.cons', direct.input=direct_input, joint.compare=joint_compare[, , optim_use],
fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, cons.fill=cons_fill,
use.ref.as.prev=use.ref.as.prev, control=nlminb_control)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Print error
if(print.progress){
cat(paste0(', ', signif(cons_fit_error_i, 3), '->', signif(cons_fit$objective, 3)))
if(any(direct_input)) cat(paste0(' {DI}'))
}
#if(print.progress) cat(paste0('(', paste0(round(cons_fit$par,3), collapse=','), ')'))
# Replace previous with optimized joint constraints
k <- 1
n <- 1
for(i in 1:length(mechanism$joint.cons)){
tform1 <- diag(3)
tform2 <- diag(3)
for(j in 1:nrow(mechanism$joint.cons[[i]])){
if(cons_fill[k] == 'v'){
tform1 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], cons_fit$par[n:(n+2)]), a=avec(mechanism$joint.cons[[i]][j, ], cons_fit$par[n:(n+2)]))
mechanism$joint.cons[[i]][j, ] <- uvector(cons_fit$par[n:(n+2)])
n <- n + 3
}else if(cons_fill[k] == 'vo'){
if(j == 2){
after_tform <- mechanism$joint.cons[[i]][j, ] %*% tform1
after_tform <- after_tform %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=cons_fit$par[n])
tform2 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], after_tform), a=avec(mechanism$joint.cons[[i]][j, ], after_tform))
}else if(j == 3){
after_tform <- mechanism$joint.cons[[i]][j, ] %*% tform2
after_tform <- after_tform %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=cons_fit$par[n])
#tform2 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], after_tform), a=avec(mechanism$joint.cons[[i]][j, ], after_tform))
}
mechanism$joint.cons[[i]][j, ] <- after_tform
# "Reset" rotation because master joint constraint is being modified
# When these parameters are used next inside animate_mechanism_error
# no rotation needs to be applied
cons_fit$par[n] <- 0
#mechanism$joint.cons[[i]][j, ] <- uvector(mechanism$joint.cons[[i]][j, ] %*% tform)
#mechanism$joint.cons[[i]][j, ] <- mechanism$joint.cons[[i]][j, ] %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=cons_fit$par[n])
n <- n + 1
}else if(cons_fill[k] == 'cprod'){
mechanism$joint.cons[[i]][j, ] <- cprod(mechanism$joint.cons[[i]][j-2, ], mechanism$joint.cons[[i]][j-1, ])
}
k <- k + 1
}
}
# Check orthogonality
#cat(paste0('{', avec(mechanism$joint.cons[[i]][1, ], mechanism$joint.cons[[i]][2, ]), ' ',
# avec(mechanism$joint.cons[[i]][1, ], mechanism$joint.cons[[i]][3, ]), ' ',
# avec(mechanism$joint.cons[[i]][2, ], mechanism$joint.cons[[i]][3, ]), '}'))
#print(mechanism$joint.cons[[1]])
cons_optim_v <- cons_fit$par
# Set final error (in case last optim block)
final_fit_error <- cons_fit$objective
}
# Replace directly calculated input parameters
if(any(direct_input)){
# Get input parameters
for(i in which(direct_input)){
input_param_optim[[i]] <- findJointInputParam(type=mechanism$joint.type[i],
coor=mechanism$joint.coor[i,], cons=mechanism$joint.cons[[i]],
ref=mechanism$body.points, poses=fit.points[, , optim_use])
cols_fill <- grepl(paste0('^',joint.names[direct_input[i]],'-'), colnames(input_optim))
input_optim[optim_use,cols_fill] <- input_param_optim[[i]]
}
}
## Optimize reference pose of each body
if(FALSE){
# This routine doesn't run/animate the model, it just takes the current
# transformation parameters for each set of fit.points and at each iteration,
# applies the corresponding transformation arrays from the mechanism run, and
# compares the error
# Run model to get updated transformations
tmarr <- suppressWarnings(animateMechanism(mechanism, input.param=input_param_optim,
joint.compare=joint_compare[, , optim_use], use.ref.as.prev=use.ref.as.prev, print.progress=FALSE,
check.inter.joint.dist=FALSE, check.joint.cons=FALSE)$tmat)
# Create vectors for initial and final error
pose_fit_errors_i <- setNames(rep(NA, mechanism$num.bodies), mechanism$body.names)
pose_fit_errors_f <- setNames(rep(NA, mechanism$num.bodies), mechanism$body.names)
# Run optimization
for(i in 1:mechanism$num.bodies){
# If no fit points associated with body, skip
if(is.na(mechanism$points.assoc[[i]][1])) next
# Get fit point matrix
if(length(mechanism$points.assoc[[i]]) == 1){
fit_point_mat <- t(fit.points[mechanism$points.assoc[[i]], , optim_use])
}else{
fit_point_mat <- fit.points[mechanism$points.assoc[[i]], , optim_use]
}
# Find initial error
if(print.progress) pose_fit_errors_i[i] <- ref_transform_error(rep(0, 6),
ref.points=mechanism$body.points[mechanism$points.assoc[[i]], ],
fit.points=fit_point_mat, transform=tmarr[, , i, ],
fit.wts=fit.wts[mechanism$points.assoc[[i]]])
# Run optimization
pose_fit <- tryCatch(
expr={
nlminb(start=rep(0, 6), objective=ref_transform_error,
lower=c(rep(-2*pi, 3), -rep(pose_optim_bounds_add, 3)),
upper=c(rep(2*pi, 3), rep(pose_optim_bounds_add, 3)),
ref.points=mechanism$body.points[mechanism$points.assoc[[i]], ],
fit.points=fit_point_mat, control=nlminb_control,
transform=tmarr[, , i, ], fit.wts=fit.wts[mechanism$points.assoc[[i]]])
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
#print(pose_fit$par)
#print(pose_optim_bounds_add)
# Save error
pose_fit_errors_f[i] <- pose_fit$objective
# Apply optimized parameters
tmat <- diag(4)
tmat[1:3, 1:3] <- rotationMatrixZYX(pose_fit$par[1:3])
tmat[1:3, 4] <- pose_fit$par[4:6]
mechanism$body.points[mechanism$points.assoc[[i]], ] <- applyTransform(mechanism$body.points[mechanism$points.assoc[[i]], ], tmat)
# Apply transformation to tmat array containing the current transformations applied to the reference points
body_points_tmarr[, , i] <- body_points_tmarr[, , i] %*% tmat
}
# Set final error
final_fit_error <- mean(pose_fit_errors_f, na.rm=TRUE)
# Print error
if(print.progress) cat(paste0(', ', signif(mean(pose_fit_errors_i, na.rm=TRUE), 3), '->', signif(final_fit_error, 3)))
}
## Optimize each joint coordinate and associated constraint individually - does it act like pose optimization?
if(pose.optim){
# Create vectors for initial and final error
pose_fit_errors_i <- setNames(rep(NA, mechanism$num.joints), mechanism$joint.names)
pose_fit_errors_f <- setNames(rep(NA, mechanism$num.joints), mechanism$joint.names)
for(i in 1:mechanism$num.joints){
# If no fit points associated with body on either side of joint, skip
if(is.na(mechanism$points.assoc[[mechanism$body.conn.num[i, 1]]][1]) && is.na(mechanism$points.assoc[[mechanism$body.conn.num[i, 2]]][1])) next
# Find initial error
if(print.progress) pose_fit_errors_i[i] <- transform_joint_error(rep(0, 6),
joint.idx=i, fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, joint.compare=joint_compare[, , optim_use],
print.progress=FALSE)
# Run optimization
pose_fit <- tryCatch(
expr={
nlminb(start=rep(0, 6), objective=transform_joint_error,
lower=c(rep(-0.4, 3), -rep(pose_optim_bounds_add, 3)),
upper=c(rep(0.4, 3), rep(pose_optim_bounds_add, 3)),
joint.idx=i, fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, joint.compare=joint_compare[, , optim_use],
print.progress=FALSE)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Save error
pose_fit_errors_f[i] <- pose_fit$objective
# Apply optimized parameters
tmat <- diag(4)
tmat[1:3, 1:3] <- rotationMatrixZYX(pose_fit$par[1:3])
tmat[1:3, 4] <- pose_fit$par[4:6]
# Apply transform to joint coordinate
mechanism$joint.coor[i, ] <- applyTransform(mechanism$joint.coor[i, ], tmat)
# Apply transform to joint constraint
mechanism$joint.cons[[i]] <- applyTransform(mechanism$joint.cons[[i]], tmat)
}
# Set final error
final_fit_error <- mean(pose_fit_errors_f, na.rm=TRUE)
# Print error
if(print.progress) cat(paste0(', ', signif(mean(pose_fit_errors_i, na.rm=TRUE), 3), '->', signif(final_fit_error, 3)))
}
# Save error after optimization
optim_errors <- c(optim_errors, final_fit_error)
# Get percent change in every other optim iter
every_other <- ''
if(any(direct.input) && optim_iter > 5){
optim_errors_eo3 <- optim_errors[seq(length(optim_errors)-2, length(optim_errors), by=2)]
percent_error_change <- mean(diff(optim_errors_eo3)) / mean(optim_errors_eo3)
if(optim_iter == 6) every_other <- ', now using mean of last 2 every other'
}else{
# Get percent change
percent_error_change <- diff(tail(optim_errors, 2)) / optim_errors[length(optim_errors)-1]
}
# Set change type
if(percent_error_change < 0){ change_types[optim_iter+1] <- 'decrease' }else{ change_types[optim_iter+1] <- 'increase' }
inc_iter_max <- ''
if(any(direct.input) && !direct_input && optim_iter == control$optim.iter.max-1){
optim_iter_max <- control$optim.iter.max + 1
inc_iter_max <- ', increasing iteration max to end on direct input'
}
turning_off <- ''
if(sum(change_types == 'increase', na.rm=TRUE) > 4 && any(direct.input)){
#direct.input <- FALSE*direct.input
#turning_off <- ', turning off direct.input'
}
if(print.progress && optim_iter > 0) cat(paste0(' (', signif(abs(percent_error_change)*100, 2), '% ', change_types[optim_iter+1], every_other, ')', turning_off, inc_iter_max))
if(print.progress) cat('\n')
# Stop if optim.to.percent threshold is reached - special cases for direct input since percent change bounces around much more
if(any(direct.input)){
if(direct_input){
if(optim_iter > 10){
optim_to_percent_test <- abs(percent_error_change)*100 < control$optim.to.percent
}else{
optim_to_percent_test <- FALSE
}
}else{
optim_to_percent_test <- FALSE
}
}else{
optim_to_percent_test <- abs(percent_error_change)*100 < control$optim.to.percent
}
# If near perfect fit stop - for perfect input testing
if(optim_to_percent_test) break
# If near perfect fit stop - for perfect input testing
if(final_fit_error < control$optim.error.stop) break
# Advance iteration tracker
optim_iter <- optim_iter + 1
}
mechanism_g <<- mechanism
input_param_g <<- input_param
input_optim_g <<- input_optim
input_fit_errors_f_g <<- input_fit_errors_f
optim_errors_g <<- optim_errors
}else{
input.param <- input_param_g
mechanism <- mechanism_g
input_optim <- input_optim_g
input_fit_errors_f <- input_fit_errors_f_g
optim_errors <- optim_errors_g
}
# Print final optimized joint coordinates
if(print.progress && optim_joint_coor){
#
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Final results of joint coordinate optimization:\n'))
for(j in 1:n_joints){
if(!j %in% joints_optim) next
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), joint.names[j], ': '))
cat(paste0('Optimized to {', paste0(signif(mechanism$joint.coor[j, ], 3), collapse=','), '} from {',
paste0(signif(joints_optim_initial[j, ], 3), collapse=','), '}\n'))
}
}
## Final optimization across all input parameters (will change slightly since coordinates, constraints, and pose fits have improved
if(print.progress) cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Final input parameter optimization...'))
if(print.progress && n_iter > 20) cat('\n')
# Fill in missing guesses for starting parameters with preceding parameter (could use
# interpolation if necessary but this seems to work pretty well)
if(n_optim < n_iter){
for(i in 1:nrow(input_optim)){
# Save last non-NA value
if(!is.na(input_optim[i, 1])) {seq_i <- input_optim[i, ];next}
# Add last non-NA value
input_optim[i, ] <- seq_i
}
}
# Save subset input fit errors
input_fit_errors_sub <- mean(input_fit_errors_f, na.rm=TRUE)*2
# Create vectors for initial and final error
input_fit_errors_i <- rep(NA, n_iter)
input_fit_errors_f <- rep(NA, n_iter)
if(print.progress && n_iter > 20) cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'At iteration: 1'))
# Create vector for iterations that likely did not converge
no_conv <- rep(FALSE, n_iter)
#cat('\n')
for(iter in 1:n_iter){
# Find initial error
if(print.progress) input_fit_errors_i[iter] <- animate_mechanism_error(input_optim[iter, ],
replace='input.param', joint.compare=joint_compare[, , iter],
fit.points=fit.points[, , iter], mechanism=mechanism, input.param=input_param,
fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev, input.param.fill=input_param_fill, iter=iter)
# Set iteration limit
it_limit <- 10
# Create vector to store errors
obj_vec <- rep(NA, it_limit)
# Create list to save input parameters
input_optim_try <- matrix(NA, nrow=it_limit, ncol=length(input_optim_vec))
# Run optimization with varying start parameters
n <- 1
input_fit <- list('objective'=input_fit_errors_sub*3)
add_to_start <- 0
converged <- FALSE
threshold <- input_fit_errors_sub
#cat(threshold, '\n')
while(!converged && n < it_limit){
# Try changing starting parameters if first run exceeds benchmark
if(n == 2) add_to_start <- -input_optim[iter, ]*1
if(n == 3) add_to_start <- input_optim[iter, ]*0.25
if(n == 4) add_to_start <- input_optim[iter, ]*1
if(n == 5) add_to_start <- -input_optim[iter, ]*0.5
if(n == 6) add_to_start <- -input_optim[iter, ]*0.1
if(n == 7) add_to_start <- input_optim[iter, ]*0.5
if(n == 8) add_to_start <- input_optim[iter, ]*0.05
if(n == 9) add_to_start <- input_optim[iter, ]*0.75
if(n == 10) add_to_start <- input_optim[iter, ]*0.1
# Save starting parameters
start_param <- input_optim[iter, ]+add_to_start
# Run optimization
input_fit <- tryCatch(
expr={
nlminb(start=start_param, objective=animate_mechanism_error,
lower=input_optim_bounds['lower', ], upper=input_optim_bounds['upper', ],
replace='input.param', joint.compare=joint_compare[, , iter],
fit.points=fit.points[, , iter], mechanism=mechanism, input.param=input_param,
fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev, input.param.fill=input_param_fill,
iter=iter)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Save objective
obj_vec[n] <- input_fit$objective
# Save input parameters
input_optim_try[n, ] <- input_fit$par
# Detect non-convergence
converged <- TRUE
if(input_fit$objective > threshold) converged <- FALSE
if(sum(abs(start_param - input_fit$par)) < 1e-4) converged <- FALSE
if(n > 3 && sd(obj_vec, na.rm=TRUE) < 1e-10) converged <- TRUE
#if(iter == 650) cat('\n')
#if(iter > 650 && iter < 660){
# cat(paste0('\t', iter, ' (', n, '): ', round(input_fit$objective, 4), ', ',
# round(threshold, 4), ', ', converged, ', ', paste0(round(obj_vec, 5), collapse=','), '\n'))
#}
# Change threshold so that error must be lower than initial if no change in parameters
if(n == 1 && sum(abs(start_param - input_fit$par)) < 1e-4) threshold <- input_fit$objective
n <- n + 1
}
# Find minimum error
try_min_error <- which.min(obj_vec)
# Save error
input_fit_errors_f[iter] <- obj_vec[try_min_error]
# Save optimized parameters
input_optim[iter, ] <- input_optim_try[try_min_error, ]
# Save input for next iteration
if(iter < n_iter && !((iter+1) %in% optim_use)) input_optim[iter+1, ] <- input_optim[iter, ]
#if(iter > 15) input_fit_errors_sub <- mean(tail(input_fit_errors_f, 15), na.rm=TRUE)*2
if(iter > 15) input_fit_errors_sub <- mean(input_fit_errors_f, na.rm=TRUE)*2
if(print.progress && iter %% 25 == 0) cat(paste0(',', iter))
}
if(print.progress) cat('\n')
# Set amount to try shifting angles by (produces equivalent rotation)
min_equ_shift <- c(0,-2*pi,2*pi,-4*pi,4*pi,-6*pi,6*pi)
# Add input parameters into list
for(i in 1:length(input_param)){
# Add optimized input parameters into main full input list
input_param[[i]][, input_param_fill[[i]][['list.idx']]] <- matrix(input_optim[, input_param_fill[[i]][['col.idx']]],
nrow=nrow(input_optim), ncol=length(input_param_fill[[i]][['col.idx']]))
# Set angles to minimum equivalent angle
if(joint.types[input.joint[i]] %in% c('R', 'U', 'O')){
for(j in 1:nrow(input_param[[i]])){
for(k in 1:ncol(input_param[[i]])){
input_param[[i]][j,k] <- input_param[[i]][j,k] + min_equ_shift[which.min(abs(input_param[[i]][j,k] + min_equ_shift))]
}
}
}
}
# Print error
if(print.progress) cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Error: ', signif(mean(input_fit_errors_i, na.rm=TRUE), 3), '->', signif(mean(input_fit_errors_f, na.rm=TRUE), 3), '\n'))
# mechanism_g <<- mechanism
#}
#mechanism <- mechanism_g
## Standardize joint coordinates equivalent
# Slide joint coordinates, when applicable, to closest points between connected bodies
if(!planar){
for(i in 1:n_joints){
if(!joint.types[i] %in% c('R')) next
if(!i %in% joints_optim) next
# Get points of bodies connected by joint
body1_pts <- mechanism$body.points[mechanism$body.assoc == mechanism$body.conn.num[i, 1], ]
body2_pts <- mechanism$body.points[mechanism$body.assoc == mechanism$body.conn.num[i, 2], ]
if(length(body1_pts) > 0 && length(body2_pts) > 0){
# Find nearest points on connected bodies
nearest_pts <- nearestPointsOnBodies(body1_pts, body2_pts)
# Find mean point
nearest_pt <- colMeans(nearest_pts)
}else if(length(body1_pts) == 0){
nearest_pt <- colMeans(body2_pts)
}else if(length(body2_pts) == 0){
nearest_pt <- colMeans(body1_pts)
}
if(joint.types[i] == 'R'){
# Find point on R-joint axis closest to nearest point between bodies
mechanism$joint.coor[i, ] <- pointNormalOnLine(nearest_pt, mechanism$joint.coor[i, ], mechanism$joint.coor[i, ]+mechanism$joint.cons[[i]])
}
}
}
# Get run time
time2 <- proc.time()
proc_time <- (time2 - time1)['user.self']
# Print run time
if(print.progress){
optim_iter_exceeded <- ''
if(optim_iter >= control$optim.iter.max) optim_iter_exceeded <- ' (maximum number of iterations met)'
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Total number of optimization iterations: ', optim_iter ,'', optim_iter_exceeded, '\n'))
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Total run time: ', signif(proc_time, 2), ' sec (', round(floor(proc_time / 60)), 'm ', signif(proc_time %% 60, 2),'s)\n'))
}
# Run animate mechanism using final parameters to get fit points and transformations
anim_mech <- animateMechanism(mechanism, input.param=input_param, use.ref.as.prev=use.ref.as.prev,
joint.compare=joint_compare)
list(
'mechanism'=mechanism,
'animate'=anim_mech,
'fit.points'=fit.points,
'body.tmarr'=body_points_tmarr,
'body.points.anim'=body_points_tmarr,
'input.param'=input_param,
'joint.compare'=joint_compare,
'rmse'=input_fit_errors_f,
'use.ref.as.prev'=use.ref.as.prev,
'optim.errors'=optim_errors[3:length(optim_errors)],
'run.time'=proc_time
)
}
aaronolsen/linkR documentation built on June 13, 2019, 5:39 p.m.
R Package Documentation
Browse R Packages
We want your feedback!
Note that we can't provide technical support on individual packages. You should contact the package authors for that.
fitMechanism <- function(mechanism, fit.points, body.assoc, input.param,
coor.optim = rep(TRUE, nrow(joint.coor)), pose.optim = TRUE, fit.wts = NULL,
planar = FALSE, coor.vectors = NULL, use.ref.as.prev = FALSE,
direct.input = FALSE, print.progress = FALSE, ref.iter = 1, control = NULL, print.level = 0){
# Start run time
time1 <- proc.time()
# Set indent
indent <- '\t'
if(print.progress) cat(paste0(paste0(rep(indent, print.level), collapse=''), 'fitMechanism()\n'))
control_default <- list(
'joint.coor.bounds'=0.1,
'fit.points.bounds.factor'=0.1,
'optim.frame.max'=21,
'optim.to.percent'=0.001,
'optim.iter.max'=30,
'optim.error.stop'=1e-5
)
# Set nlminb controls
nlminb_control <- list('abs.tol'=1e-20, 'rel.tol'=1e-10, 'x.tol'=1.5e-8, 'xf.tol'=2.2e-14) # Defaults
nlminb_control[['abs.tol']] <- 1e-12
nlminb_control[['rel.tol']] <- 1e-6
# Overwrite default controls with inputs, if applicable
if(is.null(control)){
control_new <- control_default
}else{
control_new <- control_default
for(i in 1:length(control))
if(!is.null(control[[names(control)[i]]])) control_new[[names(control)[i]]] <- control[[names(control)[i]]]
}
control <- control_new
# Set parameters from mechanism
joint.types <- mechanism$joint.types
joint.coor <- mechanism$joint.coor
joint.names <- rownames(mechanism$joint.coor)
input.body <- mechanism$input.body
input.joint <- mechanism$input.joint
input_joint_name <- joint.names[input.joint]
input.dof <- mechanism$input.dof
joint.cons <- mechanism$joint.cons
body.conn <- mechanism$body.conn
fixed.body <- mechanism$fixed.body
body_names <- mechanism$body.names
# If there's an N joint attached to fixed body, check if there are fit points associated with fixed body and add if there are not
if('N' %in% joint.types){
if(fixed.body %in% body.conn[joint.types == 'N',]){
if(!fixed.body %in% body.assoc){
new_fixed_pts <- paste0(fixed.body, '_fit_pt', 1:3)
fit_points_new <- array(NA, dim=dim(fit.points)+c(3,0,0), dimnames=list(c(new_fixed_pts, dimnames(fit.points)[[1]]), dimnames(fit.points)[[2]], dimnames(fit.points)[[3]]))
fit_points_new[new_fixed_pts,,] <- diag(3)
fit_points_new[dimnames(fit.points)[[1]],,] <- fit.points
fit.points <- fit_points_new
body.assoc <- c(rep(fixed.body, 3), body.assoc)
if(is.matrix(ref.iter)){
ref.iter <- rbind(matrix(diag(3), 3, 3, dimnames=list(new_fixed_pts, NULL)), ref.iter)
}
}
}
}
# Use motion between two bodies connected by each joint to generate initial joint
# constraint estimate
# Make sure that row names are unique
if(!is.null(rownames(fit.points))) if(length(unique(rownames(fit.points))) != nrow(fit.points)) stop("Row names in 'fit.points' must be unique.")
# Get number of iterations
n_iter <- dim(fit.points)[3]
# Standardize joint types
stand_joint_type <- list('Our'='O', 'Ou'='O', 'Ur'='U')
for(i in 1:length(stand_joint_type)){
joint.types[joint.types == names(stand_joint_type)[i]] <- stand_joint_type[[i]]
if(!is.null(input_joint_name)){
input_joint_name[input_joint_name == names(stand_joint_type)[i]] <- stand_joint_type[[i]]
}
}
# Get number of joints
n_joints <- length(joint.types)
# Set input parameters if single joint and NULL
if(is.null(input.param)){
if(n_joints > 1) stop("'input.param' can only be NULL for single joint models (i.e. joint.types of length 1).")
# Set input.param based on model DoFs
input.param <- matrix(NA, nrow=n_iter, ncol=jointDoF(joint.types))
}
# Set NA joint coordinates
#if(is.null(joint.coor)) joint.coor <- matrix(NA, nrow=n_joints, ncol=3, dimnames=list(joint.names, NULL))
#if(is.vector(joint.coor)) joint.coor <- matrix(joint.coor, nrow=1, ncol=3, dimnames=list(joint.names, NULL))
# Set joint names from coordinates if NULL
#if(is.null(joint.names)) joint.names <- rownames(joint.coor)
# If single coor.optim value and multiple joints, repeat for same number of joints
if(length(coor.optim) == 1 && n_joints > 1) coor.optim <- rep(coor.optim, n_joints)
# Set joint coordinates to optimize
joints_optim <- which(coor.optim)
# Set logical whether joint coordinates will be optimized
if(length(joints_optim) > 0){ optim_joint_coor <- TRUE }else{ optim_joint_coor <- FALSE }
# Set NA joint constraints if no set values specified
#if(is.null(joint.cons)){
# joint.cons <- list()
# for(joint_num in 1:n_joints){
# if(joint.types[joint_num] %in% c('R', 'L', 'T')) joint.cons[[joint_num]] <- matrix(NA, 1, 3)
# if(joint.types[joint_num] %in% c('X', 'U', 'P', 'PR')) joint.cons[[joint_num]] <- matrix(NA, 2, 3)
# if(joint.types[joint_num] %in% c('S', 'O')) joint.cons[[joint_num]] <- matrix(NA, 3, 3)
# if(joint.types[joint_num] %in% c('N')) joint.cons[[joint_num]] <- matrix(NA, 6, 3)
# }
#}
# Use define linkage just to get body.conn.num and other mechanism properties (skip path finding)
#mechanism <- defineMechanism(joint.coor=joint.coor, joint.types=joint.types,
# input.body=input.body, input.joint=input.joint, input.dof=input.dof, joint.cons=joint.cons, body.conn=body.conn,
# fixed.body=fixed.body, print.progress=FALSE)
# Save input formatted joint constraints
#joint.cons <- mechanism$joint.cons
joint_cons_input <- mechanism$joint.cons
# Create numeric body association
body_assoc_num <- rep(NA, length(body.assoc))
for(i in 1:length(body_assoc_num)) body_assoc_num[i] <- which(body.assoc[i] == mechanism$body.names)
# Find most dissimilar shapes for optimization
if(n_iter > control$optim.frame.max){
optim_use <- sort(whichMaxShapeDist(fit.points, max.index=min(n_iter, control$optim.frame.max)))
}else{
optim_use <- 1:n_iter
}
if(FALSE){
cols <- rainbow(n=length(dist_index), start=0, end=5/6)
svg.new(file='plot.html')
for(i in 1:length(dist_index)) svg.lines(fit.points[c(1:3,1), , dist_index[i]], col=cols[i])
for(i in 1:dim(fit.points)[3]) svg.lines(fit.points[c(1:3,1), , i], opacity=0.05)
svg.frame(fit.points[, , optim_use])
svg.close()
}
# Set number of optimization iterations
n_optim <- length(optim_use)
if(print.progress){
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Total time points: ', n_iter, '\n'))
if(is.vector(ref.iter)){
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Reference iteration: ', ref.iter, '\n'))
}else{
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Reference coordinates:\n'))
for(i in 1:nrow(ref.iter)){
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), rownames(ref.iter)[i], ': {', paste0(signif(ref.iter[i,], 3), collapse=','), '}\n'))
}
}
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Number of time points used in optimization: ', n_optim, '\n'))
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Time points (max. dispersed): ', paste0(optim_use, collapse=','), '\n'))
}
## Estimate initial joint constraints
if(print.progress) cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Estimating joint constraints...\n'))
for(joint_num in 1:n_joints){
if(joint.types[joint_num] == 'N'){
joint.coor[joint_num, ] <- c(0,0,0)
joint.cons[[joint_num]] <- rbind(diag(3), diag(3))
next
}
if(!is.na(joint.coor[joint_num, 1]) && joint.types[joint_num] == 'S'){
if(is.na(joint.cons[[joint_num]][3, 1])) joint.cons[[joint_num]] <- diag(3)
next
}
# Get two bodies connected by joint
bodies <- mechanism$body.conn.num[joint_num, ]
# Get points associated with each body
body_points <- list(which(body_assoc_num == bodies[1]), which(body_assoc_num == bodies[2]))
# Skip certain joints
if(joint.types[joint_num] %in% c('P')) next
# If fixed link among bodies
if(1 %in% bodies){
# Choose other body as mobile
mobile_body <- which(bodies != 1)
# Get fit points
fit_points <- fit.points[body_points[[mobile_body]], , ]
# If no fixed link
}else{
if(is.vector(ref.iter)){
# Fix body2 points relative to body1 (need to input body1+body2 points)
fit_points_rel <- immobilize(fit.points[unlist(body_points), , ], fit.points[body_points[[1]], , 1])
# Save body2 points only
fit_points <- fit_points_rel[dimnames(fit.points)[[1]][body_points[[2]]], , ]
}else{
# Fix body2 points relative to body1 (need to input body1+body2 points)
fit_points_rel <- immobilize(fit.points[unlist(body_points), , ], ref.iter[dimnames(fit.points)[[1]][body_points[[1]]], ])
# Save body2 points only
fit_points <- fit_points_rel[dimnames(fit.points)[[1]][body_points[[2]]], , ]
}
}
# Set initial fit type
init_fit_type <- joint.types[joint_num]
# **** Temp fix for initial guess - seems to work OK
if(joint.types[joint_num] %in% c('U', 'X', 'O')) init_fit_type <- 'R'
# Get initial joint constraint estimate
fit_joint_cons <- fitJointConstraint(coor=fit_points, type=init_fit_type, smooth=TRUE, print.progress=FALSE)
# Set center of rotation
if(joint.types[joint_num] %in% c('R', 'S', 'X', 'U', 'O'))
if(is.na(joint.coor[joint_num, 1])) joint.coor[joint_num, ] <- fit_joint_cons$cons[1:3]
# Set axes of rotation, filling in where initial joint constraints are not already specified
if(joint.types[joint_num] == 'R'){
if(is.na(joint.cons[[joint_num]][1, 1])) joint.cons[[joint_num]] <- matrix(fit_joint_cons$cons[4:6], 1, 3)
}
if(joint.types[joint_num] %in% c('X', 'U')){
if(is.na(joint.cons[[joint_num]][1, 1])) joint.cons[[joint_num]] <- rbind(fit_joint_cons$cons[4:6], rep(NA, 3))
if(is.na(joint.cons[[joint_num]][2, 1])) joint.cons[[joint_num]][2, ] <- vorthogonal(joint.cons[[joint_num]][1, ])
}
if(joint.types[joint_num] %in% c('O')){
if(is.na(joint.cons[[joint_num]][1, 1])) joint.cons[[joint_num]] <- rbind(fit_joint_cons$cons[4:6], rep(NA, 3), rep(NA, 3))
if(is.na(joint.cons[[joint_num]][2, 1])) joint.cons[[joint_num]][2, ] <- vorthogonal(joint.cons[[joint_num]][1, ])
if(is.na(joint.cons[[joint_num]][3, 1])) joint.cons[[joint_num]][3, ] <- vorthogonal(joint.cons[[joint_num]][2, ])
}
if(joint.types[joint_num] %in% c('S')){
if(is.na(joint.cons[[joint_num]][1, 1])){
# No input joint constraint vectors - use standard
joint.cons[[joint_num]] <- diag(3)
joint_cons_input[[joint_num]] <- diag(3)
}else{
if(is.na(joint.cons[[joint_num]][2, 1])){
joint.cons[[joint_num]][2, ] <- vorthogonal(joint.cons[[joint_num]][1, ])
joint.cons[[joint_num]][3, ] <- cprod(joint.cons[[joint_num]][1, ], joint.cons[[joint_num]][2, ])
}
}
}
}
#joint.cons[[1]][1,] <- c(0.1,0.3,1)
#joint.cons[[1]][2,] <- vorthogonal(joint_cons_true[1,])
#joint.cons[[1]][1,] <- joint_cons_true[1,]
#joint.cons[[1]][2,] <- joint_cons_true[2,]
#joint.cons[[1]][2,] <- vorthogonal(joint_cons_true[1,])
# If planar is TRUE, set joint coordinates into same plane and R-joint axes perpendicular to plane
if(planar){
if(sum(!coor.optim) <= 1){
# Find normal vector to plane
v123 <- cprod(joint.coor[3, ]-joint.coor[1, ], joint.coor[2, ]-joint.coor[1, ])
v124 <- cprod(joint.coor[4, ]-joint.coor[1, ], joint.coor[3, ]-joint.coor[2, ])
plane_nvector <- uvector(colMeans(rbind(v123, v124)))
}else{
# ****** Temp fix
joints_plane <- which(!coor.optim)
if(length(joints_plane) == 3){
v123 <- joint.coor[joints_plane[3], ] - joint.coor[joints_plane[1], ]
v124 <- joint.coor[joints_plane[2], ] - joint.coor[joints_plane[1], ]
}else{
v123 <- joint.coor[joints_plane[4], ] - joint.coor[joints_plane[1], ]
v124 <- joint.coor[joints_plane[3], ] - joint.coor[joints_plane[1], ]
}
plane_nvector <- uvector(cprod(v123, v124))
}
# Check if a joint is fixed
if(sum(!coor.optim) > 0){
# Set joint center as point in plane
plane_point <- joint.coor[which(!coor.optim)[1], ]
}else{
# Set joint center as point in plane
plane_point <- colMeans(joint.coor)
}
# Set all R-joint axes to normal vector
for(joint_num in 1:n_joints){
# Project joint coordinates into plane
joint.coor[joint_num, ] <- pointPlaneProj(joint.coor[joint_num, ], plane_point, plane_nvector)
if(joint.types[joint_num] == 'R') joint.cons[[joint_num]] <- plane_nvector
}
}
# Re-define linkage with initial joint coordinate and constraints
mechanism <- defineMechanism(joint.coor=joint.coor, joint.types=joint.types,
input.body=input.body, input.joint=input.joint, input.dof=input.dof, joint.cons=joint.cons,
body.conn=body.conn, fixed.body=fixed.body)
# If input.joint is NULL and a single joint, set sole joint as input
if(is.null(input.joint)){
if(n_joints > 1) stop("If the mechanism has more than one joint 'input.joint' must be specified.")
input.joint <- 1
}
# If input.joint is non-numeric, convert to numeric equivalent
if(!is.numeric(input.joint[1])){
if(sum(!input.joint %in% mechanism$joint.names) > 0) stop("'input.joint' names do not match joint names.")
input_joint_num <- rep(NA, length(input.joint))
for(i in 1:length(input.joint)) input_joint_num[i] <- which(mechanism$joint.names == input.joint[i])
input.joint <- input_joint_num
}
# Set joint compare to choose toggle positions (not necessary for single joint)
if(n_joints == 1){
joint_compare <- NULL
}else{
# Create joint coordinates animated with fit points for toggle comparisons
joint_compare <- array(NA, dim=c(dim(mechanism$joint.coor), n_iter),
dimnames=list(rownames(mechanism$joint.coor), NULL, NULL))
#
for(i in 1:dim(joint_compare)[1]){
# Find associated bodies
assoc_bodies <- mechanism$body.conn.num[i, ]
# Find associated fit points
body_names_a <- mechanism$body.names[assoc_bodies]
# Get fit point indices associated with each body
which_body1 <- which(body.assoc == body_names_a[1])
which_body2 <- which(body.assoc == body_names_a[2])
# Skip if less than 3 fit points
if(length(which_body1) < 3 && length(which_body2) < 3) next
# Choose one body
which_body <- which_body1
if(length(which_body1) < 3) which_body <- which_body2
if(length(which_body2) < 3) which_body <- which_body1
# Combine fit points and joint
if(is.vector(ref.iter)){
fit_point_joint <- rbind(fit.points[which_body, , ref.iter], 'joint.coor'=mechanism$joint.coor[i, ])
}else{
fit_point_joint <- rbind(ref.iter[dimnames(fit.points[which_body,,])[[1]],], 'joint.coor'=mechanism$joint.coor[i, ])
}
# Fill array
for(iter in 1:n_iter){
# Move joint based on fit point motion
joint_compare[i, , iter] <- bestAlign(fit.points[which_body, , iter], fit_point_joint)$mat['joint.coor', ]
}
}
}
#print(mechanism$joint.coor['SH_cra',])
#print(t(joint_compare['SH_cra',,]))
#return(1)
## Prepare optimization vectors
if(print.progress) cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Preparing optimization parameters...\n'))
## Prepare fit point pose optimization vectors
# Get range of joint coordinate positions along each axis
fit_points_range <- apply(apply(fit.points[, , 1], 2, 'range'), 2, 'diff')
# If one dimension is 0, set to minimum of other dimensions
if(sum(abs(fit_points_range) < 1e-3) > 0) fit_points_range[abs(fit_points_range) < 1e-3] <- min(fit_points_range[fit_points_range !=0])
# Set bound for translation
pose_optim_bounds_add <- fit_points_range*control$fit.points.bounds.factor
# Set amount to shift from boundary
bound_shift <- control$joint.coor.bounds*0.01
# Create vector of input parameters (start with first iteration)
input_optim_vec <- c()
input_optim_names <- c()
# Create matrix of bounds for input parameters
input_optim_bounds <- matrix(NA, nrow=2, ncol=0, dimnames=list(c('lower', 'upper'), NULL))
# Convert input.param into list of matrices for consistency
if(class(input.param) == 'numeric') input.param <- list(matrix(input.param, nrow=n_inputs, ncol=1))
if(class(input.param) == 'matrix') input.param <- list(input.param)
if(class(input.param) == 'list'){
for(i in 1:length(input.param)) if(is.vector(input.param[[i]])) input.param[[i]] <- matrix(input.param[[i]], nrow=length(input.param[[i]]), ncol=1)
}
# Create modifiable copy of input parameters
input_param <- input.param
# Create vector for number of input parameters per input
n_input <- rep(NA, length(input_param))
# Create optimization input parameter
input_param_optim <- list()
# Create list to specify which values from input_optim will be placed in which columns of input_param
input_param_fill <- list()
# Set joints for which input parameters will be calculated directly (for now only use in single joint case)
direct.input <- rep(FALSE, length(input.joint))
if(direct.input && n_joints == 1) direct.input[joint.types == 'U'] <- TRUE
# Logical for whether there are any fixed inputs
any_fixed_input <- FALSE
# Fill vector with initial values
col_idx_start <- 1
for(i in 1:length(input.joint)){
# Create list element
input_param_fill[[i]] <- list()
# Create optimization input parameters
input_param_optim[[i]] <- matrix(input_param[[i]][optim_use, ], nrow=n_optim, ncol=ncol(input_param[[i]]))
if(sum(!is.na(input_param[[i]][1,])) > 0) any_fixed_input <- TRUE
# Set column indices to pull from input_optim matrix
input_param_fill[[i]][['col.idx']] <- col_idx_start:(col_idx_start + sum(is.na(input_param[[i]][1,]))-1)
col_idx_start <- max(input_param_fill[[i]][['col.idx']]) + 1
# Set column indices where to add in input_param_fill and input_param lists
input_param_fill[[i]][['list.idx']] <- (1:ncol(input_param[[i]]))[is.na(input_param[[i]][1,])]
# Set number of columns
n_cols <- length(input_param_fill[[i]][['list.idx']])
# Add names for input_optim column names
if(is.null(joint.names)){
input_optim_names <- c(input_optim_names, paste0(joint.types[input.joint[i]], input.joint[i],
'-', which(is.na(input_param[[i]][1,]))))
}else{
input_optim_names <- c(input_optim_names, paste0(joint.names[input.joint[i]],
'-', which(is.na(input_param[[i]][1,]))))
}
# Fill with default values (for now, same for all joint types)
input_optim_vec <- c(input_optim_vec, rep(-0.1, n_cols))
input_bounds_r <- c(-6*pi, 6*pi)
#input_bounds_r <- c(0*(pi/180), 45*(pi/180))
input_bounds_t <- c(-max(fit_points_range), max(fit_points_range))
# Set input value
if(joint.types[input.joint[i]] %in% c('R', 'S', 'U', 'X', 'O')){
add_bounds <- rbind(rep(input_bounds_r[1], n_cols), rep(input_bounds_r[2], n_cols))
}else if(joint.types[input.joint[i]] %in% c('P')){
add_bounds <- rbind(rep(input_bounds_t[1], n_cols), rep(input_bounds_t[2], n_cols))
}else if(joint.types[input.joint[i]] %in% c('PR')){
add_bounds <- rbind(c(rep(input_bounds_t[1], 2), input_bounds_r[1]), c(rep(input_bounds_t[2], 2), input_bounds_r[2]))
}else if(joint.types[input.joint[i]] %in% c('N')){
add_bounds <- rbind(c(rep(input_bounds_t[1], 3), rep(input_bounds_r[1], 3)), c(rep(input_bounds_t[2], 3), rep(input_bounds_r[2], 3)))
}else{
stop("Unrecognized joint type for setting initial input parameter values.")
}
# Fill vectors and matrices
input_optim_bounds <- cbind(input_optim_bounds, add_bounds)
}
# For now, turn off direct input when any fixed input parameters are input
if(any_fixed_input) direct.input <- rep(FALSE, length(direct.input))
# Convert to matrix with row for each iteration
input_optim <- matrix(NA, nrow=n_iter, ncol=length(input_optim_vec), byrow=TRUE, dimnames=list(NULL, input_optim_names))
# Set default values for rows that will be used in optimization
input_optim[optim_use, ] <- input_optim_vec
# Add column names to bounds matrix
colnames(input_optim_bounds) <- input_optim_names
# Set initial value for reference iteration to 0 - this will also be optimized
if(is.vector(ref.iter)) input_optim[ref.iter, ] <- 0
if(print.progress){
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Joint parameter optimization:\n'))
cat(paste0(paste0(rep(indent, print.level+3), collapse=''), 'Joints for which input parameters will be calculated directly : '))
if(any(direct.input)){
cat(paste0(joint.names[direct.input], collapse=','))
}else{
cat('none')
}
cat(paste0(paste0(rep(indent, print.level+3), collapse=''), 'Input parameter columns that will be optimized: '))
cat(paste0(colnames(input_optim), collapse=', '))
cat('\n')
}
## Prepare joint coordinate optimization vectors
# Create boundary matrix for each joint in joints_optim
joints_optim_initial <- matrix(NA, nrow=n_joints, ncol=3, dimnames=list(joint.names, NULL))
joints_optim_initial[joints_optim, ] <- mechanism$joint.coor[joints_optim, ]
# Set coor vectors input
coor_vectors_input <- rep(FALSE, n_joints)
if(!is.null(coor.vectors)){
for(i in 1:length(coor.vectors)){
if(length(coor.vectors[[i]]) == 1) next
coor_vectors_input[i] <- TRUE
if(is.vector(coor.vectors[[i]])) coor.vectors[[i]] <- matrix(coor.vectors[[i]], 1, 3)
}
}
# These are values that are *added* to joint.coor
coor_optim <- c()
for(i in joints_optim){
# Use input coor vectors length if available
if(coor_vectors_input[i]){
coor_optim <- c(coor_optim, rep(0,nrow(coor.vectors[[i]])))
next
}
# Create sequence of values to add to joint coordinates
if(planar){
if(joint.types[i] == 'R') coor_optim <- c(coor_optim, rep(0,3))
}else{
if(joint.types[i] %in% c('R', 'P')){
coor_optim <- c(coor_optim, rep(0,2))
}else{
coor_optim <- c(coor_optim, rep(0,3))
}
}
}
# Print joint coordinate optimization parameters
if(print.progress){
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Joint coordinate optimization:\n'))
for(j in 1:n_joints){
cat(paste0(paste0(rep(indent, print.level+3), collapse=''), joint.names[j], ': '))
if(j %in% joints_optim){
cat(paste0('Adding values to {', paste0(signif(mechanism$joint.coor[j, ], 3), collapse=','), '}'))
if(coor_vectors_input[j]){
cat(paste0(' along input vectors {', paste0(signif(coor.vectors[[j]], 3), collapse=','), '}'))
}else if(joint.types[j] == 'R'){
cat(paste0(' along vectors in plane perpendicular to ', joint.names[j], ' axis'))
}else if(joint.types[j] == 'P'){
cat(paste0(' along vector perpendicular to ', joint.names[j], ' axis'))
}else{
cat(paste0(' along x, y, and z-axes'))
}
cat(paste0(' with bounds of +/- ', signif(control$joint.coor.bounds, 3), ' (per optim iter)'))
}else{
cat(paste0('Using {', paste0(signif(mechanism$joint.coor[j, ], 3), collapse=','), '}'))
}
cat('\n')
}
}
## Prepare joint constraint optimization vectors
# Create vector to specify which parameters will be optimized
cons_fill <- c()
cons_optim <- matrix(NA, nrow=0, ncol=3)
for(i in 1:n_joints){
for(j in 1:dim(joint_cons_input[[i]])[1]){
if(is.na(joint_cons_input[[i]][j, 1])){
if(joint.types[i] == 'R'){
cons_fill <- c(cons_fill, 'v')
cons_optim <- rbind(cons_optim, joint.cons[[i]][j, ])
}
if(joint.types[i] %in% c('X', 'U', 'O') && j == 1){
cons_fill <- c(cons_fill, 'v')
cons_optim <- rbind(cons_optim, joint.cons[[i]][j, ])
}
if(joint.types[i] %in% c('U', 'X', 'S', 'O') && j == 2){
cons_fill <- c(cons_fill, 'vo')
cons_optim <- rbind(cons_optim, c(0, NA, NA))
}
if(joint.types[i] %in% c('O') && j == 3){
cons_fill <- c(cons_fill, 'vo')
cons_optim <- rbind(cons_optim, c(0, NA, NA))
}
if(joint.types[i] %in% c('S') && j == 3){
cons_fill <- c(cons_fill, 'cprod')
cons_optim <- rbind(cons_optim, c(NA, NA, NA))
}
if(joint.types[i] %in% c('N')){
cons_fill <- c(cons_fill, 'n')
cons_optim <- rbind(cons_optim, rep(NA, 3))
}
}else{
cons_fill <- c(cons_fill, 'n')
cons_optim <- rbind(cons_optim, rep(NA, 3))
}
}
}
# Create vector of upper and lower bounds
cons_optim_bounds_low <- matrix(NA, nrow=length(cons_fill), ncol=3)
cons_optim_bounds_upp <- matrix(NA, nrow=length(cons_fill), ncol=3)
for(i in 1:length(cons_fill)){
if(cons_fill[i] == 'v'){
cons_optim_bounds_low[i, ] <- -1
cons_optim_bounds_upp[i, ] <- 1
}
if(cons_fill[i] == 'vo'){
cons_optim_bounds_low[i, 1] <- -6*pi
cons_optim_bounds_upp[i, 1] <- 6*pi
}
}
# Determine whether joint constraints will be optimized
if(sum(!cons_fill %in% c('n', 'cprod')) > 0){ optim_joint_cons <- TRUE }else { optim_joint_cons <- FALSE }
# Print joint constraint optimization parameters
if(print.progress){
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Joint constraint optimization:\n'))
n <- 1
for(i in 1:n_joints){
for(j in 1:dim(joint_cons_input[[i]])[1]){
cat(paste0(paste0(rep(indent, print.level+3), collapse=''), joint.names[i], '-', j, ': '))
if(cons_fill[n] %in% c('n', 'cprod')){
if(cons_fill[n] == 'n') cat(paste0('Using input vectors {', paste0(signif(joint.cons[[i]][j, ], 3), collapse=','), '}'))
if(cons_fill[n] == 'cprod') cat(paste0('Will be filled with cross product of ', joint.names[i], '-', j-2, ' and ', joint.names[i], '-', j-1))
}else{
if(cons_fill[n] == 'v') cat('Optimizing 3-unit vector')
if(cons_fill[n] == 'vo') cat(paste0('Optimizing vector orthogonal to ', joint.names[i], '-', j-1))
cat(paste0(' with initial value(s) {', paste0(signif(na.omit(cons_optim[n, ]), 3), collapse=','), '}'))
cat(paste0(', lower bounds {', paste0(signif(na.omit(cons_optim_bounds_low[n, ]), 3), collapse=','), '}'))
cat(paste0(', and upper bounds {', paste0(signif(na.omit(cons_optim_bounds_upp[n, ]), 3), collapse=','), '}'))
}
cat('\n')
n <- n + 1
}
}
}
# Add fit points to mechanism as body points
for(body_name in mechanism$body.names){
# Get fit points associated with body
fit_assoc <- which(body.assoc == body_name)
if(is.vector(ref.iter)){
if(length(fit_assoc) == 0) next
if(length(fit_assoc) == 1){
# Single fit point, set to position at reference
pose_ref <- matrix(fit.points[fit_assoc, , ref.iter], 1,3, dimnames=list(dimnames(fit.points)[[1]][fit_assoc], NULL))
points_connect <- list(c(1,1))
}else{
## Align coordinates across all time points using generalized procrustes analysis to
# find mean (consensus) shape scaled to mean centroid size
proc_align <- procrustes(fit.points[fit_assoc, , optim_use], scale=FALSE, rotate=TRUE, scale.to.mean.Csize=FALSE)
consensus <- proc_align$consensus
# Set initial reference pose by aligning consensus with reference time point
pose_ref <- bestAlign(fit.points[fit_assoc, , ref.iter], consensus)$mat
# Set points connect
if(length(fit_assoc) == 2){
points_connect <- list(c(1,2))
}else if(length(fit_assoc) == 3){
points_connect <- list(c(1,2), c(2,3), c(1,3))
}else{
points_connect <- list(c(1,2), c(2,3), c(1,3))
}
}
}else{
# Use input ref.iter matrix to set reference pose
pose_ref <- ref.iter[dimnames(fit.points)[[1]][fit_assoc],]
points_connect <- list(c(1,2), c(2,3), c(1,3))
}
# Associate fit points with body
mechanism <- associatePoints(mechanism, points=pose_ref, body=body_name,
points.connect=points_connect)
}
# Sort fit point weights to match mechanism$body.points
if(!is.null(fit.wts)){
names(fit.wts) <- dimnames(fit.points)[[1]]
fit.wts <- fit.wts[rownames(mechanism$body.points)]
}else{
fit.wts <- setNames(rep(1, nrow(mechanism$body.points)), rownames(mechanism$body.points))
}
# Sort fit points to match mechanism$body.points
fit.points <- fit.points[rownames(mechanism$body.points), , ]
# Create transformation array for fit points (resulting from pose optimization)
body_points_tmarr <- array(diag(4), dim=c(4,4,mechanism$num.bodies), dimnames=list(NULL, NULL, mechanism$body.names))
# Draw animation
#drawMechanism(mechanism, file=paste0('Fit mechanism.html'),
# window.title='Fit mechanism', animate.reverse=FALSE)
if(print.progress){
optimizing_which <- c(TRUE, optim_joint_coor, optim_joint_cons, pose.optim)
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Optimizing ',
paste0(c('input parameters', 'joint coordinates', 'constraint parameters', 'whole joint poses')[optimizing_which], collapse=', '),
'...\n'))
}
# Set difference threshold at which optimization will stop, as proportion of mean fit point range
#opt_to_diff <- control$optim.to.percent*mean(fit_points_range)
# Initial setting of optimization sequence parameters
optim_iter <- 0
#optim_errors <- c(mean(fit_points_range)*1000, mean(fit_points_range)*1000-opt_to_diff*2)
optim_errors <- c(10000, 1000)
# Create change types vector
change_types <- rep(NA, control$optim.iter.max)
if(TRUE){
# Cycle optimizing the input parameters, joint constraints and coordinates, and body
# pose until error changes less than difference threshold between consecutive
# optimization steps
optim_iter_max <- control$optim.iter.max
while(optim_iter < optim_iter_max){
if(print.progress) cat(paste0(paste0(rep(indent, print.level+2), collapse=''), optim_iter+1, ') Errors: '))
## Optimize input parameters
# Create vectors for initial and final error
input_fit_errors_i <- rep(NA, n_optim)
input_fit_errors_f <- rep(NA, n_optim)
# Switch back and forth between direct and optimized input (seems to improve fit)
if(any(direct.input) && optim_iter %in% seq(1,control$optim.iter.max,by=2)){direct_input <- TRUE}else{direct_input <- FALSE}
#direct_input <- FALSE
if(direct_input){ # && optim_iter == 0
# Get input parameters
for(i in which(direct_input)){
input_optim[optim_use, ] <- findJointInputParam(type=mechanism$joint.type[i],
coor=mechanism$joint.coor[i,], cons=mechanism$joint.cons[[i]], ref=mechanism$body.points,
poses=fit.points[, , optim_use])
}
for(i in 1:n_optim){
# Set iteration
iter <- optim_use[i]
# Find initial error
if(print.progress) input_fit_errors_i[i] <- animate_mechanism_error(input_optim[iter, ],
replace='input.param', joint.compare=joint_compare[, , iter], fit.points=fit.points[, , iter],
mechanism=mechanism, input.param=input_param_optim, fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev,
input.param.fill=input_param_fill, iter=i)
}
input_fit_errors_f <- input_fit_errors_i
input_error_outliers <- rep(FALSE, length(input_fit_errors_f))
}else{
# Run optimization
for(i in 1:n_optim){
# Set iteration
iter <- optim_use[i]
# Find initial error
if(print.progress) input_fit_errors_i[i] <- animate_mechanism_error(input_optim[iter, ],
replace='input.param', joint.compare=joint_compare[, , iter], fit.points=fit.points[, , iter],
mechanism=mechanism, input.param=input_param_optim,
fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev, input.param.fill=input_param_fill, iter=i)
# Run optimization
input_fit <- tryCatch(
expr={
nlminb(start=input_optim[iter, ], objective=animate_mechanism_error,
lower=input_optim_bounds['lower', ], upper=input_optim_bounds['upper', ],
replace='input.param', joint.compare=joint_compare[, , iter], fit.points=fit.points[, , iter],
mechanism=mechanism, input.param=input_param_optim,
fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev, input.param.fill=input_param_fill,
iter=i, control=nlminb_control)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Save error
input_fit_errors_f[i] <- input_fit$objective
# Save optimized parameters
input_optim[optim_use[i], ] <- input_fit$par
# If first cycle, set next input parameters to current values
if(optim_iter == 0 && i < n_optim) input_optim[optim_use[i+1], ] <- input_fit$par
}
# Get median fit error
med_fit_error <- median(input_fit_errors_f, na.rm=TRUE)
#cat(paste0('med{', med_fit_error, '}'))
# Correct for very small error in noiseless fit (otherwise many outliers are erroneously replaced)
if(med_fit_error < 1e-5) med_fit_error <- 0.1
#cat(paste0('med{', med_fit_error, '}'))
# Check for outlier input parameter fit errors (2x greater than median), likely poor convergence
#input_error_outliers <- input_fit_errors_f > med_fit_error*20
input_error_outliers <- rep(FALSE, length(input_fit_errors_f))
#print(input_fit_errors_f[input_error_outliers])
# If any found, replace with closest non-outlier
if(any(input_error_outliers)){
for(i in which(input_error_outliers)){
# Find closest non-outlier
i_replace <- which(!input_error_outliers)[which.min(abs(which(!input_error_outliers) - i))]
# Replace input parameters
input_optim[optim_use[i], ] <- input_optim[optim_use[i_replace], ]
}
}
#print(input_optim[!is.na(input_optim[, 1]), ])
#print(input_optim_bounds['lower', ])
#print(input_optim_bounds['upper', ])
# Set final fit error
final_fit_error <- mean(input_fit_errors_f, na.rm=TRUE)
}
# Print error
if(print.progress){
cat(paste0('', signif(mean(input_fit_errors_i, na.rm=TRUE), 3), '->', signif(mean(input_fit_errors_f, na.rm=TRUE), 3)))
cat(paste0(' {', sum(input_error_outliers), ' out}'))
}
# Add input parameters into list
#n <- 1
#for(j in 1:length(input_param_optim)){
# input_param_optim[[j]] <- matrix(input_optim[optim_use, n:(n + n_input[j] - 1)], nrow=n_optim, ncol=n_input[j])
# n <- n + n_input[j]
#}
# Add optimized input parameters into main optimization input list
for(i in 1:length(input_param_optim)){
input_param_optim[[i]][, input_param_fill[[i]][['list.idx']]] <-
matrix(input_optim[optim_use, input_param_fill[[i]][['col.idx']]], nrow=n_optim, ncol=length(input_param_fill[[i]][['col.idx']]))
}
## Optimize mechanism orientation
if(planar && FALSE){
# Find initial error
if(print.progress) mtfm_fit_error_i <- mechanism_transform_error(rep(0, 1),
fit.points=fit.points[, , optim_use], mechanism=mechanism, input.param=input_param_optim,
fit.wts=fit.wts, center=mechanism$joint.coor[!coor.optim, ])
# Run optimization
mtfm_fit <- tryCatch(
expr={
nlminb(start=rep(0, 1), objective=mechanism_transform_error,
lower=c(rep(-2*pi, 1)),
upper=c(rep(2*pi, 1)),
fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, center=mechanism$joint.coor[!coor.optim, ])
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Print error
if(print.progress) cat(paste0(', ', signif(mtfm_fit_error_i, 3), '->', signif(mtfm_fit$objective, 3)))
# Apply optimized parameters
tmat1 <- tmat2 <- tmat3 <- diag(4)
tmat1[1:3, 4] <- mechanism$joint.coor[which(!coor.optim)[1], ]
tmat2[1:3, 1:3] <- tMatrixEP(mechanism$joint.coor[which(!coor.optim)[4], ]-mechanism$joint.coor[which(!coor.optim)[1], ], mtfm_fit$par)
tmat3[1:3, 4] <- -mechanism$joint.coor[which(!coor.optim)[1], ]
tmat <- tmat1 %*% tmat2 %*% tmat3
# Apply transformation to joint constraints
for(i in 1:mechanism$num.joints){
if(mechanism$joint.types[i] == 'R'){
cons_vec <- rbind(mechanism$joint.coor[i,], mechanism$joint.coor[i,]+mechanism$joint.cons[[i]])
cons_vec_t <- applyTransform(cons_vec, tmat)
mechanism$joint.cons[[i]] <- cons_vec_t[2, ] - cons_vec_t[1, ]
}
}
# Apply transformation to joint coordinates
mechanism$joint.coor <- applyTransform(mechanism$joint.coor, tmat)
}
## Optimize joint coordinates
if(optim_joint_coor){
# Create list for vector components
bound_hit <- 0
coor_vectors <- as.list(rep(NA, n_joints))
coor_optim_bounds_low <- c()
coor_optim_bounds_upp <- c()
for(j in joints_optim){
if(planar){
#vo <- vorthogonal(mechanism$joint.cons[[j]])
#coor_vectors[[j]] <- rbind(uvector(vo), uvector(cprod(vo, mechanism$joint.cons[[j]])))
}else{
if(coor_vectors_input[j]){
coor_vectors[[j]] <- coor.vectors[[j]]
}else if(joint.types[j] == 'R'){
vo <- vorthogonal(mechanism$joint.cons[[j]])
coor_vectors[[j]] <- rbind(uvector(vo), uvector(cprod(vo, mechanism$joint.cons[[j]])))
}else if(joint.types[j] == 'P'){
coor_vectors[[j]] <- mechanism$joint.cons[[j]]
}else{
#coor_vectors[[j]] <- diag(3)[1:2, ]
coor_vectors[[j]] <- diag(3)
}
}
# Set upper and lower bound for parameter to add to vector
for(k in 1:nrow(coor_vectors[[j]])){
# Project initial position onto vector
pnl <- pointNormalOnLine(joints_optim_initial[j, ], mechanism$joint.coor[j, ], mechanism$joint.coor[j, ]+coor_vectors[[j]][k, ])
# Find bounds on vector from projection
int_sl <- rbind(
pnl - control$joint.coor.bounds*coor_vectors[[j]][k, ],
pnl + control$joint.coor.bounds*coor_vectors[[j]][k, ]
)
# Find distance from current position and bounds
dpp <- distPointToPoint(mechanism$joint.coor[j, ], int_sl)
# If point is at boundary
if(dpp[1] < 1e-4){
# Record that bound was hit
bound_hit <- bound_hit + 1
# Move joint in positive direction away from boundary
mechanism$joint.coor[j, ] <- mechanism$joint.coor[j, ] + bound_shift*coor_vectors[[j]][k, ]
# Update bounds
coor_optim_bounds_low <- c(coor_optim_bounds_low, -bound_shift)
coor_optim_bounds_upp <- c(coor_optim_bounds_upp, dpp[2]-bound_shift)
}else if(dpp[2] < 1e-4){
# Record that bound was hit
bound_hit <- bound_hit + 1
# Move joint in negative direction away from boundary
mechanism$joint.coor[j, ] <- mechanism$joint.coor[j, ] - bound_shift*coor_vectors[[j]][k, ]
coor_optim_bounds_low <- c(coor_optim_bounds_low, -dpp[1])
coor_optim_bounds_upp <- c(coor_optim_bounds_upp, 0)
}else{
# Try scaling vector toward one intersection
int_scale <- mechanism$joint.coor[j, ] + dpp[1]*coor_vectors[[j]][k, ]
# Set upper and lower bounds in each direction, depending on which direction the positive vector points
if(distPointToPoint(int_scale, int_sl[1, ]) < 1e-10){
coor_optim_bounds_low <- c(coor_optim_bounds_low, -dpp[2])
coor_optim_bounds_upp <- c(coor_optim_bounds_upp, dpp[1])
}else{
coor_optim_bounds_low <- c(coor_optim_bounds_low, -dpp[1])
coor_optim_bounds_upp <- c(coor_optim_bounds_upp, dpp[2])
}
}
}
}
# Get initial error
if(print.progress) coor_fit_error_i <- animate_mechanism_error(coor_optim,
replace='joint.coor', joint.compare=joint_compare[, , optim_use], planar=planar,
fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, coor.vectors=coor_vectors,
joint.optim=joints_optim, use.ref.as.prev=use.ref.as.prev)
# Run optimization
coor_fit <- tryCatch(
expr={
nlminb(start=coor_optim, objective=animate_mechanism_error,
lower=coor_optim_bounds_low, upper=coor_optim_bounds_upp,
replace='joint.coor', joint.compare=joint_compare[, , optim_use], planar=planar,
fit.points=fit.points[, , optim_use],
mechanism=mechanism, input.param=input_param_optim,
fit.wts=fit.wts, coor.vectors=coor_vectors,
joint.optim=joints_optim, use.ref.as.prev=use.ref.as.prev, control=nlminb_control)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
#cat('\n')
#print(coor_fit)
# Print error
if(print.progress){
cat(paste0(', ', signif(coor_fit_error_i, 3), '->', signif(coor_fit$objective, 3)))
if(bound_hit > 0){
if(bound_hit == 1){ cat(' {1 bound hit}') }else{ cat(paste0(' {', bound_hit, ' bounds hit}')) }
}
}
#print(coor_optim_bounds_low)
#print(coor_optim_bounds_upp)
#print(coor_fit$par)
# Replace previous with optimized joint coordinates
j <- 1
for(i in joints_optim){
if(length(coor_vectors[[i]]) == 1){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + coor_fit$par[j:(j+2)]
j <- j + 3
}else{
if(nrow(coor_vectors[[i]]) == 1){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(coor_fit$par[j]*coor_vectors[[i]])
j <- j + 1
}else if(nrow(coor_vectors[[i]]) == 2){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(coor_fit$par[j:(j+1)]*coor_vectors[[i]])
j <- j + 2
}else if(nrow(coor_vectors[[i]]) == 3){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(coor_fit$par[j:(j+2)]*coor_vectors[[i]])
j <- j + 3
}
}
if(planar){
mechanism$joint.coor[i, ] <- pointPlaneProj(mechanism$joint.coor[i, ],
p=mechanism$joint.coor[1, ], n=mechanism$joint.cons[[i]])
}
#print(joints_optim_initial[i, ])
#print(mechanism$joint.coor[i, ])
#print(distPointToPoint(joints_optim_initial[i, ], mechanism$joint.coor[i, ]))
}
# Set final error (in case last optim block)
final_fit_error <- coor_fit$objective
# No need to update coor_optim because joint coordinates are updated and coor_optim restarts at 0
}
#cat('\n');print(mechanism$joint.coor[joints_optim, ])
## Optimize joint constraints
if(!planar && optim_joint_cons){
# Convert optimization parameters to vector
if(optim_iter == 0) cons_optim_v <- na.omit(c(t(cons_optim)))
cons_optim_bounds_low_v <- na.omit(c(cons_optim_bounds_low))
cons_optim_bounds_upp_v <- na.omit(c(cons_optim_bounds_upp))
# if(optim_iter > 0) direct_input <- rep(FALSE, length(direct_input))
# Get initial error
if(print.progress) cons_fit_error_i <- animate_mechanism_error(cons_optim_v,
replace='joint.cons', direct.input=direct_input, joint.compare=joint_compare[, , optim_use],
fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, cons.fill=cons_fill,
use.ref.as.prev=use.ref.as.prev, print.progress=FALSE)
# Run optimization
cons_fit <- tryCatch(
expr={
nlminb(start=cons_optim_v, objective=animate_mechanism_error,
lower=cons_optim_bounds_low_v, upper=cons_optim_bounds_upp_v,
replace='joint.cons', direct.input=direct_input, joint.compare=joint_compare[, , optim_use],
fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, cons.fill=cons_fill,
use.ref.as.prev=use.ref.as.prev, control=nlminb_control)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Print error
if(print.progress){
cat(paste0(', ', signif(cons_fit_error_i, 3), '->', signif(cons_fit$objective, 3)))
if(any(direct_input)) cat(paste0(' {DI}'))
}
#if(print.progress) cat(paste0('(', paste0(round(cons_fit$par,3), collapse=','), ')'))
# Replace previous with optimized joint constraints
k <- 1
n <- 1
for(i in 1:length(mechanism$joint.cons)){
tform1 <- diag(3)
tform2 <- diag(3)
for(j in 1:nrow(mechanism$joint.cons[[i]])){
if(cons_fill[k] == 'v'){
tform1 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], cons_fit$par[n:(n+2)]), a=avec(mechanism$joint.cons[[i]][j, ], cons_fit$par[n:(n+2)]))
mechanism$joint.cons[[i]][j, ] <- uvector(cons_fit$par[n:(n+2)])
n <- n + 3
}else if(cons_fill[k] == 'vo'){
if(j == 2){
after_tform <- mechanism$joint.cons[[i]][j, ] %*% tform1
after_tform <- after_tform %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=cons_fit$par[n])
tform2 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], after_tform), a=avec(mechanism$joint.cons[[i]][j, ], after_tform))
}else if(j == 3){
after_tform <- mechanism$joint.cons[[i]][j, ] %*% tform2
after_tform <- after_tform %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=cons_fit$par[n])
#tform2 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], after_tform), a=avec(mechanism$joint.cons[[i]][j, ], after_tform))
}
mechanism$joint.cons[[i]][j, ] <- after_tform
# "Reset" rotation because master joint constraint is being modified
# When these parameters are used next inside animate_mechanism_error
# no rotation needs to be applied
cons_fit$par[n] <- 0
#mechanism$joint.cons[[i]][j, ] <- uvector(mechanism$joint.cons[[i]][j, ] %*% tform)
#mechanism$joint.cons[[i]][j, ] <- mechanism$joint.cons[[i]][j, ] %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=cons_fit$par[n])
n <- n + 1
}else if(cons_fill[k] == 'cprod'){
mechanism$joint.cons[[i]][j, ] <- cprod(mechanism$joint.cons[[i]][j-2, ], mechanism$joint.cons[[i]][j-1, ])
}
k <- k + 1
}
}
# Check orthogonality
#cat(paste0('{', avec(mechanism$joint.cons[[i]][1, ], mechanism$joint.cons[[i]][2, ]), ' ',
# avec(mechanism$joint.cons[[i]][1, ], mechanism$joint.cons[[i]][3, ]), ' ',
# avec(mechanism$joint.cons[[i]][2, ], mechanism$joint.cons[[i]][3, ]), '}'))
#print(mechanism$joint.cons[[1]])
cons_optim_v <- cons_fit$par
# Set final error (in case last optim block)
final_fit_error <- cons_fit$objective
}
# Replace directly calculated input parameters
if(any(direct_input)){
# Get input parameters
for(i in which(direct_input)){
input_param_optim[[i]] <- findJointInputParam(type=mechanism$joint.type[i],
coor=mechanism$joint.coor[i,], cons=mechanism$joint.cons[[i]],
ref=mechanism$body.points, poses=fit.points[, , optim_use])
cols_fill <- grepl(paste0('^',joint.names[direct_input[i]],'-'), colnames(input_optim))
input_optim[optim_use,cols_fill] <- input_param_optim[[i]]
}
}
## Optimize reference pose of each body
if(FALSE){
# This routine doesn't run/animate the model, it just takes the current
# transformation parameters for each set of fit.points and at each iteration,
# applies the corresponding transformation arrays from the mechanism run, and
# compares the error
# Run model to get updated transformations
tmarr <- suppressWarnings(animateMechanism(mechanism, input.param=input_param_optim,
joint.compare=joint_compare[, , optim_use], use.ref.as.prev=use.ref.as.prev, print.progress=FALSE,
check.inter.joint.dist=FALSE, check.joint.cons=FALSE)$tmat)
# Create vectors for initial and final error
pose_fit_errors_i <- setNames(rep(NA, mechanism$num.bodies), mechanism$body.names)
pose_fit_errors_f <- setNames(rep(NA, mechanism$num.bodies), mechanism$body.names)
# Run optimization
for(i in 1:mechanism$num.bodies){
# If no fit points associated with body, skip
if(is.na(mechanism$points.assoc[[i]][1])) next
# Get fit point matrix
if(length(mechanism$points.assoc[[i]]) == 1){
fit_point_mat <- t(fit.points[mechanism$points.assoc[[i]], , optim_use])
}else{
fit_point_mat <- fit.points[mechanism$points.assoc[[i]], , optim_use]
}
# Find initial error
if(print.progress) pose_fit_errors_i[i] <- ref_transform_error(rep(0, 6),
ref.points=mechanism$body.points[mechanism$points.assoc[[i]], ],
fit.points=fit_point_mat, transform=tmarr[, , i, ],
fit.wts=fit.wts[mechanism$points.assoc[[i]]])
# Run optimization
pose_fit <- tryCatch(
expr={
nlminb(start=rep(0, 6), objective=ref_transform_error,
lower=c(rep(-2*pi, 3), -rep(pose_optim_bounds_add, 3)),
upper=c(rep(2*pi, 3), rep(pose_optim_bounds_add, 3)),
ref.points=mechanism$body.points[mechanism$points.assoc[[i]], ],
fit.points=fit_point_mat, control=nlminb_control,
transform=tmarr[, , i, ], fit.wts=fit.wts[mechanism$points.assoc[[i]]])
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
#print(pose_fit$par)
#print(pose_optim_bounds_add)
# Save error
pose_fit_errors_f[i] <- pose_fit$objective
# Apply optimized parameters
tmat <- diag(4)
tmat[1:3, 1:3] <- rotationMatrixZYX(pose_fit$par[1:3])
tmat[1:3, 4] <- pose_fit$par[4:6]
mechanism$body.points[mechanism$points.assoc[[i]], ] <- applyTransform(mechanism$body.points[mechanism$points.assoc[[i]], ], tmat)
# Apply transformation to tmat array containing the current transformations applied to the reference points
body_points_tmarr[, , i] <- body_points_tmarr[, , i] %*% tmat
}
# Set final error
final_fit_error <- mean(pose_fit_errors_f, na.rm=TRUE)
# Print error
if(print.progress) cat(paste0(', ', signif(mean(pose_fit_errors_i, na.rm=TRUE), 3), '->', signif(final_fit_error, 3)))
}
## Optimize each joint coordinate and associated constraint individually - does it act like pose optimization?
if(pose.optim){
# Create vectors for initial and final error
pose_fit_errors_i <- setNames(rep(NA, mechanism$num.joints), mechanism$joint.names)
pose_fit_errors_f <- setNames(rep(NA, mechanism$num.joints), mechanism$joint.names)
for(i in 1:mechanism$num.joints){
# If no fit points associated with body on either side of joint, skip
if(is.na(mechanism$points.assoc[[mechanism$body.conn.num[i, 1]]][1]) && is.na(mechanism$points.assoc[[mechanism$body.conn.num[i, 2]]][1])) next
# Find initial error
if(print.progress) pose_fit_errors_i[i] <- transform_joint_error(rep(0, 6),
joint.idx=i, fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, joint.compare=joint_compare[, , optim_use],
print.progress=FALSE)
# Run optimization
pose_fit <- tryCatch(
expr={
nlminb(start=rep(0, 6), objective=transform_joint_error,
lower=c(rep(-0.4, 3), -rep(pose_optim_bounds_add, 3)),
upper=c(rep(0.4, 3), rep(pose_optim_bounds_add, 3)),
joint.idx=i, fit.points=fit.points[, , optim_use], mechanism=mechanism,
input.param=input_param_optim, fit.wts=fit.wts, joint.compare=joint_compare[, , optim_use],
print.progress=FALSE)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Save error
pose_fit_errors_f[i] <- pose_fit$objective
# Apply optimized parameters
tmat <- diag(4)
tmat[1:3, 1:3] <- rotationMatrixZYX(pose_fit$par[1:3])
tmat[1:3, 4] <- pose_fit$par[4:6]
# Apply transform to joint coordinate
mechanism$joint.coor[i, ] <- applyTransform(mechanism$joint.coor[i, ], tmat)
# Apply transform to joint constraint
mechanism$joint.cons[[i]] <- applyTransform(mechanism$joint.cons[[i]], tmat)
}
# Set final error
final_fit_error <- mean(pose_fit_errors_f, na.rm=TRUE)
# Print error
if(print.progress) cat(paste0(', ', signif(mean(pose_fit_errors_i, na.rm=TRUE), 3), '->', signif(final_fit_error, 3)))
}
# Save error after optimization
optim_errors <- c(optim_errors, final_fit_error)
# Get percent change in every other optim iter
every_other <- ''
if(any(direct.input) && optim_iter > 5){
optim_errors_eo3 <- optim_errors[seq(length(optim_errors)-2, length(optim_errors), by=2)]
percent_error_change <- mean(diff(optim_errors_eo3)) / mean(optim_errors_eo3)
if(optim_iter == 6) every_other <- ', now using mean of last 2 every other'
}else{
# Get percent change
percent_error_change <- diff(tail(optim_errors, 2)) / optim_errors[length(optim_errors)-1]
}
# Set change type
if(percent_error_change < 0){ change_types[optim_iter+1] <- 'decrease' }else{ change_types[optim_iter+1] <- 'increase' }
inc_iter_max <- ''
if(any(direct.input) && !direct_input && optim_iter == control$optim.iter.max-1){
optim_iter_max <- control$optim.iter.max + 1
inc_iter_max <- ', increasing iteration max to end on direct input'
}
turning_off <- ''
if(sum(change_types == 'increase', na.rm=TRUE) > 4 && any(direct.input)){
#direct.input <- FALSE*direct.input
#turning_off <- ', turning off direct.input'
}
if(print.progress && optim_iter > 0) cat(paste0(' (', signif(abs(percent_error_change)*100, 2), '% ', change_types[optim_iter+1], every_other, ')', turning_off, inc_iter_max))
if(print.progress) cat('\n')
# Stop if optim.to.percent threshold is reached - special cases for direct input since percent change bounces around much more
if(any(direct.input)){
if(direct_input){
if(optim_iter > 10){
optim_to_percent_test <- abs(percent_error_change)*100 < control$optim.to.percent
}else{
optim_to_percent_test <- FALSE
}
}else{
optim_to_percent_test <- FALSE
}
}else{
optim_to_percent_test <- abs(percent_error_change)*100 < control$optim.to.percent
}
# If near perfect fit stop - for perfect input testing
if(optim_to_percent_test) break
# If near perfect fit stop - for perfect input testing
if(final_fit_error < control$optim.error.stop) break
# Advance iteration tracker
optim_iter <- optim_iter + 1
}
mechanism_g <<- mechanism
input_param_g <<- input_param
input_optim_g <<- input_optim
input_fit_errors_f_g <<- input_fit_errors_f
optim_errors_g <<- optim_errors
}else{
input.param <- input_param_g
mechanism <- mechanism_g
input_optim <- input_optim_g
input_fit_errors_f <- input_fit_errors_f_g
optim_errors <- optim_errors_g
}
# Print final optimized joint coordinates
if(print.progress && optim_joint_coor){
#
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Final results of joint coordinate optimization:\n'))
for(j in 1:n_joints){
if(!j %in% joints_optim) next
cat(paste0(paste0(rep(indent, print.level+2), collapse=''), joint.names[j], ': '))
cat(paste0('Optimized to {', paste0(signif(mechanism$joint.coor[j, ], 3), collapse=','), '} from {',
paste0(signif(joints_optim_initial[j, ], 3), collapse=','), '}\n'))
}
}
## Final optimization across all input parameters (will change slightly since coordinates, constraints, and pose fits have improved
if(print.progress) cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Final input parameter optimization...'))
if(print.progress && n_iter > 20) cat('\n')
# Fill in missing guesses for starting parameters with preceding parameter (could use
# interpolation if necessary but this seems to work pretty well)
if(n_optim < n_iter){
for(i in 1:nrow(input_optim)){
# Save last non-NA value
if(!is.na(input_optim[i, 1])) {seq_i <- input_optim[i, ];next}
# Add last non-NA value
input_optim[i, ] <- seq_i
}
}
# Save subset input fit errors
input_fit_errors_sub <- mean(input_fit_errors_f, na.rm=TRUE)*2
# Create vectors for initial and final error
input_fit_errors_i <- rep(NA, n_iter)
input_fit_errors_f <- rep(NA, n_iter)
if(print.progress && n_iter > 20) cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'At iteration: 1'))
# Create vector for iterations that likely did not converge
no_conv <- rep(FALSE, n_iter)
#cat('\n')
for(iter in 1:n_iter){
# Find initial error
if(print.progress) input_fit_errors_i[iter] <- animate_mechanism_error(input_optim[iter, ],
replace='input.param', joint.compare=joint_compare[, , iter],
fit.points=fit.points[, , iter], mechanism=mechanism, input.param=input_param,
fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev, input.param.fill=input_param_fill, iter=iter)
# Set iteration limit
it_limit <- 10
# Create vector to store errors
obj_vec <- rep(NA, it_limit)
# Create list to save input parameters
input_optim_try <- matrix(NA, nrow=it_limit, ncol=length(input_optim_vec))
# Run optimization with varying start parameters
n <- 1
input_fit <- list('objective'=input_fit_errors_sub*3)
add_to_start <- 0
converged <- FALSE
threshold <- input_fit_errors_sub
#cat(threshold, '\n')
while(!converged && n < it_limit){
# Try changing starting parameters if first run exceeds benchmark
if(n == 2) add_to_start <- -input_optim[iter, ]*1
if(n == 3) add_to_start <- input_optim[iter, ]*0.25
if(n == 4) add_to_start <- input_optim[iter, ]*1
if(n == 5) add_to_start <- -input_optim[iter, ]*0.5
if(n == 6) add_to_start <- -input_optim[iter, ]*0.1
if(n == 7) add_to_start <- input_optim[iter, ]*0.5
if(n == 8) add_to_start <- input_optim[iter, ]*0.05
if(n == 9) add_to_start <- input_optim[iter, ]*0.75
if(n == 10) add_to_start <- input_optim[iter, ]*0.1
# Save starting parameters
start_param <- input_optim[iter, ]+add_to_start
# Run optimization
input_fit <- tryCatch(
expr={
nlminb(start=start_param, objective=animate_mechanism_error,
lower=input_optim_bounds['lower', ], upper=input_optim_bounds['upper', ],
replace='input.param', joint.compare=joint_compare[, , iter],
fit.points=fit.points[, , iter], mechanism=mechanism, input.param=input_param,
fit.wts=fit.wts, use.ref.as.prev=use.ref.as.prev, input.param.fill=input_param_fill,
iter=iter)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Save objective
obj_vec[n] <- input_fit$objective
# Save input parameters
input_optim_try[n, ] <- input_fit$par
# Detect non-convergence
converged <- TRUE
if(input_fit$objective > threshold) converged <- FALSE
if(sum(abs(start_param - input_fit$par)) < 1e-4) converged <- FALSE
if(n > 3 && sd(obj_vec, na.rm=TRUE) < 1e-10) converged <- TRUE
#if(iter == 650) cat('\n')
#if(iter > 650 && iter < 660){
# cat(paste0('\t', iter, ' (', n, '): ', round(input_fit$objective, 4), ', ',
# round(threshold, 4), ', ', converged, ', ', paste0(round(obj_vec, 5), collapse=','), '\n'))
#}
# Change threshold so that error must be lower than initial if no change in parameters
if(n == 1 && sum(abs(start_param - input_fit$par)) < 1e-4) threshold <- input_fit$objective
n <- n + 1
}
# Find minimum error
try_min_error <- which.min(obj_vec)
# Save error
input_fit_errors_f[iter] <- obj_vec[try_min_error]
# Save optimized parameters
input_optim[iter, ] <- input_optim_try[try_min_error, ]
# Save input for next iteration
if(iter < n_iter && !((iter+1) %in% optim_use)) input_optim[iter+1, ] <- input_optim[iter, ]
#if(iter > 15) input_fit_errors_sub <- mean(tail(input_fit_errors_f, 15), na.rm=TRUE)*2
if(iter > 15) input_fit_errors_sub <- mean(input_fit_errors_f, na.rm=TRUE)*2
if(print.progress && iter %% 25 == 0) cat(paste0(',', iter))
}
if(print.progress) cat('\n')
# Set amount to try shifting angles by (produces equivalent rotation)
min_equ_shift <- c(0,-2*pi,2*pi,-4*pi,4*pi,-6*pi,6*pi)
# Add input parameters into list
for(i in 1:length(input_param)){
# Add optimized input parameters into main full input list
input_param[[i]][, input_param_fill[[i]][['list.idx']]] <- matrix(input_optim[, input_param_fill[[i]][['col.idx']]],
nrow=nrow(input_optim), ncol=length(input_param_fill[[i]][['col.idx']]))
# Set angles to minimum equivalent angle
if(joint.types[input.joint[i]] %in% c('R', 'U', 'O')){
for(j in 1:nrow(input_param[[i]])){
for(k in 1:ncol(input_param[[i]])){
input_param[[i]][j,k] <- input_param[[i]][j,k] + min_equ_shift[which.min(abs(input_param[[i]][j,k] + min_equ_shift))]
}
}
}
}
# Print error
if(print.progress) cat(paste0(paste0(rep(indent, print.level+2), collapse=''), 'Error: ', signif(mean(input_fit_errors_i, na.rm=TRUE), 3), '->', signif(mean(input_fit_errors_f, na.rm=TRUE), 3), '\n'))
# mechanism_g <<- mechanism
#}
#mechanism <- mechanism_g
## Standardize joint coordinates equivalent
# Slide joint coordinates, when applicable, to closest points between connected bodies
if(!planar){
for(i in 1:n_joints){
if(!joint.types[i] %in% c('R')) next
if(!i %in% joints_optim) next
# Get points of bodies connected by joint
body1_pts <- mechanism$body.points[mechanism$body.assoc == mechanism$body.conn.num[i, 1], ]
body2_pts <- mechanism$body.points[mechanism$body.assoc == mechanism$body.conn.num[i, 2], ]
if(length(body1_pts) > 0 && length(body2_pts) > 0){
# Find nearest points on connected bodies
nearest_pts <- nearestPointsOnBodies(body1_pts, body2_pts)
# Find mean point
nearest_pt <- colMeans(nearest_pts)
}else if(length(body1_pts) == 0){
nearest_pt <- colMeans(body2_pts)
}else if(length(body2_pts) == 0){
nearest_pt <- colMeans(body1_pts)
}
if(joint.types[i] == 'R'){
# Find point on R-joint axis closest to nearest point between bodies
mechanism$joint.coor[i, ] <- pointNormalOnLine(nearest_pt, mechanism$joint.coor[i, ], mechanism$joint.coor[i, ]+mechanism$joint.cons[[i]])
}
}
}
# Get run time
time2 <- proc.time()
proc_time <- (time2 - time1)['user.self']
# Print run time
if(print.progress){
optim_iter_exceeded <- ''
if(optim_iter >= control$optim.iter.max) optim_iter_exceeded <- ' (maximum number of iterations met)'
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Total number of optimization iterations: ', optim_iter ,'', optim_iter_exceeded, '\n'))
cat(paste0(paste0(rep(indent, print.level+1), collapse=''), 'Total run time: ', signif(proc_time, 2), ' sec (', round(floor(proc_time / 60)), 'm ', signif(proc_time %% 60, 2),'s)\n'))
}
# Run animate mechanism using final parameters to get fit points and transformations
anim_mech <- animateMechanism(mechanism, input.param=input_param, use.ref.as.prev=use.ref.as.prev,
joint.compare=joint_compare)
list(
'mechanism'=mechanism,
'animate'=anim_mech,
'fit.points'=fit.points,
'body.tmarr'=body_points_tmarr,
'body.points.anim'=body_points_tmarr,
'input.param'=input_param,
'joint.compare'=joint_compare,
'rmse'=input_fit_errors_f,
'use.ref.as.prev'=use.ref.as.prev,
'optim.errors'=optim_errors[3:length(optim_errors)],
'run.time'=proc_time
)
}
R Package Documentation
Browse R Packages
We want your feedback!
Note that we can't provide technical support on individual packages. You should contact the package authors for that.
Embedding an R snippet on your website
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.