Exemple #1
0
/**
 * Set foot posiion and orientation based off of where the metajoint, X Y and Z, were set. 
 */
void LowerBodyLeg::setInverse(){
    checkGoalsReached();
    if(!updated){
        unsetAll();
        return;
    }

    if (!allSet()) //wait for all joints
        return;

    double foot_x = 0;
    double foot_y = 0;
    double foot_z = 0;
    double foot_yaw = 0;

    parameters[FOOT_X]->get(INTERPOLATION_STEP, foot_x);
    parameters[FOOT_Y]->get(INTERPOLATION_STEP, foot_y);
    parameters[FOOT_Z]->get(INTERPOLATION_STEP, foot_z);
    parameters[FOOT_YAW]->get(INTERPOLATION_STEP, foot_yaw);

    if(globalFlag){
        // Convert position to hip roll reference frame and set joint angles
        setHipRollXYZ(cos(foot_yaw)*foot_x + sin(foot_yaw)*foot_y, cos(foot_yaw)*foot_y - sin(foot_yaw)*foot_x, foot_z, foot_yaw);
    }
    else{
        setHipRollXYZ(foot_x, foot_y, foot_z, foot_yaw);
    }

    unsetAll();
}
Exemple #2
0
MMSButtonWidgetClass::MMSButtonWidgetClass() {
    unsetAll();
}
Exemple #3
0
MMSThemeClass::MMSThemeClass() {
    unsetAll();
}