/** * This method returns a scalar that is the ammount of feedback that needs to be added to a trajectory. It is a function of the * phase in the controller's state (between 0 and 1), the vector between the stance foot and the center of mass, and the velocity of * the center of mass. */ override public float getFeedbackContribution(SimBiController con, CWJoint j, float phi, Vector3 d, Vector3 v) { Vector3 axis = Vector3.zero; if (MathLib.getAxisIndex(feedbackProjectionAxis) == 0) { axis = con.getCharacter().getLocalLeftAxis(); } else if (MathLib.getAxisIndex(feedbackProjectionAxis) == 1) { axis = con.getCharacter().getLocalUpAxis(); } else if (MathLib.getAxisIndex(feedbackProjectionAxis) == 2) { axis = con.getCharacter().getLocalFrontAxis(); } float dToUse = Vector3.Dot(d, axis); if (reversedCD && dToUse < 0) { dToUse = -dToUse; } float vToUse = Vector3.Dot(v, axis); if (reversedCV && vToUse < 0) { vToUse = -vToUse; } if (dToUse < dMin) { dToUse = dMin; } if (vToUse < vMin) { vToUse = vMin; } if (dToUse > dMax) { dToUse = dMax; } if (vToUse > vMax) { vToUse = vMax; } return(dToUse * cd + vToUse * cv); }
/** * This method returns a scalar that is the ammount of feedback that needs to be added to a trajectory. It is a function of the * phase in the controller's state (between 0 and 1), the vector between the stance foot and the center of mass, and the velocity of * the center of mass. */ override public float getFeedbackContribution(SimBiController con, CWJoint j, float phi, Vector3 d, Vector3 v) { CWRigidBody theBody; //Vector3 tmpP; float offset = 0; if (j != null) { theBody = j.getChild(); } else { theBody = con.getCharacter().getRoot(); } //we might want to initialize it (or add to it) some default value in case we are trying to control its projection offset = 0; float dToUse = Vector3.Dot(MathLib.getComplexConjugate(theBody.getOrientation()) * con.getDoubleStanceCOMError(), feedbackProjectionAxis); if (dToUse < dMin) { dToUse = dMin; } if (dToUse > dMax) { dToUse = dMax; } float vToUse = Vector3.Dot(MathLib.getComplexConjugate(theBody.getOrientation()) * con.getComVelocity(), feedbackProjectionAxis); if (vToUse < vMin) { vToUse = vMin; } if (vToUse > vMax) { vToUse = vMax; } float err = (dToUse * cd - vToUse * cv + offset); float result = err * totalMultiplier; //if (j) // tprintf("%s gets: %lf\n", j->getName(), result); // else //tprintf("root gets: %lf\n", result); if (result < minFeedbackValue) { result = minFeedbackValue; } if (result > maxFeedbackValue) { result = maxFeedbackValue; } // if (j == NULL && cv > 0.4) // logPrint("%lf\t%lf\t%lf\t%lf\t%lf\t%lf\n", theBody->getLocalCoordinates(con->COMOffset).dotProductWith(feedbackProjectionAxis), dToUse, theBody->getLocalCoordinates(con->comVelocity).dotProductWith(feedbackProjectionAxis), vToUse, result, forceRatioWeight); return(result); }
virtual public float getFeedbackContribution(SimBiController con, CWJoint j, float phi, Vector3 d, Vector3 v) { return(0); }