Position and Posture

Measure Manipulation 3.1.36

manipulation = measure_manipulation(joints)

Manipulation is nearly zero near the Singular Positions and Workspace Boundaries

Usually, under strict conditions, less than 0.01 is considered poor flexibility, while under relaxed conditions, less than 0.001 is considered poor flexibility.

• Parameters
  • joints: Joint angles: {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14}
• Returns
  • manipulation manipulation

Example Program

cpose = {-0.3, -0.3, 0.3, 0, 0, 0}
jpose = kinematics_inverse(cpose)
if not jpose["ok"]
then
  error("kinematics_inverse failed")
end
manipulation = measure_manipulation(jpose)
print(manipulation)
if manipulation < 0.01
then
  error("manipulation too low")
end
move_hybrid(jpose, 0.1, 0.1, 0, 0)

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)