"매 robot link 의 4 parameter 의 standard description". 매 robot manipulator 의 forward kinematics. 매 (a, α, d, θ) 의 의 매 link 의 transformation. 매 modern variant: 매 modified DH (Craig).
매 핵심
매 4 parameter (classical Denavit-Hartenberg)
a (link length): 매 z_{i-1} → z_i 의 X-axis 의 distance.
α (link twist): 매 z_{i-1} → z_i 의 angle.
d (link offset): 매 x_{i-1} → x_i 의 z-axis 의 distance.
θ (joint angle): 매 x_{i-1} → x_i 의 z-axis 의 rotation.
importnumpyasnpdefdh_matrix(a,alpha,d,theta):ca,sa=np.cos(alpha),np.sin(alpha)ct,st=np.cos(theta),np.sin(theta)returnnp.array([[ct,-st*ca,st*sa,a*ct],[st,ct*ca,-ct*sa,a*st],[0,sa,ca,d],[0,0,0,1],])defforward_kinematics(dh_table,joint_angles):"""dh_table: [(a, alpha, d, theta_offset), ...]"""T=np.eye(4)for(a,alpha,d,off),qinzip(dh_table,joint_angles):T=T@dh_matrix(a,alpha,d,off+q)returnT# 매 example: PUMA 560puma=[(0,np.pi/2,0,0),(0.4318,0,0,0),(0.0203,-np.pi/2,0.15,0),(0,np.pi/2,0.4318,0),(0,-np.pi/2,0,0),(0,0,0,0),]T=forward_kinematics(puma,[0,np.pi/4,-np.pi/4,0,np.pi/2,0])print(T[:3,3])# 매 end-effector position
# URDF 의 DH 의 convertimportxml.etree.ElementTreeasETdefurdf_to_dh(urdf_path):"""매 URDF joint 의 DH-style approx."""tree=ET.parse(urdf_path)dh=[]forjointintree.findall('joint'):ifjoint.attrib['type']=='revolute':origin=joint.find('origin')xyz=[float(x)forxinorigin.attrib['xyz'].split()]rpy=[float(x)forxinorigin.attrib['rpy'].split()]# 매 simplification — true DH extraction 의 nontrivialdh.append((xyz[0],rpy[0],xyz[2],0))returndh
Workspace visualization
defworkspace_sample(dh_table,joint_limits,n=5000):points=[]for_inrange(n):q=[np.random.uniform(lo,hi)forlo,hiinjoint_limits]p=forward_kinematics(dh_table,q)[:3,3]points.append(p)returnnp.array(points)# 매 plotimportmatplotlib.pyplotaspltpts=workspace_sample(puma,[(-np.pi,np.pi)]*6)fig=plt.figure()ax=fig.add_subplot(111,projection='3d')ax.scatter(pts[:,0],pts[:,1],pts[:,2],s=1)
Modified DH (Craig)
defmdh_matrix(a,alpha,d,theta):"""매 frame at proximal end."""ca,sa=np.cos(alpha),np.sin(alpha)ct,st=np.cos(theta),np.sin(theta)returnnp.array([[ct,-st,0,a],[st*ca,ct*ca,-sa,-d*sa],[st*sa,ct*sa,ca,d*ca],[0,0,0,1],])
매 결정 기준
상황
Approach
Standard manipulator
Classical DH
Avoiding singularity
Modified DH (Craig)
Modern simulation
URDF (rich features)
Closed-form IK
Pieper's solution (last 3 axes intersect)
Numerical IK
Jacobian-based
Beyond serial (parallel)
Stewart platform — DH X
기본값: 매 manipulator 의 DH + 매 forward kinematics + 매 numerical IK + 매 URDF for sim.