MATEMATIKA &
FISIKA

Transformasi homogen adalah representasi seragam untuk translasi + rotasi dalam satu matriks 4×4. Fundamental untuk semua perhitungan pose robot.

import numpy as np

def Rx(t): c,s=np.cos(t),np.sin(t); return np.array([[1,0,0],[0,c,-s],[0,s,c]])
def Ry(t): c,s=np.cos(t),np.sin(t); return np.array([[c,0,s],[0,1,0],[-s,0,c]])
def Rz(t): c,s=np.cos(t),np.sin(t); return np.array([[c,-s,0],[s,c,0],[0,0,1]])

def T_mat(R, p):
    T = np.eye(4); T[:3,:3]=R; T[:3,3]=p; return T

def T_inv(T):
    R=T[:3,:3]; p=T[:3,3]
    Ti=np.eye(4); Ti[:3,:3]=R.T; Ti[:3,3]=-R.T@p
    return Ti

# Contoh: transformasi sensor → base frame
R_sensor = Rz(np.radians(90)) @ Rx(np.radians(-10))
p_sensor = np.array([0.15, 0, 0.30])
T_sensor = T_mat(R_sensor, p_sensor)

# Point di frame sensor → transformasi ke frame robot
p_s = np.array([1.5, 0, 0, 1])  # homogeneous
p_robot = T_sensor @ p_s
print(f"Point in robot frame: {p_robot[:3]}")

import numpy as np

def quat_mult(q1, q2):
    w1,x1,y1,z1 = q1; w2,x2,y2,z2 = q2
    return np.array([
        w1*w2 - x1*x2 - y1*y2 - z1*z2,
        w1*x2 + x1*w2 + y1*z2 - z1*y2,
        w1*y2 - x1*z2 + y1*w2 + z1*x2,
        w1*z2 + x1*y2 - y1*x2 + z1*w2])

def quat_rotate(q, v):
    qv = np.array([0, *v])
    q_conj = np.array([q[0], -q[1], -q[2], -q[3]])
    return quat_mult(quat_mult(q, qv), q_conj)[1:]

def quat_from_axis_angle(axis, angle_rad):
    n = np.array(axis) / np.linalg.norm(axis)
    s = np.sin(angle_rad/2)
    return np.array([np.cos(angle_rad/2), *n*s])

def slerp(q1, q2, t):
    """Spherical linear interpolation, t in [0,1]"""
    dot = np.clip(np.dot(q1,q2), -1,1)
    if dot < 0: q2=-q2; dot=-dot
    if dot > 0.9995: return (q1+t*(q2-q1)) / np.linalg.norm(q1+t*(q2-q1))
    theta = np.arccos(dot)
    return (np.sin((1-t)*theta)*q1 + np.sin(t*theta)*q2) / np.sin(theta)

def quat_to_euler(q):
    w,x,y,z = q
    roll  = np.arctan2(2*(w*x+y*z), 1-2*(x*x+y*y))
    pitch = np.arcsin(np.clip(2*(w*y-z*x),-1,1))
    yaw   = np.arctan2(2*(w*z+x*y), 1-2*(y*y+z*z))
    return np.degrees([roll, pitch, yaw])

import numpy as np

def skew(w):
    """Skew-symmetric matrix dari vektor 3D"""
    return np.array([[0,-w[2],w[1]],[w[2],0,-w[0]],[-w[1],w[0],0]])

def rodrigues(omega):
    """Axis-angle vector omega → rotation matrix (Rodrigues)"""
    theta = np.linalg.norm(omega)
    if theta < 1e-8: return np.eye(3)
    k = omega / theta
    K = skew(k)
    return np.eye(3) + np.sin(theta)*K + (1-np.cos(theta))*(K@K)

def log_SO3(R):
    """Rotation matrix → axis-angle vector"""
    theta = np.arccos(np.clip((np.trace(R)-1)/2,-1,1))
    if abs(theta) < 1e-8: return np.zeros(3)
    w = np.array([R[2,1]-R[1,2], R[0,2]-R[2,0], R[1,0]-R[0,1]])
    return theta / (2*np.sin(theta)) * w

import numpy as np

def geometric_jacobian(T_list, joint_types=None):
    """Compute 6×n geometric Jacobian from list of transforms T_0_i"""
    n = len(T_list) - 1
    if joint_types is None: joint_types = ['R']*n  # all revolute
    p_ee = T_list[-1][:3,3]  # end-effector position
    J = np.zeros((6, n))
    for i in range(n):
        z = T_list[i][:3,2]  # z-axis of frame i
        p = T_list[i][:3,3]  # origin of frame i
        if joint_types[i] == 'R':
            J[:3,i] = np.cross(z, p_ee - p)
            J[3:,i] = z
        else:  # Prismatic
            J[:3,i] = z
            J[3:,i] = np.zeros(3)
    return J

