N重振り子　シミュレーション～R版

こちらのシミュレーションのR版を走り書き

# calcAandB() : function to calculate matrices A and B
calcAandB <- function(Thetas, Vthetas, L, M, g){
	n <- length(Thetas)
	A <- B <- matrix(0,n,n)
	for(i in 1:n){
		for(j in 1:n){
			k <- max(i,j)
			A[i,j] <- sum(M[k:n]) * L[j] * cos(Thetas[i]-Thetas[j])
			if(i == j){
				B[i,j] <- sum(M[k:n]) * g * sin(Thetas[i])
			}else{
				B[i,j] <- sum(M[k:n]) * L[j] * Vthetas[j]^2 * sin(Thetas[i]-Thetas[j])
			}
		}
	}
	return(list(A=A,B=B))	
}

# calcX : function to calculate XY coordinates
calcX <- function(L, Thetas){
	n <- length(L)
	ret <- matrix(0,n,2)
	for(i in 1:n){
		if(i == 1){
			ret[i,] <- L[i] * c(sin(Thetas[i]), cos(Thetas[i]))
		}else{
			ret[i,] <- ret[i-1,] + L[i] * c(sin(Thetas[i]), cos(Thetas[i]))
		}
	}
	return(ret)
}

# calcAccel : function to calclate acceleration of angle velocity
calcAccel <- function(Thetas, Vthetas, L, M, g){
	AB <- calcAandB(Thetas, Vthetas, L, M, g)
	n <- length(Thetas)
	v <- matrix(-1,n,1)
	ret <- solve(AB[[1]]) %*% AB[[2]] %*% v
	return(ret)
}

# Simulation

# n: number of segments
n <- 5
# M: weight of ball at the distal tip of each segment
#M <- n:1
M <- c(100,1,1,1,1)
# L: length of each segment
L <- c(1,1,1,1,1)
# g: 重力
Thetas0 <- rep(0,n)
#Thetas0[1] <- pi/6
vThetas0 <- rep(0,n)
vThetas0[1] <- 1

n.iteration <- 10^4
deltaT <- 10^(-3)

Thetas.All <- vThetas.All <- matrix(0, n.iteration, n)
Thetas.All[1,] <- Thetas0
vThetas.All[1,] <- vThetas0

# 途中でストップする
# いつ
time.stop <- 2000
# どこを
location.stop <- c()

for(i in 2:n.iteration){
	Thetas.now <- Thetas.All[i-1,]
	vThetas.now <- vThetas.All[i-1,]
	
	accel <- calcAccel(Thetas.now, vThetas.now, L, M, g)
	
	Thetas.next <- Thetas.now + deltaT * vThetas.All[i-1,]
	vThetas.next <- vThetas.now + deltaT * accel
	Thetas.All[i,] <- Thetas.next
	vThetas.All[i,] <- vThetas.next
	if(i == time.stop){
		for(k in 1:length(location.stop)){
			Thetas.All[i,location.stop[k]] <- 0
			vThetas.All[i,location.stop[k]] <- 0
			#vThetas.All[i,location.stop[k]] <- (-1) * vThetas.All[i,location.stop[k]] 
		}
	}
	
}

matplot(Thetas.All,type="l")
matplot(vThetas.All,type="l")