Source

puma / ik.h

Full commit
Trammell Hudson ec96604 

Trammell Hudson 0644cea 












Trammell Hudson 61085f2 
Trammell Hudson 0644cea 
Trammell Hudson 61085f2 

Trammell Hudson 0644cea 




Trammell Hudson 61085f2 




Trammell Hudson ec96604 

Trammell Hudson 61085f2 
Trammell Hudson ec96604 
Trammell Hudson 1885e59 
Trammell Hudson ec96604 


Trammell Hudson 1885e59 



Trammell Hudson 61085f2 

Trammell Hudson 1885e59 




#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
);