Position and Posture

Forward Kinematics

cpose = kinematics_forward(joints)

Convert joint angles to Cartesian position and posture through the robot's forward kinematics.

• Parameters
  • joints: Joint angles: {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14}
• Returns {x, y, z, Rz, Ry, Rx, ok=true}
  • For example: {-0.36435443592898153, 0.25548551781114875, 0.1477188562117689, 0.27649950521632694, 0.913569441486944, -2.9826903521006582, ok=true}
  • ok: Whether the calculation was successful. true: success, false: failure

Example Program

p = kinematics_forward({j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14})
for k,v in ipairs(p) do
  print(k,v)
end
print(p["ok"])

Inverse Kinematics

jpose = kinematics_inverse(vector, joints)

Convert Cartesian position and posture to joint angles through the robot's inverse kinematics. The calculation result is related to the current TCP settings and current joint position.

• Parameters
  • vector: Position and posture in tool space.
  • joints: Reference position in joint space. Optional, default is current feedback joint position. When multiple solutions exist for inverse kinematics, it will choose the solution closest to joints.
• Returns {j1=a, j2=b, j3=c, j4=d, j5=e, j6=f, ok=true}
  • Corresponding to joint space, e.g., {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14, ok=true}
  • ok: Whether the calculation was successful. true: success, false: failure

Example Program

p = kinematics_inverse({1.12,2.12,3.12,4.12,.125,6.12})
for k,v in ipairs(p) do
  print(k,v)
end
print(p["ok"])

Pose Feature Coordinate System Transformation

c = pose_times(a, b)

The algorithm converts a and b to 4×4 homogeneous matrices $A$ and $B$ respectively, then calculates the matrix multiplication $C=AB$, and finally converts $C$ back to pose representation.

The physical meaning is equivalent to, with a as the user coordinate system $\{A\}$, b is the pose description relative to coordinate system $\{A\}$. Finally, it returns the pose description relative to the robot's world coordinate system, which can be used for move series instructions.

• Parameters
  • a: Pose $A$.
  • b: Pose $B$.
• Returns
  • c: Pose $C = AB$.

Example Program

local res = pose_times({-0.159,-0.342,-0.0391,-2.97,-0.017,-3.14}, {-0.044,-0.0036,-0.0004,3.89,0,0})
movej(res, 1, 1, 0, 1)

Inverse of Pose

c = pose_inverse(a)

Used to calculate the pose description of the inverse of the homogeneous matrix $A$ corresponding to pose a. Can be used to solve pose equations.

The inverse of a homogeneous matrix equals its transpose $A^{-1}=A^T$.

• Parameters
  • a: Pose $A$.
• Returns
  • c: Inverse of $A$, $A^{-1}$.

Example Program

• Given the user coordinate system pose a (can be a certain teaching point), and then teach another point b, this method can be used to calculate the description of b relative to a.
• After calculating this result, when the user coordinate system changes but the relative position of the teaching points remains unchanged, there's no need to reteach.
• $AX=B \to X=A^{-1}B$

function axb(a, b)
  return pose_times(pose_inverse(a), b)
end

pre_cup_drop = {-0.159,-0.342,-0.0391,-2.97,-0.017,-3.14}
cup_drop = {-0.044,-0.0036,-0.0004,2.89,0,0}
local relative_cup_drop = axb(pre_cup_drop, cup_drop)
print(pose_times(pre_cup_drop, relative_cup_drop))
print(cup_drop)

Addition of Pose 3.1.13

pose = pose_add(base, delta, frame)

The pose after moving delta from the base position in the frame direction.

• Parameters
  • base: Starting pose.
  • delta: Pose offset.
  • frame: Direction of pose offset, only the posture part is valid. Can be empty, default is base direction.
• Returns
  • pose: Pose after movement.

Example Program

base = {-0.4,0,0.1,-1.57,0,1.57}
frame = {0,0,0,0,-0.78,0} -- Rotate 45° around y-axis, making z-axis slant upwards
delta = {0,0,0.1,0,0,0} -- Move 0.1m along z-axis direction
pose = pose_add(base, delta, frame)
movej(base, 0.4, 0.1)
movel(pose, 0.4, 0.1)