char ASCIIShade(double x)
{
if(IsNaN(x)) return 'E';
if(IsInf(x)==1) return 'I';
else if(IsInf(x)==-1) return 'i';
int index = (int)Trunc(x*8) + 7;
if(index < 0) index=0;
if(index >= kNumAsciiShades) index=kNumAsciiShades-1;
if(index == 7) {
if(x > 0) return kAsciiShades[8];
else if(x < 0) return kAsciiShades[6];
else return kAsciiShades[7];
}
return kAsciiShades[index];
}
void MainWindow::UpdateLinkParameters(){
#define NUM(x) QString::number(x)
Robot* rob = world.robots[0];
QString link_info=QString("V [%1 %2], A [%3,%4], T [%5,%6]").arg(
NUM(rob->velMin(gui->link_index)),NUM(rob->velMax(gui->link_index)),
NUM(-rob->accMax(gui->link_index)),NUM(rob->accMax(gui->link_index)),
NUM(-rob->torqueMax(gui->link_index)),NUM(rob->torqueMax(gui->link_index)));
ui->lbl_link_info->setText(link_info);
ui->spn_link->setMinimum(rob->qMin(gui->link_index));
ui->spn_link->setMaximum(rob->qMax(gui->link_index));
if(IsInf(ui->spn_link->minimum()) || IsInf(ui->spn_link->minimum()))
ui->sld_link->setEnabled(false);
else ui->sld_link->setEnabled(true);
#undef NUM
}
void ODESimulator::GetSurfaceParameters(const ODEObjectID& a,const ODEObjectID& b,dSurfaceParameters& surface) const
{
//TODO: base the friction on the properties of the contact point
//completely rigid contact
surface.mode = dContactApprox1;
//surface.mode = 0;
surface.bounce = 0;
surface.bounce_vel = 0;
//printf("GetSurfaceParameters a = %d,%d, b = %d,%d\n",a.type,a.index,b.type,b.index);
ODETriMesh *ma,*mb;
if(a.type == 0) {
Assert(a.index < (int)envs.size());
ma = envMeshes[a.index];
}
else if(a.type == 1)
ma = robots[a.index]->triMesh(a.bodyIndex);
else if(a.type == 2)
ma = objects[a.index]->triMesh();
else Abort();
if(b.type == 0) {
Assert(b.index < (int)envs.size());
mb=envMeshes[b.index];
}
else if(b.type == 1)
mb=robots[b.index]->triMesh(b.bodyIndex);
else if(b.type == 2)
mb=objects[b.index]->triMesh();
else Abort();
const ODESurfaceProperties &propa=ma->surf(),&propb=mb->surf();
if(!IsInf(propa.kStiffness) || !IsInf(propb.kStiffness)) {
surface.mode |= dContactSoftERP | dContactSoftCFM;
Real kStiffness = 1.0/(1.0/propa.kStiffness+1.0/propb.kStiffness);
Real kDamping = 1.0/(1.0/propa.kDamping+1.0/propb.kDamping);
surface.soft_erp = ERPFromSpring(timestep,kStiffness,kDamping);
surface.soft_cfm = CFMFromSpring(timestep,kStiffness,kDamping);
//printf("Joint stiffness %g, damping %g\n",kStiffness,kDamping);
//printf("ERP = %g, CFM = %g\n",surface.soft_erp,surface.soft_cfm);
}
surface.mu = 2.0/(1.0/propa.kFriction+1.0/propb.kFriction);
//correction to account for pyramid shaped friction cone
surface.mu *= 0.707;
surface.bounce = 0.5*(propa.kRestitution+propb.kRestitution);
if(surface.bounce != 0)
surface.mode |= dContactBounce;
}
void MainWindow::UpdateDriverParameters(){
bool oldState = ui->spn_driver->blockSignals(true);
#define NUM(x) QString::number(x)
Robot* rob = world.robots[0];
RobotJointDriver dr=rob->drivers[gui->driver_index];
QString driver_info=QString("V [%1 %2], T [%3,%4], PID %5,%6,%7").arg( \
NUM(dr.vmin),NUM(dr.vmax),NUM(dr.tmin),NUM(dr.tmax),NUM(dr.servoP),NUM(dr.servoI),NUM(dr.servoD));
ui->lbl_driver_info->setText(driver_info);
Vector2 limits = rob->GetDriverLimits(gui->driver_index);
ui->spn_driver->setMinimum(limits.x);
ui->spn_driver->setMaximum(limits.y);
#undef NUM
if(IsInf(ui->spn_driver->minimum()) || IsInf(ui->spn_driver->minimum()))
ui->sld_driver->setEnabled(false);
else ui->sld_driver->setEnabled(true);
ui->spn_driver->blockSignals((oldState));
}
//return a value x in [xmin,xmax] such that |a*x - b| <= epsilon*max(|a||b|)
//for ill posed problems choose x=0
bool SafeEqSolve(dReal a,dReal b,dReal epsilon,dReal xmin,dReal xmax,dReal& x)
{
if(a < 0) {
return SafeEqSolve(-a,-b,epsilon,xmin,xmax,x);
}
//now we know a>=0
dReal epsScaled = epsilon*Max(a,Abs(b));
//infinte range
if(IsInf(xmin)==-1 && IsInf(xmax)==1) {
if(a == 0) {
x = 0.0;
return Abs(b) <= epsScaled;
}
x = b/a;
return true;
}
dReal axmin=a*xmin,axmax=a*xmax;
if(!(b + epsScaled >= axmin && b - epsScaled <= axmax)) { //ranges don't intersect
return false;
}
if(a != 0) {
x = b/a;
if(xmin <= x && x <= xmax) {
return true;
}
}
if (Abs(0.5*(axmin+axmax) - b) <= epsScaled) {
//center of range is in epsilon range
x = 0.5*(xmin+xmax);
return true;
}
if(Abs(axmax - b) <= epsScaled) {
x = xmax;
return true;
}
assert(Abs(axmin - b) <= epsScaled);
x = xmin;
return true;
}
void ErrorAccumulator::Add(Real error,Real weight)
{
if(norm == 1.0)
accumulator += Abs(error)*weight;
else if(norm == 2.0)
accumulator += error*error*weight;
else if(IsInf(norm))
accumulator = Max(accumulator,weight*Abs(error));
else
accumulator += weight*Pow(Abs(error),norm);
sumWeights += weight;
}
void ErrorAccumulator::Add(Real error)
{
if(norm == 1.0)
accumulator += Abs(error);
else if(norm == 2.0)
accumulator += error*error;
else if(IsInf(norm))
accumulator = Max(accumulator,Abs(error));
else
accumulator += Pow(Abs(error),norm);
sumWeights += 1.0;
}
Real CompositeObjective::Delta(PlannerObjectiveBase* priorGoal)
{
if(priorGoal->TypeString() != TypeString()) return Inf;
CompositeObjective* pcomposite = dynamic_cast<CompositeObjective*>(priorGoal);
if(pcomposite->components.size() != components.size()) return Inf;
ErrorAccumulator accum(norm);
for(size_t i=0;i<pcomposite->components.size();i++) {
Real w1 = (weights.empty()?1.0:weights[i]);
Real w2 = (pcomposite->weights.empty()?1.0:pcomposite->weights[i]);
Real d=components[i]->Delta(pcomposite->components[i]);
if(IsInf(d)) return Inf;
accum.Add(d,w1);
accum.Add(w1-w2);
}
return accum.Value();
}
Real ErrorAccumulator::Value() const
{
if(IsInf(norm)) return accumulator;
if(norm == 1.0) {
if(mean) return accumulator/sumWeights;
return accumulator;
}
if(norm == 2.0) {
if(mean && !root) return accumulator/sumWeights;
else if(mean && root) return Sqrt(accumulator/sumWeights);
else if(root) return Sqrt(accumulator);
return accumulator;
}
if(mean && !root) return accumulator/sumWeights;
else if(mean && root) return Pow(accumulator/sumWeights,1.0/norm);
else if(root) return Pow(accumulator,1.0/norm);
return accumulator;
}
virtual void GetRoadmap(RoadmapPlanner& roadmap) const {
//simply output all the visited grid cells
if(planner.distances.numValues()==0) return;
vector<int> index(qStart.n,0);
do {
if(!IsInf(planner.distances[index])) {
Vector config = planner.FromGrid(index);
roadmap.AddMilestone(config);
}
} while(IncrementIndex(index,planner.distances.dims)==0);
if(planner.solution.edges.empty()) {
//add debugging path
int prev = -1;
for(size_t i=0;i<planner.failedCheck.edges.size();i++) {
if(prev < 0)
prev = roadmap.AddMilestone(planner.failedCheck.GetMilestone(0));
int n = roadmap.AddMilestone(planner.failedCheck.GetMilestone(i+1));
roadmap.ConnectEdge(prev,n,planner.failedCheck.edges[i]);
prev = n;
}
}
}
// Calculate Standard Deviation for the values
void MgFeatureNumericFunctions::GetStandardDeviationCategories( VECTOR &values, int numCats,
double dataMin, double dataMax,
VECTOR &distValues)
{
// Expected categories should be more than zero
if (numCats <= 0)
{
STRING message = MgServerFeatureUtil::GetMessage(L"MgInvalidComputedProperty");
MgStringCollection arguments;
arguments.Add(message);
throw new MgFeatureServiceException(L"MgServerSelectFeatures.GetEqualCategories", __LINE__, __WFILE__, &arguments, L"", NULL);
}
// collect information about the data values
double min = DoubleMaxValue;
double max = -DoubleMaxValue;
double mean = 0;
int cnt = (int)values.size();
if (cnt <= 0) { return; } // Nothing to do, we just send back Property Definition to clients from reader
for (int i=0; i < cnt; i++)
{
double val = values[i];
if (val > max)
max = val;
if (val < min)
min = val;
mean += val;
}
// expand min and max a little to account for numerical instability
double delta = 0.0001 * (max - min);
min -= delta;
max += delta;
// compute the mean, variance and standard deviation
double count = (double)cnt; // (guaranteed to be > 0)
mean /= count;
double variance = 0;
for (int i=0; i < cnt; i++)
{
double val = values[i];
variance += (val - mean) * (val - mean);
}
double deviation = sqrt(variance / count);
// fill in the middle category/categories
double* cats = new double[numCats+1];
int midCat, highMidCat;
if (numCats % 2 == 0)
{
midCat = numCats / 2;
highMidCat = midCat;
cats[midCat] = mean;
}
else
{
midCat = (numCats - 1) / 2;
highMidCat = midCat + 1;
cats[midCat] = mean - 0.5 * deviation;
cats[highMidCat] = mean + 0.5 * deviation;
}
// fill in the other categories
for (int i=midCat-1; i>=0; i--)
cats[i] = cats[i+1] - deviation;
for (int i=highMidCat; i<=numCats; i++)
cats[i] = cats[i-1] + deviation;
// if the data method specifies strict a strict min and/or max, use them
if (!IsInf(dataMin) && !IsNan(dataMin) && (dataMin != -DoubleMaxValue))
min = dataMin;
if (!IsInf(dataMax) && !IsNan(dataMax) && (dataMax != DoubleMaxValue))
max = dataMax;
// flatten/clip any categories that extend beyond the min/max range
for (int i=0; i<=numCats; i++)
{
if (cats[i] < min)
cats[i] = min;
else if (cats[i] > max)
cats[i] = max;
}
for (int kk = 0; kk < numCats+1; kk++)
{
distValues.push_back(cats[kk]);
}
delete[] cats; // Delete the memory allocated before
}
