animate_mechanism_error <- function(p, fit.points, mechanism, input.param,
fit.wts, replace, direct.input = NULL, joint.compare = NULL, planar = FALSE,
cons.fill = NULL, coor.vectors = NULL, joint.optim = NULL,
use.ref.as.prev = FALSE, input.param.fill = NULL, iter = NULL, print.progress = FALSE){
# Replace parameter with optimize parameters
if(replace == 'input.param'){
input_param <- list()
for(i in 1:length(input.param.fill)){
input_param[[i]] <- matrix(input.param[[i]][iter, ], nrow=1)
input_param[[i]][, input.param.fill[[i]][['list.idx']]] <- matrix(p[input.param.fill[[i]][['col.idx']]], nrow=1)
}
input.param <- input_param
}else if(replace == 'joint.coor'){
j <- 1
for(i in joint.optim){
if(length(coor.vectors[[i]]) == 1){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + p[j:(j+2)]
j <- j + 3
}else{
if(nrow(coor.vectors[[i]]) == 1){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(p[j]*coor.vectors[[i]])
j <- j + 1
}else if(nrow(coor.vectors[[i]]) == 2){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(p[j:(j+1)]*coor.vectors[[i]])
j <- j + 2
}else if(nrow(coor.vectors[[i]]) == 3){
mechanism$joint.coor[i, ] <- mechanism$joint.coor[i, ] + colSums(p[j:(j+2)]*coor.vectors[[i]])
j <- j + 3
}
}
if(planar){
mechanism$joint.coor[i, ] <- pointPlaneProj(mechanism$joint.coor[i, ],
p=mechanism$joint.coor[1, ], n=mechanism$joint.cons[[i]])
}
}
}else if(replace == 'joint.cons'){
k <- 1
n <- 1
for(i in 1:length(mechanism$joint.cons)){
tform1 <- diag(3)
tform2 <- diag(3)
for(j in 1:nrow(mechanism$joint.cons[[i]])){
if(cons.fill[k] == 'v'){
tform1 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], p[n:(n+2)]), a=avec(mechanism$joint.cons[[i]][j, ], p[n:(n+2)]))
mechanism$joint.cons[[i]][j, ] <- uvector(p[n:(n+2)])
n <- n + 3
}else if(cons.fill[k] == 'vo'){
if(j == 2){
after_tform <- mechanism$joint.cons[[i]][j, ] %*% tform1
after_tform <- after_tform %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=p[n])
tform2 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], after_tform), a=avec(mechanism$joint.cons[[i]][j, ], after_tform))
}else if(j == 3){
after_tform <- mechanism$joint.cons[[i]][j, ] %*% tform2
after_tform <- after_tform %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=p[n])
#tform2 <- tMatrixEP(v=cprod(mechanism$joint.cons[[i]][j, ], after_tform), a=avec(mechanism$joint.cons[[i]][j, ], after_tform))
}
mechanism$joint.cons[[i]][j, ] <- after_tform
#mechanism$joint.cons[[i]][j, ] <- mechanism$joint.cons[[i]][j, ] %*% tform1
#mechanism$joint.cons[[i]][j, ] <- mechanism$joint.cons[[i]][j, ] %*% tMatrixEP(v=mechanism$joint.cons[[i]][j-1, ], a=p[n])
n <- n + 1
}else if(cons.fill[k] == 'cprod'){
mechanism$joint.cons[[i]][j, ] <- cprod(mechanism$joint.cons[[i]][j-2, ], mechanism$joint.cons[[i]][j-1, ])
}
k <- k + 1
}
}
}
# Replace any input parameters with direct calculation
if(!is.null(direct.input) && any(direct.input)){
# Get input parameters
for(i in which(direct.input)){
input.param[[i]] <- findJointInputParam(type=mechanism$joint.type[i],
coor=mechanism$joint.coor[i,], cons=mechanism$joint.cons[[i]][, ], ref=mechanism$body.points,
poses=fit.points)
}
}
# Run mechanism model
anim_mech <- suppressWarnings(animateMechanism(mechanism, input.param=input.param,
joint.compare=joint.compare, use.ref.as.prev=use.ref.as.prev,
print.progress=print.progress, check.inter.joint.dist=FALSE, check.joint.cons=FALSE))
# Compare simulated coordinates to ideal
if(dim(anim_mech$body.points.anim)[3] == 1){
return_error <- sqrt(mean(fit.wts*(anim_mech$body.points.anim[, , 1] - fit.points)^2))
}else{
return_error <- sqrt(mean(fit.wts*(anim_mech$body.points.anim - fit.points)^2))
}
# Add NA penalty
#if(sum(is.na(anim_mech$joint.coor)) > 0){cat(TRUE, '')}else{cat(FALSE, '')}
return_error <- return_error + 0.2*(sum(is.na(anim_mech$joint.coor)) / 3)
return(return_error)
}
Add the following code to your website.
For more information on customizing the embed code, read Embedding Snippets.