KinematicChainsArduino/Limb.h at master · KinematicsArduino/KinematicChainsArduino · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
/*
 * Limb.h
 *
 *  Created on: Apr 27, 2021
 *      Author: aksha
 */

#ifndef LIBRARIES_KINEMATICCHAINSARDUINO_LIMB_H_
#define LIBRARIES_KINEMATICCHAINSARDUINO_LIMB_H_

#include "Link.h"
#include "IKSolver.h"
#include <Geometry.h>

#define maxLinks 20

class Limb {
private:
	int limbIndex;
	int numberOfLinks = 0;
	const char *limbName;
	Link *links[maxLinks];
	Matrix<4, 4> fiducialtoLimbRoot;
	hardwareManager *hwLocal;
	IKSolver *ik = NULL;
public:
	Limb(int index, const char *Name, Matrix<4, 4> limbRoot,
			hardwareManager *hwptr);
	void addLinkPtr(Link *linkPTR);
	Matrix<4, 4>& FK(Matrix<4, 4> &GlobalTransform, float *currAngles);
	IKResult IK(Matrix<4, 4> &Target, float *Result);
	IKResult cacheValue(float valueInDegrees, int linkIndex);
	int GetNumberOfLinks();
};
#endif /* LIBRARIES_KINEMATICCHAINSARDUINO_LIMB_H_ */