Hello,
Please can I have some help about an issue as I live control our xArm6 using python ( Data send from Swift).
Information :
Pitch is set by default at 90° (using python), forcing the arm head looking forward, like you can see in the video.
For exemple :
As the robot head reach 90° on Roll and keep 90° on the pitch (like rotating head right / left but still but still looking straight), the robot head become crazy. Can you explain why please?
(Under the video I join some codes)
Thanks in advance for you help and your time.
Regard’s
PYTHON :
# Calculate the final position for each Axis
# Correcting the coordinates from Vision Pro to match the xArm's coordinate system
final_z = (z-baseAVP_z)*-1
final_x = ((x*-1)- baseAVP_x)
final_y = (y - baseAVP_y)
# Add 90° to pitch for looking froward
final_pitch = 90+pitch
# Limit the final position to a maximum / minimum of 150mm
final_z = min(max(final_z, -150), 150)
final_x = min(max(final_x, -150), 150)
final_y = min(max(final_y, -150), 150)
print(f"Final position: {roll}, {final_pitch}, {yaw}")
# Send the command to the xArm (Using servo_cartesian for smooth live movements)
code = arm.set_servo_cartesian(
[final_z, final_x, final_y, roll, final_pitch, yaw],
speed=SPEED,
mvacc=ACCELERATION,
wait=False, is_radian=False, is_tool_coord=False
)
if code != 0:
print(f"Error moving arm, code: {code}")
return
SWIFT :
let toDeviceTransform = pose.originFromAnchorTransform
let devicePosition = toDeviceTransform.translation * 1000 // Convert meters to mm
let deviceRotation = toDeviceTransform.upper3x3
let orientation = rotationMatrixToEulerAngles(matrix: deviceRotation)
print(orientation)
if (accumulatedTime >= 0.05)
{
let dataToSend : [Float] =
[
devicePosition.x,
devicePosition.y,
devicePosition.z,
orientation.pitch,
orientation.roll,
orientation.yaw,
0.0
]
udpSender.send(values: dataToSend)
accumulatedTime -= 0.05
}
func rotationMatrixToEulerAngles(matrix: simd_float3x3) -> (yaw: Float, pitch: Float, roll: Float) {
let r00 = matrix[0, 0]
let r10 = matrix[1, 0]
let r20 = matrix[2, 0]
let r21 = matrix[2, 1]
let r22 = matrix[2, 2]
let pitch = asin(-r20)
let yaw = atan2(r10, r00)
let roll = atan2(r21, r22)
// Convert to degrees
let pitchDeg = pitch * (180.0 / .pi)
let yawDeg = yaw * (180.0 / .pi)
let rollDeg = roll * (180.0 / .pi)
return (yawDeg, pitchDeg, rollDeg)
}
