import numpy as np
import matplotlib.pyplot as plt
# Constants
L1 = 0.3 # m Link 1 length
L2 = 0.3 # m Link 2 length
Lg1 = 0.15 # m Link 1 center of mass
Lg2 = 0.15 # m Link 2 center of mass
m1 = 0.5 # kg Link 1 mass
m2 = 0.5 # kg Link 2 mass
I1 = 5.4e-3 # kgm^2 Link 1 inertia moment
I2 = 5.4e-3 # kgm^2 Link 2 inertia moment
mL = 5.0 # kg End effector mass
g = 9.806199 # Gravitational acceleration
pi = 3.141592 # π
h = 0.001 # s Time step
# Initialize variables
t = 0
th1 = np.zeros(801)
th2 = np.zeros(801)
thv1 = np.zeros(801)
thv2 = np.zeros(801)
X1 = np.zeros(801)
Y1 = np.zeros(801)
X2 = np.zeros(801)
Y2 = np.zeros(801)
# Calculated constants
A1 = m1 * Lg1 * Lg1 + I1 + m2 * L1 * L1 + mL * L1 * L1
A2 = I2 + m2 * Lg2 * Lg2 + mL * L2 * L2
A3 = (m2 * Lg2 + mL * L2) * L1
B1 = (m1 * Lg1 + m2 * L1 + mL * L1) * g
B2 = (m2 * Lg2 + mL * L2) * g
# Initial conditions
z1 = 90.0 * pi / 180.0
z2 = -90.0 * pi / 180.0
w1 = 0
w2 = 0
Xp = L2
Yp = L1
Xv = 0
Yv = 0
# Simulation loop for 800 ms
for i in range(801):
t += h
Xp = L1 * np.cos(z1) + L2 * np.cos(z1 + z2)
Yp = L1 * np.sin(z1) + L2 * np.sin(z1 + z2)
Xv = (-L1 * np.sin(z1) - L2 * np.sin(z1 + z2)) * w1 + (-L2 * np.sin(z1 + z2)) * w2
Yv = (L1 * np.cos(z1) + L2 * np.cos(z1 + z2)) * w1 + (L2 * np.cos(z1 + z2)) * w2
# External force
FLx = 0
FLy = 0
# Controller (Control input)
tau1 = 0.0
tau2 = 0.0
# Runge-Kutta method Step 1
k11 = h * (w1)
k12 = h * (w2)
LL = A1 * A2 - A3 * A3 * np.cos(z2) * np.cos(z2)
a11 = A2 / LL
a12 = -(A2 + A3 * np.cos(z2)) / LL
a22 = (A1 + A2 + 2.0 * A3 * np.cos(z2)) / LL
tauL1 = (-L1 * np.sin(z1) - L2 * np.sin(z1 + z2)) * FLx + (L1 * np.cos(z1) + L2 * np.cos(z1 + z2)) * FLy
tauL2 = (-L2 * np.sin(z1 + z2)) * FLx + (L2 * np.cos(z1 + z2)) * FLy
u1 = tau1 + tauL1 + A3 * (2.0 * w1 * w2 + w2 * w2) * np.sin(z2) - B1 * np.cos(z1) - B2 * np.cos(z1 + z2)
u2 = tau2 + tauL2 - A3 * w1 * w1 * np.sin(z2) - B2 * np.cos(z1 + z2)
L11 = h * (a11 * u1 + a12 * u2)
L12 = h * (a12 * u1 + a22 * u2)
# Runge-Kutta method Step 2
z21 = z1 + k11 / 2.0
z22 = z2 + k12 / 2.0
w21 = w1 + L11 / 2.0
w22 = w2 + L12 / 2.0
k21 = h * (w21)
k22 = h * (w22)
LL = A1 * A2 - A3 * A3 * np.cos(z22) * np.cos(z22)
a11 = A2 / LL
a12 = -(A2 + A3 * np.cos(z22)) / LL
a22 = (A1 + A2 + 2.0 * A3 * np.cos(z22)) / LL
tauL1 = (-L1 * np.sin(z21) - L2 * np.sin(z21 + z22)) * FLx + (L1 * np.cos(z21) + L2 * np.cos(z21 + z22)) * FLy
tauL2 = (-L2 * np.sin(z21 + z22)) * FLx + (L2 * np.cos(z21 + z22)) * FLy
u1 = tau1 + tauL1 + A3 * (2.0 * w21 * w22 + w22 * w22) * np.sin(z22) - B1 * np.cos(z21) - B2 * np.cos(z21 + z22)
u2 = tau2 + tauL2 - A3 * w21 * w21 * np.sin(z22) - B2 * np.cos(z21 + z22)
L21 = h * (a11 * u1 + a12 * u2)
L22 = h * (a12 * u1 + a22 * u2)
# Runge-Kutta method Step 3
z31 = z1 + k21 / 2.0
z32 = z2 + k22 / 2.0
w31 = w1 + L21 / 2.0
w32 = w2 + L22 / 2.0
k31 = h * (w31)
k32 = h * (w32)
LL = A1 * A2 - A3 * A3 * np.cos(z32) * np.cos(z32)
a11 = A2 / LL
a12 = -(A2 + A3 * np.cos(z32)) / LL
a22 = (A1 + A2 + 2.0 * A3 * np.cos(z32)) / LL
tauL1 = (-L1 * np.sin(z31) - L2 * np.sin(z31 + z32)) * FLx + (L1 * np.cos(z31) + L2 * np.cos(z31 + z32)) * FLy
tauL2 = (-L2 * np.sin(z31 + z32)) * FLx + (L2 * np.cos(z31 + z32)) * FLy
u1 = tau1 + tauL1 + A3 * (2.0 * w31 * w32 + w32 * w32) * np.sin(z32) - B1 * np.cos(z31) - B2 * np.cos(z31 + z32)
u2 = tau2 + tauL2 - A3 * w31 * w31 * np.sin(z32) - B2 * np.cos(z31 + z32)
L31 = h * (a11 * u1 + a12 * u2)
L32 = h * (a12 * u1 + a22 * u2)
# Runge-Kutta method Step 4
z41 = z1 + k31
z42 = z2 + k32
w41 = w1 + L31
w42 = w2 + L32
k41 = h * (w41)
k42 = h * (w42)
LL = A1 * A2 - A3 * A3 * np.cos(z42) * np.cos(z42)
a11 = A2 / LL
a12 = -(A2 + A3 * np.cos(z42)) / LL
a22 = (A1 + A2 + 2.0 * A3 * np.cos(z42)) / LL
tauL1 = (-L1 * np.sin(z41) - L2 * np.sin(z41 + z42)) * FLx + (L1 * np.cos(z41) + L2 * np.cos(z41 + z42)) * FLy
tauL2 = (-L2 * np.sin(z41 + z42)) * FLx + (L2 * np.cos(z41 + z42)) * FLy
u1 = tau1 + tauL1 + A3 * (2.0 * w41 * w42 + w42 * w42) * np.sin(z42) - B1 * np.cos(z41) - B2 * np.cos(z41 + z42)
u2 = tau2 + tauL2 - A3 * w41 * w41 * np.sin(z42) - B2 * np.cos(z41 + z42)
L41 = h * (a11 * u1 + a12 * u2)
L42 = h * (a12 * u1 + a22 * u2)
# Combine Runge-Kutta steps
z1 = z1 + (k11 + 2.0 * k21 + 2.0 * k31 + k41) / 6.0
z2 = z2 + (k12 + 2.0 * k22 + 2.0 * k32 + k42) / 6.0
w1 = w1 + (L11 + 2.0 * L21 + 2.0 * L31 + L41) / 6.0
w2 = w2 + (L12 + 2.0 * L22 + 2.0 * L32 + L42) / 6.0
th1[i] = z1
th2[i] = z2
thv1[i] = w1
thv2[i] = w2
X1[i] = L1 * np.cos(z1)
Y1[i] = L1 * np.sin(z1)
X2[i] = L1 * np.cos(z1) + L2 * np.cos(z1 + z2)
Y2[i] = L1 * np.sin(z1) + L2 * np.sin(z1 + z2)
if i % 1 == 0:
print(f"i={i:3d} z1={th1[i]*180.0/pi:7.2f} z2={th2[i]*180.0/pi:7.2f} "
f"X={X2[i]:6.3f} Y={Y2[i]:6.3f} "
f"tu1={tau1:7.3f} tu2={tau2:7.3f}")
# Plot results
plt.figure()
plt.subplot(2, 1, 1)
plt.plot(np.arange(801) * h, th1 * 180 / pi, label='θ1 (degree)')
plt.plot(np.arange(801) * h, th2 * 180 / pi, label='θ2 (degree)')
plt.xlabel('Time [s]')
plt.ylabel('Angle [degree]')
plt.legend()
plt.title('Joint Angles over Time')
plt.subplot(2, 1, 2)
plt.plot(np.arange(801) * h, thv1 * 180 / pi, label='ω1 (degree/s)')
plt.plot(np.arange(801) * h, thv2 * 180 / pi, label='ω2 (degree/s)')
plt.xlabel('Time [s]')
plt.ylabel('Angular Velocity [degree/s]')
plt.legend()
plt.title('Joint Angular Velocities over Time')
plt.tight_layout()
plt.show()
plt.figure()
plt.plot(X2, Y2, label='End Effector Path')
plt.xlabel('X Position [m]')
plt.ylabel('Y Position [m]')
plt.legend()
plt.title('End Effector Path')
plt.axis('equal')
plt.grid(True)
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
# Robot arm parameters
L1 = 0.3
L2 = 0.3
Lg1 = 0.15
Lg2 = 0.15
m1 = 0.5
m2 = 0.5
I1 = 5.4e-3
I2 = 5.4e-3
mL = 5.0
g = 9.806199
h = 0.001
def robot_arm_dynamics(t, state, tau1, tau2):
z1, z2, w1, w2 = state
A1 = m1 * Lg1**2 + I1 + m2 * L1**2 + mL * L1**2
A2 = I2 + m2 * Lg2**2 + mL * L2**2
A3 = (m2 * Lg2 + mL * L2) * L1
B1 = (m1 * Lg1 + m2 * L1 + mL * L1) * g
B2 = (m2 * Lg2 + mL * L2) * g
LL = A1 * A2 - A3**2 * np.cos(z2)**2
a11 = A2 / LL
a12 = -(A2 + A3 * np.cos(z2)) / LL
a22 = (A1 + A2 + 2.0 * A3 * np.cos(z2)) / LL
tauL1 = (-L1 * np.sin(z1) - L2 * np.sin(z1 + z2)) * 0.0
tauL2 = (-L2 * np.sin(z1 + z2)) * 0.0
u1 = tau1 + tauL1 + A3 * (2.0 * w1 * w2 + w2**2) * np.sin(z2) - B1 * np.cos(z1) - B2 * np.cos(z1 + z2)
u2 = tau2 + tauL2 - A3 * w1**2 * np.sin(z2) - B2 * np.cos(z1 + z2)
dz1_dt = w1
dz2_dt = w2
dw1_dt = h * (a11 * u1 + a12 * u2)
dw2_dt = h * (a12 * u1 + a22 * u2)
return [dz1_dt, dz2_dt, dw1_dt, dw2_dt]
# Initial conditions
z1_init = np.deg2rad(90.0)
z2_init = np.deg2rad(-90.0)
w1_init = 0.0
w2_init = 0.0
initial_state = [z1_init, z2_init, w1_init, w2_init]
# Time span and evaluation
t_span = (0, 0.8) # simulation for 800 ms
t_eval = np.linspace(t_span[0], t_span[1], 801)
# Solve the ODE
solution = solve_ivp(robot_arm_dynamics, t_span, initial_state, t_eval=t_eval, args=(0.0, 0.0))
# Extract results
t = solution.t
z1 = solution.y[0]
z2 = solution.y[1]
X1 = L1 * np.cos(z1)
Y1 = L1 * np.sin(z1)
X2 = L1 * np.cos(z1) + L2 * np.cos(z1 + z2)
Y2 = L1 * np.sin(z1) + L2 * np.sin(z1 + z2)
# Plot the results
plt.figure(figsize=(12, 6))
plt.subplot(1, 2, 1)
plt.plot(X2, Y2)
plt.title('End-Effector Path')
plt.xlabel('X position (m)')
plt.ylabel('Y position (m)')
plt.subplot(1, 2, 2)
plt.plot(t, np.rad2deg(z1), label='Theta 1')
plt.plot(t, np.rad2deg(z2), label='Theta 2')
plt.title('Joint Angles Over Time')
plt.xlabel('Time (s)')
plt.ylabel('Angle (degrees)')
plt.legend()
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.animation as animation
# 定数の設定
L1 = 0.3 # リンク1の長さ
L2 = 0.3 # リンク2の長さ
g = 9.806199 # 重力加速度
h = 0.001 # タイムステップ
# 初期パラメータの設定
theta1_init = np.deg2rad(90) # リンク1の初期角度 (ラジアン)
theta2_init = np.deg2rad(-90) # リンク2の初期角度 (ラジアン)
omega1_init = 0.0 # リンク1の初期角速度
omega2_init = 0.0 # リンク2の初期角速度
# 時間の設定
time_steps = 800
time = np.linspace(0, time_steps * h, time_steps)
# 角度と角速度の配列
theta1 = np.zeros(time_steps)
theta2 = np.zeros(time_steps)
omega1 = np.zeros(time_steps)
omega2 = np.zeros(time_steps)
# 初期条件の設定
theta1[0] = theta1_init
theta2[0] = theta2_init
omega1[0] = omega1_init
omega2[0] = omega2_init
# シミュレーションの計算
for i in range(1, time_steps):
# ロボットアームの角度と角速度の計算
# この部分は簡略化されています。実際のシミュレーションでは、運動方程式を解く必要があります。
theta1[i] = theta1[i - 1] + omega1[i - 1] * h
theta2[i] = theta2[i - 1] + omega2[i - 1] * h
omega1[i] = omega1[i - 1]
omega2[i] = omega2[i - 1]
# アニメーションの作成
fig, ax = plt.subplots()
ax.set_xlim(-0.8, 0.8)
ax.set_ylim(-0.8, 0.8)
ax.set_aspect('equal')
line, = ax.plot([], [], 'o-', lw=2)
def init():
line.set_data([], [])
return line,
def update(frame):
x1 = L1 * np.cos(theta1[frame])
y1 = L1 * np.sin(theta1[frame])
x2 = x1 + L2 * np.cos(theta1[frame] + theta2[frame])
y2 = y1 + L2 * np.sin(theta1[frame] + theta2[frame])
line.set_data([0, x1, x2], [0, y1, y2])
return line,
ani = animation.FuncAnimation(fig, update, frames=range(time_steps), init_func=init, blit=True)
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# Define the function for the differential equation dy/dt = -y
def func(y):
return -y
# Parameters
n = 20
h = 0.1
t = np.zeros(n)
y = np.zeros(n)
# Initial conditions
t[0] = 0
y[0] = 1.0
# Runge-Kutta 4th order method
for k in range(n - 1):
L1 = h * func(y[k]) # 1st stage
L2 = h * func(y[k] + L1 / 2.0) # 2nd stage
L3 = h * func(y[k] + L2 / 2.0) # 3rd stage
L4 = h * func(y[k] + L3) # 4th stage
y[k + 1] = y[k] + (L1 + 2.0 * L2 + 2.0 * L3 + L4) / 6.0
t[k + 1] = t[k] + h
print(f"t[{k}]={t[k]:.6f}, y[{k}]={y[k]:.6f}, e-t={np.exp(-t[k]):.6f}")
# Plotting the results
plt.plot(t, y, 'b-', label='Runge-Kutta Solution')
plt.plot(t, np.exp(-t), 'r--', label='Exact Solution')
plt.xlabel('t')
plt.ylabel('y')
plt.title('Runge-Kutta Method vs Exact Solution')
plt.legend()
plt.grid()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# リンクの長さ
l1 = 1.0
l2 = 1.0
# 関節角度の範囲
theta1 = np.linspace(-np.pi, np.pi, 100)
theta2 = np.linspace(-np.pi, np.pi, 100)
# エンドエフェクターの位置を計算
X = l1 * np.cos(theta1[:, None]) + l2 * np.cos(theta1[:, None] + theta2[None, :])
Y = l1 * np.sin(theta1[:, None]) + l2 * np.sin(theta1[:, None] + theta2[None, :])
# プロット
plt.figure(figsize=(8, 8))
plt.plot(X, Y, 'o', markersize=1)
plt.title('2-Link Robot Arm End-Effector Positions')
plt.xlabel('X position')
plt.ylabel('Y position')
plt.grid(True)
plt.axis('equal')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection
# Function to create a rotation matrix from roll, pitch, yaw
def rotation_matrix(roll, pitch, yaw):
R_x = np.array([[1, 0, 0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]])
R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)],
[0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]])
R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]])
return np.dot(R_z, np.dot(R_y, R_x))
# Create a cube
def create_cube():
points = np.array([[-1, -1, -1],
[1, -1, -1],
[1, 1, -1],
[-1, 1, -1],
[-1, -1, 1],
[1, -1, 1],
[1, 1, 1],
[-1, 1, 1]])
return points
# Rotate the cube
def rotate_cube(points, roll, pitch, yaw):
R = rotation_matrix(roll, pitch, yaw)
return np.dot(points, R.T)
# Plot the cube
def plot_cube(points, ax):
vertices = [[points[j] for j in [0, 1, 2, 3]],
[points[j] for j in [4, 5, 6, 7]],
[points[j] for j in [0, 1, 5, 4]],
[points[j] for j in [2, 3, 7, 6]],
[points[j] for j in [1, 2, 6, 5]],
[points[j] for j in [4, 7, 3, 0]]]
faces = Poly3DCollection(vertices, linewidths=1, edgecolors='k')
faces.set_facecolor((0,0,1,0.1))
ax.add_collection3d(faces)
# Initialize the figure
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# Create and plot original cube
cube = create_cube()
plot_cube(cube, ax)
# Define roll, pitch, yaw angles in radians
roll = np.radians(30)
pitch = np.radians(45)
yaw = np.radians(60)
# Rotate and plot the cube
rotated_cube = rotate_cube(cube, roll, pitch, yaw)
plot_cube(rotated_cube, ax)
# Set the axes limits
ax.set_xlim([-2, 2])
ax.set_ylim([-2, 2])
ax.set_zlim([-2, 2])
# Labels and title
ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')
ax.set_title('Cube Rotation: Roll, Pitch, Yaw')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# Function to create a direction cosine matrix from an angle
def direction_cosine_matrix(theta):
return np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
# Function to perform forward kinematics of a 2-link robotic arm
def forward_kinematics(l1, l2, theta1, theta2):
# Base frame to first joint
R0_1 = direction_cosine_matrix(theta1)
T0_1 = np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
# First joint to end effector
R1_2 = direction_cosine_matrix(theta2)
T1_2 = np.array([l2 * np.cos(theta2), l2 * np.sin(theta2)])
# Compute the position of the end effector
end_effector_pos = T0_1 + R0_1.dot(T1_2)
return T0_1, end_effector_pos
# Link lengths
l1 = 1.0
l2 = 1.0
# Angles in radians
theta1 = np.radians(45)
theta2 = np.radians(30)
# Compute the position of the end effector
joint_pos, end_effector_pos = forward_kinematics(l1, l2, theta1, theta2)
# Plot the robotic arm
plt.figure()
plt.plot([0, joint_pos[0]], [0, joint_pos[1]], 'r-o', label='Link 1')
plt.plot([joint_pos[0], end_effector_pos[0]], [joint_pos[1], end_effector_pos[1]], 'b-o', label='Link 2')
plt.scatter(end_effector_pos[0], end_effector_pos[1], color='g', label='End Effector')
plt.xlim(-2, 2)
plt.ylim(-2, 2)
plt.axhline(0, color='black', linewidth=0.5)
plt.axvline(0, color='black', linewidth=0.5)
plt.grid(color='gray', linestyle='--', linewidth=0.5)
plt.legend()
plt.title('2-Link Robotic Arm Forward Kinematics')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# Function to solve inverse kinematics for a 2-link robotic arm
def inverse_kinematics(l1, l2, target):
x, y = target
# Calculate the distance from the base to the target
r = np.sqrt(x**2 + y**2)
if r > l1 + l2:
raise ValueError("Target is out of reach")
# Calculate the angle of the second joint
cos_theta2 = (r**2 - l1**2 - l2**2) / (2 * l1 * l2)
sin_theta2 = np.sqrt(1 - cos_theta2**2)
theta2 = np.arctan2(sin_theta2, cos_theta2)
# Calculate the angle of the first joint
k1 = l1 + l2 * cos_theta2
k2 = l2 * sin_theta2
theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
return theta1, theta2
# Function to perform forward kinematics of a 2-link robotic arm
def forward_kinematics(l1, l2, theta1, theta2):
# Calculate the positions of the joints
joint1 = np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
end_effector = joint1 + np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)])
return joint1, end_effector
# Link lengths
l1 = 1.0
l2 = 1.0
# Target trajectory (circle)
t = np.linspace(0, 2*np.pi, 100)
x = 1.5 * np.cos(t)
y = 1.5 * np.sin(t)
# Solve inverse kinematics for each point in the trajectory
theta1_list = []
theta2_list = []
joint1_list = []
end_effector_list = []
for target in zip(x, y):
try:
theta1, theta2 = inverse_kinematics(l1, l2, target)
joint1, end_effector = forward_kinematics(l1, l2, theta1, theta2)
theta1_list.append(theta1)
theta2_list.append(theta2)
joint1_list.append(joint1)
end_effector_list.append(end_effector)
except ValueError as e:
print(e)
# Convert lists to numpy arrays for plotting
joint1_list = np.array(joint1_list)
end_effector_list = np.array(end_effector_list)
# Plot the robotic arm following the trajectory
plt.figure()
for joint1, end_effector in zip(joint1_list, end_effector_list):
plt.plot([0, joint1[0]], [0, joint1[1]], 'r-o')
plt.plot([joint1[0], end_effector[0]], [joint1[1], end_effector[1]], 'b-o')
plt.plot(x, y, 'g--', label='Target Trajectory')
plt.scatter(end_effector_list[:, 0], end_effector_list[:, 1], color='g', label='End Effector Path')
plt.xlim(-2, 2)
plt.ylim(-2, 2)
plt.axhline(0, color='black', linewidth=0.5)
plt.axvline(0, color='black', linewidth=0.5)
plt.grid(color='gray', linestyle='--', linewidth=0.5)
plt.legend()
plt.title('2-Link Robotic Arm Inverse Kinematics')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# Function to compute the Jacobian matrix for a 2-DOF robotic arm
def jacobian(l1, l2, theta1, theta2):
J = np.array([
[-l1 * np.sin(theta1) - l2 * np.sin(theta1 + theta2), -l2 * np.sin(theta1 + theta2)],
[l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2), l2 * np.cos(theta1 + theta2)]
])
return J
# Function to perform forward kinematics of a 2-link robotic arm
def forward_kinematics(l1, l2, theta1, theta2):
joint1 = np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
end_effector = joint1 + np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)])
return joint1, end_effector
# Function to compute the inverse kinematics for a 2-DOF robotic arm
def inverse_kinematics(l1, l2, target):
x, y = target
r = np.sqrt(x**2 + y**2)
if r > l1 + l2:
raise ValueError("Target is out of reach")
cos_theta2 = (r**2 - l1**2 - l2**2) / (2 * l1 * l2)
sin_theta2 = np.sqrt(1 - cos_theta2**2)
theta2 = np.arctan2(sin_theta2, cos_theta2)
k1 = l1 + l2 * cos_theta2
k2 = l2 * sin_theta2
theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
return theta1, theta2
# Function to compute torques using the Jacobian transpose method
def compute_torques(J, F):
return J.T.dot(F)
# Link lengths
l1 = 1.0
l2 = 1.0
# Target trajectory (circle)
t = np.linspace(0, 2 * np.pi, 100)
x = 1.5 * np.cos(t)
y = 1.5 * np.sin(t)
# External force applied to the end effector (for simplicity, assume constant force)
F = np.array([0, -9.81]) # Simulating gravity acting on the end effector
# Solve inverse kinematics for each point in the trajectory
theta1_list = []
theta2_list = []
torque1_list = []
torque2_list = []
joint1_list = []
end_effector_list = []
for target in zip(x, y):
try:
theta1, theta2 = inverse_kinematics(l1, l2, target)
J = jacobian(l1, l2, theta1, theta2)
torques = compute_torques(J, F)
joint1, end_effector = forward_kinematics(l1, l2, theta1, theta2)
theta1_list.append(theta1)
theta2_list.append(theta2)
torque1_list.append(torques[0])
torque2_list.append(torques[1])
joint1_list.append(joint1)
end_effector_list.append(end_effector)
except ValueError as e:
print(e)
# Convert lists to numpy arrays for plotting
joint1_list = np.array(joint1_list)
end_effector_list = np.array(end_effector_list)
# Plot the robotic arm following the trajectory
plt.figure()
for joint1, end_effector in zip(joint1_list, end_effector_list):
plt.plot([0, joint1[0]], [0, joint1[1]], 'r-o')
plt.plot([joint1[0], end_effector[0]], [joint1[1], end_effector[1]], 'b-o')
plt.plot(x, y, 'g--', label='Target Trajectory')
plt.scatter(end_effector_list[:, 0], end_effector_list[:, 1], color='g', label='End Effector Path')
plt.xlim(-2, 2)
plt.ylim(-2, 2)
plt.axhline(0, color='black', linewidth=0.5)
plt.axvline(0, color='black', linewidth=0.5)
plt.grid(color='gray', linestyle='--', linewidth=0.5)
plt.legend()
plt.title('2-Link Robotic Arm Inverse Kinematics and Trajectory Control')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.show()
# Plot the torques
plt.figure()
plt.plot(t, torque1_list, label='Torque 1')
plt.plot(t, torque2_list, label='Torque 2')
plt.title('Joint Torques Over Time')
plt.xlabel('Time')
plt.ylabel('Torque')
plt.legend()
plt.grid()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# Function to compute the Jacobian matrix for a 2-DOF robotic arm
def jacobian(l1, l2, theta1, theta2):
J = np.array([
[-l1 * np.sin(theta1) - l2 * np.sin(theta1 + theta2), -l2 * np.sin(theta1 + theta2)],
[l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2), l2 * np.cos(theta1 + theta2)]
])
return J
# Function to perform forward kinematics of a 2-link robotic arm
def forward_kinematics(l1, l2, theta1, theta2):
joint1 = np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)])
end_effector = joint1 + np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)])
return joint1, end_effector
# Function to compute the inverse kinematics for a 2-DOF robotic arm
def inverse_kinematics(l1, l2, target):
x, y = target
r = np.sqrt(x**2 + y**2)
if r > l1 + l2:
raise ValueError("Target is out of reach")
cos_theta2 = (r**2 - l1**2 - l2**2) / (2 * l1 * l2)
sin_theta2 = np.sqrt(1 - cos_theta2**2)
theta2 = np.arctan2(sin_theta2, cos_theta2)
k1 = l1 + l2 * cos_theta2
k2 = l2 * sin_theta2
theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
return theta1, theta2
# Function to compute torques using the Lagrangian method
def lagrangian_dynamics(l1, l2, m1, m2, I1, I2, theta1, theta2, dtheta1, dtheta2, ddtheta1, ddtheta2):
g = 9.81 # acceleration due to gravity
# Kinetic energy
T1 = 0.5 * I1 * dtheta1**2 + 0.5 * m1 * (l1/2)**2 * dtheta1**2
T2 = 0.5 * I2 * (dtheta1 + dtheta2)**2 + 0.5 * m2 * (l1**2 * dtheta1**2 + l2**2 * (dtheta1 + dtheta2)**2 + 2 * l1 * l2 * dtheta1 * (dtheta1 + dtheta2) * np.cos(theta2))
T = T1 + T2
# Potential energy
V1 = m1 * g * (l1/2) * np.cos(theta1)
V2 = m2 * g * (l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2))
V = V1 + V2
# Lagrangian
L = T - V
# Compute partial derivatives for torques
# For simplicity, the following equations are based on symbolic differentiation
# where we assume ddtheta1 and ddtheta2 are known inputs
tau1 = (m1 * (l1/2) * g * np.sin(theta1) + m2 * l1 * g * np.sin(theta1) + I1 * ddtheta1 +
I2 * (ddtheta1 + ddtheta2) + m2 * l2 * g * np.sin(theta1 + theta2) + m2 * l2 * ddtheta2)
tau2 = (I2 * ddtheta2 + m2 * l2 * g * np.sin(theta1 + theta2))
return tau1, tau2
# Link lengths and masses
l1 = 1.0
l2 = 1.0
m1 = 1.0
m2 = 1.0
I1 = 0.1 # Moment of inertia for link 1
I2 = 0.1 # Moment of inertia for link 2
# Target trajectory (circle)
t = np.linspace(0, 2 * np.pi, 100)
x = 1.5 * np.cos(t)
y = 1.5 * np.sin(t)
# Solve inverse kinematics for each point in the trajectory
theta1_list = []
theta2_list = []
torque1_list = []
torque2_list = []
joint1_list = []
end_effector_list = []
# Compute joint angles for each target position
theta1 = np.zeros_like(t)
theta2 = np.zeros_like(t)
for i, target in enumerate(zip(x, y)):
try:
theta1[i], theta2[i] = inverse_kinematics(l1, l2, target)
except ValueError as e:
print(e)
# Compute the time derivatives of angles
dtheta1 = np.gradient(theta1, t)
dtheta2 = np.gradient(theta2, t)
ddtheta1 = np.gradient(dtheta1, t)
ddtheta2 = np.gradient(dtheta2, t)
# Compute torques
for i in range(len(t)):
tau1, tau2 = lagrangian_dynamics(l1, l2, m1, m2, I1, I2, theta1[i], theta2[i], dtheta1[i], dtheta2[i], ddtheta1[i], ddtheta2[i])
joint1, end_effector = forward_kinematics(l1, l2, theta1[i], theta2[i])
theta1_list.append(theta1[i])
theta2_list.append(theta2[i])
torque1_list.append(tau1)
torque2_list.append(tau2)
joint1_list.append(joint1)
end_effector_list.append(end_effector)
# Convert lists to numpy arrays for plotting
joint1_list = np.array(joint1_list)
end_effector_list = np.array(end_effector_list)
# Plot the robotic arm following the trajectory
plt.figure()
for joint1, end_effector in zip(joint1_list, end_effector_list):
plt.plot([0, joint1[0]], [0, joint1[1]], 'r-o')
plt.plot([joint1[0], end_effector[0]], [joint1[1], end_effector[1]], 'b-o')
plt.plot(x, y, 'g--', label='Target Trajectory')
plt.scatter(end_effector_list[:, 0], end_effector_list[:, 1], color='g', label='End Effector Path')
plt.xlim(-2, 2)
plt.ylim(-2, 2)
plt.axhline(0, color='black', linewidth=0.5)
plt.axvline(0, color='black', linewidth=0.5)
plt.grid(color='gray', linestyle='--', linewidth=0.5)
plt.legend()
plt.title('2-Link Robotic Arm Inverse Kinematics and Trajectory Control')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.show()
# Plot the torques
plt.figure()
plt.plot(t, torque1_list, label='Torque 1')
plt.plot(t, torque2_list, label='Torque 2')
plt.title('Joint Torques Over Time')
plt.xlabel('Time')
plt.ylabel('Torque')
plt.legend()
plt.grid()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# リンクの長さ
L1 = 1.0
L2 = 1.0
def forward_kinematics(theta1, theta2):
# 関節角度からエンドエフェクタの位置を計算
x = L1 * np.cos(theta1) + L2 * np.cos(theta1 + theta2)
y = L1 * np.sin(theta1) + L2 * np.sin(theta1 + theta2)
return x, y
# 関節角度の設定
theta1 = np.radians(45)
theta2 = np.radians(30)
# エンドエフェクタの位置を計算
x, y = forward_kinematics(theta1, theta2)
# 結果のプロット
plt.figure(figsize=(8, 8))
plt.plot([0, L1 * np.cos(theta1), x], [0, L1 * np.sin(theta1), y], 'ro-')
plt.xlim(-2, 2)
plt.ylim(-2, 2)
plt.xlabel('X')
plt.ylabel('Y')
plt.title('2-DOF Robot Arm Forward Kinematics')
plt.grid()
plt.gca().set_aspect('equal', adjustable='box')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
# リンクの長さと質量
L1 = 1.0
L2 = 1.0
m1 = 1.0
m2 = 1.0
g = 9.81 # 重力加速度
# ニュートン-オイラー方程式によるロボットの動力学モデル
def newton_euler_dynamics(t, y):
theta1, theta2, omega1, omega2 = y
# ロボットのパラメータ
I1 = m1 * L1**2 / 12
I2 = m2 * (L2**2 / 12 + L1**2 + 2 * L1 * L2 * np.cos(theta2))
# モーメント方程式
H11 = I1 + I2 + m2 * L1**2 + 2 * m2 * L1 * L2 * np.cos(theta2)
H12 = I2 + m2 * L1 * L2 * np.cos(theta2)
H21 = H12
H22 = I2
C1 = -m2 * L1 * L2 * np.sin(theta2) * omega2
C2 = m2 * L1 * L2 * np.sin(theta2) * omega1
G1 = (m1 * L1 + m2 * L1) * g * np.cos(theta1) + m2 * g * L2 * np.cos(theta1 + theta2)
G2 = m2 * g * L2 * np.cos(theta1 + theta2)
# 加速度の計算
ddtheta1 = (1 / H11) * (G1 - C1 - H12 * omega2**2)
ddtheta2 = (1 / H22) * (G2 - C2 - H12 * omega1 * omega2)
return [omega1, omega2, ddtheta1, ddtheta2]
# 初期条件
theta1_0 = np.pi / 4
theta2_0 = np.pi / 4
omega1_0 = 0
omega2_0 = 0
y0 = [theta1_0, theta2_0, omega1_0, omega2_0]
# 時間範囲
t_span = (0, 10)
t_eval = np.linspace(t_span[0], t_span[1], 500)
# 微分方程式の解法
sol = solve_ivp(newton_euler_dynamics, t_span, y0, t_eval=t_eval, method='RK45')
# 結果のプロット
plt.figure(figsize=(12, 6))
# 関節角度のプロット
plt.subplot(2, 1, 1)
plt.plot(sol.t, sol.y[0], label='Theta1 (rad)')
plt.plot(sol.t, sol.y[1], label='Theta2 (rad)')
plt.xlabel('Time (s)')
plt.ylabel('Angle (rad)')
plt.title('Joint Angles over Time')
plt.legend()
plt.grid()
# 角速度のプロット
plt.subplot(2, 1, 2)
plt.plot(sol.t, sol.y[2], label='Omega1 (rad/s)')
plt.plot(sol.t, sol.y[3], label='Omega2 (rad/s)')
plt.xlabel('Time (s)')
plt.ylabel('Angular Velocity (rad/s)')
plt.title('Joint Angular Velocities over Time')
plt.legend()
plt.grid()
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# リンクの長さ
L1 = 1.0
L2 = 1.0
def inverse_kinematics(x, y):
"""
逆運動学の計算を行う関数。
(x, y): 末端エフェクタの目標位置。
戻り値: 関節角度 (theta1, theta2)。
"""
# 目標位置の計算
r = np.sqrt(x**2 + y**2)
if r > L1 + L2:
raise ValueError("目標位置が到達範囲を超えています")
# 関節角度の計算
cos_theta2 = (x**2 + y**2 - L1**2 - L2**2) / (2 * L1 * L2)
theta2 = np.arccos(cos_theta2)
k1 = L1 + L2 * np.cos(theta2)
k2 = L2 * np.sin(theta2)
theta1 = np.arctan2(y, x) - np.arctan2(k2, k1)
return theta1, theta2
# 目標位置の設定
x_target = 1.5
y_target = 0.5
# 逆運動学の計算
theta1, theta2 = inverse_kinematics(x_target, y_target)
# 関節角度の出力
print(f"関節角度 theta1: {np.degrees(theta1):.2f}°")
print(f"関節角度 theta2: {np.degrees(theta2):.2f}°")
# 末端エフェクタの位置をプロット
plt.figure(figsize=(8, 6))
plt.plot([0, L1 * np.cos(theta1), x_target], [0, L1 * np.sin(theta1), y_target], 'bo-')
plt.plot(x_target, y_target, 'ro', label='Target Position')
plt.xlabel('X position')
plt.ylabel('Y position')
plt.title('Inverse Kinematics for 2-Link Robot Arm')
plt.legend()
plt.grid()
plt.axis('equal')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp
from scipy.signal import lti, step
# ロボットパラメータ
L1 = 1.0 # リンク1の長さ
L2 = 1.0 # リンク2の長さ
m1 = 1.0 # リンク1の質量
m2 = 1.0 # リンク2の質量
g = 9.81 # 重力加速度
# ラグランジュ方程式の動力学モデル
def dynamics(t, y, u):
theta1, theta2, omega1, omega2 = y
u1, u2 = u
# 運動エネルギー
T1 = 0.5 * m1 * (L1 * omega1)**2
T2 = 0.5 * m2 * (L1**2 * omega1**2 + L2**2 * omega2**2 + 2 * L1 * L2 * omega1 * omega2 * np.cos(theta2))
T = T1 + T2
# ポテンシャルエネルギー
V1 = m1 * g * (L1 / 2) * np.cos(theta1)
V2 = m2 * g * (L1 * np.cos(theta1) + L2 / 2 * np.cos(theta1 + theta2))
V = V1 + V2
# 運動方程式
M11 = m1 * L1**2 + m2 * (L1**2 + L2**2 + 2 * L1 * L2 * np.cos(theta2))
M12 = m2 * (L2**2 + L1 * L2 * np.cos(theta2))
M21 = M12
M22 = m2 * L2**2
C1 = -m2 * L1 * L2 * np.sin(theta2) * omega2
C2 = m2 * L1 * L2 * np.sin(theta2) * omega1
G1 = -m1 * g * (L1 / 2) * np.sin(theta1) - m2 * g * (L1 * np.sin(theta1) + L2 / 2 * np.sin(theta1 + theta2))
G2 = -m2 * g * (L2 / 2) * np.sin(theta1 + theta2)
dtheta1_dt = omega1
dtheta2_dt = omega2
domega1_dt = (u1 - C1 - G1) / M11
domega2_dt = (u2 - C2 - G2) / M22
return [dtheta1_dt, dtheta2_dt, domega1_dt, domega2_dt]
# PID制御パラメータ
Kp = 10
Ki = 0.1
Kd = 1
# 制御信号の計算
def pid_control(t, setpoint, y, integral, prev_error):
theta1, theta2, omega1, omega2 = y
error = setpoint - theta1
integral += error * t
derivative = (error - prev_error) / t if t > 0 else 0
u = Kp * error + Ki * integral + Kd * derivative
return u, integral, error
# 初期条件
theta1_0 = 0
theta2_0 = 0
omega1_0 = 0
omega2_0 = 0
y0 = [theta1_0, theta2_0, omega1_0, omega2_0]
setpoint = 1.0 # 目標位置
t_span = (0, 10)
t_eval = np.linspace(t_span[0], t_span[1], 500)
integral = 0
prev_error = 0
# PID制御とシミュレーション
def controlled_dynamics(t, y):
global integral, prev_error
u, integral, prev_error = pid_control(t, setpoint, y, integral, prev_error)
return dynamics(t, y, [u, u])
sol = solve_ivp(controlled_dynamics, t_span, y0, t_eval=t_eval)
# 結果のプロット
plt.figure(figsize=(12, 6))
# 関節角度のプロット
plt.subplot(2, 1, 1)
plt.plot(sol.t, sol.y[0], label='Theta1')
plt.plot(sol.t, sol.y[1], label='Theta2')
plt.xlabel('Time')
plt.ylabel('Angle (radians)')
plt.title('Joint Angles Over Time')
plt.legend()
plt.grid()
# 角速度のプロット
plt.subplot(2, 1, 2)
plt.plot(sol.t, sol.y[2], label='Omega1')
plt.plot(sol.t, sol.y[3], label='Omega2')
plt.xlabel('Time')
plt.ylabel('Angular Velocity (rad/s)')
plt.title('Joint Angular Velocities Over Time')
plt.legend()
plt.grid()
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# パラメータ設定
k = 10 # バネ定数 [N/m]
F_external = 5 # 外部力 [N]
x_original = 1 # 元の位置 [m]
# 仮想変位の範囲
delta_x = np.linspace(-0.5, 0.5, 100) # 仮想変位 [m]
# 仮想仕事の計算
virtual_work_spring = -k * x_original * delta_x
virtual_work_external = F_external * delta_x
# プロット
plt.figure(figsize=(10, 6))
# バネによる仮想仕事
plt.plot(delta_x, virtual_work_spring, label='Virtual Work by Spring Force', color='blue')
# 外部力による仮想仕事
plt.plot(delta_x, virtual_work_external, label='Virtual Work by External Force', color='red')
plt.xlabel('Virtual Displacement (m)')
plt.ylabel('Virtual Work (J)')
plt.title('Virtual Work Principle in 1D System')
plt.legend()
plt.grid(True)
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# パラメータ設定
l1 = 1.0 # リンク1の長さ [m]
l2 = 1.0 # リンク2の長さ [m]
# 角度の範囲
theta1 = np.linspace(0, 2 * np.pi, 100)
theta2 = np.linspace(0, 2 * np.pi, 100)
Theta1, Theta2 = np.meshgrid(theta1, theta2)
# エンドエフェクタの位置
x = l1 * np.cos(Theta1) + l2 * np.cos(Theta1 + Theta2)
y = l1 * np.sin(Theta1) + l2 * np.sin(Theta1 + Theta2)
# ヤコビ行列の計算
def jacobian(theta1, theta2):
J = np.array([
[-l1 * np.sin(theta1) - l2 * np.sin(theta1 + theta2), -l2 * np.sin(theta1 + theta2)],
[l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2), l2 * np.cos(theta1 + theta2)]
])
return J
# 特異点の評価
def singularity_condition(J):
return np.linalg.det(J) == 0
# 特異点のプロット
singularities = np.zeros_like(x)
for i in range(len(theta1)):
for j in range(len(theta2)):
J = jacobian(theta1[i], theta2[j])
if singularity_condition(J):
singularities[i, j] = 1
# プロット
plt.figure(figsize=(12, 6))
plt.subplot(1, 2, 1)
plt.contourf(Theta1, Theta2, x, cmap='viridis')
plt.colorbar(label='x-coordinate')
plt.title('End-Effector X Position')
plt.subplot(1, 2, 2)
plt.contourf(Theta1, Theta2, singularities, cmap='coolwarm', levels=[0, 1])
plt.colorbar(label='Singularity')
plt.title('Singularity Map')
plt.xlabel('Theta1 (rad)')
plt.ylabel('Theta2 (rad)')
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
# 7.4.2 Decoupled Acceleration Control
# Parameters
mass = 1.0 # Mass [kg]
Kp = 1.0 # Proportional gain
Kd = 0.5 # Derivative gain
# Desired acceleration
a_desired = np.array([1.0, 0.5]) # Desired acceleration in x and y directions
# System dynamics
def system_dynamics(y, t):
x, v_x, y, v_y = y
a_x, a_y = a_desired
# Acceleration control law
u_x = Kp * (a_x - v_x) + Kd * (0 - v_x) # Control input for x
u_y = Kp * (a_y - v_y) + Kd * (0 - v_y) # Control input for y
# State equations
dxdt = v_x
dv_xdt = u_x / mass
dydt = v_y
dv_ydt = u_y / mass
return [dxdt, dv_xdt, dydt, dv_ydt]
# Initial conditions
y0 = [0, 0, 0, 0] # Initial position and velocity
# Time vector
t = np.linspace(0, 10, 100)
# Solve ODE
solution = odeint(system_dynamics, y0, t)
x = solution[:, 0]
y = solution[:, 2]
# Plot
plt.figure(figsize=(12, 6))
plt.subplot(1, 2, 1)
plt.plot(t, x, label='Position x')
plt.plot(t, y, label='Position y')
plt.xlabel('Time [s]')
plt.ylabel('Position [m]')
plt.title('Decoupled Acceleration Control')
plt.legend()
plt.grid(True)
# 7.5 Adaptive Control
# Adaptive control parameters
adaptive_gain = 0.1
error = np.zeros_like(t)
adaptive_control_signal = np.zeros_like(t)
# Simulate adaptive control
for i in range(1, len(t)):
error[i] = a_desired[0] - solution[i, 1] # Error in x direction
adaptive_control_signal[i] = adaptive_gain * error[i]
# Plot
plt.subplot(1, 2, 2)
plt.plot(t, adaptive_control_signal, label='Adaptive Control Signal', color='red')
plt.xlabel('Time [s]')
plt.ylabel('Control Signal')
plt.title('Adaptive Control')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
# 8.3 Hybrid Control
# Parameters for position and force control
mass = 1.0 # Mass [kg]
Kp_pos = 1.0 # Proportional gain for position control
Kd_pos = 0.5 # Derivative gain for position control
Kp_force = 0.5 # Proportional gain for force control
Kd_force = 0.2 # Derivative gain for force control
# Desired position and force
position_desired = 1.0 # Desired position [m]
force_desired = 2.0 # Desired force [N]
# System dynamics
def hybrid_system_dynamics(y, t):
x, v_x = y
# Position control
u_pos = Kp_pos * (position_desired - x) + Kd_pos * (0 - v_x) # Control input for position
# Force control
force = u_pos * mass
u_force = Kp_force * (force_desired - force) + Kd_force * (0 - force) # Control input for force
# State equations
dxdt = v_x
dv_xdt = (u_pos + u_force) / mass
return [dxdt, dv_xdt]
# Initial conditions
y0 = [0, 0] # Initial position and velocity
# Time vector
t = np.linspace(0, 10, 100)
# Solve ODE
solution = odeint(hybrid_system_dynamics, y0, t)
x = solution[:, 0]
# 8.4 Dynamic Force Control Using Decoupled Acceleration Method
# Parameters
Kp_acc = 1.0 # Proportional gain for acceleration control
Kd_acc = 0.5 # Derivative gain for acceleration control
# Desired acceleration
a_desired = 1.0 # Desired acceleration [m/s^2]
# System dynamics for force control
def dynamic_force_system_dynamics(y, t):
x, v_x = y
# Acceleration control
u_acc = Kp_acc * (a_desired - v_x) + Kd_acc * (0 - v_x) # Control input for acceleration
# State equations
dxdt = v_x
dv_xdt = u_acc
return [dxdt, dv_xdt]
# Initial conditions
y0_acc = [0, 0] # Initial position and velocity
# Solve ODE for force control
solution_acc = odeint(dynamic_force_system_dynamics, y0_acc, t)
x_acc = solution_acc[:, 0]
# Plot
plt.figure(figsize=(12, 6))
# Plot for Hybrid Control
plt.subplot(1, 2, 1)
plt.plot(t, x, label='Position')
plt.xlabel('Time [s]')
plt.ylabel('Position [m]')
plt.title('Hybrid Control')
plt.legend()
plt.grid(True)
# Plot for Dynamic Force Control
plt.subplot(1, 2, 2)
plt.plot(t, x_acc, label='Position with Dynamic Force Control', color='red')
plt.xlabel('Time [s]')
plt.ylabel('Position [m]')
plt.title('Dynamic Force Control')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
# 8.4 Dynamic Force Control Using Decoupled Acceleration Method
# Parameters
mass = 1.0 # Mass [kg]
Kp_acc = 1.0 # Proportional gain for acceleration control
Kd_acc = 0.5 # Derivative gain for acceleration control
# Desired acceleration
a_desired = 1.0 # Desired acceleration [m/s^2]
# Define system dynamics for force control
def dynamic_force_system_dynamics(y, t):
x, v_x = y
# Acceleration control
u_acc = Kp_acc * (a_desired - v_x) + Kd_acc * (0 - v_x) # Control input for acceleration
# State equations
dxdt = v_x
dv_xdt = u_acc
return [dxdt, dv_xdt]
# Initial conditions
y0_acc = [0, 0] # Initial position and velocity
# Time vector
t = np.linspace(0, 10, 100)
# Solve ODE for force control
solution_acc = odeint(dynamic_force_system_dynamics, y0_acc, t)
x_acc = solution_acc[:, 0]
v_x_acc = solution_acc[:, 1]
# Plot results
plt.figure(figsize=(12, 6))
# Position Plot
plt.subplot(1, 2, 1)
plt.plot(t, x_acc, label='Position')
plt.xlabel('Time [s]')
plt.ylabel('Position [m]')
plt.title('Position vs. Time (Dynamic Force Control)')
plt.legend()
plt.grid(True)
# Velocity Plot
plt.subplot(1, 2, 2)
plt.plot(t, v_x_acc, label='Velocity', color='red')
plt.xlabel('Time [s]')
plt.ylabel('Velocity [m/s]')
plt.title('Velocity vs. Time (Dynamic Force Control)')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
# Impedance control parameters
K = 100.0 # Stiffness [N/m]
B = 10.0 # Damping [Ns/m]
M = 1.0 # Mass [kg]
# Desired position and velocity
position_desired = 1.0 # Desired position [m]
velocity_desired = 0.0 # Desired velocity [m/s]
# System dynamics
def impedance_system_dynamics(y, t):
x, v = y
# Impedance control forces
f_stiffness = K * (position_desired - x)
f_damping = B * (velocity_desired - v)
# Total control force
f_control = f_stiffness + f_damping
# State equations
dxdt = v
dvdt = f_control / M
return [dxdt, dvdt]
# Initial conditions
y0 = [0, 0] # Initial position and velocity
# Time vector
t = np.linspace(0, 10, 100)
# Solve ODE
solution = odeint(impedance_system_dynamics, y0, t)
x = solution[:, 0]
v = solution[:, 1]
# Plot results
plt.figure(figsize=(12, 6))
# Position Plot
plt.subplot(1, 2, 1)
plt.plot(t, x, label='Position')
plt.axhline(y=position_desired, color='red', linestyle='--', label='Desired Position')
plt.xlabel('Time [s]')
plt.ylabel('Position [m]')
plt.title('Position vs. Time (Impedance Control)')
plt.legend()
plt.grid(True)
# Velocity Plot
plt.subplot(1, 2, 2)
plt.plot(t, v, label='Velocity', color='green')
plt.axhline(y=velocity_desired, color='red', linestyle='--', label='Desired Velocity')
plt.xlabel('Time [s]')
plt.ylabel('Velocity [m/s]')
plt.title('Velocity vs. Time (Impedance Control)')
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# Parameters
mass = 1.0 # Mass [kg]
stiffness = 100.0 # Stiffness [N/m]
damping = 10.0 # Damping [Ns/m]
# Frequency range
frequencies = np.logspace(-1, 2, 500) # Frequencies from 0.1 to 100 Hz
omega = 2 * np.pi * frequencies # Angular frequencies [rad/s]
# Compute impedance
Z_real = (mass * omega**2 - stiffness) / (mass**2 * omega**2 + damping**2)
Z_imag = (damping * omega) / (mass**2 * omega**2 + damping**2)
# Magnitude and Phase
Z_magnitude = np.sqrt(Z_real**2 + Z_imag**2)
Z_phase = np.arctan2(Z_imag, Z_real)
# Plot impedance magnitude and phase
plt.figure(figsize=(12, 6))
# Magnitude Plot
plt.subplot(1, 2, 1)
plt.loglog(frequencies, Z_magnitude)
plt.xlabel('Frequency [Hz]')
plt.ylabel('Magnitude [Ω]')
plt.title('Mechanical Impedance Magnitude')
plt.grid(True, which='both', linestyle='--', linewidth=0.5)
# Phase Plot
plt.subplot(1, 2, 2)
plt.semilogx(frequencies, Z_phase * 180 / np.pi)
plt.xlabel('Frequency [Hz]')
plt.ylabel('Phase [degrees]')
plt.title('Mechanical Impedance Phase')
plt.grid(True, which='both', linestyle='--', linewidth=0.5)
plt.tight_layout()
plt.show()
import numpy as np
def canonical_transformation(q, p, A):
# q: 位置ベクトル
# p: 運動量ベクトル
# A: 変換行列
q_new = np.dot(A, q)
p_new = np.dot(np.linalg.inv(A).T, p)
return q_new, p_new
# 例として、2次元の位置と運動量、変換行列を定義
q = np.array([1.0, 2.0])
p = np.array([0.5, 1.0])
A = np.array([[0, 1], [1, 0]])
q_new, p_new = canonical_transformation(q, p, A)
print(f"変換後の位置: {q_new}")
print(f"変換後の運動量: {p_new}")
import numpy as np
import matplotlib.pyplot as plt
def hamilton_jacobi(x, t):
# ここでは、簡単な例として一つの変数xについての解を計算
return np.sin(x + t)
x = np.linspace(0, 2 * np.pi, 100)
t = 1.0
y = hamilton_jacobi(x, t)
plt.plot(x, y)
plt.xlabel('x')
plt.ylabel('Hamilton-Jacobi solution')
plt.title('Hamilton-Jacobi Equation Solution')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
def phase_space_trajectory(t):
# 例として簡単な調和振動子の運動
x = np.sin(t)
p = np.cos(t)
return x, p
t = np.linspace(0, 10, 100)
x, p = phase_space_trajectory(t)
plt.plot(x, p)
plt.xlabel('Position (x)')
plt.ylabel('Momentum (p)')
plt.title('Phase Space Trajectory')
plt.show()
import numpy as np
def canonical_coordinates_momentum(q, p):
# 例として、正準座標と正準運動量の単純な表示
return q, p
q = np.array([1.0, 2.0])
p = np.array([0.5, 1.0])
q_canon, p_canon = canonical_coordinates_momentum(q, p)
print(f"正準座標: {q_canon}")
print(f"正準運動量: {p_canon}")
from scipy.optimize import minimize
def action_functional(x):
# 簡単な例としての作用汎関数
return np.sum(x**2)
x0 = np.array([1.0, 2.0])
result = minimize(action_functional, x0)
print(f"最小化された作用汎関数の値: {result.fun}")
print(f"最適化された変数: {result.x}")
import sympy as sp
# 変数とラグランジアンの定義
t = sp.Symbol('t')
q = sp.Function('q')(t)
L = sp.Function('L')(q, sp.Derivative(q, t))
# オイラー・ラグランジュ方程式
dL_dq = sp.diff(L, q)
dL_dq_dot = sp.diff(L, sp.Derivative(q, t))
eq = sp.Eq(sp.diff(dL_dq_dot, t) - dL_dq, 0)
solution = sp.solve(eq, sp.Derivative(q, t, t))
print(f"オイラー・ラグランジュ方程式の解: {solution}")
import sympy as sp
# 変数とハミルトニアンの定義
q, p = sp.symbols('q p')
H = sp.Function('H')(q, p)
f = sp.Function('f')(q, p)
g = sp.Function('g')(q, p)
# ポアソン括弧の計算
poisson_bracket = sp.diff(f, q) * sp.diff(g, p) - sp.diff(f, p) * sp.diff(g, q)
print(f"ポアソン括弧: {poisson_bracket}")
import numpy as np
def calculate_jacobian_transpose(L1, L2, theta1, theta2):
"""
ヤコビ行列の転置を計算する関数
Parameters:
L1 (float): リンク1の長さ
L2 (float): リンク2の長さ
theta1 (float): 関節角度1（ラジアン）
theta2 (float): 関節角度2（ラジアン）
Returns:
np.array: ヤコビ行列の転置
"""
# 角度の三角関数
S1 = np.sin(theta1)
C1 = np.cos(theta1)
S12 = np.sin(theta1 + theta2)
C12 = np.cos(theta1 + theta2)
# ヤコビ行列の転置を計算
JT = np.array([
[-L1 * S1 - L2 * S12, L1 * C1 + L2 * C12],
[-L2 * S12, L2 * C12]
])
return JT
# 例: 与えられた条件で計算
L1 = 0.25 # リンク1の長さ
L2 = 0.3 # リンク2の長さ
theta1 = 0 # θ1 = 0
theta2 = np.pi / 2 # θ2 = π/2
JT = calculate_jacobian_transpose(L1, L2, theta1, theta2)
print(JT)
import numpy as np
def M_matrix(theta2, A1, A2, A3):
"""
慣性行列 M(θ) を計算する関数
"""
M = np.array([
[A1 + A2 + 2 * A3 * np.cos(theta2), A2 + A3 * np.cos(theta2)],
[A2 + A3 * np.cos(theta2), A2]
])
return M
def h_vector(theta2, theta_dot1, theta_dot2, A3):
"""
コリオリ力と遠心力のベクトル h(θ,θ̇) を計算する関数
"""
h = np.array([
-A3 * (2 * theta_dot1 * theta_dot2 + theta_dot2**2) * np.sin(theta2),
A3 * theta_dot1**2 * np.sin(theta2)
])
return h
def g_vector(theta1, theta2, B1, B2):
"""
重力ベクトル g(θ) を計算する関数
"""
g = np.array([
B1 * np.cos(theta1) + B2 * np.cos(theta1 + theta2),
B2 * np.cos(theta1 + theta2)
])
return g
def calculate_dynamics(theta1, theta2, theta_dot1, theta_dot2, theta_ddot1, theta_ddot2, A1, A2, A3, B1, B2, tau_L):
"""
動力学方程式 M(θ)θ̈ + h(θ,θ̇) + g(θ) = τ + τL を計算する関数
"""
theta_ddot = np.array([theta_ddot1, theta_ddot2])
tau_L = np.array(tau_L)
M = M_matrix(theta2, A1, A2, A3)
h = h_vector(theta2, theta_dot1, theta_dot2, A3)
g = g_vector(theta1, theta2, B1, B2)
tau = np.dot(M, theta_ddot) + h + g - tau_L
return tau
# 例のパラメータ
theta1 = 0.5
theta2 = 0.3
theta_dot1 = 0.1
theta_dot2 = 0.2
theta_ddot1 = 0.05
theta_ddot2 = 0.05
A1 = 1.0
A2 = 0.5
A3 = 0.3
B1 = 9.8
B2 = 9.8
tau_L = [0.1, 0.2]
# 動力学を計算
tau = calculate_dynamics(theta1, theta2, theta_dot1, theta_dot2, theta_ddot1, theta_ddot2, A1, A2, A3, B1, B2, tau_L)
print("計算されたトルク τ: ", tau)
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# Robot arm parameters
l1 = 1.0 # Length from the first joint to the second joint
l2 = 1.0 # Length from the second joint to the third joint
# Compute the Jacobian matrix
def compute_jacobian(theta1, theta2, theta3):
# Calculate the position of each joint
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
x2 = x1 + l2 * np.cos(theta1 + theta2)
y2 = y1 + l2 * np.sin(theta1 + theta2)
x3 = x2 + l2 * np.cos(theta1 + theta2 + theta3)
y3 = y2 + l2 * np.sin(theta1 + theta2 + theta3)
# Calculate the Jacobian matrix
J = np.array([
[-l1 * np.sin(theta1) - l2 * np.sin(theta1 + theta2) - l2 * np.sin(theta1 + theta2 + theta3),
-l2 * np.sin(theta1 + theta2) - l2 * np.sin(theta1 + theta2 + theta3),
-l2 * np.sin(theta1 + theta2 + theta3)],
[ l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2) + l2 * np.cos(theta1 + theta2 + theta3),
l2 * np.cos(theta1 + theta2) + l2 * np.cos(theta1 + theta2 + theta3),
l2 * np.cos(theta1 + theta2 + theta3)]
])
return J
# Compute the inverse kinematics
def inverse_kinematics(x_target, y_target):
# Initial guess
theta1 = np.pi / 4
theta2 = np.pi / 4
theta3 = np.pi / 4
learning_rate = 0.01
tolerance = 1e-6
max_iterations = 1000
for _ in range(max_iterations):
# Compute the Jacobian matrix
J = compute_jacobian(theta1, theta2, theta3)
# Calculate the end-effector position
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
x2 = x1 + l2 * np.cos(theta1 + theta2)
y2 = y1 + l2 * np.sin(theta1 + theta2)
x3 = x2 + l2 * np.cos(theta1 + theta2 + theta3)
y3 = y2 + l2 * np.sin(theta1 + theta2 + theta3)
# Difference between the end-effector position and target position
error = np.array([x_target - x3, y_target - y3])
# Pseudo-inverse of the Jacobian matrix
J_pseudo_inv = np.linalg.pinv(J)
# Update joint angles
delta_theta = J_pseudo_inv @ error
theta1 += learning_rate * delta_theta[0]
theta2 += learning_rate * delta_theta[1]
theta3 += learning_rate * delta_theta[2]
# Exit if the error is small enough
if np.linalg.norm(error) < tolerance:
break
return theta1, theta2, theta3
# Target position
x_target = 1.5
y_target = 1.0
# Execute inverse kinematics
theta1, theta2, theta3 = inverse_kinematics(x_target, y_target)
# Display the results
print(f"Joint angles: θ1 = {np.degrees(theta1):.2f}°, θ2 = {np.degrees(theta2):.2f}°, θ3 = {np.degrees(theta3):.2f}°")
# 3D plot
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# Plot the initial position and target position
x1 = l1 * np.cos(theta1)
y1 = l1 * np.sin(theta1)
x2 = x1 + l2 * np.cos(theta1 + theta2)
y2 = y1 + l2 * np.sin(theta1 + theta2)
x3 = x2 + l2 * np.cos(theta1 + theta2 + theta3)
y3 = y2 + l2 * np.sin(theta1 + theta2 + theta3)
# Plotting
ax.plot([0, x1, x2, x3], [0, y1, y2, y3], [0, 0, 0, 0], marker='o', label='Robot Arm')
ax.scatter([x_target], [y_target], [0], color='r', label='Target Position')
ax.set_xlabel('X-axis')
ax.set_ylabel('Y-axis')
ax.set_zlabel('Z-axis')
ax.set_title('3D Plot of Robot Arm')
ax.legend()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# Robot arm parameters
l = [1.0, 1.0, 1.0] # Lengths of the robot arm segments
n_dof = len(l) # Number of degrees of freedom
# Compute the Jacobian matrix for a multi-DOF robot arm
def compute_jacobian(theta):
J = np.zeros((2, n_dof))
x = 0
y = 0
for i in range(n_dof):
x += l[i] * np.cos(np.sum(theta[:i+1]))
y += l[i] * np.sin(np.sum(theta[:i+1]))
J[0, i] = -l[i] * np.sin(np.sum(theta[:i+1]))
J[1, i] = l[i] * np.cos(np.sum(theta[:i+1]))
return J
# Compute the torques required at each joint to counteract external forces
def compute_torques(theta, external_forces):
J = compute_jacobian(theta)
torques = J.T @ external_forces
return torques
# Compute the end-effector position
def forward_kinematics(theta):
x = 0
y = 0
for i in range(n_dof):
x += l[i] * np.cos(np.sum(theta[:i+1]))
y += l[i] * np.sin(np.sum(theta[:i+1]))
return np.array([x, y])
# Example usage
theta = np.array([np.pi / 4, np.pi / 4, np.pi / 4]) # Example joint angles
external_forces = np.array([0.5, 0.5]) # Example external forces (assumed 2D for simplicity)
# Calculate the Jacobian matrix
J = compute_jacobian(theta)
# Calculate the required torques
torques = compute_torques(theta, external_forces)
print(f"Torques required: {torques}")
# Calculate the end-effector position
end_effector_position = forward_kinematics(theta)
print(f"End-effector position: {end_effector_position}")
# 3D plot of the robot arm (simplified to 2D for visualization)
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# Joint positions
x_positions = [0]
y_positions = [0]
z_positions = [0]
x = 0
y = 0
for i in range(n_dof):
x += l[i] * np.cos(np.sum(theta[:i+1]))
y += l[i] * np.sin(np.sum(theta[:i+1]))
x_positions.append(x)
y_positions.append(y)
z_positions.append(0)
# Plot the robot arm
ax.plot(x_positions, y_positions, z_positions, marker='o', label='Robot Arm')
ax.scatter(end_effector_position[0], end_effector_position[1], 0, color='r', label='End-Effector')
ax.set_xlabel('X-axis')
ax.set_ylabel('Y-axis')
ax.set_zlabel('Z-axis')
ax.set_title('3D Plot of Multi-DOF Robot Arm')
ax.legend()
plt.show()
import numpy as np
# Define the parameters of the robot arm
m1, m2 = 1.0, 1.0 # Masses of the links
l1, l2 = 1.0, 1.0 # Lengths of the links
g = 9.81 # Acceleration due to gravity
# Define the joint angles and angular velocities
theta1, theta2 = np.pi / 4, np.pi / 4 # Joint angles
theta1_dot, theta2_dot = 0.0, 0.0 # Angular velocities
theta1_ddot, theta2_ddot = 0.0, 0.0 # Angular accelerations
# Compute the forces and torques
def newton_equations(theta1, theta2, theta1_dot, theta2_dot, theta1_ddot, theta2_ddot):
# Compute the forces (torques) on each joint
T1 = (m1 * g * l1 / 2 * np.cos(theta1) +
m2 * g * (l1 * np.cos(theta1) + l2 / 2 * np.cos(theta1 + theta2)))
T2 = m2 * g * l2 / 2 * np.cos(theta1 + theta2)
# Calculate the torques required to achieve desired accelerations
tau1 = T1 - m1 * l1**2 * theta1_ddot - m2 * (l1**2 + l2**2 + 2 * l1 * l2 * np.cos(theta2)) * theta1_ddot
tau2 = T2 - m2 * l2**2 * theta2_ddot
return np.array([tau1, tau2])
# Compute the torques
torques = newton_equations(theta1, theta2, theta1_dot, theta2_dot, theta1_ddot, theta2_ddot)
print(f"Torques: {torques}")
# Define robot parameters and state variables
n_links = 2 # Number of links
m = [1.0, 1.0] # Masses of the links
l = [1.0, 1.0] # Lengths of the links
r = [l[0] / 2, l[1] / 2] # Lengths to the center of mass
g = 9.81 # Acceleration due to gravity
# Define joint angles, velocities, and accelerations
theta = np.array([np.pi / 4, np.pi / 4])
theta_dot = np.array([0.0, 0.0])
theta_ddot = np.array([0.0, 0.0])
# Initialize forces and torques
forces = np.zeros(n_links)
torques = np.zeros(n_links)
# Compute forces and torques using the Newton-Euler method
for i in range(n_links):
# Compute forces due to gravity
F_g = m[i] * g
# Compute torques for current link
torque = F_g * r[i] * np.sin(theta[i]) # Simplified model for demonstration
forces[i] = F_g
torques[i] = torque
print(f"Gravitational Forces: {forces}")
print(f"Torques: {torques}")
import sympy as sp
import numpy as np
import matplotlib.pyplot as plt
# Define symbolic variables
theta1, theta2 = sp.symbols('theta1 theta2')
theta1_dot, theta2_dot = sp.symbols('theta1_dot theta2_dot')
theta1_ddot, theta2_ddot = sp.symbols('theta1_ddot theta2_ddot')
# Define parameters
m1, m2 = sp.symbols('m1 m2')
l1, l2 = sp.symbols('l1 l2')
g = sp.Symbol('g')
# Kinetic energy
T1 = 0.5 * m1 * (l1**2 * theta1_dot**2)
T2 = 0.5 * m2 * ((l1**2 * theta1_dot**2) + (l2**2 * theta2_dot**2) + 2 * l1 * l2 * theta1_dot * theta2_dot * sp.cos(theta2))
T = T1 + T2
# Potential energy
U1 = m1 * g * (l1 / 2) * sp.cos(theta1)
U2 = m2 * g * (l1 * sp.cos(theta1) + (l2 / 2) * sp.cos(theta1 + theta2))
U = U1 + U2
# Lagrangian function
L = T - U
# Equations of motion using Lagrange's equation: d/dt(dL/d(theta_dot)) - dL/d(theta) = 0
dL_dtheta1_dot = sp.diff(L, theta1_dot)
dL_dtheta2_dot = sp.diff(L, theta2_dot)
dL_dtheta1 = sp.diff(L, theta1)
dL_dtheta2 = sp.diff(L, theta2)
dL_dtheta1_dot_dt = sp.diff(dL_dtheta1_dot, theta1) * theta1_dot + sp.diff(dL_dtheta1_dot, theta1_dot) * theta1_ddot
dL_dtheta2_dot_dt = sp.diff(dL_dtheta2_dot, theta2) * theta2_dot + sp.diff(dL_dtheta2_dot, theta2_dot) * theta2_ddot
# Compute equations of motion
eq1 = dL_dtheta1_dot_dt - dL_dtheta1
eq2 = dL_dtheta2_dot_dt - dL_dtheta2
# Simplify equations
eq1_simplified = sp.simplify(eq1)
eq2_simplified = sp.simplify(eq2)
# Define numerical values for parameters
params = {
'm1': 1.0,
'm2': 1.0,
'l1': 1.0,
'l2': 1.0,
'g': 9.81
}
# Substitute parameters into the equations
eq1_numeric = eq1_simplified.subs(params)
eq2_numeric = eq2_simplified.subs(params)
# Create a grid of theta1 and theta2 values
theta1_vals = np.linspace(-np.pi, np.pi, 100)
theta2_vals = np.linspace(-np.pi, np.pi, 100)
Theta1, Theta2 = np.meshgrid(theta1_vals, theta2_vals)
# Compute the corresponding values for the equations of motion
Eq1_vals = np.zeros_like(Theta1)
Eq2_vals = np.zeros_like(Theta2)
for i in range(Theta1.shape[0]):
for j in range(Theta1.shape[1]):
# Substitute numerical values
eq1_val = eq1_numeric.evalf(subs={theta1: Theta1[i, j], theta2: Theta2[i, j], theta1_dot: 0, theta2_dot: 0, theta1_ddot: 0, theta2_ddot: 0})
eq2_val = eq2_numeric.evalf(subs={theta1: Theta1[i, j], theta2: Theta2[i, j], theta1_dot: 0, theta2_dot: 0, theta1_ddot: 0, theta2_ddot: 0})
Eq1_vals[i, j] = eq1_val
Eq2_vals[i, j] = eq2_val
# Plot the results
fig = plt.figure(figsize=(12, 6))
# Plot for eq1
ax1 = fig.add_subplot(121, projection='3d')
ax1.plot_surface(Theta1, Theta2, Eq1_vals, cmap='viridis')
ax1.set_xlabel('Theta1')
ax1.set_ylabel('Theta2')
ax1.set_zlabel('Equation 1 Value')
ax1.set_title('Equation of Motion 1')
# Plot for eq2
ax2 = fig.add_subplot(122, projection='3d')
ax2.plot_surface(Theta1, Theta2, Eq2_vals, cmap='viridis')
ax2.set_xlabel('Theta1')
ax2.set_ylabel('Theta2')
ax2.set_zlabel('Equation 2 Value')
ax2.set_title('Equation of Motion 2')
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# 定数の定義
K = 100.0 # 剛性
B = 10.0 # 減衰
M = 1.0 # 質量
dt = 0.01 # タイムステップ
t_max = 5 # シミュレーション時間
# 時間の定義
time = np.arange(0, t_max, dt)
# 目標位置、速度、加速度
x_target = 1.0
v_target = 0.0
a_target = 0.0
# 初期条件
x = 0.0
v = 0.0
a = 0.0
# 保存用リスト
x_list = []
v_list = []
f_list = []
# シミュレーションループ
for t in time:
# 目標値との誤差を計算
delta_x = x_target - x
delta_v = v_target - v
delta_a = a_target - a
# インピーダンス制御力の計算
F = K * delta_x + B * delta_v + M * delta_a
# 運動方程式を用いて次の状態を計算
a = F / M
v = v + a * dt
x = x + v * dt
# データを保存
x_list.append(x)
v_list.append(v)
f_list.append(F)
# 結果のプロット
plt.figure(figsize=(12, 6))
# 位置のプロット
plt.subplot(311)
plt.plot(time, x_list, label='Position')
plt.axhline(y=x_target, color='r', linestyle='--', label='Target Position')
plt.xlabel('Time [s]')
plt.ylabel('Position [m]')
plt.legend()
plt.grid()
# 速度のプロット
plt.subplot(312)
plt.plot(time, v_list, label='Velocity', color='g')
plt.xlabel('Time [s]')
plt.ylabel('Velocity [m/s]')
plt.legend()
plt.grid()
# 力のプロット
plt.subplot(313)
plt.plot(time, f_list, label='Force', color='b')
plt.xlabel('Time [s]')
plt.ylabel('Force [N]')
plt.legend()
plt.grid()
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# ロボットアームのパラメータ
l1 = 1.0 # 第一関節から第二関節までの長さ
l2 = 1.0 # 第二関節から先端までの長さ
# 逆運動学の計算
def inverse_kinematics(x_target, y_target):
# ロボットのパラメータ
d = np.sqrt(x_target**2 + y_target**2)
if d > l1 + l2:
raise ValueError("目標位置がロボットの可動範囲を超えています")
# 逆運動学の計算
theta2 = np.arccos((x_target**2 + y_target**2 - l1**2 - l2**2) / (2 * l1 * l2))
theta1 = np.arctan2(y_target, x_target) - np.arctan2(l2 * np.sin(theta2), l1 + l2 * np.cos(theta2))
return theta1, theta2
# 軌道の生成（円形）
t = np.linspace(0, 2 * np.pi, 100)
x_target = 0.5 * (l1 + l2) * np.cos(t)
y_target = 0.5 * (l1 + l2) * np.sin(t)
# 関節角度の計算
theta1_list = []
theta2_list = []
for x, y in zip(x_target, y_target):
theta1, theta2 = inverse_kinematics(x, y)
theta1_list.append(theta1)
theta2_list.append(theta2)
# 3Dプロットの準備
fig = plt.figure(figsize=(10, 5))
ax = fig.add_subplot(111)
# 軌道のプロット
ax.plot(x_target, y_target, label='Desired Path', linestyle='--', color='r')
# ロボットアームのシミュレーション
x1_list = [l1 * np.cos(theta) for theta in theta1_list]
y1_list = [l1 * np.sin(theta) for theta in theta1_list]
x2_list = [x1 + l2 * np.cos(theta1 + theta2) for x1, theta1, theta2 in zip(x1_list, theta1_list, theta2_list)]
y2_list = [y1 + l2 * np.sin(theta1 + theta2) for y1, theta1, theta2 in zip(y1_list, theta1_list, theta2_list)]
# アームの動きをプロット
ax.plot(x2_list, y2_list, label='End-Effector Path', color='b')
ax.set_xlabel('X Position')
ax.set_ylabel('Y Position')
ax.set_title('End-Effector Trajectory')
ax.legend()
ax.grid()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# ロール、ピッチ、ヨーの角度をラジアンで指定
def rotation_matrix(roll, pitch, yaw):
# ロール、ピッチ、ヨーの回転行列を計算
R_x = np.array([
[1, 0, 0],
[0, np.cos(roll), -np.sin(roll)],
[0, np.sin(roll), np.cos(roll)]
])
R_y = np.array([
[np.cos(pitch), 0, np.sin(pitch)],
[0, 1, 0],
[-np.sin(pitch), 0, np.cos(pitch)]
])
R_z = np.array([
[np.cos(yaw), -np.sin(yaw), 0],
[np.sin(yaw), np.cos(yaw), 0],
[0, 0, 1]
])
# 合成回転行列
R = R_z @ R_y @ R_x
return R
# 点の回転
def rotate_point(point, roll, pitch, yaw):
R = rotation_matrix(roll, pitch, yaw)
rotated_point = R @ point
return rotated_point
# 回転の例
point = np.array([1, 1, 1]) # 回転させる点
roll = np.radians(30) # ロール角
pitch = np.radians(45) # ピッチ角
yaw = np.radians(60) # ヨー角
rotated_point = rotate_point(point, roll, pitch, yaw)
# 結果の表示
print(f"Original Point: {point}")
print(f"Rotated Point: {rotated_point}")
# 3Dプロット
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
# 元の点と回転後の点をプロット
ax.scatter(*point, color='r', label='Original Point')
ax.scatter(*rotated_point, color='b', label='Rotated Point')
# 軸の設定
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('3D Rotation')
ax.legend()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
import sympy as sp
# Define symbolic variables
theta1, theta2 = sp.symbols('theta1 theta2')
theta1_dot, theta2_dot = sp.symbols('theta1_dot theta2_dot')
theta1_ddot, theta2_ddot = sp.symbols('theta1_ddot theta2_ddot')
# Define parameters
m1, m2 = sp.symbols('m1 m2')
l1, l2 = sp.symbols('l1 l2')
r1, r2 = l1 / 2, l2 / 2
g = sp.Symbol('g')
# Define symbolic expressions for kinetic and potential energies
x1 = l1 * sp.cos(theta1) / 2
y1 = l1 * sp.sin(theta1) / 2
x2 = l1 * sp.cos(theta1) + l2 * sp.cos(theta1 + theta2) / 2
y2 = l1 * sp.sin(theta1) + l2 * sp.sin(theta1 + theta2) / 2
# Kinetic energy
T1 = 0.5 * m1 * (l1**2 * theta1_dot**2)
T2 = 0.5 * m2 * (l1**2 * theta1_dot**2 + l2**2 * theta2_dot**2 + 2 * l1 * l2 * theta1_dot * theta2_dot * sp.cos(theta2))
T = T1 + T2
# Potential energy
U1 = m1 * g * y1
U2 = m2 * g * y2
U = U1 + U2
# Simplify expressions
T_simplified = sp.simplify(T)
U_simplified = sp.simplify(U)
# Define numerical values for parameters
m1_val = 1.0
m2_val = 1.0
l1_val = 1.0
l2_val = 1.0
g_val = 9.81
# Define ranges for joint angles
theta1_vals = np.linspace(-np.pi, np.pi, 100)
theta2_vals = np.linspace(-np.pi, np.pi, 100)
Theta1, Theta2 = np.meshgrid(theta1_vals, theta2_vals)
# Substitute values into the simplified expressions
T_func = sp.lambdify((theta1, theta2, theta1_dot, theta2_dot), T_simplified.subs({m1: m1_val, m2: m2_val, l1: l1_val, l2: l2_val, g: g_val, theta1_dot: 0, theta2_dot: 0}), 'numpy')
U_func = sp.lambdify((theta1, theta2), U_simplified.subs({m1: m1_val, m2: m2_val, l1: l1_val, l2: l2_val, g: g_val}), 'numpy')
# Compute kinetic and potential energies
T_vals = T_func(Theta1, Theta2)
U_vals = U_func(Theta1, Theta2)
# Plotting
fig = plt.figure(figsize=(12, 6))
# Kinetic Energy Plot
ax1 = fig.add_subplot(121, projection='3d')
ax1.plot_surface(Theta1, Theta2, T_vals, cmap='viridis')
ax1.set_xlabel('Theta1 (rad)')
ax1.set_ylabel('Theta2 (rad)')
ax1.set_zlabel('Kinetic Energy (J)')
ax1.set_title('Kinetic Energy of 2-DOF Robot Arm')
# Potential Energy Plot
ax2 = fig.add_subplot(122, projection='3d')
ax2.plot_surface(Theta1, Theta2, U_vals, cmap='plasma')
ax2.set_xlabel('Theta1 (rad)')
ax2.set_ylabel('Theta2 (rad)')
ax2.set_zlabel('Potential Energy (J)')
ax2.set_title('Potential Energy of 2-DOF Robot Arm')
plt.tight_layout()
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.signal import square
# パラメータ設定
TABLE_LEN = 256
CTRL_RATE = 20000
FREQ_LEN = 9
TABLE_SIZE = 2 * TABLE_LEN # 周波数テーブルのサイズ
PWM_PERIOD = 1 / CTRL_RATE
# 周波数テーブル（例として適当な値を設定）
freq_table = np.array([175, 196, 223, 233, 262, 294, 311, 350, 400])
# 正弦波と三角波の生成
def generate_sin_wave(frequency, sample_rate, duration):
t = np.arange(0, duration, 1/sample_rate)
return np.sin(2 * np.pi * frequency * t)
def generate_tri_wave(frequency, sample_rate, duration):
t = np.arange(0, duration, 1/sample_rate)
return 2 * (t * frequency - np.floor(t * frequency + 0.5))
# PWM信号の生成
def generate_pwm_signal(sin_wave, tri_wave):
pwm_signal = sin_wave > tri_wave
return pwm_signal.astype(int)
# パラメータ設定
sample_rate = CTRL_RATE
duration = 0.1 # 0.1秒のデータを生成
# 信号波の周波数設定
signal_freq = 50 # 例: 50Hz
tri_freq = 50 # 例: 50Hz
# 正弦波と三角波の生成
sin_wave = generate_sin_wave(signal_freq, sample_rate, duration)
tri_wave = generate_tri_wave(tri_freq, sample_rate, duration)
# PWM信号の生成
pwm_signal = generate_pwm_signal(sin_wave, tri_wave)
# 結果のプロット
t = np.arange(0, duration, 1/sample_rate)
plt.figure(figsize=(10, 6))
plt.subplot(3, 1, 1)
plt.title('Sine Wave')
plt.plot(t, sin_wave, label='Sine Wave')
plt.grid()
plt.subplot(3, 1, 2)
plt.title('Triangular Wave')
plt.plot(t, tri_wave, label='Triangular Wave', color='orange')
plt.grid()
plt.subplot(3, 1, 3)
plt.title('PWM Signal')
plt.plot(t, pwm_signal, label='PWM Signal', color='red')
plt.grid()
plt.tight_layout()
plt.show()
import numpy as np
# 2自由度のロボットアームの長さ
l1 = 1.0
l2 = 1.0
# 関節角度
theta1 = np.pi / 4
theta2 = np.pi / 6
# ヤコビ行列の計算
J = np.array([
[-l1*np.sin(theta1) - l2*np.sin(theta1 + theta2), -l2*np.sin(theta1 + theta2)],
[l1*np.cos(theta1) + l2*np.cos(theta1 + theta2), l2*np.cos(theta1 + theta2)]
])
print("Jacobian Matrix:\n", J)
# 末端効果器の速度
end_effector_velocity = np.array([0.5, 0.2]) # [vx, vy]
# 関節速度の計算
joint_velocity = np.linalg.pinv(J).dot(end_effector_velocity)
print("Joint Velocities:\n", joint_velocity)
import matplotlib.pyplot as plt
# 目標位置
target_position = np.array([1.5, 1.5])
# 現在の末端効果器の位置
current_position = np.array([0.0, 0.0])
# 設定
learning_rate = 0.1
tolerance = 0.01
max_iterations = 1000
iterations = 0
# シミュレーションループ
while np.linalg.norm(target_position - current_position) > tolerance and iterations < max_iterations:
# ヤコビ行列の計算（関節角度は更新される）
theta1, theta2 = np.arctan2(current_position[1], current_position[0]), 0
J = np.array([
[-l1*np.sin(theta1) - l2*np.sin(theta1 + theta2), -l2*np.sin(theta1 + theta2)],
[l1*np.cos(theta1) + l2*np.cos(theta1 + theta2), l2*np.cos(theta1 + theta2)]
])
# 末端効果器の位置から関節速度を計算
end_effector_velocity = target_position - current_position
joint_velocity = np.linalg.pinv(J).dot(end_effector_velocity)
# 関節角度の更新
theta1 += learning_rate * joint_velocity[0]
theta2 += learning_rate * joint_velocity[1]
# 現在の末端効果器の位置の更新
current_position = np.array([l1*np.cos(theta1) + l2*np.cos(theta1 + theta2),
l1*np.sin(theta1) + l2*np.sin(theta1 + theta2)])
iterations += 1
# 結果のプロット
plt.figure(figsize=(10, 6))
plt.plot([0, l1*np.cos(theta1), l1*np.cos(theta1) + l2*np.cos(theta1 + theta2)],
[0, l1*np.sin(theta1), l1*np.sin(theta1) + l2*np.sin(theta1 + theta2)], 'o-', label='Arm Position')
plt.plot(target_position[0], target_position[1], 'rx', label='Target Position')
plt.xlabel('X Position')
plt.ylabel('Y Position')
plt.title('Robot Arm Position Control')
plt.legend()
plt.grid(True)
plt.show()
print("Final Joint Angles:\n", [theta1, theta2])
import numpy as np
import matplotlib.pyplot as plt
# ロボットのパラメータ
l1 = 1.0 # 第1リンクの長さ (m)
l2 = 1.0 # 第2リンクの長さ (m)
m1 = 1.0 # 第1リンクの質量 (kg)
m2 = 1.0 # 第2リンクの質量 (kg)
g = 9.81 # 重力加速度 (m/s^2)
# 関節角度
theta1 = np.linspace(-np.pi, np.pi, 100)
theta2 = np.linspace(-np.pi, np.pi, 100)
THETA1, THETA2 = np.meshgrid(theta1, theta2)
# 動力学の計算
def mass_matrix(theta1, theta2):
M11 = m1*l1**2 + m2*(l1**2 + l2**2 + 2*l1*l2*np.cos(theta2)) # M11
M12 = m2*(l2**2 + l1*l2*np.cos(theta2)) # M12
M21 = M12 # M21
M22 = m2*l2**2 # M22
return np.array([[M11, M12], [M21, M22]])
def gravity_vector(theta1, theta2):
G1 = (m1*l1 + m2*l1)*g*np.cos(theta1) + m2*l2*g*np.cos(theta1 + theta2)
G2 = m2*l2*g*np.cos(theta1 + theta2)
return np.array([G1, G2])
# 各関節のトルク計算
torques = np.zeros(THETA1.shape)
for i in range(THETA1.shape[0]):
for j in range(THETA1.shape[1]):
M = mass_matrix(THETA1[i, j], THETA2[i, j])
G = gravity_vector(THETA1[i, j], THETA2[i, j])
torques[i, j] = np.linalg.norm(np.dot(M, G))
# プロット
plt.figure(figsize=(12, 6))
contour = plt.contourf(THETA1, THETA2, torques, 20, cmap='viridis')
plt.colorbar(contour)
plt.xlabel('Theta1 (rad)')
plt.ylabel('Theta2 (rad)')
plt.title('Torque Contour Plot for 2-DOF Robot Arm')
plt.grid(True)
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
# システムパラメータ
a11, a12 = -1, 2
a21, a22 = 1, -1
b1, b2 = 1, 0
# 状態空間モデルの定義
def state_space_model(x, t, A, B, u):
return np.dot(A, x) + B * u(t)
# 行列の設定
A = np.array([[a11, a12], [a21, a22]])
B = np.array([b1, b2])
# 入力関数の定義
def u(t):
return np.sin(t) # 入力信号として正弦波を使用
# 初期状態
x0 = [0, 0]
# 時間の設定
time = np.linspace(0, 10, 100)
# 微分方程式の解を求める
sol = odeint(state_space_model, x0, time, args=(A, B, u))
# 結果のプロット
plt.figure(figsize=(12, 6))
plt.plot(time, sol[:, 0], label='x1(t)')
plt.plot(time, sol[:, 1], label='x2(t)')
plt.xlabel('Time (s)')
plt.ylabel('State Variables')
plt.title('State Variables Over Time')
plt.legend()
plt.grid(True)
plt.show()
# 速度台形則のパラメータ
v_max = 1.0 # 最大速度
a_max = 0.5 # 最大加速度
x0, xf = 0, 1 # 開始位置と終了位置
def trapezoidal_trajectory(t, x0, xf, v_max, a_max):
total_distance = xf - x0
t_acc = v_max / a_max
distance_acc = 0.5 * a_max * t_acc**2
if total_distance <= 2 * distance_acc:
t_acc = np.sqrt(total_distance / a_max)
t_cruise = (total_distance - 2 * distance_acc) / v_max
t_total = 2 * t_acc + t_cruise
if t < t_acc:
return x0 + 0.5 * a_max * t**2
elif t < t_acc + t_cruise:
return x0 + distance_acc + v_max * (t - t_acc)
elif t < t_total:
t_dec = t - (t_acc + t_cruise)
return xf - 0.5 * a_max * t_dec**2
else:
return xf
# プロット
t = np.linspace(0, 2 * (v_max / a_max) + (xf - 2 * (0.5 * a_max * (v_max / a_max)**2)) / v_max, 100)
x = np.array([trapezoidal_trajectory(i, x0, xf, v_max, a_max) for i in t])
plt.figure(figsize=(10, 6))
plt.plot(t, x, label='Trapezoidal Trajectory')
plt.xlabel('Time (s)')
plt.ylabel('Position')
plt.title('Trapezoidal Velocity Profile')
plt.legend()
plt.grid(True)
plt.show()
from sympy import symbols, cos, sin, Matrix
# シンボルの定義
theta1, theta2, l1, l2 = symbols('theta1 theta2 l1 l2')
# シンボリックなヤコビ行列の計算
x = l1 * cos(theta1) + l2 * cos(theta1 + theta2)
y = l1 * sin(theta1) + l2 * sin(theta1 + theta2)
J_sym = Matrix([
[x.diff(theta1), x.diff(theta2)],
[y.diff(theta1), y.diff(theta2)]
])
print("Symbolic Jacobian Matrix:")
print(J_sym)
# 関節速度
q_dot = np.array([0.1, 0.1]) # [dq1/dt, dq2/dt]
# 手先の速度の計算
def end_effector_velocity(J, q_dot):
return J @ q_dot
velocity = end_effector_velocity(J, q_dot)
print("End-Effector Velocity:")
print(velocity)
import numpy as np
import matplotlib.pyplot as plt
# パラメータ
l1 = 1.0
l2 = 0.8
dt = 0.1
time = np.arange(0, 10, dt)
# 目標位置
target_position = np.array([1.5, 1.0])
# 初期状態
theta1 = 0.0
theta2 = 0.0
theta1_dot = 0.1
theta2_dot = 0.1
# 軌道追跡
theta1_traj = []
theta2_traj = []
for t in time:
# ヤコビ行列の計算
J = jacobian(theta1, theta2, l1, l2)
J_inv = np.linalg.pinv(J)
# 目標位置のエラー
x_error = target_position[0] - (l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2))
y_error = target_position[1] - (l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2))
# 速度指令の計算
velocity_command = np.array([x_error, y_error])
joint_velocity = J_inv @ velocity_command
# 関節角度の更新
theta1 += joint_velocity[0] * dt
theta2 += joint_velocity[1] * dt
theta1_traj.append(theta1)
theta2_traj.append(theta2)
# プロット
plt.figure(figsize=(12, 6))
plt.plot(time, theta1_traj, label='Theta1')
plt.plot(time, theta2_traj, label='Theta2')
plt.xlabel('Time (s)')
plt.ylabel('Joint Angles (rad)')
plt.title('Trajectory of Joint Angles')
plt.legend()
plt.grid(True)
plt.show()
import numpy as np
# 手先の速度（例）
end_effector_velocity = np.array([0.1, 0.1]) # [vx, vy]
# 微分逆運動学の計算
def differential_inverse_kinematics(end_effector_velocity, J):
J_inv = np.linalg.pinv(J)
return J_inv @ end_effector_velocity
# ヤコビ行列の計算
theta1 = np.pi / 4
theta2 = np.pi / 6
J = jacobian(theta1, theta2, l1, l2)
# 関節速度の計算
joint_velocity = differential_inverse_kinematics(end_effector_velocity, J)
print("Joint Velocity:")
print(joint_velocity)
import numpy as np
import matplotlib.pyplot as plt
# ロボットパラメータ
l1 = 1.0
l2 = 0.8
dt = 0.1
time = np.arange(0, 10, dt)
# 目標位置
target_position = np.array([1.5, 1.0])
# 初期状態
theta1 = 0.0
theta2 = 0.0
# 軌道追跡
theta1_traj = []
theta2_traj = []
for t in time:
# ヤコビ行列の計算
J = jacobian(theta1, theta2, l1, l2)
J_inv = np.linalg.pinv(J)
# 目標位置のエラー
x_error = target_position[0] - (l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2))
y_error = target_position[1] - (l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2))
# 速度指令の計算
velocity_command = np.array([x_error, y_error])
joint_velocity = J_inv @ velocity_command
# 関節角度の更新
theta1 += joint_velocity[0] * dt
theta2 += joint_velocity[1] * dt
theta1_traj.append(theta1)
theta2_traj.append(theta2)
# プロット
plt.figure(figsize=(12, 6))
plt.plot(time, theta1_traj, label='Theta1')
plt.plot(time, theta2_traj, label='Theta2')
plt.xlabel('Time (s)')
plt.ylabel('Joint Angles (rad)')
plt.title('Trajectory of Joint Angles with Velocity Control')
plt.legend()
plt.grid(True)
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# カメラ画像の特徴
def image_features(theta1, theta2, l1, l2):
x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
return np.array([x, y])
# 目標画像特徴
target_features = np.array([1.5, 1.0])
# 初期状態
theta1 = 0.0
theta2 = 0.0
dt = 0.1
time = np.arange(0, 10, dt)
# ビジュアルサーボ
theta1_traj = []
theta2_traj = []
for t in time:
# 画像特徴の計算
features = image_features(theta1, theta2, l1, l2)
# 画像特徴のエラー
feature_error = target_features - features
# ヤコビ行列の計算
J = jacobian(theta1, theta2, l1, l2)
J_inv = np.linalg.pinv(J)
# 関節速度の計算
joint_velocity = J_inv @ feature_error
# 関節角度の更新
theta1 += joint_velocity[0] * dt
theta2 += joint_velocity[1] * dt
theta1_traj.append(theta1)
theta2_traj.append(theta2)
# プロット
plt.figure(figsize=(12, 6))
plt.plot(time, theta1_traj, label='Theta1')
plt.plot(time, theta2_traj, label='Theta2')
plt.xlabel('Time (s)')
plt.ylabel('Joint Angles (rad)')
plt.title('Trajectory of Joint Angles with Visual Servoing')
plt.legend()
plt.grid(True)
plt.show()
import numpy as np
# 仮想仕事の計算
def virtual_work(J, joint_velocity, external_force):
# 力と速度の内積から仮想仕事を計算
return np.dot(external_force, J @ joint_velocity)
# ロボットパラメータ
l1 = 1.0
l2 = 0.8
# 初期状態
theta1 = np.pi / 4
theta2 = np.pi / 6
J = jacobian(theta1, theta2, l1, l2)
# 外力の設定
external_force = np.array([0.1, 0.1])
# 関節速度の設定
joint_velocity = np.array([0.1, 0.1])
# 仮想仕事の計算
work = virtual_work(J, joint_velocity, external_force)
print(f"Virtual Work: {work}")
import numpy as np
import matplotlib.pyplot as plt
# コンプライアンス制御
def compliance_control(position_error, K):
# Kはコンプライアンスゲイン
return K * position_error
# ロボットパラメータ
l1 = 1.0
l2 = 0.8
K = 0.1
# 初期状態
theta1 = 0.0
theta2 = 0.0
target_position = np.array([1.5, 1.0])
dt = 0.1
time = np.arange(0, 10, dt)
# コンプライアンス制御
theta1_traj = []
theta2_traj = []
for t in time:
# 現在の位置を計算
current_position = np.array([l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2),
l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)])
# 位置のエラー
position_error = target_position - current_position
# コンプライアンス制御の計算
control_input = compliance_control(position_error, K)
# ヤコビ行列の計算
J = jacobian(theta1, theta2, l1, l2)
J_inv = np.linalg.pinv(J)
# 関節速度の計算
joint_velocity = J_inv @ control_input
# 関節角度の更新
theta1 += joint_velocity[0] * dt
theta2 += joint_velocity[1] * dt
theta1_traj.append(theta1)
theta2_traj.append(theta2)
# プロット
plt.figure(figsize=(12, 6))
plt.plot(time, theta1_traj, label='Theta1')
plt.plot(time, theta2_traj, label='Theta2')
plt.xlabel('Time (s)')
plt.ylabel('Joint Angles (rad)')
plt.title('Trajectory of Joint Angles with Compliance Control')
plt.legend()
plt.grid(True)
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# ロボットパラメータ
l1 = 1.0
l2 = 0.8
m1 = 1.0
m2 = 0.8
g = 9.81
# 関節角度
theta1 = np.pi / 4
theta2 = np.pi / 6
theta1_dot = 0.1
theta2_dot = 0.1
# 慣性行列の計算
def inertia_matrix(theta1, theta2, l1, l2, m1, m2):
I11 = m1 * l1**2 + m2 * (l1**2 + l2**2 + 2 * l1 * l2 * np.cos(theta2))
I12 = m2 * (l2**2 + l1 * l2 * np.cos(theta2))
I21 = I12
I22 = m2 * l2**2
return np.array([[I11, I12], [I21, I22]])
# 遠心力とコリオリ力の計算
def coriolis_centrifugal_forces(theta1, theta2, theta1_dot, theta2_dot, l1, l2, m1, m2):
C11 = -m2 * l1 * l2 * np.sin(theta2) * theta2_dot
C12 = -m2 * l1 * l2 * np.sin(theta2) * (theta1_dot + theta2_dot)
C21 = m2 * l1 * l2 * np.sin(theta2) * theta1_dot
C22 = 0.0
C = np.array([C11, C12])
# 遠心力の計算
F = np.array([
m1 * g * l1 * np.cos(theta1) + m2 * g * (l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)),
m2 * g * l2 * np.cos(theta1 + theta2)
])
return C, F
# 計算
inertia = inertia_matrix(theta1, theta2, l1, l2, m1, m2)
coriolis, centrifugal = coriolis_centrifugal_forces(theta1, theta2, theta1_dot, theta2_dot, l1, l2, m1, m2)
print("Inertia Matrix:\n", inertia)
print("Coriolis and Centrifugal Forces:\n", coriolis)
print("Gravity Forces:\n", centrifugal)
import numpy as np
# ロボットパラメータ
l1 = 1.0
l2 = 0.8
m1 = 1.0
m2 = 0.8
g = 9.81
# 関節角度と速度
theta1 = np.pi / 4
theta2 = np.pi / 6
theta1_dot = 0.1
theta2_dot = 0.1
theta1_ddot = 0.0
theta2_ddot = 0.0
# 慣性行列の計算
def inertia_matrix(theta1, theta2, l1, l2, m1, m2):
I11 = m1 * l1**2 + m2 * (l1**2 + l2**2 + 2 * l1 * l2 * np.cos(theta2))
I12 = m2 * (l2**2 + l1 * l2 * np.cos(theta2))
I21 = I12
I22 = m2 * l2**2
return np.array([[I11, I12], [I21, I22]])
# コリオリ力と遠心力の計算
def coriolis_centrifugal_forces(theta1, theta2, theta1_dot, theta2_dot, l1, l2, m1, m2):
C11 = -m2 * l1 * l2 * np.sin(theta2) * theta2_dot
C12 = -m2 * l1 * l2 * np.sin(theta2) * (theta1_dot + theta2_dot)
C21 = m2 * l1 * l2 * np.sin(theta2) * theta1_dot
C22 = 0.0
C = np.array([C11, C12])
# 重力の計算
G1 = m1 * g * l1 * np.cos(theta1) + m2 * g * (l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2))
G2 = m2 * g * l2 * np.cos(theta1 + theta2)
G = np.array([G1, G2])
return C, G
# ニュートン・オイラー法による動力学方程式の計算
def newton_euler_dynamics(theta1, theta2, theta1_dot, theta2_dot, theta1_ddot, theta2_ddot, l1, l2, m1, m2):
inertia = inertia_matrix(theta1, theta2, l1, l2, m1, m2)
coriolis, gravity = coriolis_centrifugal_forces(theta1, theta2, theta1_dot, theta2_dot, l1, l2, m1, m2)
# 計算
q_ddot = np.linalg.inv(inertia) @ (np.array([0, 0]) - coriolis - gravity)
return q_ddot
# 計算
acceleration = newton_euler_dynamics(theta1, theta2, theta1_dot, theta2_dot, theta1_ddot, theta2_ddot, l1, l2, m1, m2)
print("Joint Accelerations:\n", acceleration)
import numpy as np
import matplotlib.pyplot as plt
# ロボットパラメータ
l1 = 1.0 # リンク1の長さ
l2 = 0.8 # リンク2の長さ
m1 = 1.0 # リンク1の質量
m2 = 0.8 # リンク2の質量
g = 9.81 # 重力加速度
# 関節角度と速度
theta1 = np.pi / 4 # 関節1の角度 (ラジアン)
theta2 = np.pi / 6 # 関節2の角度 (ラジアン)
theta1_dot = 0.1 # 関節1の角速度 (rad/s)
theta2_dot = 0.1 # 関節2の角速度 (rad/s)
theta1_ddot = 0.0 # 関節1の角加速度 (rad/s^2)
theta2_ddot = 0.0 # 関節2の角加速度 (rad/s^2)
# 慣性行列の計算
def inertia_matrix(theta1, theta2, l1, l2, m1, m2):
I11 = m1 * l1**2 + m2 * (l1**2 + l2**2 + 2 * l1 * l2 * np.cos(theta2))
I12 = m2 * (l2**2 + l1 * l2 * np.cos(theta2))
I21 = I12
I22 = m2 * l2**2
return np.array([[I11, I12], [I21, I22]])
# コリオリ力と遠心力の計算
def coriolis_centrifugal_forces(theta1, theta2, theta1_dot, theta2_dot, l1, l2, m1, m2):
C11 = -m2 * l1 * l2 * np.sin(theta2) * theta2_dot
C12 = -m2 * l1 * l2 * np.sin(theta2) * (theta1_dot + theta2_dot)
C21 = m2 * l1 * l2 * np.sin(theta2) * theta1_dot
C22 = 0.0
C = np.array([C11, C12])
# 重力の計算
G1 = m1 * g * l1 * np.cos(theta1) + m2 * g * (l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2))
G2 = m2 * g * l2 * np.cos(theta1 + theta2)
G = np.array([G1, G2])
return C, G
# ニュートン・オイラー法による動力学方程式の計算
def newton_euler_dynamics(theta1, theta2, theta1_dot, theta2_dot, theta1_ddot, theta2_ddot, l1, l2, m1, m2):
inertia = inertia_matrix(theta1, theta2, l1, l2, m1, m2)
coriolis, gravity = coriolis_centrifugal_forces(theta1, theta2, theta1_dot, theta2_dot, l1, l2, m1, m2)
# 動力学方程式の計算
torques = np.linalg.inv(inertia) @ (np.array([theta1_ddot, theta2_ddot]) - coriolis - gravity)
return torques
# トルクの計算
torques = newton_euler_dynamics(theta1, theta2, theta1_dot, theta2_dot, theta1_ddot, theta2_ddot, l1, l2, m1, m2)
print("Torques:\n", torques)
# グラフ表示
fig, ax = plt.subplots()
ax.plot([1, 2], torques, 'bo-')
ax.set_xticks([1, 2])
ax.set_xticklabels(['Joint 1', 'Joint 2'])
ax.set_ylabel('Torque (Nm)')
ax.set_title('Torques for the 2-DOF Arm using Newton-Euler Method')
plt.grid(True)
plt.show()
import numpy as np
# ロボットパラメータ
l1 = 1.0 # リンク1の長さ (m)
l2 = 0.8 # リンク2の長さ (m)
m1 = 1.0 # リンク1の質量 (kg)
m2 = 0.8 # リンク2の質量 (kg)
g = 9.81 # 重力加速度 (m/s^2)
# 関節角度、速度、加速度
theta1 = np.pi / 4 # 関節1の角度 (rad)
theta2 = np.pi / 6 # 関節2の角度 (rad)
theta1_dot = 0.1 # 関節1の角速度 (rad/s)
theta2_dot = 0.1 # 関節2の角速度 (rad/s)
theta1_ddot = 0.0 # 関節1の角加速度 (rad/s^2)
theta2_ddot = 0.0 # 関節2の角加速度 (rad/s^2)
# 慣性行列の計算
def inertia_matrix(theta1, theta2, l1, l2, m1, m2):
I11 = m1 * l1**2 + m2 * (l1**2 + l2**2 + 2 * l1 * l2 * np.cos(theta2))
I12 = m2 * (l2**2 + l1 * l2 * np.cos(theta2))
I21 = I12
I22 = m2 * l2**2
return np.array([[I11, I12], [I21, I22]])
# コリオリ力と遠心力の計算
def coriolis_centrifugal_forces(theta1, theta2, theta1_dot, theta2_dot, l1, l2, m1, m2):
C11 = -m2 * l1 * l2 * np.sin(theta2) * theta2_dot
C12 = -m2 * l1 * l2 * np.sin(theta2) * (theta1_dot + theta2_dot)
C21 = m2 * l1 * l2 * np.sin(theta2) * theta1_dot
C22 = 0.0
C = np.array([C11, C12])
# 重力の計算
G1 = m1 * g * l1 * np.cos(theta1) + m2 * g * (l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2))
G2 = m2 * g * l2 * np.cos(theta1 + theta2)
G = np.array([G1, G2])
return C, G
# ニュートン・オイラー法による動力学方程式の計算
def newton_euler_dynamics(theta1, theta2, theta1_dot, theta2_dot, theta1_ddot, theta2_ddot, l1, l2, m1, m2):
inertia = inertia_matrix(theta1, theta2, l1, l2, m1, m2)
coriolis, gravity = coriolis_centrifugal_forces(theta1, theta2, theta1_dot, theta2_dot, l1, l2, m1, m2)
# 動力学方程式の計算
torques = np.linalg.inv(inertia) @ (np.array([theta1_ddot, theta2_ddot]) - coriolis - gravity)
return torques
# トルクの計算
torques = newton_euler_dynamics(theta1, theta2, theta1_dot, theta2_dot, theta1_ddot, theta2_ddot, l1, l2, m1, m2)
print("Torques:\n", torques)
import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import odeint
# ロボットアームのパラメータ
l1 = 1.0 # リンク1の長さ (m)
l2 = 1.0 # リンク2の長さ (m)
# 初期状態
theta1_init = 0.0
theta2_init = 0.0
theta_dot1_init = 0.0
theta_dot2_init = 0.0
# 目標位置
target_pos = np.array([1.0, 1.0])
# 動力学モデルの定義
def dynamics(state, t, Kp, Kd):
theta1, theta2, theta_dot1, theta_dot2 = state
# ロボットの位置
x = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
y = l1 * np.sin(theta1) + l2 * np.sin(theta1 + theta2)
# 目標位置に対する誤差
error = np.array([target_pos[0] - x, target_pos[1] - y])
# 制御入力
control_input = Kp * error - Kd * np.array([theta_dot1, theta_dot2])
# 運動方程式
theta_ddot1 = control_input[0]
theta_ddot2 = control_input[1]
return [theta_dot1, theta_dot2, theta_ddot1, theta_ddot2]
# シミュレーションパラメータ
t = np.linspace(0, 10, 500)
Kp = np.array([1.0, 1.0])
Kd = np.array([0.5, 0.5])
initial_state = [theta1_init, theta2_init, theta_dot1_init, theta_dot2_init]
# シミュレーション
solution = odeint(dynamics, initial_state, t, args=(Kp, Kd))
# 結果のプロット
plt.figure(figsize=(12, 6))
plt.plot(t, solution[:, 0], label='Theta 1')
plt.plot(t, solution[:, 1], label='Theta 2')
plt.xlabel('Time (s)')
plt.ylabel('Joint Angles (rad)')
plt.title('Trajectory of Joint Angles Over Time')
plt.legend()
plt.grid(True)
plt.show()
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
# ロボットアームのパラメータ
l1 = 1.0 # リンク1の長さ (m)
l2 = 1.0 # リンク2の長さ (m)
# 関節角度の範囲
theta1 = np.linspace(-np.pi, np.pi, 100)
theta2 = np.linspace(-np.pi, np.pi, 100)
THETA1, THETA2 = np.meshgrid(theta1, theta2)
# ヤコビ行列の特異値を計算するための関数
def compute_jacobian(theta1, theta2):
J = np.zeros((2, 2))
J[0, 0] = -l1 * np.sin(theta1) - l2 * np.sin(theta1 + theta2)
J[0, 1] = -l2 * np.sin(theta1 + theta2)
J[1, 0] = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
J[1, 1] = l2 * np.cos(theta1 + theta2)
return J
# ヤコビ行列の特異値を計算
singular_values = np.zeros(THETA1.shape)
for i in range(THETA1.shape[0]):
for j in range(THETA1.shape[1]):
J = compute_jacobian(THETA1[i, j], THETA2[i, j])
_, s, _ = np.linalg.svd(J)
singular_values[i, j] = s[0] # 最大特異値を使用
# 3Dプロット
fig = plt.figure(figsize=(12, 8))
ax = fig.add_subplot(111, projection='3d')
# プロットデータ
ax.plot_surface(THETA1, THETA2, singular_values, cmap='viridis')
# ラベルとタイトル
ax.set_xlabel('Theta 1 (rad)')
ax.set_ylabel('Theta 2 (rad)')
ax.set_zlabel('Singular Value')
ax.set_title('3D Plot of the Singular Value of the Jacobian Matrix')
plt.show()
import numpy as np
import matplotlib.pyplot as plt
# ロボットアームのパラメータ
l1 = 1.0 # リンク1の長さ (m)
l2 = 1.0 # リンク2の長さ (m)
# 関節角度の範囲
theta1 = np.linspace(-np.pi, np.pi, 100)
theta2 = np.linspace(-np.pi, np.pi, 100)
THETA1, THETA2 = np.meshgrid(theta1, theta2)
# ヤコビ行列を計算する関数
def compute_jacobian(theta1, theta2):
J = np.zeros((2, 2))
J[0, 0] = -l1 * np.sin(theta1) - l2 * np.sin(theta1 + theta2)
J[0, 1] = -l2 * np.sin(theta1 + theta2)
J[1, 0] = l1 * np.cos(theta1) + l2 * np.cos(theta1 + theta2)
J[1, 1] = l2 * np.cos(theta1 + theta2)
return J
# ヤコビ行列の要素を計算
J11 = np.zeros(THETA1.shape)
J12 = np.zeros(THETA1.shape)
J21 = np.zeros(THETA1.shape)
J22 = np.zeros(THETA1.shape)
for i in range(THETA1.shape[0]):
for j in range(THETA1.shape[1]):
J = compute_jacobian(THETA1[i, j], THETA2[i, j])
J11[i, j] = J[0, 0]
J12[i, j] = J[0, 1]
J21[i, j] = J[1, 0]
J22[i, j] = J[1, 1]
# プロット
fig, axs = plt.subplots(2, 2, figsize=(12, 10))
# J11
c1 = axs[0, 0].contourf(THETA1, THETA2, J11, cmap='viridis')
fig.colorbar(c1, ax=axs[0, 0])
axs[0, 0].set_title('J11')
axs[0, 0].set_xlabel('Theta 1 (rad)')
axs[0, 0].set_ylabel('Theta 2 (rad)')
# J12
c2 = axs[0, 1].contourf(THETA1, THETA2, J12, cmap='viridis')
fig.colorbar(c2, ax=axs[0, 1])
axs[0, 1].set_title('J12')
axs[0, 1].set_xlabel('Theta 1 (rad)')
axs[0, 1].set_ylabel('Theta 2 (rad)')
# J21
c3 = axs[1, 0].contourf(THETA1, THETA2, J21, cmap='viridis')
fig.colorbar(c3, ax=axs[1, 0])
axs[1, 0].set_title('J21')
axs[1, 0].set_xlabel('Theta 1 (rad)')
axs[1, 0].set_ylabel('Theta 2 (rad)')
# J22
c4 = axs[1, 1].contourf(THETA1, THETA2, J22, cmap='viridis')
fig.colorbar(c4, ax=axs[1, 1])
axs[1, 1].set_title('J22')
axs[1, 1].set_xlabel('Theta 1 (rad)')
axs[1, 1].set_ylabel('Theta 2 (rad)')
plt.tight_layout()
plt.show()