R/applyTransformationsChain.R
In aaronolsen/linkR: 3D Lever and Linkage Mechanism Modeling
applyTransformationsChain <- function(linkage, linkage_r, joint_cons, joints_unknown,
link_points_tform, itr, path, solve_chain, joint_init, joint_base, unknown_changed,
call=1, joints_excl=c(), print.progress = FALSE){
if(print.progress) cat('\t\t\tapplyTransformationsChain()\n')
#print(linkage$joint.links)
# SET SOLVE TYPE
type_solve <- names(solve_chain)[1]
#cat('Ji:', joint_init, ' Jb:', joint_base, ' (', type_solve, ')\n', sep='')
#print(solve_chain)
# FIND CONNECTED JOINTS
if(linkage$num.links > 2){
joints_1 <- unique(c(linkage$joint.links[rowSums(linkage$joint.links[, c('Joint1', 'Joint2')] == joint_init) > 0, c('Joint1', 'Joint2')]))
# REMOVE GROUND AND INPUT JOINT
joints_1 <- joints_1[!joints_1 %in% c(0, joint_init, linkage$ground.joints, joints_excl)]
# REMOVE ALREADY DETERMINED JOINT POSITIONS FOR ROTATION
if(type_solve == 'r') if(length(path) > 1) joints_1 <- joints_1[grepl('p', joints_unknown[joints_1])]
if(type_solve == 't') joints_1 <- joints_1[grepl('t|p', joints_unknown[joints_1])]
if(type_solve == 'p') joints_1 <- joints_1[grepl('t', joints_unknown[joints_1])]
if(joint_init != joint_base){
joints_1_excl <- c()
for(joint_1 in joints_1){
# FIND CONNECTED JOINTS
joints_2 <- unique(c(linkage$joint.links[rowSums(linkage$joint.links[, c('Joint1', 'Joint2')] == joint_1) > 0, c('Joint1', 'Joint2')]))
joints_2 <- joints_2[!joints_2 %in% c(joint_init, joint_base, joints_1)]
# SKIP SECONDARY JOINTS CONNECTED TO AN L-, P- OR R-JOINT
if(sum(linkage$joint.types[joints_2] %in% c('L', 'P', 'R', 'U')) > 0) joints_1_excl <- joint_1
}
joints_1 <- joints_1[!joints_1 %in% joints_1_excl]
}
}else{
#joints_1 <- unique(c(linkage$joint.links[sum(linkage$joint.links[, c('Joint1', 'Joint2')] == joint_init) > 0, c('Joint1', 'Joint2')]))
joints_1 <- c()
}
# IF NO CONNECTED JOINTS, THEN SINGLE JOINT LINKAGE
if(linkage$num.links == 2){
#joints_1 <- joint_init
links <- 1
}else{
# FIND ASSOCIATED LINK(S)
links <- c()
#print(joints_1)
#print(linkage$joint.links)
for(joint_1 in joints_1){
curr_and_path <- (rowSums(joint_init == linkage$joint.links[, c('Joint1', 'Joint2')]) == 1) * (rowSums(joint_1 == linkage$joint.links[, c('Joint1', 'Joint2')]) == 1) == 1
links <- c(links, linkage$joint.links[curr_and_path, c('Link.idx')][1])
}
links <- unique(links)
}
#cat('---\n')
# SINGLE UNKNOWN JOINT WILL RETURN 0 LINKS BECAUSE ALL OTHER JOINTS DETERMINED, NO LINK THAT SHARES JOINTS
if(is.null(links)){
# FIND ADJOINING JOINT IN PATH
type_path <- paste(linkage$joint.types[path], collapse='')
if(type_path == 'SSR' || type_path == 'RSS'){
link_match <- linkage$joint.links[rowSums(matrix(linkage$joint.links[, c('Joint1', 'Joint2')] %in% c(joint_init, path[2]), nrow=nrow(linkage$joint.links))) == 2, 'Link.idx']
if(length(link_match) > 0) links <- link_match
}
}
# FIND ASSOCIATED POINTS
points_t <- NULL
#print(links)
if(!is.null(linkage$link.points)) points_t <- as.vector(unlist(linkage$point.assoc[linkage$link.names[links+1]]))
# NO JOINTS TO TRANSFORM
if(linkage$num.links > 2 && length(joints_1) == 0){
#cat('Insert return() here.\n')
return(list(
'linkage_r' = linkage_r, 'joint_cons' = joint_cons, 'joints_unknown' = joints_unknown,
'link_points_tform' = link_points_tform, 'unknown_changed' = unknown_changed
))
}
#if(print.progress) cat('\t\t\t\tApply tranformations chain\n')
if(type_solve %in% 'r'){
# MAKE SURE VECTOR IS NOT NA
if(is.na(joint_cons[[joint_base]][1])) stop("Joint constraint vector for body rotation has NA values.")
if(print.progress) cat(paste0('\t\t\t\tapplyTransformationsChain(): rotate ', paste(linkage$link.names[links+1], collapse=', '), ' link(s)\n'))
if(length(joints_1) > 0){
# ROTATE ASSOCIATED JOINTS
linkage_r$joint.coor[joints_1, , itr] <- rotateBody(m=linkage_r$joint.coor[joints_1, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]], a=solve_chain[[type_solve]])
if(joint_init == joint_base) joints_unknown[joints_1] <- gsub('p', '', joints_unknown[joints_1])
# APPLY ROTATION TO CONSTRAINT VECTORS
for(joint_1 in joints_1){
joint_cons_m <- rotateBody(m=rbind(linkage_r$joint.coor[joint_1, , itr], linkage_r$joint.coor[joint_1, , itr]+linkage$joint.cons[[joint_1]]),
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]], a=solve_chain[[type_solve]])
joint_cons[[joint_1]] <- joint_cons_m[2, ] - joint_cons_m[1, ]
}
}
# SET CHANGE
unknown_changed <- TRUE
# REMOVE POSITIONS AND ROTATIONS FROM UNKNOWNS
#print(joints_unknown)
if(joint_init == joint_base){
joints_unknown[joint_init] <- gsub('r', '', joints_unknown[joint_init])
}
# ROTATE ASSOCIATED POINTS
if(!is.null(points_t)){
linkage_r$link.points[points_t, , itr] <- rotateBody(m=linkage_r$link.points[points_t, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][1:3], a=solve_chain[[type_solve]][1])
if(linkage$joint.type[joint_base] %in% c('U', 'S')){
linkage_r$link.points[points_t, , itr] <- rotateBody(m=linkage_r$link.points[points_t, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][4:6], a=solve_chain[[type_solve]][2])
}
if(linkage$joint.type[joint_base] %in% c('S')){
linkage_r$link.points[points_t, , itr] <- rotateBody(m=linkage_r$link.points[points_t, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][7:9], a=solve_chain[[type_solve]][2])
}
}
# ROTATE ASSOCIATED LOCAL COORDINATE SYSTEMS
for(link_name in linkage$link.names[links+1]){
linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][1:3], a=solve_chain[[type_solve]][1])
if(linkage$joint.type[joint_base] %in% c('U', 'S')){
linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][4:6], a=solve_chain[[type_solve]][2])
}
if(linkage$joint.type[joint_base] %in% c('S')){
linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][7:9], a=solve_chain[[type_solve]][2])
}
#linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
# p=linkage_r$link.lcs[[link_name]][1, , itr], v=joint_cons[[joint_base]], a=solve_chain[[type_solve]])
#if(link_name != 'Link2'){
#}
}
# SET TRANSFORMED LINK
link_points_tform[links+1] <- TRUE
}
if(type_solve %in% c('t', 'p')){
# CHECK IF TRANSLATION SET IN PREVIOUS LOOP
if(joint_init == joint_base && !grepl(type_solve, joints_unknown[joint_init])){
#print(joints_unknown)
return(list(
'linkage_r' = linkage_r, 'joint_cons' = joint_cons, 'joints_unknown' = joints_unknown,
'link_points_tform' = link_points_tform, 'unknown_changed' = unknown_changed
))
}
if(print.progress){
if(!'r' %in% names(solve_chain)){
cat(paste0('\t\t\t\tapplyTransformationsChain(): translate ', paste(linkage$link.names[links+1], collapse=', '), ' link(s)\n'))
}else{
cat(paste0('\t\t\t\tapplyTransformationsChain(): translate and rotate ', paste(linkage$link.names[links+1], collapse=', '), ' link(s)\n'))
}
}
# SET POSITION OF JOINT
if(joints_unknown[joint_init] != ''){
if(!'r' %in% names(solve_chain)){
linkage_r$joint.coor[joint_init, , itr] <- linkage_r$joint.coor[joint_init, , itr] + solve_chain[[type_solve]]
}else{
linkage_r$joint.coor[joint_init, , itr] <- rotateBody(m=linkage_r$joint.coor[joint_init, , itr],
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
linkage_r$joint.coor[joint_init, , itr] <- linkage_r$joint.coor[joint_init, , itr] + solve_chain[['t']]
}
# SET CHANGE
unknown_changed <- TRUE
# REMOVE POSITION FROM UNKNOWN
joints_unknown[joint_init] <- gsub(type_solve, '', joints_unknown[joint_init])
}
if(length(joints_1) > 0){
# TRANSLATE ASSOCIATED JOINTS
if(!'r' %in% names(solve_chain)){
linkage_r$joint.coor[joints_1, , itr] <- linkage_r$joint.coor[joints_1, , itr] +
matrix(solve_chain[[type_solve]], nrow=length(joints_1), ncol=3, byrow=TRUE)
}else{
linkage_r$joint.coor[joints_1, , itr] <- rotateBody(m=linkage_r$joint.coor[joints_1, , itr],
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
linkage_r$joint.coor[joints_1, , itr] <- linkage_r$joint.coor[joints_1, , itr] +
matrix(solve_chain[['t']], nrow=length(joints_1), ncol=3, byrow=TRUE)
}
# APPLY ROTATION TO CONSTRAINT VECTORS
for(joint_1 in joints_1){
if(!linkage$joint.types[joint_1] %in% c('R', 'U')) next
joint_cons_m <- rotateBody(m=rbind(linkage_r$joint.coor[joint_1, , itr],
linkage_r$joint.coor[joint_1, , itr]+linkage$joint.cons[[joint_1]]),
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
joint_cons[[joint_1]] <- joint_cons_m[2, ] - joint_cons_m[1, ]
}
# SET CHANGE
unknown_changed <- TRUE
# REMOVE TRANSLATION FROM UNKNOWN
joints_unknown[joints_1] <- gsub('t|p', '', joints_unknown[joints_1])
}
if(type_solve == 't' && !is.null(points_t)){
# TRANSLATE ASSOCIATED POINTS
if(!is.null(points_t)){
if(!'r' %in% names(solve_chain)){
linkage_r$link.points[points_t, , itr] <- linkage_r$link.points[points_t, , itr] +
matrix(solve_chain[[type_solve]], nrow=length(points_t), ncol=3, byrow=TRUE)
}else{
linkage_r$link.points[points_t, , itr] <- rotateBody(m=linkage_r$link.points[points_t, , itr],
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
linkage_r$link.points[points_t, , itr] <- linkage_r$link.points[points_t, , itr] +
matrix(solve_chain[['t']], nrow=length(points_t), ncol=3, byrow=TRUE)
}
}
# SET TRANSFORMED LINK
link_points_tform[links+1] <- TRUE
}
# TRANSLATE ASSOCIATED LOCAL COORDINATE SYSTEMS
for(link_name in linkage$link.names[links+1]){
if(!'r' %in% names(solve_chain)){
linkage_r$link.lcs[[link_name]][, , itr] <- linkage_r$link.lcs[[link_name]][, , itr] +
matrix(solve_chain[[type_solve]], nrow=dim(linkage_r$link.lcs[[link_name]])[1], ncol=3, byrow=TRUE)
}else{
linkage_r$link.lcs[[link_name]][, , itr] <- linkage_r$link.lcs[[link_name]][, , itr] +
matrix(solve_chain[['t']], nrow=dim(linkage_r$link.lcs[[link_name]])[1], ncol=3, byrow=TRUE)
linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
}
}
}
#cat('\n')
if(linkage$num.links > 2 && length(path) == 1 && joint_init == joint_base){
for(joint_1 in joints_1){
# DO NOT APPLY TRANSFORMATION TO LINKS CONNECTED THROUGH S JOINTS
if(linkage$joint.types[joint_1] %in% c('S')) next
if(print.progress) cat(paste0('\t\t\t\tRecursive applyTransformationsChain() for links associated through ', rownames(linkage$joint.coor)[joint_1], '\n'))
apply_t <- applyTransformationsChain(linkage, linkage_r, joint_cons, joints_unknown, link_points_tform, itr,
path, solve_chain, joint_1, joint_init, unknown_changed, call=2, joints_excl=joints_1,
print.progress=print.progress)
linkage_r <- apply_t$linkage_r
joint_cons <- apply_t$joint_cons
joints_unknown <- apply_t$joints_unknown
link_points_tform <- apply_t$link_points_tform
unknown_changed <- apply_t$unknown_changed
}
}
#print(joints_unknown)
return(list(
'linkage_r' = linkage_r,
'joint_cons' = joint_cons,
'joints_unknown' = joints_unknown,
'link_points_tform' = link_points_tform,
'unknown_changed' = unknown_changed
))
}
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.
applyTransformationsChain <- function(linkage, linkage_r, joint_cons, joints_unknown,
link_points_tform, itr, path, solve_chain, joint_init, joint_base, unknown_changed,
call=1, joints_excl=c(), print.progress = FALSE){
if(print.progress) cat('\t\t\tapplyTransformationsChain()\n')
#print(linkage$joint.links)
# SET SOLVE TYPE
type_solve <- names(solve_chain)[1]
#cat('Ji:', joint_init, ' Jb:', joint_base, ' (', type_solve, ')\n', sep='')
#print(solve_chain)
# FIND CONNECTED JOINTS
if(linkage$num.links > 2){
joints_1 <- unique(c(linkage$joint.links[rowSums(linkage$joint.links[, c('Joint1', 'Joint2')] == joint_init) > 0, c('Joint1', 'Joint2')]))
# REMOVE GROUND AND INPUT JOINT
joints_1 <- joints_1[!joints_1 %in% c(0, joint_init, linkage$ground.joints, joints_excl)]
# REMOVE ALREADY DETERMINED JOINT POSITIONS FOR ROTATION
if(type_solve == 'r') if(length(path) > 1) joints_1 <- joints_1[grepl('p', joints_unknown[joints_1])]
if(type_solve == 't') joints_1 <- joints_1[grepl('t|p', joints_unknown[joints_1])]
if(type_solve == 'p') joints_1 <- joints_1[grepl('t', joints_unknown[joints_1])]
if(joint_init != joint_base){
joints_1_excl <- c()
for(joint_1 in joints_1){
# FIND CONNECTED JOINTS
joints_2 <- unique(c(linkage$joint.links[rowSums(linkage$joint.links[, c('Joint1', 'Joint2')] == joint_1) > 0, c('Joint1', 'Joint2')]))
joints_2 <- joints_2[!joints_2 %in% c(joint_init, joint_base, joints_1)]
# SKIP SECONDARY JOINTS CONNECTED TO AN L-, P- OR R-JOINT
if(sum(linkage$joint.types[joints_2] %in% c('L', 'P', 'R', 'U')) > 0) joints_1_excl <- joint_1
}
joints_1 <- joints_1[!joints_1 %in% joints_1_excl]
}
}else{
#joints_1 <- unique(c(linkage$joint.links[sum(linkage$joint.links[, c('Joint1', 'Joint2')] == joint_init) > 0, c('Joint1', 'Joint2')]))
joints_1 <- c()
}
# IF NO CONNECTED JOINTS, THEN SINGLE JOINT LINKAGE
if(linkage$num.links == 2){
#joints_1 <- joint_init
links <- 1
}else{
# FIND ASSOCIATED LINK(S)
links <- c()
#print(joints_1)
#print(linkage$joint.links)
for(joint_1 in joints_1){
curr_and_path <- (rowSums(joint_init == linkage$joint.links[, c('Joint1', 'Joint2')]) == 1) * (rowSums(joint_1 == linkage$joint.links[, c('Joint1', 'Joint2')]) == 1) == 1
links <- c(links, linkage$joint.links[curr_and_path, c('Link.idx')][1])
}
links <- unique(links)
}
#cat('---\n')
# SINGLE UNKNOWN JOINT WILL RETURN 0 LINKS BECAUSE ALL OTHER JOINTS DETERMINED, NO LINK THAT SHARES JOINTS
if(is.null(links)){
# FIND ADJOINING JOINT IN PATH
type_path <- paste(linkage$joint.types[path], collapse='')
if(type_path == 'SSR' || type_path == 'RSS'){
link_match <- linkage$joint.links[rowSums(matrix(linkage$joint.links[, c('Joint1', 'Joint2')] %in% c(joint_init, path[2]), nrow=nrow(linkage$joint.links))) == 2, 'Link.idx']
if(length(link_match) > 0) links <- link_match
}
}
# FIND ASSOCIATED POINTS
points_t <- NULL
#print(links)
if(!is.null(linkage$link.points)) points_t <- as.vector(unlist(linkage$point.assoc[linkage$link.names[links+1]]))
# NO JOINTS TO TRANSFORM
if(linkage$num.links > 2 && length(joints_1) == 0){
#cat('Insert return() here.\n')
return(list(
'linkage_r' = linkage_r, 'joint_cons' = joint_cons, 'joints_unknown' = joints_unknown,
'link_points_tform' = link_points_tform, 'unknown_changed' = unknown_changed
))
}
#if(print.progress) cat('\t\t\t\tApply tranformations chain\n')
if(type_solve %in% 'r'){
# MAKE SURE VECTOR IS NOT NA
if(is.na(joint_cons[[joint_base]][1])) stop("Joint constraint vector for body rotation has NA values.")
if(print.progress) cat(paste0('\t\t\t\tapplyTransformationsChain(): rotate ', paste(linkage$link.names[links+1], collapse=', '), ' link(s)\n'))
if(length(joints_1) > 0){
# ROTATE ASSOCIATED JOINTS
linkage_r$joint.coor[joints_1, , itr] <- rotateBody(m=linkage_r$joint.coor[joints_1, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]], a=solve_chain[[type_solve]])
if(joint_init == joint_base) joints_unknown[joints_1] <- gsub('p', '', joints_unknown[joints_1])
# APPLY ROTATION TO CONSTRAINT VECTORS
for(joint_1 in joints_1){
joint_cons_m <- rotateBody(m=rbind(linkage_r$joint.coor[joint_1, , itr], linkage_r$joint.coor[joint_1, , itr]+linkage$joint.cons[[joint_1]]),
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]], a=solve_chain[[type_solve]])
joint_cons[[joint_1]] <- joint_cons_m[2, ] - joint_cons_m[1, ]
}
}
# SET CHANGE
unknown_changed <- TRUE
# REMOVE POSITIONS AND ROTATIONS FROM UNKNOWNS
#print(joints_unknown)
if(joint_init == joint_base){
joints_unknown[joint_init] <- gsub('r', '', joints_unknown[joint_init])
}
# ROTATE ASSOCIATED POINTS
if(!is.null(points_t)){
linkage_r$link.points[points_t, , itr] <- rotateBody(m=linkage_r$link.points[points_t, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][1:3], a=solve_chain[[type_solve]][1])
if(linkage$joint.type[joint_base] %in% c('U', 'S')){
linkage_r$link.points[points_t, , itr] <- rotateBody(m=linkage_r$link.points[points_t, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][4:6], a=solve_chain[[type_solve]][2])
}
if(linkage$joint.type[joint_base] %in% c('S')){
linkage_r$link.points[points_t, , itr] <- rotateBody(m=linkage_r$link.points[points_t, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][7:9], a=solve_chain[[type_solve]][2])
}
}
# ROTATE ASSOCIATED LOCAL COORDINATE SYSTEMS
for(link_name in linkage$link.names[links+1]){
linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][1:3], a=solve_chain[[type_solve]][1])
if(linkage$joint.type[joint_base] %in% c('U', 'S')){
linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][4:6], a=solve_chain[[type_solve]][2])
}
if(linkage$joint.type[joint_base] %in% c('S')){
linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
p=linkage_r$joint.coor[joint_base, , itr], v=joint_cons[[joint_base]][7:9], a=solve_chain[[type_solve]][2])
}
#linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
# p=linkage_r$link.lcs[[link_name]][1, , itr], v=joint_cons[[joint_base]], a=solve_chain[[type_solve]])
#if(link_name != 'Link2'){
#}
}
# SET TRANSFORMED LINK
link_points_tform[links+1] <- TRUE
}
if(type_solve %in% c('t', 'p')){
# CHECK IF TRANSLATION SET IN PREVIOUS LOOP
if(joint_init == joint_base && !grepl(type_solve, joints_unknown[joint_init])){
#print(joints_unknown)
return(list(
'linkage_r' = linkage_r, 'joint_cons' = joint_cons, 'joints_unknown' = joints_unknown,
'link_points_tform' = link_points_tform, 'unknown_changed' = unknown_changed
))
}
if(print.progress){
if(!'r' %in% names(solve_chain)){
cat(paste0('\t\t\t\tapplyTransformationsChain(): translate ', paste(linkage$link.names[links+1], collapse=', '), ' link(s)\n'))
}else{
cat(paste0('\t\t\t\tapplyTransformationsChain(): translate and rotate ', paste(linkage$link.names[links+1], collapse=', '), ' link(s)\n'))
}
}
# SET POSITION OF JOINT
if(joints_unknown[joint_init] != ''){
if(!'r' %in% names(solve_chain)){
linkage_r$joint.coor[joint_init, , itr] <- linkage_r$joint.coor[joint_init, , itr] + solve_chain[[type_solve]]
}else{
linkage_r$joint.coor[joint_init, , itr] <- rotateBody(m=linkage_r$joint.coor[joint_init, , itr],
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
linkage_r$joint.coor[joint_init, , itr] <- linkage_r$joint.coor[joint_init, , itr] + solve_chain[['t']]
}
# SET CHANGE
unknown_changed <- TRUE
# REMOVE POSITION FROM UNKNOWN
joints_unknown[joint_init] <- gsub(type_solve, '', joints_unknown[joint_init])
}
if(length(joints_1) > 0){
# TRANSLATE ASSOCIATED JOINTS
if(!'r' %in% names(solve_chain)){
linkage_r$joint.coor[joints_1, , itr] <- linkage_r$joint.coor[joints_1, , itr] +
matrix(solve_chain[[type_solve]], nrow=length(joints_1), ncol=3, byrow=TRUE)
}else{
linkage_r$joint.coor[joints_1, , itr] <- rotateBody(m=linkage_r$joint.coor[joints_1, , itr],
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
linkage_r$joint.coor[joints_1, , itr] <- linkage_r$joint.coor[joints_1, , itr] +
matrix(solve_chain[['t']], nrow=length(joints_1), ncol=3, byrow=TRUE)
}
# APPLY ROTATION TO CONSTRAINT VECTORS
for(joint_1 in joints_1){
if(!linkage$joint.types[joint_1] %in% c('R', 'U')) next
joint_cons_m <- rotateBody(m=rbind(linkage_r$joint.coor[joint_1, , itr],
linkage_r$joint.coor[joint_1, , itr]+linkage$joint.cons[[joint_1]]),
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
joint_cons[[joint_1]] <- joint_cons_m[2, ] - joint_cons_m[1, ]
}
# SET CHANGE
unknown_changed <- TRUE
# REMOVE TRANSLATION FROM UNKNOWN
joints_unknown[joints_1] <- gsub('t|p', '', joints_unknown[joints_1])
}
if(type_solve == 't' && !is.null(points_t)){
# TRANSLATE ASSOCIATED POINTS
if(!is.null(points_t)){
if(!'r' %in% names(solve_chain)){
linkage_r$link.points[points_t, , itr] <- linkage_r$link.points[points_t, , itr] +
matrix(solve_chain[[type_solve]], nrow=length(points_t), ncol=3, byrow=TRUE)
}else{
linkage_r$link.points[points_t, , itr] <- rotateBody(m=linkage_r$link.points[points_t, , itr],
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
linkage_r$link.points[points_t, , itr] <- linkage_r$link.points[points_t, , itr] +
matrix(solve_chain[['t']], nrow=length(points_t), ncol=3, byrow=TRUE)
}
}
# SET TRANSFORMED LINK
link_points_tform[links+1] <- TRUE
}
# TRANSLATE ASSOCIATED LOCAL COORDINATE SYSTEMS
for(link_name in linkage$link.names[links+1]){
if(!'r' %in% names(solve_chain)){
linkage_r$link.lcs[[link_name]][, , itr] <- linkage_r$link.lcs[[link_name]][, , itr] +
matrix(solve_chain[[type_solve]], nrow=dim(linkage_r$link.lcs[[link_name]])[1], ncol=3, byrow=TRUE)
}else{
linkage_r$link.lcs[[link_name]][, , itr] <- linkage_r$link.lcs[[link_name]][, , itr] +
matrix(solve_chain[['t']], nrow=dim(linkage_r$link.lcs[[link_name]])[1], ncol=3, byrow=TRUE)
linkage_r$link.lcs[[link_name]][, , itr] <- rotateBody(m=linkage_r$link.lcs[[link_name]][, , itr],
p=solve_chain[['p']], v=joint_cons[[joint_init]], a=solve_chain[['r']])
}
}
}
#cat('\n')
if(linkage$num.links > 2 && length(path) == 1 && joint_init == joint_base){
for(joint_1 in joints_1){
# DO NOT APPLY TRANSFORMATION TO LINKS CONNECTED THROUGH S JOINTS
if(linkage$joint.types[joint_1] %in% c('S')) next
if(print.progress) cat(paste0('\t\t\t\tRecursive applyTransformationsChain() for links associated through ', rownames(linkage$joint.coor)[joint_1], '\n'))
apply_t <- applyTransformationsChain(linkage, linkage_r, joint_cons, joints_unknown, link_points_tform, itr,
path, solve_chain, joint_1, joint_init, unknown_changed, call=2, joints_excl=joints_1,
print.progress=print.progress)
linkage_r <- apply_t$linkage_r
joint_cons <- apply_t$joint_cons
joints_unknown <- apply_t$joints_unknown
link_points_tform <- apply_t$link_points_tform
unknown_changed <- apply_t$unknown_changed
}
}
#print(joints_unknown)
return(list(
'linkage_r' = linkage_r,
'joint_cons' = joint_cons,
'joints_unknown' = joints_unknown,
'link_points_tform' = link_points_tform,
'unknown_changed' = unknown_changed
))
}
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.