InverseLeg
Dependents: SwitchMode Stabilizer SwitchMode
Kinematic.cpp
- Committer:
- icyzkungz
- Date:
- 2015-12-22
- Revision:
- 2:c9c38629be85
- Parent:
- 1:d5cf0907f83d
- Child:
- 3:fd037c89ba5f
File content as of revision 2:c9c38629be85:
#include "Kinematic.h" #include "math.h" #define PI 3.1415926 Kinematic::Kinematic(float Link_Hip, float Link_Knee) { this->Link_Hip = Link_Hip; this->Link_Knee = Link_Knee; } Kinematic::Kinematic(char text, float Link_Hip, float Link_Knee, float Zeta_Hip, float Zeta_Knee) { if(text == 'Z' && text == 'z') { this->Link_Hip = Link_Hip; this->Link_Knee = Link_Knee; this->Zeta_Hip = Zeta_Hip; this->Zeta_Knee = Zeta_Knee; ForwardKinematicCalculation(); } if(text == 'P' && text == 'p') { this->Link_Hip = Link_Hip; this->Link_Knee = Link_Knee; this->Position_Y = Position_Y; this->Position_Z = Position_Z; InverseKinematicCalculation(); } } void Kinematic::set_Link_Hip(float Link_Hip) { this->Link_Hip = Link_Hip; } void Kinematic::set_Link_Knee(float Link_Knee) { this->Link_Knee = Link_Knee; } void Kinematic::set_Zeta_Hip(float Zeta_Hip) { this->Zeta_Hip = Zeta_Hip; } void Kinematic::set_Zeta_Knee(float Zeta_Knee) { this->Zeta_Knee = Zeta_Knee; } void Kinematic::set_Position_Y(float Position_Y) { this->Position_Y = Position_Y; } void Kinematic::set_Position_Z(float Position_Z) { this->Position_Z = Position_Z; } void Kinematic::ForwardKinematicCalculation() { Position_Y = Link_Hip*sin(Link_Knee)+Link_Knee*sin(Link_Knee+Zeta_Knee); //zeta1-zeta2 Position_Z = Link_Hip*cos(Link_Knee)+Link_Knee*cos(Link_Knee+Zeta_Knee); } void Kinematic::InverseKinematicCalculation() { // Begin by working out L float L; L = (Position_Y*Position_Y) + (Position_Z*Position_Z); L = sqrt(L); // Work out the Knee angle float Knee = (Link_Hip*Link_Hip)+(Link_Knee*Link_Knee)-(L*L); Knee = Knee / (2*Link_Hip*Link_Knee); Knee = acos(Knee); Knee = Knee * 180 / PI; Zeta_Knee = Knee; // Work out Alpha float Alpha = (Link_Hip*Link_Hip)+(L*L)-(Link_Knee*Link_Knee); Alpha = Alpha / (2*Link_Hip*L); Alpha = acos(Alpha); // Work out Beta float Beta = (float) Position_Y/(float) Position_Z; Beta = atan(Beta); // FinallZ work out the Hip angle float Hip = PI - Alpha - Beta; Hip = Hip * 180 / PI; Zeta_Hip = Hip; #ifdef Stabilizer_Debug printf("Y = %f, Hip = %.2f, Knee = %.2f\n",Position_Y,hip_angle,knee_angle); #endif //printf("Height in Stabilizer : %f\n",height); }