returnValue PlotWindow::addSubplot( const Expression& _expressionX,
const Expression& _expressionY,
const char* const _title,
const char* const _xLabel,
const char* const _yLabel,
PlotMode _plotMode,
double _xRangeLowerLimit,
double _xRangeUpperLimit,
double _yRangeLowerLimit,
double _yRangeUpperLimit
)
{
ASSERT( _expressionX.getDim() == _expressionY.getDim() );
uint run1;
for( run1 = 0; run1 < _expressionX.getDim(); run1++ ){
PlotWindowSubplot* newSubplot = new PlotWindowSubplot( _expressionX(run1),_expressionY(run1),
_title,_xLabel,_yLabel,_plotMode,
_xRangeLowerLimit,_xRangeUpperLimit,
_yRangeLowerLimit,_yRangeUpperLimit );
if ( number == 0 )
{
first = newSubplot;
last = newSubplot;
}
else
{
if ( last->setNext( newSubplot ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_OPTIONS_LIST_CORRUPTED );
last = newSubplot;
}
Expression tmpX( _expressionX(run1) );
if ( addPlotDataItem( &tmpX ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_LOG_RECORD_CORRUPTED );
Expression tmpY( _expressionY(run1) );
if ( addPlotDataItem( &tmpY ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_LOG_RECORD_CORRUPTED );
++number;
}
return SUCCESSFUL_RETURN;
}
void quotRemainder(const BigReal &x, const BigReal &y, BigInt *quotient, BigReal *remainder) {
DEFINEMETHODNAME;
if(y.isZero()) {
throwBigRealInvalidArgumentException(method, _T("Division by zero. (%s / 0)."), x.toString().cstr());
}
if(quotient == remainder) { // also takes care of the stupid situation where both are NULL
throwBigRealInvalidArgumentException(method, _T("quotient is the same variable as remainder"));
}
if(quotient == &x || quotient == &y) {
throwBigRealInvalidArgumentException(method, _T("quotient cannot be the same variable as x or y"));
}
if(remainder == &x || remainder == &y) {
throwBigRealInvalidArgumentException(method, _T("remainder cannot be the same variable as x or y"));
}
if(x.isZero()) {
if(quotient ) quotient->setToZero();
if(remainder) remainder->setToZero();
return;
}
DigitPool *pool = x.getDigitPool();
const int cmpAbs = compareAbs(x, y);
if(cmpAbs < 0) {
if(remainder) *remainder = x;
if(quotient) quotient->setToZero();
return;
} else if(cmpAbs == 0) {
if(remainder) remainder->setToZero();
if(quotient) {
*quotient = quotient->getDigitPool()->get1();
}
return;
}
BigReal tmpX(x);
tmpX.setPositive();
BigReal tmpY(y);
tmpY.setPositive();
BigInt q = floor(quot(tmpX, tmpY, modulusC1));
BigReal mod = tmpX - q * tmpY;
if(mod.isNegative()) {
if(remainder) {
*remainder = mod + tmpY;
}
if(quotient) {
--q;
*quotient = q;
}
} else if(mod >= tmpY) {
if(remainder) {
*remainder = mod - tmpY;
}
if(quotient) {
++q;
*quotient = q;
}
} else {
if(remainder) {
*remainder = mod;
}
if(quotient) {
*quotient = q;
}
}
if(remainder && (remainder->m_negative != x.m_negative)) {
remainder->m_negative = x.m_negative; // sign(x % y) = sign(x), equivalent to build-in % operator
}
}
bool ArmMap::query(const YVector &arm, const YVector &head)
{
_headKinematics.update(head);
// _fkinematics.update(arm, head);
const Y3DVector &cart = _headKinematics.fixationPolar();
Y3DVector tmp;
tmp = cart;
if (!_checkReachability(cart))
return false;
// tmp(1) = tmp(1) + __azimuthOffset;
// tmp(2) = tmp(2) + __elevationOffset;
// tmp(3) = tmp(3) + __distanceOffset;
int x, y; //retinal position (for closed loop)
x = 128;
y = 128;
if (_mode == atnr)
_command = _atr.query(head) + _noise.query();
else if (_mode == learning)
{
// _nnet.sim(tmp.data(), _command.data());
_command = _command + _noise.query();
}
else
{
// _nnet.sim(tmp.data(), _command.data());
YVector tmpY(3);
_rfnet.Simulate(tmp, 0.001, tmpY);
_command = 0.0;
_command(1) = tmpY(1);
_command(2) = tmpY(2);
_command(3) = tmpY(3);
#define USE_JACOBIAN 1
#if USE_JACOBIAN
_fkinematics.update(_command, head);
_fkinematics.computeJacobian(x,y); // compute from center
YVector tmpArm(6);
// tmpArm(1) = _command(1);
// tmpArm(2) = _command(2);
// tmpArm(3) = _command(3);
// tmpArm(1) = _command(1); //copy 1 joint from map
// tmpArm(2) = arm(2);
// tmpArm(3) = arm(3);
// tmpArm(4) = arm(4);
// tmpArm(5) = arm(5);
// tmpArm(6) = arm(6);
tmpArm=_command;
_commandCL = _fkinematics.computeCommandThreshold(tmpArm , x, y); // to center
#endif
// _sendTrajectory();
#ifndef TEST_REACHING
_formTrajectory(_commandCL, _commandCL);
#else
_formTrajectory(_command, _commandCL);
#endif
}
return true;
}
