R/resolveJointPaths.R
In aaronolsen/linkR: 3D Lever and Linkage Mechanism Modeling
resolveJointPaths <- function(mechanism, iter, print.progress = FALSE, indent = '\t', indent.level=3){
# Path syntax
# J/D: Jointed/disjointed
if(print.progress) cat(paste0(paste0(rep(indent, indent.level), collapse=''), 'resolveJointPaths()\n'))
if(mechanism[['num.paths.closed']] == 0){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''), 'No closed paths\n'))
return(mechanism)
}
# Set max number of recursive loops
ct_max <- 20
# Create vector of solved paths
if(is.null(mechanism[['paths.solved']])){
# Start by searching all paths
save_paths_solved <- TRUE
path_search_idx <- 1:mechanism[['num.paths.closed']]
path_search_len <- mechanism[['num.paths.closed']]
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''), 'Searching all paths: 1-', tail(path_search_idx, 1), '\n'))
}else{
# Only search solved paths from previous run
save_paths_solved <- FALSE
path_search_idx <- mechanism[['paths.solved']]
path_search_len <- length(mechanism[['paths.solved']])
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''), 'Searching previously solved paths: ', paste0(path_search_idx, collapse=','), '\n'))
}
#if(print.progress) print_joint_status(mechanism, indent, indent.level+2)
#if(iter == 1) print.progress <- TRUE
ct <- 0
while(ct < ct_max){
# Keep track if path was solved
path_solved <- FALSE
# For each path
i <- 1
while(i <= path_search_len){
# Set path index
path_idx <- path_search_idx[i]
# Get joint indices
joint_idx <- mechanism[['paths.closed']][[path_idx]]
if(print.progress) if(ct == ct_max-1){ ct_print <- 'max' }else{ ct_print <- ct }
# Skip if any joints are already solved
if(any(rowSums(mechanism[['status']][['solved']][joint_idx, ] > 0) > 1)){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Skipping path ', path_idx, ' with solved joints, run num=', ct_print, '\n'))
i <- i + 1
next
}
# Get path bodies
path_bodies <- mechanism[['paths.closed.bodies']][[path_idx]]
# Create joint-body string
path_strings <- createJointPathString(bodies=path_bodies, mechanism=mechanism,
mode=c('ts', 'tj', 'tjs'), joints=joint_idx, path=path_idx, print.progress=print.progress)
# Check string against solvable joint type and status strings
solvable_ts <- ifelse(path_strings[['ts']] %in% linkR_sp[['str']][['ts']], TRUE, FALSE)
#print(linkR_sp[['str']][['ts']])
# If no match, skip
if(!solvable_ts){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Skipping unsolvable path ', path_strings[['p']], ', [', path_strings[['ts']], '], path num=', path_idx, ', run num=', ct_print, '\n'))
i <- i + 1
next
}
# Check string against solvable joint type, jointed and status strings
solvable_tjs <- ifelse(path_strings[['tjs']] %in% linkR_sp[['str']][['tjs']], TRUE, FALSE)
# Save solved path index
if(save_paths_solved) mechanism[['paths.solved']] <- c(mechanism[['paths.solved']], path_idx)
if(!solvable_tjs){
path_standardized <- FALSE
if(print.progress) print_progress_standardize <- paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Standardizing path ', path_strings[['p']], ', path num=', path_idx, ', run num=', ct_print, '\n')
if(path_strings[['tj']] %in% c('P(D)-1-S(J)-2-U(D)-3-S(J)-4-S(J)')){
if(print.progress) cat(paste0(print_progress_standardize, paste0(rep(indent, indent.level+2), collapse=''),
'Standardizing path by joining ', mechanism[['joint.names']][joint_idx[3]], ' (', joint_idx[3], ')\n'))
# Find transformation to restore disjointed joint and then extend across any subsequent disjoints
mechanism <- extendTransformation(mechanism, joint=joint_idx[3], body=path_bodies[2],
body.excl=path_bodies[2], iter=iter, recursive=TRUE, print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
path_standardized <- TRUE
}
if(path_strings[['tj']] %in% c('R(J)-1-R(D)-2-R(D)', 'R(J)-1-R(D)-2-R(J)')){
if(print.progress) cat(paste0(print_progress_standardize, paste0(rep(indent, indent.level+2), collapse=''),
'Standardizing path by joining ', mechanism[['joint.names']][joint_idx[2]], ' (', joint_idx[2], ')\n'))
# Find transformation to restore disjointed joint and then extend across any subsequent disjoints
mechanism <- extendTransformation(mechanism, joint=joint_idx[2], body=path_bodies[2],
body.excl=path_bodies[1], iter=iter, recursive=TRUE, print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
path_standardized <- TRUE
}
if(path_strings[['tj']] %in% c('R(D)-1-S(J)-2-P(D)-2-S(J)-3-S(J)', 'U(D)-1-R(J)-2-S(J)', 'R(D)-1-S(J)-2-S(J)', 'R(D)-1-R(J)-2-R(D)', 'R(D)-1-R(D)-2-R(D)', 'R(D)-1-R(J)-2-R(J)')){
if(print.progress) cat(paste0(print_progress_standardize, paste0(rep(indent, indent.level+2), collapse=''),
'Standardizing path by joining ', mechanism[['joint.names']][joint_idx[1]], ' (', joint_idx[1], ')\n'))
# Find transformation to restore disjointed joint and then extend across any subsequent disjoints
mechanism <- extendTransformation(mechanism, joint=joint_idx[1], body=path_bodies[1],
body.excl=path_bodies[1], iter=iter, recursive=TRUE, print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
path_standardized <- TRUE
#if(path_strings[['p']] == 'R2(DSN)-R3(JNN)-R4(DNS)'){ ct_max <- 21; break }
}
if(!path_standardized) i <- i + 1
next
}
# Print solving path
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Solving path ', path_strings[['p']], ', [', path_strings[['tjs']], '], path num=', path_idx, ', run num=', ct_print, '\n'))
# Get whether halves are solved
joint_sets <- mechanism[['paths.closed.set']][[path_idx]]
# Get original distances between joints in path (used for solving some paths)
path_dists <- mechanism[['paths.closed.dist']][[path_idx]]
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
# Get previous point for toggle position comparison
if(path_strings[['tjs']] %in% c('U(JSN)-1-R(JNN)-2-S(DNS)', 'R(JSN)-1-S(JNN)-2-P(DNS)',
'R(JSN)-1-S(JNN)-2-S(DNS)', 'R(JSN)-1-S(JNN)-2-S(DSN)', 'R(JSN)-1-R(JNN)-2-R(DNS)',
'R(JSN)-1-S(JNN)-2-P(DNS)-2-S(JNN)-3-S(DNS)', 'P(DSN)-1-S(JNN)-2-U(JNS)-3-S(DSN)-4-S(JNN)')){
if(path_strings[['tjs']] == 'R(JSN)-1-S(JNN)-2-P(DNS)-2-S(JNN)-3-S(DNS)'){
which_jt <- 3
}else if (path_strings[['tjs']] == 'P(DSN)-1-S(JNN)-2-U(JNS)-3-S(DSN)-4-S(JNN)'){
which_jt <- c(2,5)
}else{
which_jt <- 2
}
if(iter == 1){
if(is.null(mechanism[['joint.compare']])){
toggle_jt_compare <- mechanism[['joint.coor']][joint_idx[which_jt], ]
}else{
toggle_jt_compare <- mechanism[['joint.compare']][joint_idx[which_jt],,1]
}
}else{
if(is.null(mechanism[['joint.compare']])){
if(length(which_jt) == 1){
toggle_jt_compare <- applyJointTransform(mechanism, joint=joint_idx[which_jt], iter=iter-1)$coor[1, , 1]
}else if(length(which_jt) == 2){
toggle_jt_compare <- rbind(applyJointTransform(mechanism, joint=joint_idx[which_jt[1]], iter=iter-1)$coor[1, , 1],
applyJointTransform(mechanism, joint=joint_idx[which_jt[2]], iter=iter-1)$coor[1, , 1])
}
}else{
toggle_jt_compare <- mechanism[['joint.compare']][joint_idx[which_jt],,iter]
}
}
}
if(path_strings[['tjs']] %in% c('S(JSN)-1-S(DNS)')){
# Explain solution
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding rotation at S-joint ', mechanism[['joint.names']][joint_idx[1]],
'(', joint_idx[1], ') to bring S-joint ', mechanism[['joint.names']][joint_idx[2]],
'(', joint_idx[2], ') into alignment\n')
cat(solve_str)
}
## Find transformation of B4, rotation about joint 5
V_pre <- joint_current$coor[2,,joint_sets[2,1]] - joint_current$coor[1,,1]
V_new <- joint_current$coor[2,,joint_sets[2,2]] - joint_current$coor[1,,1]
# Get axis to rotate body about
J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- joint_current$coor[1,,1]
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- -joint_current$coor[1,,1]
tmat_B <- tmat_B_1 %*% tmat_B_2 %*% tmat_B_3
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], tmat=tmat_B,
iter=iter, recursive=TRUE, joint=joint_idx[1], status.solved.to=1,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[2]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[2], ] <- FALSE
#if(print.progress) print('hi')
}
# Solve path
if(path_strings[['tjs']] %in% c('U(JSN)-1-R(JNN)-2-S(DNS)')){
# Example: Solving path LegL_FootL(DSN)-1-FemurL_LegL(JNN)-2-Pelvis_FemurL(JNS)
# Find distance from J2 to J3
J23_length <- distPointToPoint(mechanism[['joint.coor']][joint_idx[2], ], mechanism[['joint.coor']][joint_idx[3], ])
# Set params
params <- list('U_axes'=rbind(joint_current$cons[[1]][1,,1], joint_current$cons[[1]][2,,2]),
'U_cor'=joint_current$coor[1,,1], 'R_mat'=rbind('R_cor'=joint_current$coor[2,,1],
'R_axis_pt'=joint_current$coor[2,,1]+joint_current$cons[[2]][1,,1], 'R_pt'=joint_current$coor[3,,joint_sets[3,1]]),
'S_cor_S'=joint_current$coor[3,,joint_sets[3,2]], 'J2_compare'=toggle_jt_compare,
'J23_length'=J23_length, 'return.J2'=FALSE, 'return.tmat'=FALSE)
# Explain optimization
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding optimal rotations of U-joint ', mechanism[['joint.names']][joint_idx[1]],
'(', joint_idx[1], ') to minimize the distance of joint ', mechanism[['joint.names']][joint_idx[3]],
'(', joint_idx[3], ') from the circle defined by the hinge axis and center of rotation of joint ',
mechanism[['joint.names']][joint_idx[2]], '\n')
cat(solve_str)
}
# Set starting values to try - does not converge for some starting values
try_start <- list(c(0.1,0.2), c(0.1,-0.2), c(0,0), c(-0.2,-0.1))
# Run optimization with several starting values
list_optim_runs <- list()
toggle_dist <- rep(NA, length(try_start))
list_optim_errs <- rep(NA, length(try_start))
for(try_num in 1:length(try_start)){
u_jt_fit <- tryCatch(
expr={
nlminb(start=try_start[[try_num]], objective=path_min_j3dtc, lower=rep(-2*pi, 2),
upper=rep(2*pi, 2), params=params)
},
error=function(cond) {return(NULL)},
warning=function(cond) {return(NULL)}
)
if(is.null(u_jt_fit)) next
# Save optimization and error
list_optim_runs[[try_num]] <- u_jt_fit
list_optim_errs[try_num] <- u_jt_fit$objective
#
params[['return.J2']] <- TRUE
toggle_dist[try_num] <- distPointToPoint(toggle_jt_compare, path_min_j3dtc(u_jt_fit$par, params))
params[['return.J2']] <- FALSE
# If low error is reached stop
#if(u_jt_fit$objective < 1e-5) break
#if(sum(abs(u_jt_fit$par - try_start[[try_num]])) > 1e-5) break
}
# Find best guess of closest toggle distance (min for now - should look for lowest value occurring at least twice?)
min_toggle_dist <- min(toggle_dist, na.rm=TRUE)
# Limit search to fits where toggle is lower
runs_at_min_toggle <- which(abs(toggle_dist-min_toggle_dist) < 1e-3)
list_optim_runs <- list_optim_runs[runs_at_min_toggle]
list_optim_errs <- list_optim_errs[runs_at_min_toggle]
# Save fit having lowest error and closest position to toggle
u_jt_fit <- list_optim_runs[[which.min(list_optim_errs)]]
# Report optimization error
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Error after optimization: ', signif(u_jt_fit$objective, 3), '\n')
cat(solve_str)
}
# Get optimal transformation
params[['return.tmat']] <- TRUE
body_1_tmat <- path_min_j3dtc(u_jt_fit$par, params)
# Transform bodies and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], tmat=body_1_tmat,
iter=iter, recursive=TRUE, joint=joint_idx[1], status.solved.to=1,
print.progress=print.progress, indent=indent, indent.level=indent.level+2)
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
## Find transformation of B4, rotation about joint 5
V_pre <- joint_current$coor[3,,joint_sets[3,1]] - joint_current$coor[2,,1]
V_new <- joint_current$coor[3,,joint_sets[3,2]] - joint_current$coor[2,,1]
# Get rotation matrix
RM <- tMatrixEP(joint_current$cons[[2]][,,1], avec(V_pre, V_new, axis=joint_current$cons[[2]][,,1], about.axis=TRUE))
# Get transformation
tmat_B2_1 <- tmat_B2_2 <- tmat_B2_3 <- diag(4)
tmat_B2_1[1:3, 4] <- joint_current$coor[2,,1]
tmat_B2_2[1:3, 1:3] <- RM
tmat_B2_3[1:3, 4] <- -joint_current$coor[2,,1]
tmat_B2 <- tmat_B2_1 %*% tmat_B2_2 %*% tmat_B2_3
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=tmat_B2,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=1,
print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[3]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[3], ] <- FALSE
path_solved <- TRUE
}
# Solve path
if(path_strings[['tjs']] %in% c('R(JSN)-1-S(JNN)-2-P(DNS)-2-S(JNN)-3-S(DNS)')){
# Check that all middle joints are coincident
if(sum(apply(mechanism[['joint.coor']][joint_idx[2:4], ], 2, 'sd') > 1e-10)){
stop(paste0('Joints "', paste0(joint_idx[2:4], collapse=','), '" (',
paste0(mechanism[['joint.names']][2:4], collapse=', '),
') must be coincident for current version of path solver.'))
}
# Get R and P vectors
R_vec <- joint_current$cons[[1]][, , 1]
P_vec <- joint_current$cons[[3]][3, , joint_sets[3,2]]
# Check that R and P vectors are parallel
if(avec(R_vec, P_vec, max.pi=TRUE) > 1e-10){
stop(paste0('Axis of joint ', joint_idx[1], ' (', mechanism[['joint.names']][joint_idx[1]],
') and normal vector at joint ', joint_idx[3], ' (', mechanism[['joint.names']][joint_idx[3]],
') must be parallel.'))
}
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding ', mechanism[['joint.names']][joint_idx[3]], '(', joint_idx[3],
') in plane defined by point {', paste0(signif(joint_current$coor[3, , joint_sets[3,2]], 3), collapse=','),
'} and normal vector {', paste0(signif(P_vec, 3), collapse=','), '} from R-joint and S-joint\n')
cat(solve_str)
}
# Find original distance from joint centers in P-plane
J1_o_proj <- pointPlaneProj(mechanism[['joint.coor']][joint_idx[1], ],
mechanism[['joint.coor']][joint_idx[3], ], mechanism[['joint.cons']][[joint_idx[3]]][3,])
J12_o_dist <- distPointToPoint(J1_o_proj, mechanism[['joint.coor']][joint_idx[3], ])
# Project new point into plane
J1_c_proj <- pointPlaneProj(joint_current$coor[1, , 1], joint_current$coor[3, , joint_sets[3,2]], P_vec)
# Find circle from R-joint side
R_circle <- defineCircle(center=J1_c_proj, nvector=R_vec, radius=J12_o_dist)
# Find intersection of sphere and plane for S-joint side (circles in plane)
int_sph_pln <- intersectSpherePlane(c=joint_current$coor[5, , joint_sets[5,2]],
r=path_dists[4], p=joint_current$coor[3, , joint_sets[3,2]], n=P_vec)
S_circle <- defineCircle(center=int_sph_pln$center, nvector=int_sph_pln$nvector, radius=int_sph_pln$radius)
# Find intersection of circles
int_circles <- intersectCircles(R_circle, S_circle)
if(int_circles$type %in% c('two', 'one')){
if(int_circles$type == 'two'){
# Find point closest to compare point (toggle)
which_min <- which.min(distPointToPoint(rbind(int_circles[[1]], int_circles[[2]]), toggle_jt_compare))
# Set solved joint
J3_solve <- int_circles[[which_min]]
}else{
# Set solved joint
J3_solve <- int_circles[[1]]
}
## Find transformation of B1, rotation about J1
V_pre <- joint_current$coor[2, , 1] - J1_c_proj
V_new <- J3_solve - J1_c_proj
# Get axis to rotate link about
J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation
tmat_B1_1 <- tmat_B1_2 <- tmat_B1_3 <- diag(4)
tmat_B1_1[1:3, 4] <- joint_current$coor[1,,1]
tmat_B1_2[1:3, 1:3] <- RM
tmat_B1_3[1:3, 4] <- -joint_current$coor[1,,1]
tmat_B1 <- tmat_B1_1 %*% tmat_B1_2 %*% tmat_B1_3
#tmat_B1 <- diag(4)
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], tmat=tmat_B1,
iter=iter, recursive=TRUE, joint=joint_idx[1], status.solved.to=1,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[1]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[1], ] <- FALSE
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
## Find transformation of B3, rotation about J3
V_pre <- joint_current$coor[5, , joint_sets[5,1]] - J3_solve
V_new <- joint_current$coor[5, , joint_sets[5,2]] - J3_solve
# Get axis to rotate link about
J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation
tmat_B3_1 <- tmat_B3_2 <- tmat_B3_3 <- diag(4)
tmat_B3_1[1:3, 4] <- J3_solve
tmat_B3_2[1:3, 1:3] <- RM
tmat_B3_3[1:3, 4] <- -J3_solve
tmat_B3 <- tmat_B3_1 %*% tmat_B3_2 %*% tmat_B3_3
# Transform first body and extend transformation
# path_bodies has 4 elements - middle body is repeated
mechanism <- extendTransformation(mechanism, body=path_bodies[4], tmat=tmat_B3,
iter=iter, recursive=TRUE, joint=joint_idx[4], status.solved.to=1,
body.excl=path_bodies[4], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[5]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[5], ] <- FALSE
path_solved <- TRUE
}
}
# Solve path
if(path_strings[['tjs']] %in% c('P(DSN)-1-S(JNN)-2-U(JNS)-3-S(DSN)-4-S(JNN)')){
# Example: Solving path LJSYM_NEURO(DSN)-1-LJAWL_LJSYM(JNN)-2-SUSPL_LJAWL(JNS)-3-OPERL_INOPL(DSN)-4-INOPL_LJAWL(JNN)
# Find distance from J2 to J1-plane
dptp_J12 <- distPointToPlane(p=mechanism[['joint.coor']][joint_idx[2], ], n=mechanism[['joint.cons']][[joint_idx[1]]][3,], q=mechanism[['joint.coor']][joint_idx[1], ])
# Find distance from J4 to J5
J45_length <- distPointToPoint(mechanism[['joint.coor']][joint_idx[4], ], mechanism[['joint.coor']][joint_idx[5], ])
# Find point in plane of final J2
pt_in_J2_plane <- joint_current$coor[1,,joint_sets[1,1]] + dptp_J12*joint_current$cons[[1]][3, , joint_sets[1,1]]
# Find fixed distance between J2 and J3
dptp_J23 <- distPointToPoint(mechanism[['joint.coor']][joint_idx[2], ], mechanism[['joint.coor']][joint_idx[3], ])
## Find circle of potential positions of J2 in plane as intersection of plane and sphere surrounding U-joint
int_sph_pln <- intersectSpherePlane(c=joint_current$coor[3,,joint_sets[3,2]], r=dptp_J23,
p=pt_in_J2_plane, n=joint_current$cons[[1]][3, , joint_sets[1,1]])
# If no intersection - add handling of this
if(is.null(int_sph_pln)) stop("Sphere of U-joint and plane of P-joint do not intersect. Add operation.")
# Define as circle
int_sph_pln <- defineCircle(center=int_sph_pln$center, nvector=int_sph_pln$nvector, radius=int_sph_pln$radius)
# Get tp2 toggle point
# Without joint.compare the transformed joint position is used - this worked ok during initial testing but may not always work
if(is.null(mechanism[['joint.compare']])){
tp2 <- joint_current$coor[5,,1]
}else{
tp2 <- mechanism[['joint.compare']][joint_idx[5],,iter]
}
# Set params
params <- list('circle'=int_sph_pln, 'u.center'=joint_current$coor[3,,1],
'u.axes'=rbind(joint_current$cons[[3]][2,,2], joint_current$cons[[3]][1,,1]),
'p1'=joint_current$coor[2,,1], 'p2'=joint_current$coor[5,,1], 'p3'=joint_current$coor[4,,joint_sets[4,1]],
'd23'=J45_length, 'tp2'=tp2, 'return.tmat'=FALSE)
#print(joint_current$coor)
#print(mechanism[['joint.coor']][joint_idx[5], ])
# Explain optimization
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding position of joint ', mechanism[['joint.names']][joint_idx[2]], '(', joint_idx[2], ')',
' on a circle with center {', paste0(signif(params[['circle']]$C, 3), collapse=','), '}',
' and normal vector {', paste0(signif(params[['circle']]$N, 3), collapse=','),
'} by finding rotations of U-joint ', mechanism[['joint.names']][joint_idx[3]], '(', joint_idx[3], ')',
' that places joint ', mechanism[['joint.names']][joint_idx[5]], '(', joint_idx[5], ')',
' at the initial length of ', signif(J45_length, 3), ' from joint ',
mechanism[['joint.names']][joint_idx[4]], '(', joint_idx[4], ') at {', paste0(signif(joint_current$coor[4,,joint_sets[4,1]], 3), collapse=','), '}',
' and using previous position of joint ',
mechanism[['joint.names']][joint_idx[2]], ' {', paste0(signif(toggle_jt_compare[1,], 3), collapse=','),
'} as a starting point for the optimization\n')
cat(solve_str)
}
# Find nearest point on circle from toggle compare point
p_cir_cmp <- pointMinDistOnCircle(params[['circle']], toggle_jt_compare[1,])
# Find starting parameter corresponding to compare point
t_start <- circleAngle(params[['circle']], p_cir_cmp)
t_start <- t_start - 0.1
# Try optimization with limited range around starting value (compare point)
# to get point that doesn't toggle too far away
n_tries <- 0
for(try_add in c(0, -0.1, 0.15, -0.15, -0.05, 0.1, 0.05)){
# Set starting value to try
t_start_try <- t_start + try_add
# Find optimal value
u_jt_fit <- tryCatch(
expr={
nlminb(start=t_start_try, objective=path_min_p1c_u_dp23, lower=t_start_try-pi, upper=t_start_try+pi, params=params)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Update number of tries
n_tries <- n_tries + 1
# If error is below threshold stop trying values
if(u_jt_fit$objective < 1e-7) break
}
# What to do if no solution... need to add something here
if(is.null(u_jt_fit)) stop("No solution found for path_min_p1c_u_dp23 optimization. Add operation.")
# Report optimization error
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Error from optimization after ', n_tries, ' try/tries: ', signif(u_jt_fit$objective, 3), '\n')
cat(solve_str)
}
# Get optimal transformation
params[['return.tmat']] <- TRUE
body_2_tmat <- path_min_p1c_u_dp23(u_jt_fit$par, params)
# Transform bodies and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=body_2_tmat,
iter=iter, recursive=TRUE, joint=joint_idx[3], status.solved.to=1,
body.excl=path_bodies[2], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
# Get initial position of J2
# *** This wont work if B1 has been re-oriented in space
# Will need to save transformations of bodies to reconstruct original position of J2 relative to J1
J2_coor_i <- (mechanism[['joint.coor']][joint_idx[2], ] - mechanism[['joint.coor']][joint_idx[1], ]) + joint_current$coor[1,,joint_sets[1,1]]
# Find translation vector
B1_t_vec <- joint_current$coor[2,,1] - J2_coor_i
# Get two coordinate sets to compare 1:translation only, 2: translation and rotation
B1_t_tmat <- diag(4)
B1_t_tmat[1:3,4] <- B1_t_vec
B1_tfm1 <- applyTransform(diag(3), B1_t_tmat)
B1_tfm2 <- applyTransform(diag(3), mechanism[['tmat']][,,path_bodies[1],iter])
# Find transformation at J2 of B1 so that B1 only translates
best_align <- bestAlign(B1_tfm1, B1_tfm2)
# Extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], tmat=best_align$tmat,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=1,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[1]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[1], ] <- FALSE
## Find transformation of B4, rotation about joint 5
V_pre <- joint_current$coor[4,,joint_sets[4,2]] - joint_current$coor[5,,1]
V_new <- joint_current$coor[4,,joint_sets[4,1]] - joint_current$coor[5,,1]
# Get axis to rotate link about
J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation
tmat_B4_1 <- tmat_B4_2 <- tmat_B4_3 <- diag(4)
tmat_B4_1[1:3, 4] <- joint_current$coor[5,,1]
tmat_B4_2[1:3, 1:3] <- RM
tmat_B4_3[1:3, 4] <- -joint_current$coor[5,,1]
tmat_B4 <- tmat_B4_1 %*% tmat_B4_2 %*% tmat_B4_3
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[4], tmat=tmat_B4,
iter=iter, recursive=TRUE, joint=joint_idx[5], status.solved.to=1,
body.excl=path_bodies[4], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[4]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[4], ] <- FALSE
path_solved <- TRUE
}
# Solve path
if(path_strings[['tjs']] %in% c('R(JSN)-1-S(JNN)-2-P(DNS)')){
# Get current circle radius, center and normal vector
circle_pt_on_rad <- joint_current$coor[2, , 1] # Also point in plane
circle_center <- joint_current$coor[1, , 1]
circle_norm <- joint_current$cons[[1]][, , 1]
# Get point in plane
plane_point <- joint_current$coor[3, , 2]
# Get plane normal vector
plane_norm <- joint_current$cons[[3]][3, , 2]
# Check whether vectors are parallel - if they are then there is not a unique solution
a_vecs <- avec(circle_norm, plane_norm, max.pi=TRUE)
if(a_vecs == 0){
# Vectors are parallel - there is no unique solution
}else{
#
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding ', mechanism[['joint.names']][joint_idx[2]], '(', joint_idx[2],
') on circle with center {', paste0(signif(circle_center, 3), collapse=','),
'} at ', mechanism[['joint.names']][joint_idx[1]],
'(', joint_idx[1], '), ', 'point on radius {', paste0(signif(circle_pt_on_rad, 3), collapse=','), '}, and normal {' ,
paste0(signif(circle_norm, 3), collapse=','), '}', ' at intersection with plane defined by point={',
paste0(signif(plane_point, 3), collapse=','), '} and normal vector {',
paste0(signif(uvector(plane_norm), 3), collapse=','), '} and closest to point {',
paste0(signif(toggle_jt_compare, 3), collapse=','), '}', '\n')
cat(solve_str)
}
# Define circle
circle <- defineCircle(center=circle_center, nvector=circle_norm,
point_on_radius=circle_pt_on_rad, redefine_center=TRUE)
# Find intersection of circle and plane
int_cp <- intersectCirclePlane(circle=circle, P=plane_point, N=plane_norm)
# Find point on circle
J2_solve <- circlePoint(circle=circle, T=int_cp)
# If no solution, return NULL
if((is.vector(J2_solve) && is.na(J2_solve[1])) || (!is.vector(J2_solve) && is.na(J2_solve[1,1]))){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+2), collapse=''), 'No solution found.\n'))
return(mechanism)
}
# Find point closest to compare point
which_min <- which.min(distPointToPoint(J2_solve, toggle_jt_compare))
# Set solved joint
J2_solve <- J2_solve[which_min,]
# Get vectors and axis to find first body transformation
V_pre <- joint_current$coor[2, , 1] - joint_current$coor[1, , 1]
V_new <- J2_solve - joint_current$coor[1, , 1]
# Get axis to rotate link about
J_axis <- joint_current$cons[[1]][, , 1]
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation of first body
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- -joint_current$coor[1, , 1]
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- joint_current$coor[1, , 1]
tmat_B1 <- tmat_B_3 %*% tmat_B_2 %*% tmat_B_1
# Find vector for second body transformation
tmat_B2 <- diag(4)
tmat_B2[1:3, 4] <- J2_solve - circle_pt_on_rad
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=tmat_B2,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=1,
body.excl=path_bodies[1:2], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], joint=joint_idx[1],
status.solved.to=1, tmat=tmat_B1, iter=iter, recursive=TRUE,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent, indent.level=indent.level+2)
# Set last joint as jointed
mechanism[['status']][['jointed']][joint_idx[3]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[3], ] <- FALSE
path_solved <- TRUE
}
}
# Solve path
if(path_strings[['tjs']] %in% c('R(JSN)-1-L(JNN)-2-R(DNS)', 'S(JSN)-1-L(JNN)-2-S(DNS)')){
# Find target length between solved halves of rotational joints
len_target <- distPointToPoint(joint_current$coor[1, , 1], joint_current$coor[3, , joint_sets[3,2]])
# Get previous point for toggle position comparison
# **** Should this be iter-1???
# **** Also need to add in joint.compare
if(iter == 1){
t_target_prev <- mechanism[['joint.coor']][joint_idx[3], ]
}else{
t_target_prev <- joint_current$coor[3, , 1]
}
# Get translation vector
tvec <- joint_current$cons[[2]][1, , 1]
# Find intersection of sphere and line
t_target <- intersectSphereLine(c=joint_current$coor[1, , 1], r=len_target,
x=joint_current$coor[3, , joint_sets[3,1]], l=tvec, point.compare=t_target_prev)
# Get translation magnitude
tmag <- distPointToPoint(t_target, joint_current$coor[3, , joint_sets[3,1]])
# Set magnitude sign
tpos <- sum(abs((joint_current$coor[3, , joint_sets[3,1]] + tmag*tvec - t_target)))
tneg <- sum(abs((joint_current$coor[3, , joint_sets[3,1]] - tmag*tvec - t_target)))
if(tpos < tneg){ tmag <- tmag }else { tmag <- -tmag }
# Get transformation of second body
tmat_B <- diag(4)
tmat_B[1:3, 4] <- tmag*tvec
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=tmat_B,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=0,
body.excl=path_bodies[1:2], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Update joint coordinates after transformation
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
# Get vectors and axis to find first body transformation
V_pre <- joint_current$coor[3, , joint_sets[3,1]]-joint_current$coor[1, , 1]
V_new <- joint_current$coor[3, , joint_sets[3,2]]-joint_current$coor[1, , 1]
# Get axis to rotate link about
if(path_strings[['tjs']] %in% c('R(JSN)-1-L(JNN)-2-R(DNS)')) J_axis <- joint_current$cons[[1]][, , 1]
if(path_strings[['tjs']] %in% c('S(JSN)-1-L(JNN)-2-S(DNS)')) J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation of first and second body
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- joint_current$coor[1, , 1]
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- -joint_current$coor[1, , 1]
tmat_B <- tmat_B_1 %*% tmat_B_2 %*% tmat_B_3
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], joint=joint_idx[1],
status.solved.to=1, tmat=tmat_B, iter=iter, recursive=TRUE,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent, indent.level=indent.level+2)
# Update joint coordinates after transformation
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
len_current <- distPointToPoint(joint_current$coor[1, , 1], joint_current$coor[3, , joint_sets[3,1]])
# Set remaining joints as jointed and solved
mechanism[['status']][['jointed']][joint_idx[3]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[3], ] <- FALSE
mechanism[['status']][['solved']][joint_idx[2], ] <- 1
mechanism[['status']][['solved']][joint_idx[3], joint_sets[3,1]] <- 1
path_solved <- TRUE
}
# Solve path
if(path_strings[['tjs']] %in% c('R(JSN)-1-S(JNN)-2-S(DSN)', 'R(JSN)-1-S(JNN)-2-S(DNS)', 'R(JSN)-1-R(JNN)-2-R(DNS)')){
# Set 3rd joint set to use
if(grepl('2-S[(]DNS[)]', path_strings[['tjs']])){
joint_set3_use <- c(1,2)
}else{
joint_set3_use <- c(2,1)
}
# Get current line length -- easiest to calculate in real time in case link is composite of other joints
line_len <- distPointToPoint(joint_current$coor[2, , 1], joint_current$coor[3, , joint_sets[3,joint_set3_use[1]]])
# Get point at distance from circle
line_point <- joint_current$coor[3, , joint_sets[3,joint_set3_use[2]]]
# Get current circle radius, center and normal vector
circle_pt_on_rad <- joint_current$coor[2, , 1]
circle_center <- joint_current$coor[1, , 1]
circle_norm <- joint_current$cons[[1]][, , 1]
#
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding ', mechanism[['joint.names']][joint_idx[2]], '(', joint_idx[2],
') on circle with center {', paste0(signif(circle_center, 3), collapse=','),
'} at ', mechanism[['joint.names']][joint_idx[1]],
'(', joint_idx[1], '), ', 'point on radius={', paste0(signif(circle_pt_on_rad, 3), collapse=','), '}, and normal={' ,
paste0(signif(circle_norm, 3), collapse=','), '}', ' at distance=',
signif(line_len, 3), ' from {', paste0(signif(line_point, 3), collapse=','),
'} at ', mechanism[['joint.names']][joint_idx[3]], '(', joint_idx[3],
') and closest to point {', paste0(signif(toggle_jt_compare, 3), collapse=','), '}\n')
cat(solve_str)
}
# Define circle
circle <- defineCircle(center=circle_center, nvector=circle_norm,
point_on_radius=circle_pt_on_rad, redefine_center=TRUE)
# Find angle on circle at distance from point
circle_t <- angleOnCircleFromPoint(circle=circle, dist=line_len, P=line_point, point_compare=toggle_jt_compare)
# Find point on circle
J2_solve <- circlePoint(circle=circle, T=circle_t)
# If no solution, return NULL
if((is.vector(J2_solve) && is.na(J2_solve[1])) || (!is.vector(J2_solve) && is.na(J2_solve[1,1]))){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+2), collapse=''), 'No solution found.\n'))
return(mechanism)
}
# Get vectors and axis to find first body transformation
V_pre <- joint_current$coor[2, , 1] - joint_current$coor[1, , 1]
V_new <- J2_solve - joint_current$coor[1, , 1]
# Get axis to rotate link about
J_axis <- joint_current$cons[[1]][, , 1]
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation of first body
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- -joint_current$coor[1, , 1]
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- joint_current$coor[1, , 1]
tmat_B <- tmat_B_3 %*% tmat_B_2 %*% tmat_B_1
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], joint=joint_idx[1],
status.solved.to=1, tmat=tmat_B, iter=iter, recursive=TRUE,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent, indent.level=indent.level+2)
# Update 3rd joint coordinate and constraint after transformation
joint_current3 <- applyJointTransform(mechanism, joint=joint_idx[3], iter=iter)
# Get vectors and axis to find second body transformation
V_pre <- joint_current3$coor[1, , joint_sets[3, joint_set3_use[1]]] - J2_solve
V_new <- line_point - J2_solve
# Get axis to rotate link about
if(path_strings[['tjs']] %in% c('R(JSN)-1-R(JNN)-2-R(DNS)')){
joint_current2 <- applyJointTransform(mechanism, joint=joint_idx[2], iter=iter)
J_axis <- joint_current2$cons[[1]][, , 1]
}
if(path_strings[['tjs']] %in% c('R(JSN)-1-S(JNN)-2-S(DNS)', 'R(JSN)-1-S(JNN)-2-S(DSN)')) J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation of second body
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- J2_solve
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- -J2_solve
tmat_B <- tmat_B_1 %*% tmat_B_2 %*% tmat_B_3
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=tmat_B,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=1,
body.excl=path_bodies[1:2], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
#print(line_point)
#print(applyJointTransform(mechanism, joint=joint_idx[3], iter=iter)$coor[, , joint_sets[3,joint_set3_use[2]]])
# Set last joint as jointed
mechanism[['status']][['jointed']][joint_idx[3]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[3], ] <- FALSE
path_solved <- TRUE
}
if(!path_solved && print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Could not find solver for path [', path_strings[['tjs']], '], path num=', path_idx, ', run num=', ct_print, '\n'))
# Extend solved status through mechanism
mechanism <- extendSolvedStatus(mechanism, print.progress=print.progress, indent=indent, indent.level=indent.level+2)
#if(print.progress) print_joint_status(mechanism, indent, indent.level+2)
#if(path_strings[['p']] == 'R2(JSN)-R3(JNN)-R4(DNS)'){ path_solved <- FALSE; break }
i <- i + 1
}
# If no path was solved, break
if(!path_solved){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'No additional paths found to solve during previous run, stopping path solving\n'))
break
}
# Advance count
ct <- ct + 1
}
# Save solved path index
if(save_paths_solved) mechanism[['paths.solved']] <- unique(mechanism[['paths.solved']])
#if(print.progress) print_joint_status(mechanism, indent)
mechanism
}
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.
resolveJointPaths <- function(mechanism, iter, print.progress = FALSE, indent = '\t', indent.level=3){
# Path syntax
# J/D: Jointed/disjointed
if(print.progress) cat(paste0(paste0(rep(indent, indent.level), collapse=''), 'resolveJointPaths()\n'))
if(mechanism[['num.paths.closed']] == 0){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''), 'No closed paths\n'))
return(mechanism)
}
# Set max number of recursive loops
ct_max <- 20
# Create vector of solved paths
if(is.null(mechanism[['paths.solved']])){
# Start by searching all paths
save_paths_solved <- TRUE
path_search_idx <- 1:mechanism[['num.paths.closed']]
path_search_len <- mechanism[['num.paths.closed']]
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''), 'Searching all paths: 1-', tail(path_search_idx, 1), '\n'))
}else{
# Only search solved paths from previous run
save_paths_solved <- FALSE
path_search_idx <- mechanism[['paths.solved']]
path_search_len <- length(mechanism[['paths.solved']])
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''), 'Searching previously solved paths: ', paste0(path_search_idx, collapse=','), '\n'))
}
#if(print.progress) print_joint_status(mechanism, indent, indent.level+2)
#if(iter == 1) print.progress <- TRUE
ct <- 0
while(ct < ct_max){
# Keep track if path was solved
path_solved <- FALSE
# For each path
i <- 1
while(i <= path_search_len){
# Set path index
path_idx <- path_search_idx[i]
# Get joint indices
joint_idx <- mechanism[['paths.closed']][[path_idx]]
if(print.progress) if(ct == ct_max-1){ ct_print <- 'max' }else{ ct_print <- ct }
# Skip if any joints are already solved
if(any(rowSums(mechanism[['status']][['solved']][joint_idx, ] > 0) > 1)){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Skipping path ', path_idx, ' with solved joints, run num=', ct_print, '\n'))
i <- i + 1
next
}
# Get path bodies
path_bodies <- mechanism[['paths.closed.bodies']][[path_idx]]
# Create joint-body string
path_strings <- createJointPathString(bodies=path_bodies, mechanism=mechanism,
mode=c('ts', 'tj', 'tjs'), joints=joint_idx, path=path_idx, print.progress=print.progress)
# Check string against solvable joint type and status strings
solvable_ts <- ifelse(path_strings[['ts']] %in% linkR_sp[['str']][['ts']], TRUE, FALSE)
#print(linkR_sp[['str']][['ts']])
# If no match, skip
if(!solvable_ts){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Skipping unsolvable path ', path_strings[['p']], ', [', path_strings[['ts']], '], path num=', path_idx, ', run num=', ct_print, '\n'))
i <- i + 1
next
}
# Check string against solvable joint type, jointed and status strings
solvable_tjs <- ifelse(path_strings[['tjs']] %in% linkR_sp[['str']][['tjs']], TRUE, FALSE)
# Save solved path index
if(save_paths_solved) mechanism[['paths.solved']] <- c(mechanism[['paths.solved']], path_idx)
if(!solvable_tjs){
path_standardized <- FALSE
if(print.progress) print_progress_standardize <- paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Standardizing path ', path_strings[['p']], ', path num=', path_idx, ', run num=', ct_print, '\n')
if(path_strings[['tj']] %in% c('P(D)-1-S(J)-2-U(D)-3-S(J)-4-S(J)')){
if(print.progress) cat(paste0(print_progress_standardize, paste0(rep(indent, indent.level+2), collapse=''),
'Standardizing path by joining ', mechanism[['joint.names']][joint_idx[3]], ' (', joint_idx[3], ')\n'))
# Find transformation to restore disjointed joint and then extend across any subsequent disjoints
mechanism <- extendTransformation(mechanism, joint=joint_idx[3], body=path_bodies[2],
body.excl=path_bodies[2], iter=iter, recursive=TRUE, print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
path_standardized <- TRUE
}
if(path_strings[['tj']] %in% c('R(J)-1-R(D)-2-R(D)', 'R(J)-1-R(D)-2-R(J)')){
if(print.progress) cat(paste0(print_progress_standardize, paste0(rep(indent, indent.level+2), collapse=''),
'Standardizing path by joining ', mechanism[['joint.names']][joint_idx[2]], ' (', joint_idx[2], ')\n'))
# Find transformation to restore disjointed joint and then extend across any subsequent disjoints
mechanism <- extendTransformation(mechanism, joint=joint_idx[2], body=path_bodies[2],
body.excl=path_bodies[1], iter=iter, recursive=TRUE, print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
path_standardized <- TRUE
}
if(path_strings[['tj']] %in% c('R(D)-1-S(J)-2-P(D)-2-S(J)-3-S(J)', 'U(D)-1-R(J)-2-S(J)', 'R(D)-1-S(J)-2-S(J)', 'R(D)-1-R(J)-2-R(D)', 'R(D)-1-R(D)-2-R(D)', 'R(D)-1-R(J)-2-R(J)')){
if(print.progress) cat(paste0(print_progress_standardize, paste0(rep(indent, indent.level+2), collapse=''),
'Standardizing path by joining ', mechanism[['joint.names']][joint_idx[1]], ' (', joint_idx[1], ')\n'))
# Find transformation to restore disjointed joint and then extend across any subsequent disjoints
mechanism <- extendTransformation(mechanism, joint=joint_idx[1], body=path_bodies[1],
body.excl=path_bodies[1], iter=iter, recursive=TRUE, print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
path_standardized <- TRUE
#if(path_strings[['p']] == 'R2(DSN)-R3(JNN)-R4(DNS)'){ ct_max <- 21; break }
}
if(!path_standardized) i <- i + 1
next
}
# Print solving path
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Solving path ', path_strings[['p']], ', [', path_strings[['tjs']], '], path num=', path_idx, ', run num=', ct_print, '\n'))
# Get whether halves are solved
joint_sets <- mechanism[['paths.closed.set']][[path_idx]]
# Get original distances between joints in path (used for solving some paths)
path_dists <- mechanism[['paths.closed.dist']][[path_idx]]
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
# Get previous point for toggle position comparison
if(path_strings[['tjs']] %in% c('U(JSN)-1-R(JNN)-2-S(DNS)', 'R(JSN)-1-S(JNN)-2-P(DNS)',
'R(JSN)-1-S(JNN)-2-S(DNS)', 'R(JSN)-1-S(JNN)-2-S(DSN)', 'R(JSN)-1-R(JNN)-2-R(DNS)',
'R(JSN)-1-S(JNN)-2-P(DNS)-2-S(JNN)-3-S(DNS)', 'P(DSN)-1-S(JNN)-2-U(JNS)-3-S(DSN)-4-S(JNN)')){
if(path_strings[['tjs']] == 'R(JSN)-1-S(JNN)-2-P(DNS)-2-S(JNN)-3-S(DNS)'){
which_jt <- 3
}else if (path_strings[['tjs']] == 'P(DSN)-1-S(JNN)-2-U(JNS)-3-S(DSN)-4-S(JNN)'){
which_jt <- c(2,5)
}else{
which_jt <- 2
}
if(iter == 1){
if(is.null(mechanism[['joint.compare']])){
toggle_jt_compare <- mechanism[['joint.coor']][joint_idx[which_jt], ]
}else{
toggle_jt_compare <- mechanism[['joint.compare']][joint_idx[which_jt],,1]
}
}else{
if(is.null(mechanism[['joint.compare']])){
if(length(which_jt) == 1){
toggle_jt_compare <- applyJointTransform(mechanism, joint=joint_idx[which_jt], iter=iter-1)$coor[1, , 1]
}else if(length(which_jt) == 2){
toggle_jt_compare <- rbind(applyJointTransform(mechanism, joint=joint_idx[which_jt[1]], iter=iter-1)$coor[1, , 1],
applyJointTransform(mechanism, joint=joint_idx[which_jt[2]], iter=iter-1)$coor[1, , 1])
}
}else{
toggle_jt_compare <- mechanism[['joint.compare']][joint_idx[which_jt],,iter]
}
}
}
if(path_strings[['tjs']] %in% c('S(JSN)-1-S(DNS)')){
# Explain solution
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding rotation at S-joint ', mechanism[['joint.names']][joint_idx[1]],
'(', joint_idx[1], ') to bring S-joint ', mechanism[['joint.names']][joint_idx[2]],
'(', joint_idx[2], ') into alignment\n')
cat(solve_str)
}
## Find transformation of B4, rotation about joint 5
V_pre <- joint_current$coor[2,,joint_sets[2,1]] - joint_current$coor[1,,1]
V_new <- joint_current$coor[2,,joint_sets[2,2]] - joint_current$coor[1,,1]
# Get axis to rotate body about
J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- joint_current$coor[1,,1]
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- -joint_current$coor[1,,1]
tmat_B <- tmat_B_1 %*% tmat_B_2 %*% tmat_B_3
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], tmat=tmat_B,
iter=iter, recursive=TRUE, joint=joint_idx[1], status.solved.to=1,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[2]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[2], ] <- FALSE
#if(print.progress) print('hi')
}
# Solve path
if(path_strings[['tjs']] %in% c('U(JSN)-1-R(JNN)-2-S(DNS)')){
# Example: Solving path LegL_FootL(DSN)-1-FemurL_LegL(JNN)-2-Pelvis_FemurL(JNS)
# Find distance from J2 to J3
J23_length <- distPointToPoint(mechanism[['joint.coor']][joint_idx[2], ], mechanism[['joint.coor']][joint_idx[3], ])
# Set params
params <- list('U_axes'=rbind(joint_current$cons[[1]][1,,1], joint_current$cons[[1]][2,,2]),
'U_cor'=joint_current$coor[1,,1], 'R_mat'=rbind('R_cor'=joint_current$coor[2,,1],
'R_axis_pt'=joint_current$coor[2,,1]+joint_current$cons[[2]][1,,1], 'R_pt'=joint_current$coor[3,,joint_sets[3,1]]),
'S_cor_S'=joint_current$coor[3,,joint_sets[3,2]], 'J2_compare'=toggle_jt_compare,
'J23_length'=J23_length, 'return.J2'=FALSE, 'return.tmat'=FALSE)
# Explain optimization
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding optimal rotations of U-joint ', mechanism[['joint.names']][joint_idx[1]],
'(', joint_idx[1], ') to minimize the distance of joint ', mechanism[['joint.names']][joint_idx[3]],
'(', joint_idx[3], ') from the circle defined by the hinge axis and center of rotation of joint ',
mechanism[['joint.names']][joint_idx[2]], '\n')
cat(solve_str)
}
# Set starting values to try - does not converge for some starting values
try_start <- list(c(0.1,0.2), c(0.1,-0.2), c(0,0), c(-0.2,-0.1))
# Run optimization with several starting values
list_optim_runs <- list()
toggle_dist <- rep(NA, length(try_start))
list_optim_errs <- rep(NA, length(try_start))
for(try_num in 1:length(try_start)){
u_jt_fit <- tryCatch(
expr={
nlminb(start=try_start[[try_num]], objective=path_min_j3dtc, lower=rep(-2*pi, 2),
upper=rep(2*pi, 2), params=params)
},
error=function(cond) {return(NULL)},
warning=function(cond) {return(NULL)}
)
if(is.null(u_jt_fit)) next
# Save optimization and error
list_optim_runs[[try_num]] <- u_jt_fit
list_optim_errs[try_num] <- u_jt_fit$objective
#
params[['return.J2']] <- TRUE
toggle_dist[try_num] <- distPointToPoint(toggle_jt_compare, path_min_j3dtc(u_jt_fit$par, params))
params[['return.J2']] <- FALSE
# If low error is reached stop
#if(u_jt_fit$objective < 1e-5) break
#if(sum(abs(u_jt_fit$par - try_start[[try_num]])) > 1e-5) break
}
# Find best guess of closest toggle distance (min for now - should look for lowest value occurring at least twice?)
min_toggle_dist <- min(toggle_dist, na.rm=TRUE)
# Limit search to fits where toggle is lower
runs_at_min_toggle <- which(abs(toggle_dist-min_toggle_dist) < 1e-3)
list_optim_runs <- list_optim_runs[runs_at_min_toggle]
list_optim_errs <- list_optim_errs[runs_at_min_toggle]
# Save fit having lowest error and closest position to toggle
u_jt_fit <- list_optim_runs[[which.min(list_optim_errs)]]
# Report optimization error
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Error after optimization: ', signif(u_jt_fit$objective, 3), '\n')
cat(solve_str)
}
# Get optimal transformation
params[['return.tmat']] <- TRUE
body_1_tmat <- path_min_j3dtc(u_jt_fit$par, params)
# Transform bodies and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], tmat=body_1_tmat,
iter=iter, recursive=TRUE, joint=joint_idx[1], status.solved.to=1,
print.progress=print.progress, indent=indent, indent.level=indent.level+2)
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
## Find transformation of B4, rotation about joint 5
V_pre <- joint_current$coor[3,,joint_sets[3,1]] - joint_current$coor[2,,1]
V_new <- joint_current$coor[3,,joint_sets[3,2]] - joint_current$coor[2,,1]
# Get rotation matrix
RM <- tMatrixEP(joint_current$cons[[2]][,,1], avec(V_pre, V_new, axis=joint_current$cons[[2]][,,1], about.axis=TRUE))
# Get transformation
tmat_B2_1 <- tmat_B2_2 <- tmat_B2_3 <- diag(4)
tmat_B2_1[1:3, 4] <- joint_current$coor[2,,1]
tmat_B2_2[1:3, 1:3] <- RM
tmat_B2_3[1:3, 4] <- -joint_current$coor[2,,1]
tmat_B2 <- tmat_B2_1 %*% tmat_B2_2 %*% tmat_B2_3
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=tmat_B2,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=1,
print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[3]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[3], ] <- FALSE
path_solved <- TRUE
}
# Solve path
if(path_strings[['tjs']] %in% c('R(JSN)-1-S(JNN)-2-P(DNS)-2-S(JNN)-3-S(DNS)')){
# Check that all middle joints are coincident
if(sum(apply(mechanism[['joint.coor']][joint_idx[2:4], ], 2, 'sd') > 1e-10)){
stop(paste0('Joints "', paste0(joint_idx[2:4], collapse=','), '" (',
paste0(mechanism[['joint.names']][2:4], collapse=', '),
') must be coincident for current version of path solver.'))
}
# Get R and P vectors
R_vec <- joint_current$cons[[1]][, , 1]
P_vec <- joint_current$cons[[3]][3, , joint_sets[3,2]]
# Check that R and P vectors are parallel
if(avec(R_vec, P_vec, max.pi=TRUE) > 1e-10){
stop(paste0('Axis of joint ', joint_idx[1], ' (', mechanism[['joint.names']][joint_idx[1]],
') and normal vector at joint ', joint_idx[3], ' (', mechanism[['joint.names']][joint_idx[3]],
') must be parallel.'))
}
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding ', mechanism[['joint.names']][joint_idx[3]], '(', joint_idx[3],
') in plane defined by point {', paste0(signif(joint_current$coor[3, , joint_sets[3,2]], 3), collapse=','),
'} and normal vector {', paste0(signif(P_vec, 3), collapse=','), '} from R-joint and S-joint\n')
cat(solve_str)
}
# Find original distance from joint centers in P-plane
J1_o_proj <- pointPlaneProj(mechanism[['joint.coor']][joint_idx[1], ],
mechanism[['joint.coor']][joint_idx[3], ], mechanism[['joint.cons']][[joint_idx[3]]][3,])
J12_o_dist <- distPointToPoint(J1_o_proj, mechanism[['joint.coor']][joint_idx[3], ])
# Project new point into plane
J1_c_proj <- pointPlaneProj(joint_current$coor[1, , 1], joint_current$coor[3, , joint_sets[3,2]], P_vec)
# Find circle from R-joint side
R_circle <- defineCircle(center=J1_c_proj, nvector=R_vec, radius=J12_o_dist)
# Find intersection of sphere and plane for S-joint side (circles in plane)
int_sph_pln <- intersectSpherePlane(c=joint_current$coor[5, , joint_sets[5,2]],
r=path_dists[4], p=joint_current$coor[3, , joint_sets[3,2]], n=P_vec)
S_circle <- defineCircle(center=int_sph_pln$center, nvector=int_sph_pln$nvector, radius=int_sph_pln$radius)
# Find intersection of circles
int_circles <- intersectCircles(R_circle, S_circle)
if(int_circles$type %in% c('two', 'one')){
if(int_circles$type == 'two'){
# Find point closest to compare point (toggle)
which_min <- which.min(distPointToPoint(rbind(int_circles[[1]], int_circles[[2]]), toggle_jt_compare))
# Set solved joint
J3_solve <- int_circles[[which_min]]
}else{
# Set solved joint
J3_solve <- int_circles[[1]]
}
## Find transformation of B1, rotation about J1
V_pre <- joint_current$coor[2, , 1] - J1_c_proj
V_new <- J3_solve - J1_c_proj
# Get axis to rotate link about
J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation
tmat_B1_1 <- tmat_B1_2 <- tmat_B1_3 <- diag(4)
tmat_B1_1[1:3, 4] <- joint_current$coor[1,,1]
tmat_B1_2[1:3, 1:3] <- RM
tmat_B1_3[1:3, 4] <- -joint_current$coor[1,,1]
tmat_B1 <- tmat_B1_1 %*% tmat_B1_2 %*% tmat_B1_3
#tmat_B1 <- diag(4)
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], tmat=tmat_B1,
iter=iter, recursive=TRUE, joint=joint_idx[1], status.solved.to=1,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[1]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[1], ] <- FALSE
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
## Find transformation of B3, rotation about J3
V_pre <- joint_current$coor[5, , joint_sets[5,1]] - J3_solve
V_new <- joint_current$coor[5, , joint_sets[5,2]] - J3_solve
# Get axis to rotate link about
J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation
tmat_B3_1 <- tmat_B3_2 <- tmat_B3_3 <- diag(4)
tmat_B3_1[1:3, 4] <- J3_solve
tmat_B3_2[1:3, 1:3] <- RM
tmat_B3_3[1:3, 4] <- -J3_solve
tmat_B3 <- tmat_B3_1 %*% tmat_B3_2 %*% tmat_B3_3
# Transform first body and extend transformation
# path_bodies has 4 elements - middle body is repeated
mechanism <- extendTransformation(mechanism, body=path_bodies[4], tmat=tmat_B3,
iter=iter, recursive=TRUE, joint=joint_idx[4], status.solved.to=1,
body.excl=path_bodies[4], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[5]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[5], ] <- FALSE
path_solved <- TRUE
}
}
# Solve path
if(path_strings[['tjs']] %in% c('P(DSN)-1-S(JNN)-2-U(JNS)-3-S(DSN)-4-S(JNN)')){
# Example: Solving path LJSYM_NEURO(DSN)-1-LJAWL_LJSYM(JNN)-2-SUSPL_LJAWL(JNS)-3-OPERL_INOPL(DSN)-4-INOPL_LJAWL(JNN)
# Find distance from J2 to J1-plane
dptp_J12 <- distPointToPlane(p=mechanism[['joint.coor']][joint_idx[2], ], n=mechanism[['joint.cons']][[joint_idx[1]]][3,], q=mechanism[['joint.coor']][joint_idx[1], ])
# Find distance from J4 to J5
J45_length <- distPointToPoint(mechanism[['joint.coor']][joint_idx[4], ], mechanism[['joint.coor']][joint_idx[5], ])
# Find point in plane of final J2
pt_in_J2_plane <- joint_current$coor[1,,joint_sets[1,1]] + dptp_J12*joint_current$cons[[1]][3, , joint_sets[1,1]]
# Find fixed distance between J2 and J3
dptp_J23 <- distPointToPoint(mechanism[['joint.coor']][joint_idx[2], ], mechanism[['joint.coor']][joint_idx[3], ])
## Find circle of potential positions of J2 in plane as intersection of plane and sphere surrounding U-joint
int_sph_pln <- intersectSpherePlane(c=joint_current$coor[3,,joint_sets[3,2]], r=dptp_J23,
p=pt_in_J2_plane, n=joint_current$cons[[1]][3, , joint_sets[1,1]])
# If no intersection - add handling of this
if(is.null(int_sph_pln)) stop("Sphere of U-joint and plane of P-joint do not intersect. Add operation.")
# Define as circle
int_sph_pln <- defineCircle(center=int_sph_pln$center, nvector=int_sph_pln$nvector, radius=int_sph_pln$radius)
# Get tp2 toggle point
# Without joint.compare the transformed joint position is used - this worked ok during initial testing but may not always work
if(is.null(mechanism[['joint.compare']])){
tp2 <- joint_current$coor[5,,1]
}else{
tp2 <- mechanism[['joint.compare']][joint_idx[5],,iter]
}
# Set params
params <- list('circle'=int_sph_pln, 'u.center'=joint_current$coor[3,,1],
'u.axes'=rbind(joint_current$cons[[3]][2,,2], joint_current$cons[[3]][1,,1]),
'p1'=joint_current$coor[2,,1], 'p2'=joint_current$coor[5,,1], 'p3'=joint_current$coor[4,,joint_sets[4,1]],
'd23'=J45_length, 'tp2'=tp2, 'return.tmat'=FALSE)
#print(joint_current$coor)
#print(mechanism[['joint.coor']][joint_idx[5], ])
# Explain optimization
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding position of joint ', mechanism[['joint.names']][joint_idx[2]], '(', joint_idx[2], ')',
' on a circle with center {', paste0(signif(params[['circle']]$C, 3), collapse=','), '}',
' and normal vector {', paste0(signif(params[['circle']]$N, 3), collapse=','),
'} by finding rotations of U-joint ', mechanism[['joint.names']][joint_idx[3]], '(', joint_idx[3], ')',
' that places joint ', mechanism[['joint.names']][joint_idx[5]], '(', joint_idx[5], ')',
' at the initial length of ', signif(J45_length, 3), ' from joint ',
mechanism[['joint.names']][joint_idx[4]], '(', joint_idx[4], ') at {', paste0(signif(joint_current$coor[4,,joint_sets[4,1]], 3), collapse=','), '}',
' and using previous position of joint ',
mechanism[['joint.names']][joint_idx[2]], ' {', paste0(signif(toggle_jt_compare[1,], 3), collapse=','),
'} as a starting point for the optimization\n')
cat(solve_str)
}
# Find nearest point on circle from toggle compare point
p_cir_cmp <- pointMinDistOnCircle(params[['circle']], toggle_jt_compare[1,])
# Find starting parameter corresponding to compare point
t_start <- circleAngle(params[['circle']], p_cir_cmp)
t_start <- t_start - 0.1
# Try optimization with limited range around starting value (compare point)
# to get point that doesn't toggle too far away
n_tries <- 0
for(try_add in c(0, -0.1, 0.15, -0.15, -0.05, 0.1, 0.05)){
# Set starting value to try
t_start_try <- t_start + try_add
# Find optimal value
u_jt_fit <- tryCatch(
expr={
nlminb(start=t_start_try, objective=path_min_p1c_u_dp23, lower=t_start_try-pi, upper=t_start_try+pi, params=params)
},
error=function(cond) {print(cond);return(NULL)},
warning=function(cond) {print(cond);return(NULL)}
)
# Update number of tries
n_tries <- n_tries + 1
# If error is below threshold stop trying values
if(u_jt_fit$objective < 1e-7) break
}
# What to do if no solution... need to add something here
if(is.null(u_jt_fit)) stop("No solution found for path_min_p1c_u_dp23 optimization. Add operation.")
# Report optimization error
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Error from optimization after ', n_tries, ' try/tries: ', signif(u_jt_fit$objective, 3), '\n')
cat(solve_str)
}
# Get optimal transformation
params[['return.tmat']] <- TRUE
body_2_tmat <- path_min_p1c_u_dp23(u_jt_fit$par, params)
# Transform bodies and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=body_2_tmat,
iter=iter, recursive=TRUE, joint=joint_idx[3], status.solved.to=1,
body.excl=path_bodies[2], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Apply current transformation to get current joint coordinate and constraint vectors
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
# Get initial position of J2
# *** This wont work if B1 has been re-oriented in space
# Will need to save transformations of bodies to reconstruct original position of J2 relative to J1
J2_coor_i <- (mechanism[['joint.coor']][joint_idx[2], ] - mechanism[['joint.coor']][joint_idx[1], ]) + joint_current$coor[1,,joint_sets[1,1]]
# Find translation vector
B1_t_vec <- joint_current$coor[2,,1] - J2_coor_i
# Get two coordinate sets to compare 1:translation only, 2: translation and rotation
B1_t_tmat <- diag(4)
B1_t_tmat[1:3,4] <- B1_t_vec
B1_tfm1 <- applyTransform(diag(3), B1_t_tmat)
B1_tfm2 <- applyTransform(diag(3), mechanism[['tmat']][,,path_bodies[1],iter])
# Find transformation at J2 of B1 so that B1 only translates
best_align <- bestAlign(B1_tfm1, B1_tfm2)
# Extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], tmat=best_align$tmat,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=1,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[1]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[1], ] <- FALSE
## Find transformation of B4, rotation about joint 5
V_pre <- joint_current$coor[4,,joint_sets[4,2]] - joint_current$coor[5,,1]
V_new <- joint_current$coor[4,,joint_sets[4,1]] - joint_current$coor[5,,1]
# Get axis to rotate link about
J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation
tmat_B4_1 <- tmat_B4_2 <- tmat_B4_3 <- diag(4)
tmat_B4_1[1:3, 4] <- joint_current$coor[5,,1]
tmat_B4_2[1:3, 1:3] <- RM
tmat_B4_3[1:3, 4] <- -joint_current$coor[5,,1]
tmat_B4 <- tmat_B4_1 %*% tmat_B4_2 %*% tmat_B4_3
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[4], tmat=tmat_B4,
iter=iter, recursive=TRUE, joint=joint_idx[5], status.solved.to=1,
body.excl=path_bodies[4], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Set as jointed
mechanism[['status']][['jointed']][joint_idx[4]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[4], ] <- FALSE
path_solved <- TRUE
}
# Solve path
if(path_strings[['tjs']] %in% c('R(JSN)-1-S(JNN)-2-P(DNS)')){
# Get current circle radius, center and normal vector
circle_pt_on_rad <- joint_current$coor[2, , 1] # Also point in plane
circle_center <- joint_current$coor[1, , 1]
circle_norm <- joint_current$cons[[1]][, , 1]
# Get point in plane
plane_point <- joint_current$coor[3, , 2]
# Get plane normal vector
plane_norm <- joint_current$cons[[3]][3, , 2]
# Check whether vectors are parallel - if they are then there is not a unique solution
a_vecs <- avec(circle_norm, plane_norm, max.pi=TRUE)
if(a_vecs == 0){
# Vectors are parallel - there is no unique solution
}else{
#
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding ', mechanism[['joint.names']][joint_idx[2]], '(', joint_idx[2],
') on circle with center {', paste0(signif(circle_center, 3), collapse=','),
'} at ', mechanism[['joint.names']][joint_idx[1]],
'(', joint_idx[1], '), ', 'point on radius {', paste0(signif(circle_pt_on_rad, 3), collapse=','), '}, and normal {' ,
paste0(signif(circle_norm, 3), collapse=','), '}', ' at intersection with plane defined by point={',
paste0(signif(plane_point, 3), collapse=','), '} and normal vector {',
paste0(signif(uvector(plane_norm), 3), collapse=','), '} and closest to point {',
paste0(signif(toggle_jt_compare, 3), collapse=','), '}', '\n')
cat(solve_str)
}
# Define circle
circle <- defineCircle(center=circle_center, nvector=circle_norm,
point_on_radius=circle_pt_on_rad, redefine_center=TRUE)
# Find intersection of circle and plane
int_cp <- intersectCirclePlane(circle=circle, P=plane_point, N=plane_norm)
# Find point on circle
J2_solve <- circlePoint(circle=circle, T=int_cp)
# If no solution, return NULL
if((is.vector(J2_solve) && is.na(J2_solve[1])) || (!is.vector(J2_solve) && is.na(J2_solve[1,1]))){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+2), collapse=''), 'No solution found.\n'))
return(mechanism)
}
# Find point closest to compare point
which_min <- which.min(distPointToPoint(J2_solve, toggle_jt_compare))
# Set solved joint
J2_solve <- J2_solve[which_min,]
# Get vectors and axis to find first body transformation
V_pre <- joint_current$coor[2, , 1] - joint_current$coor[1, , 1]
V_new <- J2_solve - joint_current$coor[1, , 1]
# Get axis to rotate link about
J_axis <- joint_current$cons[[1]][, , 1]
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation of first body
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- -joint_current$coor[1, , 1]
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- joint_current$coor[1, , 1]
tmat_B1 <- tmat_B_3 %*% tmat_B_2 %*% tmat_B_1
# Find vector for second body transformation
tmat_B2 <- diag(4)
tmat_B2[1:3, 4] <- J2_solve - circle_pt_on_rad
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=tmat_B2,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=1,
body.excl=path_bodies[1:2], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], joint=joint_idx[1],
status.solved.to=1, tmat=tmat_B1, iter=iter, recursive=TRUE,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent, indent.level=indent.level+2)
# Set last joint as jointed
mechanism[['status']][['jointed']][joint_idx[3]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[3], ] <- FALSE
path_solved <- TRUE
}
}
# Solve path
if(path_strings[['tjs']] %in% c('R(JSN)-1-L(JNN)-2-R(DNS)', 'S(JSN)-1-L(JNN)-2-S(DNS)')){
# Find target length between solved halves of rotational joints
len_target <- distPointToPoint(joint_current$coor[1, , 1], joint_current$coor[3, , joint_sets[3,2]])
# Get previous point for toggle position comparison
# **** Should this be iter-1???
# **** Also need to add in joint.compare
if(iter == 1){
t_target_prev <- mechanism[['joint.coor']][joint_idx[3], ]
}else{
t_target_prev <- joint_current$coor[3, , 1]
}
# Get translation vector
tvec <- joint_current$cons[[2]][1, , 1]
# Find intersection of sphere and line
t_target <- intersectSphereLine(c=joint_current$coor[1, , 1], r=len_target,
x=joint_current$coor[3, , joint_sets[3,1]], l=tvec, point.compare=t_target_prev)
# Get translation magnitude
tmag <- distPointToPoint(t_target, joint_current$coor[3, , joint_sets[3,1]])
# Set magnitude sign
tpos <- sum(abs((joint_current$coor[3, , joint_sets[3,1]] + tmag*tvec - t_target)))
tneg <- sum(abs((joint_current$coor[3, , joint_sets[3,1]] - tmag*tvec - t_target)))
if(tpos < tneg){ tmag <- tmag }else { tmag <- -tmag }
# Get transformation of second body
tmat_B <- diag(4)
tmat_B[1:3, 4] <- tmag*tvec
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=tmat_B,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=0,
body.excl=path_bodies[1:2], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
# Update joint coordinates after transformation
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
# Get vectors and axis to find first body transformation
V_pre <- joint_current$coor[3, , joint_sets[3,1]]-joint_current$coor[1, , 1]
V_new <- joint_current$coor[3, , joint_sets[3,2]]-joint_current$coor[1, , 1]
# Get axis to rotate link about
if(path_strings[['tjs']] %in% c('R(JSN)-1-L(JNN)-2-R(DNS)')) J_axis <- joint_current$cons[[1]][, , 1]
if(path_strings[['tjs']] %in% c('S(JSN)-1-L(JNN)-2-S(DNS)')) J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation of first and second body
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- joint_current$coor[1, , 1]
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- -joint_current$coor[1, , 1]
tmat_B <- tmat_B_1 %*% tmat_B_2 %*% tmat_B_3
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], joint=joint_idx[1],
status.solved.to=1, tmat=tmat_B, iter=iter, recursive=TRUE,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent, indent.level=indent.level+2)
# Update joint coordinates after transformation
joint_current <- applyJointTransform(mechanism, joint=joint_idx, iter=iter)
len_current <- distPointToPoint(joint_current$coor[1, , 1], joint_current$coor[3, , joint_sets[3,1]])
# Set remaining joints as jointed and solved
mechanism[['status']][['jointed']][joint_idx[3]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[3], ] <- FALSE
mechanism[['status']][['solved']][joint_idx[2], ] <- 1
mechanism[['status']][['solved']][joint_idx[3], joint_sets[3,1]] <- 1
path_solved <- TRUE
}
# Solve path
if(path_strings[['tjs']] %in% c('R(JSN)-1-S(JNN)-2-S(DSN)', 'R(JSN)-1-S(JNN)-2-S(DNS)', 'R(JSN)-1-R(JNN)-2-R(DNS)')){
# Set 3rd joint set to use
if(grepl('2-S[(]DNS[)]', path_strings[['tjs']])){
joint_set3_use <- c(1,2)
}else{
joint_set3_use <- c(2,1)
}
# Get current line length -- easiest to calculate in real time in case link is composite of other joints
line_len <- distPointToPoint(joint_current$coor[2, , 1], joint_current$coor[3, , joint_sets[3,joint_set3_use[1]]])
# Get point at distance from circle
line_point <- joint_current$coor[3, , joint_sets[3,joint_set3_use[2]]]
# Get current circle radius, center and normal vector
circle_pt_on_rad <- joint_current$coor[2, , 1]
circle_center <- joint_current$coor[1, , 1]
circle_norm <- joint_current$cons[[1]][, , 1]
#
if(print.progress){
solve_str <- paste0(paste0(rep(indent, indent.level+2), collapse=''),
'Finding ', mechanism[['joint.names']][joint_idx[2]], '(', joint_idx[2],
') on circle with center {', paste0(signif(circle_center, 3), collapse=','),
'} at ', mechanism[['joint.names']][joint_idx[1]],
'(', joint_idx[1], '), ', 'point on radius={', paste0(signif(circle_pt_on_rad, 3), collapse=','), '}, and normal={' ,
paste0(signif(circle_norm, 3), collapse=','), '}', ' at distance=',
signif(line_len, 3), ' from {', paste0(signif(line_point, 3), collapse=','),
'} at ', mechanism[['joint.names']][joint_idx[3]], '(', joint_idx[3],
') and closest to point {', paste0(signif(toggle_jt_compare, 3), collapse=','), '}\n')
cat(solve_str)
}
# Define circle
circle <- defineCircle(center=circle_center, nvector=circle_norm,
point_on_radius=circle_pt_on_rad, redefine_center=TRUE)
# Find angle on circle at distance from point
circle_t <- angleOnCircleFromPoint(circle=circle, dist=line_len, P=line_point, point_compare=toggle_jt_compare)
# Find point on circle
J2_solve <- circlePoint(circle=circle, T=circle_t)
# If no solution, return NULL
if((is.vector(J2_solve) && is.na(J2_solve[1])) || (!is.vector(J2_solve) && is.na(J2_solve[1,1]))){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+2), collapse=''), 'No solution found.\n'))
return(mechanism)
}
# Get vectors and axis to find first body transformation
V_pre <- joint_current$coor[2, , 1] - joint_current$coor[1, , 1]
V_new <- J2_solve - joint_current$coor[1, , 1]
# Get axis to rotate link about
J_axis <- joint_current$cons[[1]][, , 1]
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation of first body
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- -joint_current$coor[1, , 1]
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- joint_current$coor[1, , 1]
tmat_B <- tmat_B_3 %*% tmat_B_2 %*% tmat_B_1
# Transform first body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[1], joint=joint_idx[1],
status.solved.to=1, tmat=tmat_B, iter=iter, recursive=TRUE,
body.excl=path_bodies[1], print.progress=print.progress, indent=indent, indent.level=indent.level+2)
# Update 3rd joint coordinate and constraint after transformation
joint_current3 <- applyJointTransform(mechanism, joint=joint_idx[3], iter=iter)
# Get vectors and axis to find second body transformation
V_pre <- joint_current3$coor[1, , joint_sets[3, joint_set3_use[1]]] - J2_solve
V_new <- line_point - J2_solve
# Get axis to rotate link about
if(path_strings[['tjs']] %in% c('R(JSN)-1-R(JNN)-2-R(DNS)')){
joint_current2 <- applyJointTransform(mechanism, joint=joint_idx[2], iter=iter)
J_axis <- joint_current2$cons[[1]][, , 1]
}
if(path_strings[['tjs']] %in% c('R(JSN)-1-S(JNN)-2-S(DNS)', 'R(JSN)-1-S(JNN)-2-S(DSN)')) J_axis <- cprod(V_pre, V_new)
# Get rotation matrix
RM <- tMatrixEP(J_axis, avec(V_pre, V_new, axis=J_axis, about.axis=TRUE))
# Get transformation of second body
tmat_B_1 <- tmat_B_2 <- tmat_B_3 <- diag(4)
tmat_B_1[1:3, 4] <- J2_solve
tmat_B_2[1:3, 1:3] <- RM
tmat_B_3[1:3, 4] <- -J2_solve
tmat_B <- tmat_B_1 %*% tmat_B_2 %*% tmat_B_3
# Transform second body and extend transformation
mechanism <- extendTransformation(mechanism, body=path_bodies[2], tmat=tmat_B,
iter=iter, recursive=TRUE, joint=joint_idx[2], status.solved.to=1,
body.excl=path_bodies[1:2], print.progress=print.progress, indent=indent,
indent.level=indent.level+2)
#print(line_point)
#print(applyJointTransform(mechanism, joint=joint_idx[3], iter=iter)$coor[, , joint_sets[3,joint_set3_use[2]]])
# Set last joint as jointed
mechanism[['status']][['jointed']][joint_idx[3]] <- TRUE
mechanism[['status']][['transformed']][joint_idx[3], ] <- FALSE
path_solved <- TRUE
}
if(!path_solved && print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'Could not find solver for path [', path_strings[['tjs']], '], path num=', path_idx, ', run num=', ct_print, '\n'))
# Extend solved status through mechanism
mechanism <- extendSolvedStatus(mechanism, print.progress=print.progress, indent=indent, indent.level=indent.level+2)
#if(print.progress) print_joint_status(mechanism, indent, indent.level+2)
#if(path_strings[['p']] == 'R2(JSN)-R3(JNN)-R4(DNS)'){ path_solved <- FALSE; break }
i <- i + 1
}
# If no path was solved, break
if(!path_solved){
if(print.progress) cat(paste0(paste0(rep(indent, indent.level+1), collapse=''),
'No additional paths found to solve during previous run, stopping path solving\n'))
break
}
# Advance count
ct <- ct + 1
}
# Save solved path index
if(save_paths_solved) mechanism[['paths.solved']] <- unique(mechanism[['paths.solved']])
#if(print.progress) print_joint_status(mechanism, indent)
mechanism
}
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.