Source

puma / ik.h

#pragma once

/** Combine the DH parameters for FK/IK and limits/scaling for drive
 *
 * Rotations are right-hand rule!  (Counter clockwise)
 *
 * 1. Translate by d along the new z axis.
 * 2. Rotate by the new theta about the new z axis.
 * 3. Translate by a along the old x axis.
 * 4. Rotate by a about the old x axis.
 *
 * Note that for the PUMA the straight-up home position is
 * [0, -90, 90, 0, 0, 0], not all zeros.  This is since the X
 * axis is aligned with the horizontal plane.
 */
typedef struct {
	// DH parameters
	double d;
	double a;
	double alpha;
	double angle; // offset from "0" in the DH map

	// Mechanical details about the joint
	double coupled; // amount this axis is cross coupled to the previous
	double scale; // rads to counter ticks
	int min;
	int max;
} joint_cfg_t;

extern int
ik_first(
	const joint_cfg_t * const joints,
	double * const theta, // output commands
	const double * const xyz, // desired position (in mm)
	const int right, // right arm = +1, left arm = -1
	const int above // elbow above arm = +1, below arm = -1
);


extern int
ik_wrist(
	const joint_cfg_t * const joints,
	double * const theta, // input(0-2)/output(3-5) commands
	const double * const xyz, // desired position (in mm)
	const double * const a, // desired approach vector
	const double * const s, // desired sliding vector (for hand opening)
	const double * const n, // desired normal vector
	const int wrist // wrist up = +1, wrist down = -1
);