def damped_pseudoinverse(J, lam=0.01):
    """Damped least squares: lebih stabil di singularity"""
    return J.T @ np.linalg.inv([email protected] + lam**2*np.eye(J.shape[0]))

import numpy as np

def robot_dynamics_2dof(q, qdot, tau,
                          m1=1.0, m2=0.5,
                          l1=0.3, l2=0.2,
                          lc1=0.15, lc2=0.10,
                          I1=0.05, I2=0.02, g=9.81):
    """2-DOF planar robot dynamics via Lagrangian"""
    q1,q2 = q; dq1,dq2 = qdot
    c2 = np.cos(q2)

    # Inertia matrix M(q)
    M11 = m1*lc1**2 + I1 + m2*(l1**2 + lc2**2 + 2*l1*lc2*c2) + I2
    M12 = m2*(lc2**2 + l1*lc2*c2) + I2
    M22 = m2*lc2**2 + I2
    M = np.array([[M11,M12],[M12,M22]])

    # Coriolis + centripetal C(q,qdot)
    h = -m2*l1*lc2*np.sin(q2)
    C = np.array([[h*dq2, h*(dq1+dq2)],[-h*dq1, 0]])

    # Gravity G(q)
    G1 = (m1*lc1 + m2*l1)*g*np.cos(q1) + m2*g*lc2*np.cos(q1+q2)
    G2 = m2*g*lc2*np.cos(q1+q2)
    G = np.array([G1,G2])

    # Acceleration: M·qddot = tau - C·qdot - G
    qddot = np.linalg.solve(M, np.array(tau) - C@np.array(qdot) - G)
    return qddot, M, C, G

import numpy as np

class KalmanFilter:
    def __init__(self, F, H, Q, R, x0, P0):
        self.F=np.array(F); self.H=np.array(H)
        self.Q=np.array(Q); self.R=np.array(R)
        self.x=np.array(x0,dtype=float)
        self.P=np.array(P0,dtype=float)
        self.n=self.x.shape[0]

    def predict(self, u=None, B=None):
        self.x = self.F @ self.x
        if u is not None and B is not None:
            self.x += np.array(B) @ np.array(u)
        self.P = self.F @ self.P @ self.F.T + self.Q

    def update(self, z):
        z = np.array(z)
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ (z - self.H @ self.x)
        self.P = (np.eye(self.n) - K @ self.H) @ self.P

# Contoh: estimasi posisi 1D dari GPS noisy
dt=0.1
kf=KalmanFilter(
  F=[[1,dt],[0,1]], H=[[1,0]],
  Q=[[0.01,0],[0,0.1]], R=[[0.5]],
  x0=[0,0], P0=np.eye(2)*1.0)
for z in [0.1,0.25,0.51,0.78,1.02]:
  kf.predict(); kf.update([z])
  print(f"pos={kf.x[0]:.3f}, vel={kf.x[1]:.3f}")

import numpy as np

class RobotEKF:
    """EKF untuk lokalisasi mobile robot: state=[x,y,θ,v,ω]"""
    def __init__(self, dt=0.02):
        self.dt=dt; self.x=np.zeros(5)
        self.P=np.diag([0.1,0.1,0.05,0.01,0.01])
        self.Q=np.diag([1e-4,1e-4,1e-4,0.001,0.001])

    def f(self, x, u=None):
        dt=self.dt; xn=x.copy()
        xn[0]+=x[3]*np.cos(x[2])*dt
        xn[1]+=x[3]*np.sin(x[2])*dt
        xn[2]+=x[4]*dt
        return xn

    def F_jac(self, x):
        dt=self.dt; th=x[2]; v=x[3]
        F=np.eye(5)
        F[0,2]=-v*np.sin(th)*dt; F[0,3]=np.cos(th)*dt
        F[1,2]= v*np.cos(th)*dt; F[1,3]=np.sin(th)*dt
        F[2,4]=dt
        return F

    def predict(self):
        F=self.F_jac(self.x)
        self.x=self.f(self.x)
        [email protected]@F.T+self.Q

    def update_gps(self, gx, gy):
        H=np.zeros((2,5)); H[0,0]=H[1,1]=1
        R=np.diag([0.5,0.5])
        z=np.array([gx,gy]); [email protected]
        [email protected]@H.T+R; [email protected]@np.linalg.inv(S)
        self.x+=K@y; self.P=(np.eye(5)-K@H)@self.P

