ロボット工学メモ



```

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()


```
https://www.ohmsha.co.jp/book/9784274224300/



```
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()
```

```


```


```


```