//----------------------------------------------------------------------------------------------------------------------
void SMCAgent::init()
{
m_radius = 0.02;
if (SETTINGS->hasChild("Config/GA/Evolvable"))
{
const ci::XmlTree& xml = SETTINGS->getChild("Config/GA/Evolvable");
// Todo: move to respective sensor class!!!
// Distance sensor
if (xml.getChild("DistanceSensor").getValue<bool>(0))
{
m_distanceSensor = new DistanceSensor();
m_distanceSensor->fromXml(xml.getChild("DistanceSensor"));
setSensorMode(xml.getChild("DistanceSensor").getAttributeValue<std::string>("Mode", "Absolute"));
}
else
{
m_distanceSensor = NULL;
}
// Gradient sensor
if (xml.getChild("GradientSensor").getValue<bool>(0))
{
m_gradientSensor = new GradientSensor();
m_gradientSensor->fromXml(xml.getChild("GradientSensor"));
}
else
{
m_gradientSensor = NULL;
}
// Torus sensor
if (xml.getChild("TorusSensor").getValue<bool>(0))
{
m_torusSensor = new TorusSensor();
m_torusSensor->fromXml(xml.getChild("TorusSensor"));
}
else
{
m_torusSensor = NULL;
}
setMaxSpeed(xml.getChild("MaxSpeed").getValue<double>(1.0));
setMaxAngularSpeed(degreesToRadians(xml.getChild("MaxAngularSpeed").getValue<double>(180)));
setMaxAngle(degreesToRadians(xml.getChild("MaxAngle").getValue<double>(90)));
setMaxPosition(xml.getChild("MaxPosition").getValue<double>(0.5));
setAngleWraps(xml.getChild("AngleWraps").getValue<bool>(1));
setPositionWraps(xml.getChild("PositionWraps").getValue<bool>(1));
m_energyInitial = xml.getChild("Energy").getAttributeValue<float>("initial", 30.0);
m_energySpeedTresh = xml.getChild("Energy").getAttributeValue<float>("threshForSpeed", -1);
m_evMin = xml.getChild("Energy").getAttributeValue<float>("evMin", 0);
m_evMax = xml.getChild("Energy").getAttributeValue<float>("evMax", 10);
m_engReplFoodSens = xml.getChild("Energy").getAttributeValue<bool>("engReplFoodSens", false);
getEnvironment().fromXml(xml.getChild("Environment"));
}
reset();
}
/**
* Set the motor limits in the motor hardware.
*
* @param minAngle Minimum for the angle, in radians, the StepperMotor can travel on the theoretical plane.
* @param maxAngle Maximum for the angle, in radians, the StepperMotor can travel on the theoretical plane.
**/
void StepperMotor::setMotorLimits(double minAngle, double maxAngle){
// Set motors limits.
setMinAngle(minAngle);
setMaxAngle(maxAngle);
}
extern shapeop_err shapeop_editConstraint(ShapeOpSolver *op,
const char *constraintType,
int constraint_id,
const ShapeOpScalar *scalars,
int nb_scl) {
if (strcmp(constraintType, "EdgeStrain") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::EdgeStrainConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
if (nb_scl != 3) { return SO_INVALID_ARGUMENT_LENGTH; }
c->setEdgeLength(scalars[0]);
c->setRangeMin(scalars[1]);
c->setRangeMax(scalars[2]);
return SO_SUCCESS;
}
if (strcmp(constraintType, "TriangleStrain") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::TriangleStrainConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
if (nb_scl != 2) { return SO_INVALID_ARGUMENT_LENGTH; }
c->setRangeMin(scalars[0]);
c->setRangeMax(scalars[1]);
return SO_SUCCESS;
}
if (strcmp(constraintType, "TetrahedronStrain") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::TetrahedronStrainConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
if (nb_scl != 2) { return SO_INVALID_ARGUMENT_LENGTH; }
c->setRangeMin(scalars[0]);
c->setRangeMax(scalars[1]);
return SO_SUCCESS;
}
if (strcmp(constraintType, "Area") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::AreaConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
if (nb_scl != 2) { return SO_INVALID_ARGUMENT_LENGTH; }
c->setRangeMin(scalars[0]);
c->setRangeMax(scalars[1]);
return SO_SUCCESS;
}
if (strcmp(constraintType, "Volume") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::VolumeConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
if (nb_scl != 2) { return SO_INVALID_ARGUMENT_LENGTH; }
c->setRangeMin(scalars[0]);
c->setRangeMax(scalars[1]);
return SO_SUCCESS;
}
if (strcmp(constraintType, "Bending") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::BendingConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
if (nb_scl != 2) { return SO_INVALID_ARGUMENT_LENGTH; }
c->setRangeMin(scalars[0]);
c->setRangeMax(scalars[1]);
return SO_SUCCESS;
}
if (strcmp(constraintType, "Closeness") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::ClosenessConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
if (nb_scl != 3) { return SO_INVALID_ARGUMENT_LENGTH; }
Eigen::Map<const ShapeOp::Vector3> p(scalars, 3, 1);
c->setPosition(p);
return SO_SUCCESS;
}
if (strcmp(constraintType, "Similarity") == 0 || strcmp(constraintType, "Rigid") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::SimilarityConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
int nI = static_cast<int>(c->nIndices());
if ((nb_scl % (nI * 3)) != 0) { return SO_INVALID_ARGUMENT_LENGTH; }
std::vector<ShapeOp::Matrix3X> shapes;
int nShapes = nb_scl / (nI * 3);
for (int i = 0; i < nShapes; ++i) {
Eigen::Map<const ShapeOp::Matrix3X> s(scalars + i * nI * 3, 3, nI);
shapes.push_back(s);
}
c->setShapes(shapes);
return SO_SUCCESS;
}
if (strcmp(constraintType, "Angle") == 0) {
auto c = std::dynamic_pointer_cast<ShapeOp::AngleConstraint>(op->s->getConstraint(constraint_id));
if (!c) { return SO_UNMATCHING_CONSTRAINT_ID; }
if (nb_scl != 2) { return SO_INVALID_ARGUMENT_LENGTH; }
c->setMinAngle(scalars[0]);
c->setMaxAngle(scalars[1]);
return SO_SUCCESS;
}
return SO_INVALID_CONSTRAINT_TYPE;
}