class PID:
    def __init__(self, kp, ki, kd, i_max=100, dt=0.02, d_filter=0.1):
        self.kp=kp; self.ki=ki; self.kd=kd
        self.i_max=i_max; self.dt=dt
        self.i=0; self.e_prev=0; self.d_filt=0
        self.alpha=d_filter  # D term low-pass coeff

    def compute(self, setpoint, measured):
        e = setpoint - measured
        self.i += e * self.dt
        self.i  = max(-self.i_max, min(self.i_max, self.i))
        d_raw = (e - self.e_prev) / self.dt
        self.d_filt = self.alpha*d_raw + (1-self.alpha)*self.d_filt
        out = self.kp*e + self.ki*self.i + self.kd*self.d_filt
        self.e_prev = e
        return out

    def reset(self): self.i=0; self.e_prev=0; self.d_filt=0

# Contoh: velocity PID untuk robot wheel
vel_pid = PID(kp=200, ki=15, kd=8, i_max=80)
pwm = vel_pid.compute(target_vel_ms, actual_vel_ms)

Cascade Control (Position + Velocity)

import numpy as np
from scipy.linalg import solve_continuous_are, expm
from scipy.signal import place_poles

def lqr(A, B, Q, R):
    """Linear Quadratic Regulator — optimal state feedback gain"""
    P = solve_continuous_are(A, B, Q, R)
    K = np.linalg.inv(R) @ B.T @ P
    return K, P

# Contoh: inverted pendulum balancing (linearized)
g=9.81; L=0.3; m=0.1; M=0.5
A=np.array([[0,1,0,0],[0,0,-m*g/M,0],[0,0,0,1],[0,0,g*(M+m)/(M*L),0]])
B=np.array([[0],[1/M],[0],[-1/(M*L)]])
Q=np.diag([1,1,10,1])
R=np.array([[0.1]])
K,P = lqr(A,B,Q,R)
print("LQR gain K:", K)

# Discrete simulation:
dt=0.01
Ad = expm(A*dt)
Bd = np.linalg.solve(A, (Ad-np.eye(4))) @ B
x=np.array([0,0,0.1,0])  # initial: 0.1 rad tilt
for _ in range(500):
    u = -K @ x
    x = Ad @ x + Bd @ u
print(f"Final angle: {np.degrees(x[2]):.4f}°")

import numpy as np

class Adam:
    def __init__(self, params, lr=0.001, b1=0.9, b2=0.999, eps=1e-8):
        self.p=np.array(params,dtype=float)
        self.lr=lr; self.b1=b1; self.b2=b2; self.eps=eps
        self.m=np.zeros_like(self.p); self.v=np.zeros_like(self.p); self.t=0

    def step(self, grad):
        self.t+=1; g=np.array(grad)
        self.m=self.b1*self.m+(1-self.b1)*g
        self.v=self.b2*self.v+(1-self.b2)*g**2
        mh=self.m/(1-self.b1**self.t)
        vh=self.v/(1-self.b2**self.t)
        self.p-=self.lr*mh/(np.sqrt(vh)+self.eps)
        return self.p.copy()

# Nelder-Mead untuk PID tuning otomatis:
from scipy.optimize import minimize

def pid_cost(params):
    kp,ki,kd = params
    if any(x < 0 for x in params): return 1e9
    sim = simulate_robot(kp, ki, kd)
    overshoot = max(0, sim.max()-1) * 100
    settle = sim.settle_time()
    sse = np.sum((sim.response-1)**2)
    return overshoot*0.5 + settle*2 + sse*0.1

result = minimize(pid_cost, [100,10,5], method='Nelder-Mead')
print("Optimal PID:", result.x)

Library Ekosistem

import sympy as sp

# Symbolic Jacobian derivation
q1, q2, L1, L2 = sp.symbols('q1 q2 L1 L2')

# Forward kinematics 2-DOF planar
x = L1*sp.cos(q1) + L2*sp.cos(q1+q2)
y = L1*sp.sin(q1) + L2*sp.sin(q1+q2)

# Symbolic Jacobian
J = sp.Matrix([[sp.diff(x,q1), sp.diff(x,q2)],
               [sp.diff(y,q1), sp.diff(y,q2)]])
J_simplified = sp.simplify(J)
print(J_simplified)

# Determinant (untuk singularity analysis)
det_J = sp.det(J_simplified)
print("det(J) =", sp.simplify(det_J))
# det = 0 → singularity condition

# Convert to numpy function:
J_func = sp.lambdify([q1,q2,L1,L2], J_simplified, 'numpy')
import numpy as np
J_num = J_func(np.radians(30), np.radians(45), 0.15, 0.12)
print("Numeric J:", J_num)