bool Event::isInfoEqual(const std::string& Key, const DoubleValue& Value) const
{
DoubleValue TmpValue;
return (getInfoAsDoubleValue(Key,TmpValue) &&
openfluid::scientific::isCloseEnough(TmpValue.get(),Value.get(),0.00001));
}
/**
* If getObservedValue() is NULL, then this method does nothing.
* If there is an observedValue set AND the value is a subtype of
* DoubleValue or IntValue, then the content of this value is set.
*
* Users may override this default implementation in subclasses to obtain
* a different calculation behavior. The default implementation is only
* there for convendience, as this way of usage is very common.
*
* @param index the index to write the calculated values to.
* @return true if successful, false if there was an observed value with incompatible type.
*/
bool StatisticCalculator::calculateNextValue(int index) {
if(mObservedValue == 0) {
return true;
}
DoubleValue *doubleValue = dynamic_cast<DoubleValue*>(mObservedValue);
if(doubleValue != 0) {
setValue(index, doubleValue->get());
return true;
}
IntValue *intValue = dynamic_cast<IntValue*>(mObservedValue);
if(intValue != 0) {
setValue(index, intValue->get());
return true;
}
return false;
}
double DynamicsPlotterUtil::getMeanValue(const QList<DoubleValue*> &valuesList) {
if(valuesList.isEmpty()) {
reportProblem("DynamicsPlotterUtil::getMeanValue : Empty list. Nothing to do.");
return 0;
}
int nrValues = valuesList.size();
double meanValue = 0;
for(int currVal = 0; currVal < nrValues; ++currVal) {
DoubleValue* dVal = valuesList.at(currVal);
if(dVal == 0) {
reportProblem("DynamicsPlotterUtil::getMeanValue : Encountered NULL element.");
return 0;
}
meanValue += dVal->get();
}
return meanValue / nrValues;
}
/**
* Updates the history with the current value of the observed Value object.
* Does nothing, if the history is set to static or if no Value object is set.
*/
void ValuePlotterItem::update() {
if(mValue == 0 || mHasStaticHistory) {
return;
}
double currentValue = 0.0;
{
//if DoubleValue
DoubleValue *dv = dynamic_cast<DoubleValue*>(mValue);
if(dv != 0) {
currentValue = dv->get();
}
else {
IntValue *iv = dynamic_cast<IntValue*>(mValue);
if(iv != 0) {
currentValue = (double) iv->get();
}
else {
qDebug("PlotterItem: Value was not DoubleValue or IntValue!");
}
}
}
addToHistory(currentValue);
return;
}
void BasinPlotter::calculateData() {
// get program core
Core *core = Core::getInstance();
// get network
ModularNeuralNetwork *network = getCurrentNetwork();
if(network == 0) {
Core::log("BasinPlotter: Could not find a neural network to work with! Aborting.", true);
return;
}
QList<NeuralNetworkElement*> networkElements;
network->getNetworkElements(networkElements);
QList<DoubleValue*> networkValues =
DynamicsPlotterUtil::getNetworkValues(networkElements);
// Get parameters for varied elements
QString variedX = mVariedX->get();
QString variedY = mVariedY->get();
if(variedX.isEmpty() || variedY.isEmpty()) {
reportProblem("BasinPlotter: No elements to vary.");
return;
}
DoubleValue *variedValX = DynamicsPlotterUtil::getElementValue(
variedX, networkElements, &mNeuronsWithActivationsToTransfer);
DoubleValue *variedValY = DynamicsPlotterUtil::getElementValue(
variedY, networkElements, &mNeuronsWithActivationsToTransfer);
if(variedValX == 0 || variedValY == 0) {
reportProblem("BasinPlotter: NULL pointer for varied element. Aborting.");
return;
}
QList<double> variedRangeX =
DynamicsPlotterUtil::getDoublesFromString(mVariedRangeX->get());
QList<double> variedRangeY =
DynamicsPlotterUtil::getDoublesFromString(mVariedRangeY->get());
if(variedRangeX.size() != 2 || variedRangeY.size() != 2) {
reportProblem("BasinPlotter: Not a valid range given.");
return;
}
int resolutionX = mResolutionX->get();
int resolutionY = mResolutionY->get();
//avoid division by zero!
if(resolutionX < 2 || resolutionY < 2) {
reportProblem("BasinPlotter: Invalid resolution given.");
return;
}
// projected elements
int nrProjections = 0;
QString projectionsX = mProjectionsX->get();
QString projectionsY = mProjectionsY->get();
QList< QList<DoubleValue*> > projectionValuesX;
QList< QList<DoubleValue*> > projectionValuesY;
QList<double> projectionRangesX;
QList<double> projectionRangesY;
if(projectionsX != "0" && projectionsY != "0") {
QList<QStringList> projectionListX =
DynamicsPlotterUtil::parseElementString(projectionsX);
QList<QStringList> projectionListY =
DynamicsPlotterUtil::parseElementString(projectionsY);
projectionValuesX =
DynamicsPlotterUtil::getElementValues(projectionListX, networkElements);
projectionValuesY =
DynamicsPlotterUtil::getElementValues(projectionListY, networkElements);
if(projectionValuesX.isEmpty() || projectionValuesY.isEmpty()) {
reportProblem("BasinPlotter: Could not find specified elements to project onto.");
return;
}
if(projectionValuesX.size() != projectionValuesY.size()) {
reportProblem("BasinPlotter: Mismatching number of projected elements for the two axes.");
return;
}
projectionRangesX =
DynamicsPlotterUtil::getDoublesFromString(mProjectionRangesX->get());
projectionRangesY =
DynamicsPlotterUtil::getDoublesFromString(mProjectionRangesY->get());
if(projectionRangesX.size() != 2*projectionValuesX.size() ||
projectionRangesY.size() != 2*projectionValuesY.size()) {
reportProblem("BasinPlotter: Given ranges for projection don't match number of elements.");
return;
}
nrProjections = projectionValuesX.size();
}
// save original values for clean-up
QList<double> variedValuesOrig;
variedValuesOrig.append(QList<double>() << variedValX->get()
<< variedValY->get());
bool resetNetworkActivation = mResetNetworkActivation->get();
storeCurrentNetworkActivities();
/* store network configuration (bias terms, synapse weights,
observable parameters of TFs, AFs, SFs. */
bool restoreNetConfiguration = mRestoreNetworkConfiguration->get();
storeNetworkConfiguration();
//This is important when the physical simulator is activated!
bool resetSimulation = mResetSimulator->get();
triggerReset();
// PREPARE data matrix
double xStart = variedRangeX.first();
double xEnd = variedRangeX.last();
double xStepSize = (xEnd - xStart) / (double) (resolutionX - 1);
int roundDigits = mRoundDigits->get();
double xVal;
QList<double> xValues;
double yStart = variedRangeY.first();
double yEnd = variedRangeY.last();
double yStepSize = (yEnd - yStart) / (double) (resolutionY - 1);
double yVal;
QList<double> yValues;
{
//Thread safety of matrix.
QMutexLocker guard(mDynamicsPlotManager->getMatrixLocker());
mData->clear();
mData->resize(resolutionX + 1, resolutionY + 1, 3 + nrProjections);
mData->fill(0);
// calculate values and draw axes
for(int x = 1; x <= resolutionX; ++x) {
xVal = xStart + (x - 1) * xStepSize;
mData->set(Math::round(xVal, 5), x, 0, 0);
mData->set(Math::round(xVal, 5), x, 0, 1);
mData->set(Math::round(xVal, 5), x, 0, 2);
if(roundDigits >= 0) {
xVal = Math::round(xVal, roundDigits);
}
xValues.append(xVal);
}
for(int y = 1; y <= resolutionY; ++y) {
yVal = yStart + (y - 1) * yStepSize;
mData->set(Math::round(yVal, 5), 0, y, 0);
mData->set(Math::round(yVal, 5), 0, y, 1);
mData->set(Math::round(yVal, 5), 0, y, 2);
if(roundDigits >= 0) {
yVal = Math::round(yVal, roundDigits);
}
yValues.append(yVal);
}
// same for additional projections
for(int currProj = 0; currProj < nrProjections; ++currProj) {
double pStartX = projectionRangesX.at(currProj * 2);
double pEndX = projectionRangesX.at(currProj * 2 + 1);
double pStepX = (pEndX - pStartX) / (double) (resolutionX - 1);
for(int x = 1; x <= resolutionX; ++x) {
mData->set(Math::round((pStartX + (x - 1) * pStepX), 5), x, 0, 3 + currProj);
}
double pStartY = projectionRangesY.at(currProj * 2);
double pEndY = projectionRangesY.at(currProj * 2 + 1);
double pStepY = (pEndY - pStartY) / (double) (resolutionY - 1);
for(int y = 1; y <= resolutionY; ++y) {
mData->set(Math::round((pStartY + (y - 1) * pStepY), 5), 0, y, 3 + currProj);
}
}
}
// MAIN LOOP over x parameter points
int stepsRun = mStepsToRun->get();
int stepsCheck = mStepsToCheck->get();
double accuracy = mAccuracy->get();
QList< QList<double> > attractors;
for(int x = 1; x <= resolutionX && mActiveValue->get(); ++x) {
mProgressPercentage->set((double)(100 * x / resolutionX));
// INNER LOOP over y parameter points
for(int y = 1; y <= resolutionY && mActiveValue->get(); ++y) {
if(resetSimulation) {
triggerReset();
}
if(restoreNetConfiguration) {
restoreNetworkConfiguration();
}
if(resetNetworkActivation) {
restoreCurrentNetworkActivites();
}
// set x parameter
variedValX->set(xValues.at(x - 1));
// set y parameter
variedValY->set(yValues.at(y - 1));
if(!notifyNetworkParametersChanged(network)) {
return;
}
for(int runStep = 0; runStep < stepsRun && mActiveValue->get(); ++runStep) {
// let the network run for 1 timestep
triggerNetworkStep();
}
QList< QList<double> > networkStates;
QList<double> networkState;
QList< QPair<double,double> > variedPositions;
QList< QPair< QList<double>, QList<double> > > projectionPositions;
bool foundMatch = false;
int attrPeriod = 0;
for(int checkStep = 0; checkStep <= stepsCheck && !foundMatch && mActiveValue->get(); ++checkStep) {
triggerNetworkStep();
// get current network state
networkState = DynamicsPlotterUtil::getNetworkState(networkValues);
// abort on empty state
if(networkState.isEmpty()) {
reportProblem("BasinPlotter: Encountered empty network state.");
return;
}
// compare states to find attractors
for(int period = 1; period <= checkStep && !foundMatch; ++period) {
foundMatch = DynamicsPlotterUtil::compareNetworkStates(
networkStates.at(checkStep-period),
networkState,
accuracy);
attrPeriod = period;
}
// save current state as last one
networkStates.append(networkState);
variedPositions.append(QPair<double,double>(variedValX->get(), variedValY->get()));
if(nrProjections > 0) {
QPair< QList<double>, QList<double> > currentPositions;
currentPositions.first = DynamicsPlotterUtil::getMeanValues(projectionValuesX);
currentPositions.second = DynamicsPlotterUtil::getMeanValues(projectionValuesY);
projectionPositions.append(currentPositions);
}
}
// at this point, either an attractor has been found
if(foundMatch && mActiveValue->get()) {
// check for past attractors
bool attrMatch = false;
int attrNo = 1;
while(attrNo <= attractors.size() && !attrMatch) {
for(int state = 1; state <= attrPeriod && !attrMatch; ++state) {
attrMatch = DynamicsPlotterUtil::compareNetworkStates(
attractors.at(attrNo-1),
networkStates.at(networkStates.size()-state),
// was: size()-1-state
accuracy);
}
attrNo++;
}
//Thread safety of matrix.
QMutexLocker guard(mDynamicsPlotManager->getMatrixLocker());
// write matrix
mData->set(attrNo, x, y, 0);
mData->set(attrPeriod, x, y, 1);
// calculate and plot attractor position
int nrPositions = variedPositions.size();
for(int periodPos = 1; periodPos <= attrPeriod; ++periodPos) {
int currPosition = nrPositions - periodPos;
double currValX = variedPositions.at(currPosition).first;
double currValY = variedPositions.at(currPosition).second;
int attrPosX = ceil((currValX - xStart) / xStepSize + 1);
int attrPosY = ceil((currValY - yStart) / yStepSize + 1);
mData->set(attrNo, attrPosX, attrPosY, 2);
for(int currProj = 0; currProj < nrProjections; ++currProj) {
double xVal = projectionPositions.at(currPosition).first.at(currProj);
double yVal = projectionPositions.at(currPosition).second.at(currProj);
double pStartX = projectionRangesX.at(currProj * 2);
double pEndX = projectionRangesX.at(currProj * 2 + 1);
double pStepX = (pEndX - pStartX) / (double) (resolutionX - 1);
double pStartY = projectionRangesY.at(currProj * 2);
double pEndY = projectionRangesY.at(currProj * 2 + 1);
double pStepY = (pEndY - pStartY) / (double) (resolutionY - 1);
int xPos = floor((xVal - pStartX) / pStepX + 1);
int yPos = floor((yVal - pStartY) / pStepY + 1);
mData->set(attrNo, xPos, yPos, 3 + currProj);
}
}
if(!attrMatch) {
attractors.append(networkStates.last());
}
}
// or not, but then there's nothing to do :D
// runtime maintencance
if(core->isShuttingDown()) {
return;
}
core->executePendingTasks();
}
}
// CLEAN UP
variedValX->set(variedValuesOrig.at(0));
variedValY->set(variedValuesOrig.at(1));
notifyNetworkParametersChanged(network);
triggerReset();
restoreNetworkConfiguration();
restoreCurrentNetworkActivites();
}
void AccelSensor::setup() {
SimObject::setup();
if(mInitialized == false) {
mHostBody = Physics::getPhysicsManager()->getSimBody(mReferenceBodyName->get());
if(mHostBody == 0) {
Core::log("AccelSensor: Sensor has no valid reference body. Name was ["
+ mReferenceBodyName->get() + "]! Ignoring", true);
return;
}
createSensor();
mFirstSensorValue->set(0.0);
mSecondSensorValue->set(0.0);
mThirdSensorValue->set(0.0);
mTimeStepSize = dynamic_cast<DoubleValue*>(Core::getInstance()->
getValueManager()->getValue(SimulationConstants::VALUE_TIME_STEP_SIZE));
DoubleValue *mGravitationValue = dynamic_cast<DoubleValue*>(Core::getInstance()->
getValueManager()->getValue("/Simulation/Gravitation"));
if(mTimeStepSize == 0 || mGravitationValue == 0) {
Core::log("AccelSensor: Required Event or Value could not be found.");
return;
}
mGravitation = mGravitationValue->get();
mLastAxisOneMeasurement = 0.0;
mLastAxisTwoMeasurement = 0.0;
mLastAxisThreeMeasurement = 0.0;
mInitialized = true;
}
// rotate chosen axis about the localRotation of this sensor
Quaternion localOrientation = mLocalOrientation->get();
localOrientation.normalize();
mLocalOrientation->set(localOrientation);
Quaternion localOrientationInverse = localOrientation.getInverse();
localOrientationInverse.normalize();
Quaternion xAxis(0.0,
mSensorAxisOneValue->getX(),
mSensorAxisOneValue->getY(),
mSensorAxisOneValue->getZ());
Quaternion xAxisRotated = localOrientation * xAxis * localOrientationInverse;
mSensorAxisOne.set(xAxisRotated.getX(), xAxisRotated.getY(), xAxisRotated.getZ());
mRotatedSensorAxisOne.set(xAxisRotated.getX(),
xAxisRotated.getY(),
xAxisRotated.getZ());
Quaternion yAxis(0.0,
mSensorAxisTwoValue->getX(),
mSensorAxisTwoValue->getY(),
mSensorAxisTwoValue->getZ());
Quaternion yAxisRotated = localOrientation * yAxis * localOrientationInverse;
mSensorAxisTwo.set(yAxisRotated.getX(), yAxisRotated.getY(), yAxisRotated.getZ());
mRotatedSensorAxisTwo.set(yAxisRotated.getX(),
yAxisRotated.getY(),
yAxisRotated.getZ());
Quaternion zAxis(0.0,
mSensorAxisThreeValue->getX(),
mSensorAxisThreeValue->getY(),
mSensorAxisThreeValue->getZ());
Quaternion zAxisRotated = localOrientation * zAxis * localOrientationInverse;
mSensorAxisThree.set(zAxisRotated.getX(), zAxisRotated.getY(), zAxisRotated.getZ());
mRotatedSensorAxisThree.set(zAxisRotated.getX(),
zAxisRotated.getY(),
zAxisRotated.getZ());
if(mSensorBody == 0 || mHostBody == 0) {
Core::log("AccelSensor: The sensor has no valid host or sensor-body.");
return;
}
mSensorBody->setTextureType("None");
mSensorBody->setFaceTexture(5, "AccelBoard");
mSensorGeometry->setLocalOrientation(mLocalOrientation->get());
mSensorBody->getGeometry()->setLocalPosition(mLocalPosition->get());
Quaternion localPos(0,
mLocalPosition->getX(),
mLocalPosition->getY(),
mLocalPosition->getZ());
Quaternion bodyOrientationInverse = mHostBody->getQuaternionOrientationValue()->get().getInverse();
Quaternion rotatedLocalPosQuat = mHostBody->getQuaternionOrientationValue()->get()
* localPos * bodyOrientationInverse;
Vector3D rotatedLocalPos(rotatedLocalPosQuat.getX(),
rotatedLocalPosQuat.getY(),
rotatedLocalPosQuat.getZ());
mLastPosition = mHostBody->getPositionValue()->get() + rotatedLocalPos;
if(mLowPassFilterDelayValue != 0) {
mLowPassFilterDelay = mLowPassFilterDelayValue->get();
}
mValueOneHistory.clear();
mValueTwoHistory.clear();
mValueThreeHistory.clear();
mGlobalNoiseDeviation = mGlobalNoiseDeviationValue->get();
}
void LyapunovExponent::calculateData() {
// get program core
Core *core = Core::getInstance();
// get network
ModularNeuralNetwork *network = getCurrentNetwork();
QList<NeuralNetworkElement*> networkElements;
network->getNetworkElements(networkElements);
QList<DoubleValue*> networkValues =
DynamicsPlotterUtil::getNetworkValues(networkElements);
// Get parameters for varied element
QString variedElement = mVariedElement->get();
if(variedElement.isEmpty()) {
reportProblem("LyapunovExponent: No element to vary.");
return;
}
DoubleValue *variedValue =
DynamicsPlotterUtil::getElementValue(variedElement, networkElements);
if(variedValue == 0) {
reportProblem("LyapunovExponent: Invalid value or specifier.");
return;
}
QList<double> variedRange =
DynamicsPlotterUtil::getDoublesFromString(mVariedRange->get());
if(variedRange.size() != 2) {
reportProblem("LyapunovExponent: Invalid parameter range.");
return;
}
int resolutionX = mResolutionX->get();
int resolutionY = mResolutionY->get();
//avoid division by zero!
if(resolutionX < 2 || resolutionY < 2) {
reportProblem("LyapunovExponent: Invalid resolution given.");
return;
}
// Let costraint resolver run properly (order matters!)
storeNetworkConfiguration();
storeCurrentNetworkActivities();
triggerReset();
restoreCurrentNetworkActivites();
restoreNetworkConfiguration();
notifyNetworkParametersChanged(network);
// save original value
double originalValue = variedValue->get();
double valStep = (variedRange.at(1) - variedRange.at(0)) / (double) (resolutionX - 1);
QList<double> variedValues;
// prepare data matrix
{
//Thread safety of matrix.
QMutexLocker guard(mDynamicsPlotManager->getMatrixLocker());
mData->clear();
mData->resize(resolutionX + 1, resolutionY + 1, 1);
mData->fill(0);
for(int x = 1; x <= resolutionX; ++x) {
double val = variedRange.at(0) + (x-1) * valStep;
variedValues.append(val);
mData->set(val, x, 0, 0);
}
}
int stepsPrePlot = mStepsPrePlot->get();
int stepsToPlot = mStepsToPlot->get();
bool drawNL = mDrawNL->get();
QList<double> ynum;
double eps = pow(10,-9);
for(int x = 0; x < variedValues.size(); ++x) {
// set initial conditions of this run/trajectory
variedValue->set(variedValues.at(x));
notifyNetworkParametersChanged(network);
// calculate activation after X PrePlot-Steps
for(int s = 0; s < stepsPrePlot && mActiveValue->get(); ++s) {
triggerNetworkStep();
}
// list for states
QList< QList<double> > networkStates;
for(int s = 0; s < stepsToPlot && mActiveValue->get(); ++s) {
triggerNetworkStep();
// get current state of the network
QList<double> networkState =
DynamicsPlotterUtil::getNetworkState(networkValues);
// save to list
networkStates.append(networkState);
}
double ljanum = 0;
int c = 0;
for(int i = 0; i < networkStates.size() - 1; ++i) {
double dy = 10000000, df = 100000000;
bool found = false;
for(int j = 0; j < networkStates.size() - 1; ++j) {
double d = DynamicsPlotterUtil::getDistance(
networkStates.at(i), networkStates.at(j));
if(d < dy && d > eps) {
dy = d;
df = DynamicsPlotterUtil::getDistance(
networkStates.at(i + 1), networkStates.at(j + 1));
found = true;
}
}
if(found && dy != 0 && df != 0) {
ljanum += log(df / dy);
c++;
}
}
// save current hightest exponent
ynum.append(ljanum / c);
// find smallest and biggest exponent
double ymin = ynum.first(); double ymax = ynum.first();
for(int i = 1; i < ynum.size(); ++i) {
double y = ynum.at(i);
if(y < ymin) {
ymin = y;
}
if(y > ymax) {
ymax = y;
}
}
double ystep = (ymax - ymin) / (double)(resolutionY - 1);
if(ystep == 0) {
reportProblem("LyapunovExponent: No suitable data found.");
ymin = 1;
ymax = 1;
}
{
//Thread safety of matrix.
QMutexLocker guard(mDynamicsPlotManager->getMatrixLocker());
// clear data matrix
mData->fill(0);
// rescale
for(int y = 1; y <= resolutionY; ++y) {
double v = ymin + (y-1) * ystep;
mData->set(Math::round(v, 5), 0, y, 0);
}
// fill rescaled matrix again
for(int x = 1; x <= ynum.size(); ++x) {
double v = min(max(ymin, ynum.at(x - 1)), ymax);
int y = ceil(((v - ymin) / ystep) + 1);
mData->set(1, x, y, 0);
}
// find null position (if any)
int ny = ceil(((-ymin)/ystep)+1);
// and draw red line indicating y=0
if(drawNL && ny < resolutionY && ny > 0) {
for(int x = 0; x < resolutionX; ++x) {
if(mData->get(x, ny, 0) == 0) {
mData->set(2, x, ny, 0);
}
}
}
}
// runtime maintencance
if(core->isShuttingDown()) {
return;
}
core->executePendingTasks();
}
// re-set original parameter value
variedValue->set(originalValue);
// CLEAN UP
notifyNetworkParametersChanged(network);
triggerReset();
restoreNetworkConfiguration();
restoreCurrentNetworkActivites();
}
dJointID ODE_PID_PassiveActuatorModel::createJoint(dBodyID body1, dBodyID body2) {
Vector3DValue *jointAxisPoint1 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint1"));
Vector3DValue *jointAxisPoint2 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint2"));
DoubleValue *minAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MinAngle"));
DoubleValue *maxAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MaxAngle"));
if(jointAxisPoint1 == 0 || jointAxisPoint2 == 0 || minAngleValue == 0 || maxAngleValue == 0) {
Core::log("ODE_PID_PassiveActuatorModel: Could not find all required parameter values.");
return 0;
}
if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) {
Core::log("ODE_PID_PassiveActuatorModel: Invalid axis points " + jointAxisPoint1->getValueAsString() + " and " + jointAxisPoint2->getValueAsString() + ".");
return 0;
}
if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) {
Core::log("Invalid axes for ODE_PID_PassiveActuatorModel.");
return 0;
}
Vector3D anchor = jointAxisPoint1->get();
Vector3D axis = jointAxisPoint2->get() - jointAxisPoint1->get();
dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup);
dJointAttach(joint, body1, body2);
dJointSetAMotorMode(joint, dAMotorEuler);
dJointSetAMotorParam(joint, dParamVel, 0.0);
dJointSetAMotorParam(joint, dParamFMax, 1.0);
dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get());
dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get());
dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get());
dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get());
dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get());
axis.normalize();
Vector3D perpedicular;
if(axis.getY() != 0.0 || axis.getX() != 0.0) {
perpedicular.set(-axis.getY(), axis.getX(), 0);
}
else {
perpedicular.set(0, -axis.getZ(), axis.getY());
}
perpedicular.normalize();
// If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added.
if(body1 == 0) {
dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ());
}
else {
dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ());
}
if(body2 == 0) {
dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(),
-perpedicular.getZ());
}
else {
dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(),
perpedicular.getZ());
}
mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
dJointAttach(mHingeJoint, body1, body2);
dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ());
dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ());
double minAngle = (minAngleValue->get() * Math::PI) / 180.0;
double maxAngle = (maxAngleValue->get() * Math::PI) / 180.0;
if(body1 == 0) {
double tmp = minAngle;
minAngle = -1.0 * maxAngle;
maxAngle = -1.0 * tmp;
}
dJointSetHingeParam(mHingeJoint, dParamLoStop, minAngle);
dJointSetHingeParam(mHingeJoint, dParamHiStop, maxAngle);
dJointSetHingeParam(mHingeJoint, dParamVel, 0.0);
return joint;
}
void TestSignals::testSignalSinus() {
Core::resetCore();
// Test SinusSignal
// Create the TimeStepSizeValue
DoubleValue *timeStepSizeValue = new DoubleValue(0.01); Core::getInstance()->getValueManager()->addValue(SimulationConstants::VALUE_TIME_STEP_SIZE, timeStepSizeValue);
// Create the CurrentStepValue
IntValue *currentStepValue = new IntValue(0);
Core::getInstance()->getValueManager()->addValue(SimulationConstants::VALUE_EXECUTION_CURRENT_STEP, currentStepValue);
// Create the NextStepEvent
Event* nextStepEvent = Core::getInstance()->getEventManager()->createEvent(NerdConstants::EVENT_EXECUTION_NEXT_STEP);
QVERIFY(nextStepEvent != 0);
// Create the signal and initialize the core
SignalSinus* mySignalSinus = new SignalSinus("MySinus", "/Signals");
Core::getInstance()->init();
// CurrentValue und IdleSteps Parameter should be there
DoubleValue* currentValue = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("CurrentValue"));
QVERIFY(currentValue != 0);
IntValue* idleSteps = dynamic_cast<IntValue*>(mySignalSinus->getParameter("IdleSteps"));
QVERIFY(idleSteps != 0);
QVERIFY(currentValue->get() == 0.0);
QVERIFY(idleSteps->get() == 0);
// Current value should be 0.0
QVERIFY(mySignalSinus->getCurrentValue() == 0.0);
// Test getName()
QVERIFY(mySignalSinus->getName() == "MySinus");
// Period, Amplitude, Offset and AngularPhaseShift Paramaters should be there
DoubleValue* period = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("Period"));
QVERIFY(period != 0);
QVERIFY(period->get() == 1.0);
DoubleValue* amplitude = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("Amplitude"));
QVERIFY(amplitude != 0);
QVERIFY(amplitude->get() == 1.0);
DoubleValue* offset = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("Offset"));
QVERIFY(offset != 0);
QVERIFY(offset->get() == 0.0);
DoubleValue* phase = dynamic_cast<DoubleValue*>(mySignalSinus->getParameter("Phase"));
QVERIFY(phase != 0);
QVERIFY(phase->get() == 0.0);
// Test computation
double desiredValue;
currentStepValue->set(1);
nextStepEvent->trigger();
desiredValue = Math::sin(1.0 / 100.0 * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
currentStepValue->set(30);
nextStepEvent->trigger();
desiredValue = Math::sin(30.0 / 100.0 * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
timeStepSizeValue->set(0.02);
nextStepEvent->trigger();
desiredValue = Math::sin(2.0 * 30.0 / 100.0 * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
idleSteps->set(10);
nextStepEvent->trigger();
desiredValue = Math::sin(2.0 * 20.0 / 100.0 * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
period->set(1.5);
nextStepEvent->trigger();
desiredValue = Math::sin((30 - 10) / (1.5 / 0.02) * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
amplitude->set(3.3);
nextStepEvent->trigger();
desiredValue = 3.3 * Math::sin((30 - 10) / (1.5 / 0.02) * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
offset->set(0.7);
nextStepEvent->trigger();
desiredValue = 0.7 + 3.3 * Math::sin((30 - 10) / (1.5 / 0.02) * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
phase->set(0.12);
nextStepEvent->trigger();
desiredValue = 0.7 + 3.3 * Math::sin(0.12 + (30 - 10) / (1.5 / 0.02) * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
// Test addDestinationValue mechanism
DoubleValue *destValue1 = new DoubleValue(0.0);
DoubleValue *destValue2 = new DoubleValue(0.0);
DoubleValue *destValue3 = new DoubleValue(0.0);
mySignalSinus->addDestinationValue(destValue1);
mySignalSinus->addDestinationValue(destValue2);
mySignalSinus->addDestinationValue(destValue3);
period->set(1.0);
amplitude->set(1.0);
offset->set(0.0);
phase->set(0.0);
idleSteps->set(0);
timeStepSizeValue->set(0.01);
currentStepValue->set(1);
nextStepEvent->trigger();
desiredValue = Math::sin(1.0 / 100.0 * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
QCOMPARE(destValue1->get(), mySignalSinus->getCurrentValue());
QCOMPARE(destValue2->get(), mySignalSinus->getCurrentValue());
QCOMPARE(destValue3->get(), mySignalSinus->getCurrentValue());
// Test removeDestinationValue mechanism
mySignalSinus->removeDestinationValue(destValue3);
currentStepValue->set(2);
nextStepEvent->trigger();
desiredValue = Math::sin(2.0 / 100.0 * 2 * Math::PI);
QVERIFY(Math::compareDoubles(mySignalSinus->getCurrentValue(), desiredValue, 10));
QCOMPARE(destValue1->get(), mySignalSinus->getCurrentValue());
QCOMPARE(destValue2->get(), mySignalSinus->getCurrentValue());
QVERIFY(destValue3->get() != mySignalSinus->getCurrentValue());
delete destValue1;
delete destValue2;
delete destValue3;
Core::resetCore();
}
//chris
void TestValueTransferController::testConstructor() {
Core::resetCore();
ValueTransferController *vtf = new ValueTransferController("TFController");
QCOMPARE(vtf->getParameters().size(), 12);
StringValue *nameOfSource = dynamic_cast<StringValue*>(vtf->getParameter("SourceValueName"));
StringValue *nameOfTarget = dynamic_cast<StringValue*>(vtf->getParameter("TargetValueName"));
StringValue *customControllerName = dynamic_cast<StringValue*>(vtf->getParameter("CustomControllerName"));
StringValue *customSensorName = dynamic_cast<StringValue*>(vtf->getParameter("CustomSensorName"));
InterfaceValue *controller = dynamic_cast<InterfaceValue*>(vtf->getParameter("Control"));
InterfaceValue *sensor = dynamic_cast<InterfaceValue*>(vtf->getParameter("Sensor"));
IntValue *mode = dynamic_cast<IntValue*>(vtf->getParameter("TransferMode"));
DoubleValue *rate = dynamic_cast<DoubleValue*>(vtf->getParameter("TransferRate"));
DoubleValue *cost = dynamic_cast<DoubleValue*>(vtf->getParameter("TransferCost"));
QVERIFY(nameOfSource != 0);
QVERIFY(nameOfTarget != 0);
QVERIFY(customControllerName != 0);
QVERIFY(customSensorName != 0);
QVERIFY(controller != 0);
QVERIFY(sensor != 0);
QVERIFY(mode != 0);
QVERIFY(rate != 0);
QVERIFY(cost != 0);
QCOMPARE(vtf->getOutputValues().size(), 1);
QVERIFY(vtf->getOutputValues().contains(sensor));
QCOMPARE(vtf->getInputValues().size(), 1);
QVERIFY(vtf->getInputValues().contains(controller));
QVERIFY(controller->getName() == "TFController/Control");
QVERIFY(sensor->getName() == "TFController/Transfer");
QCOMPARE(controller->getMin(), -1.0);
QCOMPARE(controller->getMax(), 1.0);
QCOMPARE(sensor->getMin(), -1.0);
QCOMPARE(sensor->getMax(), 1.0);
//change the custom names
customControllerName->set("NewName");
QVERIFY(controller->getName() == "TFController/NewName");
customSensorName->set("NewSensorName");
QVERIFY(sensor->getName() == "TFController/NewSensorName");
//change some values
nameOfSource->set("MySource");
nameOfTarget->set("MyTarget");
mode->set(10);
rate->set(0.12345);
cost->set(-5.123);
//*************************************************************************************
//copy constructor
ValueTransferController *copy = dynamic_cast<ValueTransferController*>(vtf->createCopy());
QVERIFY(copy != 0);
copy->setName("MyCopy");
StringValue *nameOfSourceC = dynamic_cast<StringValue*>(copy->getParameter("SourceValueName"));
StringValue *nameOfTargetC = dynamic_cast<StringValue*>(copy->getParameter("TargetValueName"));
StringValue *customControllerNameC = dynamic_cast<StringValue*>(copy->getParameter("CustomControllerName"));
StringValue *customSensorNameC = dynamic_cast<StringValue*>(copy->getParameter("CustomSensorName"));
InterfaceValue *controllerC = dynamic_cast<InterfaceValue*>(copy->getParameter("Control"));
InterfaceValue *sensorC = dynamic_cast<InterfaceValue*>(copy->getParameter("Sensor"));
IntValue *modeC = dynamic_cast<IntValue*>(copy->getParameter("TransferMode"));
DoubleValue *rateC = dynamic_cast<DoubleValue*>(copy->getParameter("TransferRate"));
DoubleValue *costC = dynamic_cast<DoubleValue*>(copy->getParameter("TransferCost"));
QVERIFY(nameOfSourceC != 0);
QVERIFY(nameOfTargetC != 0);
QVERIFY(customControllerNameC != 0);
QVERIFY(customSensorNameC != 0);
QVERIFY(controllerC != 0);
QVERIFY(sensorC != 0);
QVERIFY(modeC != 0);
QVERIFY(rateC != 0);
QVERIFY(costC != 0);
QVERIFY(vtf->getSource() == 0);
QVERIFY(vtf->getTarget() == 0);
QCOMPARE(copy->getOutputValues().size(), 1);
QVERIFY(copy->getOutputValues().contains(sensorC));
QCOMPARE(copy->getInputValues().size(), 1);
QVERIFY(copy->getInputValues().contains(controllerC));
QVERIFY(sensor != sensorC);
QVERIFY(controller != controllerC);
QVERIFY(controllerC->getName() == "MyCopy/NewName");
QVERIFY(sensorC->getName() == "MyCopy/NewSensorName");
QCOMPARE(modeC->get(), 10);
QCOMPARE(rateC->get(), 0.12345);
QCOMPARE(costC->get(), -5.123);
QVERIFY(nameOfSourceC->get() == "MySource");
QVERIFY(nameOfTargetC->get() == "MyTarget");
QVERIFY(customControllerNameC->get() == "NewName");
QVERIFY(customSensorNameC->get() == "NewSensorName");
}
void TestSphereBody::testSphereBody() {
Core::resetCore();
SphereBody *sphere = new SphereBody("Sphere");
QCOMPARE(sphere->getParameters().size(), 14);
QVERIFY(sphere->getName().compare("Sphere") == 0);
QVERIFY(sphere->getInputValues().size() == 0);
QVERIFY(sphere->getOutputValues().size() == 0);
QCOMPARE(sphere->getParameters().size(), 14);
QVERIFY(sphere->getParameter("CenterOfMass") != 0);
QVERIFY(sphere->getParameter("Position") != 0);
QVERIFY(sphere->getParameter("Orientation") != 0);
QVERIFY(sphere->getParameter("DynamicFriction") != 0);
QVERIFY(sphere->getParameter("StaticFriction") != 0);
QVERIFY(sphere->getParameter("Elasticity") != 0);
QVERIFY(sphere->getParameter("Dynamic") != 0);
QVERIFY(sphere->getParameter("Name") != 0);
QVERIFY(sphere->getParameter("Mass") != 0);
QVERIFY(sphere->getParameter("Color") != 0);
DoubleValue *radius = dynamic_cast<DoubleValue*>(sphere->getParameter("Radius"));
QVERIFY(radius != 0);
QCOMPARE(radius->get(), 0.0);
QCOMPARE(sphere->getCollisionObjects().size(), 1);
QCOMPARE(sphere->getGeometries().size(), 1);
SphereGeom *sphereGeom = dynamic_cast<SphereGeom*>(sphere->getGeometries().at(0));
QVERIFY(sphereGeom != 0);
QCOMPARE(sphereGeom->getRadius(), 0.0);
CollisionObject *collisionObject = sphere->getCollisionObjects().at(0);
QVERIFY(collisionObject->getHostBody() == sphere);
QVERIFY(collisionObject->getOwner() == 0);
SphereGeom *collisionObjectGeom = dynamic_cast<SphereGeom*>(collisionObject->getGeometry());
QVERIFY(collisionObjectGeom != 0);
QCOMPARE(collisionObjectGeom->getRadius(), 0.0);
radius->set(1.0);
QCOMPARE(collisionObjectGeom->getRadius(), 0.0);
QCOMPARE(sphereGeom->getRadius(), 0.0);
sphere->setup();
QVERIFY(collisionObject->getHostBody() == sphere);
QVERIFY(collisionObject->getOwner() == sphere);
QCOMPARE(collisionObjectGeom->getRadius(), 1.0);
QCOMPARE(sphereGeom->getRadius(), 1.0);
SphereBody *sphere2 = new SphereBody("Sphere2", 5.14);
DoubleValue *radius2 = dynamic_cast<DoubleValue*>(sphere2->getParameter("Radius"));
QVERIFY(radius2 != 0);
QCOMPARE(radius2->get(), 5.14);
SphereGeom *sphereGeom2 = dynamic_cast<SphereGeom*>(sphere2->getCollisionObjects().at(0)->getGeometry());
QCOMPARE(sphereGeom2->getRadius(), 5.14);
QCOMPARE(sphere2->getParameters().size(), 14);
sphere2->getParameter("Position")->setValueFromString("(1.11, 2.2, 3.9)");
Vector3DValue *position = dynamic_cast<Vector3DValue*>(sphere2->getParameter("Position"));
QVERIFY(position != 0);
QCOMPARE(position->getX(), 1.11);
QCOMPARE(position->getY(), 2.2);
QCOMPARE(position->getZ(), 3.9);
SphereBody *copy = dynamic_cast<SphereBody*>(sphere2->createCopy());
QVERIFY(copy != 0);
QCOMPARE(copy->getParameters().size(), 14);
QVERIFY(copy->getCollisionObjects().size() == 1);
QVERIFY(copy->getGeometries().size() == 1);
Vector3DValue *copyPosition = dynamic_cast<Vector3DValue*>(copy->getParameter("Position"));
QVERIFY(copyPosition != 0);
QCOMPARE(copyPosition->getX(), 1.11);
QCOMPARE(copyPosition->getY(), 2.2);
QCOMPARE(copyPosition->getZ(), 3.9);
SphereGeom *copyGeometry = dynamic_cast<SphereGeom*>(copy->getCollisionObjects().at(0)->getGeometry());
QVERIFY(copyGeometry != 0);
QCOMPARE(copyGeometry->getRadius(), 5.14);
CollisionObject *copyCollisionObject = copy->getCollisionObjects().at(0);
QVERIFY(copyCollisionObject->getHostBody() == copy);
QVERIFY(copyCollisionObject->getOwner() == 0);
copy->setup();
QVERIFY(copyCollisionObject->getHostBody() == copy);
QVERIFY(copyCollisionObject->getOwner() == copy);
radius2->set(-10.0);
sphere2->setup();
QVERIFY(sphereGeom2->getRadius() > 0.0);
delete copy;
delete sphere2;
delete sphere;
}