Position and Orientation

Estimate Pose Manipulability 3.1.36

Near singular positions and workspace boundaries, manipulability approaches 0.

Generally, values less than 0.001 under strict conditions are considered poor, and less than 0.0001 under relaxed conditions.

• Function: measure_manipulation from the lua module
• Parameters:
  1. joints: [JointPose] Joint angles (radians), e.g. {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14}
• Returns: [float] Manipulability, 0~1, higher is more flexible

Example

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.001 then
  error("manipulation too low")
end
movel(jpose, 0.1, 0.1, 0, 0)

Forward Kinematics

Converts joint angles to Cartesian position and orientation via robot forward kinematics.

• Function: kinematics_forward from the lua module
• Parameters:
  1. joints: [JointPose] Joint angles (radians), e.g. {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14}
• Returns: [KinResult] Cartesian position and orientation, format {x, y, z, Rz, Ry, Rx, ok}, e.g. {-0.364354, 0.255486, 0.147719, 0.276500, 0.913569, -2.982690, ok=true}. Where ok indicates whether solving was successful, true: success, false: failure

Example

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

Converts Cartesian position and orientation to joint angles via robot inverse kinematics. The result depends on current TCP settings and current joint position.

• Function: kinematics_inverse from the lua module
• Parameters:
  1. vector: [CartesianPose] Tool space position and orientation
  2. joints: [JointPose] Joint space reference position. Optional, defaults to current feedback joint position. When multiple solutions exist, the one closest to joints is selected
• Returns: [KinResult] Joint position, format {j1=a, j2=b, j3=c, j4=d, j5=e, j6=f, ok}, e.g. {j1=0, j2=1.57, j3=-1.57, j4=0, j5=3.14, j6=3.14, ok=true}. Where ok indicates whether solving was successful, true: success, false: failure

Example

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

Pose Frame Transformation

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

Physical meaning: taking a as user frame $\{A\}$, b is the pose description relative to frame $\{A\}$. Returns pose description relative to robot world frame, which can be used for move commands.

• Function: pose_times from the lua module
• Parameters:
  1. a: [Pose] Pose $A$
  2. b: [Pose] Pose $B$
• Returns: [CartesianPose] Pose $C = AB$

Example

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)

Pose Inverse

Used to find the pose inverse of pose a's corresponding homogeneous matrix $A$, the inverse's pose description. Can be used to solve pose equations.

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

Given user frame pose a (can be a teaching point), then teach another point b. Using this method, you can find b's description relative to a. After calculating this result, when the user frame changes but the relative position of teaching points remains unchanged, re-teaching is not required. $AX=B \to X=A^{-1}B$

• Function: pose_inverse from the lua module
• Parameters:
  1. a: [Pose] Pose $A$
• Returns: [CartesianPose] Inverse of $A$, $A^{-1}$

Example

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

pre_drop = {-0.159, -0.342, -0.0391, -2.97, -0.017, -3.14}
drop = {-0.044, -0.0036, -0.0004, 2.89, 0, 0}
local relative_drop = axb(pre_drop, drop)
print(pose_times(pre_drop, relative_drop))
print(drop)

Pose Addition 3.1.13

From base position, moves along frame direction by delta to get new pose.

• Function: pose_add from the lua module
• Parameters:
  1. base: [Pose] Starting pose
  2. delta: [CartesianPose] Pose offset
  3. frame: [CartesianPose] Pose offset direction, only orientation part is valid. Optional, defaults to base direction
• Returns: [CartesianPose] Pose after movement

Example

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 tilt upward
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)

LuaApi Pose to SDK Pose 3.1.24

Converts LuaApi pose to SDK pose.

• Function: sdk_pose from the lua module
• Parameters:
  1. p: [Pose] Lua API format pose, e.g. {j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}
• Returns: [Pose] SDK format pose, e.g. {0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}

Example

p = {j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}
pose = sdk_pose(p) -- {0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}

SDK Pose to LuaApi Pose 3.1.24

Converts SDK pose to LuaApi pose.

• Function: lua_pose from the lua module
• Parameters:
  1. p: [Pose] SDK format pose, e.g. {0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}
• Returns: [Pose] Lua API format pose, e.g. {j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}

Example

p = {0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}
pose = lua_pose(p) -- {j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}