config/mp-robot-d.h
// relation between physical and logical angles based on robot kinematic coupling.
relation between physical and logical angles based on robot kinematic coupling.
relation between physical and logical angles based on robot kinematic coupling .
void logicalToPhysicalAngles(float angles[6]) {
    float alpha  = angles[2];
    float alpha_ = alpha + PI / 2;
    float diag_2 = sqrt(v_1 * v_1 - 2 * v_1 * h_3 * cos(alpha_) + h_3 * h_3);
    float beta_1 = acos((-h_2 * h_2 + h_1 * h_1 + diag_2 * diag_2) / (2 * h_1 * diag_2));
    float beta_2 = acos((-h_3 * h_3 + v_1 * v_1 + diag_2 * diag_2) / (2 * v_1 * diag_2));
    angles[2] = PI / 2 - (beta_1 + beta_2);
    angles[2] += angles[1];
}
void logicalToPhysicalAngles ( float angles [ 6 ] ) { float alpha = angles [ 2 ] ; float alpha_ = alpha + PI / 2 ; float diag_2 = sqrt ( v_1 * v_1 - 2 * v_1 * h_3 * cos ( alpha_ ) + h_3 * h_3 ) ; float beta_1 = acos ( ( - h_2 * h_2 + h_1 * h_1 + diag_2 * diag_2 ) / ( 2 * h_1 * diag_2 ) ) ; float beta_2 = acos ( ( - h_3 * h_3 + v_1 * v_1 + diag_2 * diag_2 ) / ( 2 * v_1 * diag_2 ) ) ; angles [ 2 ] = PI / 2 - ( beta_1 + beta_2 ) ; angles [ 2 ] += angles [ 1 ] ; }
relation between physical and logical angles based on robot kinematic coupling.
relation between physical and logical angles based on robot kinematic coupling .