Added tgSphere functionality to the YAML builder. To use: create a no…

…de with the tag of a builder. See example in src/dev/apsabelhaus/YAMLtests/stick_with_ball.

src/dev/apsabelhaus/YAMLtests/stick_with_ball.yaml

@@ -0,0 +1,31 @@
+# This will be a test of adding tgSphere functionality to the YAML builder.
+
+nodes:
+  # Add a random stick.
+  center: [0, 1, 0]
+  endpoint: [0, 11, 0]
+  # Spheres will be created whenever a node has the tag of
+  # a builder. So, no need to do a pair group, just place a node!
+  endingsphere: [0, 11, 0]
+
+pair_groups:
+  # First, just a stick.
+  stickbox:
+    - [center, endpoint]
+  # Just to emphasize again: no pairs needed for spheres!
+
+# Builders. First, the box:
+builders:
+
+  stickbox:
+    class: tgBoxInfo
+    parameters:
+      density: 0.5
+      width: 0.5
+      height: 0.5
+
+  endingsphere:
+    class: tgSphereInfo
+    parameters:
+      density: 0.5
+      radius: 2

src/yamlbuilder/TensegrityModel.cpp

@@ -32,12 +32,14 @@
 #include "core/tgKinematicActuator.h"
 #include "core/tgRod.h"
 #include "core/tgBox.h"
+#include "core/tgSphere.h"
 #include "tgcreator/tgBasicActuatorInfo.h"
 #include "tgcreator/tgBasicContactCableInfo.h"
 #include "tgcreator/tgKinematicActuatorInfo.h"
 #include "tgcreator/tgKinematicContactCableInfo.h"
 #include "tgcreator/tgRodInfo.h"
 #include "tgcreator/tgBoxInfo.h"
+#include "tgcreator/tgSphereInfo.h"
 #include "tgcreator/tgStructureInfo.h"
 
 /**
@@ -84,12 +86,13 @@ void TensegrityModel::setup(tgWorld& world) {
     // create the build spec that uses tags to turn the structure into a model
     tgBuildSpec spec;
 
-    // add default builders (rods, strings, boxes) that match the tags (rods, strings, boxes)
+    // add default builders (rods, strings, boxes) that match the tags (rods, strings, boxes, spheres)
     // (these will be overwritten if a different builder is specified for those tags)
     Yam emptyYam = Yam();
     addRodBuilder("tgRodInfo", "rod", emptyYam, spec);
     addBasicActuatorBuilder("tgBasicActuatorInfo", "string", emptyYam, spec);
     addBoxBuilder("tgBoxInfo", "box", emptyYam, spec);
+    addSphereBuilder("tgSphereInfo", "sphere", emptyYam, spec);
 
     tgStructure structure;
     buildStructure(structure, topLvlStructurePath, spec);
@@ -612,6 +615,9 @@ void TensegrityModel::addBuilders(tgBuildSpec& spec, const Yam& builders) {
 	else if (builderClass == "tgBoxInfo") {
             addBoxBuilder(builderClass, tagMatch, parameters, spec);
         }
+	else if (builderClass == "tgSphereInfo") {
+	    addSphereBuilder(builderClass, tagMatch, parameters, spec);
+	}
         // add more builders here if they use a different Config
         else {
             throw std::invalid_argument("Unsupported builder class: " + builderClass);
@@ -818,6 +824,60 @@ void TensegrityModel::addBoxBuilder(const std::string& builderClass, const std::
     }
 }
 
+void TensegrityModel::addSphereBuilder(const std::string& builderClass, const std::string& tagMatch, const Yam& parameters, tgBuildSpec& spec){
+  /**
+   * Builder prcedure:
+   * (1) create list of all possible parameters, as with box (for example)
+   * (2) substitute in any passed-in paremeters in the YAML file
+   * (3) VALIDATION: for the sphere, we must demand that BOTH NODES be the same.
+   *     This is a pretty terrible hack at the moment, but the YAML parser only
+   *     supports two-node objects, whereas the sphere is a single-node object.
+   * (4) create the tgSphereInfo object and add it to the list of builders.
+   */
+
+  // (1)
+  // Parameters to be used in tgSphere::Config. See core/tgSphere.h
+  // Spheres are simple: one point, one radius, and then same
+  // rigid body parameters as the rest of the gang.
+  // Create a map of strings to doubles that will hold the sphere parameters
+  std::map<std::string, double> sp;
+  sp["radius"] = sphereRadius;
+  sp["density"] = sphereDensity;
+  sp["friction"] = sphereFriction;
+  sp["roll_friction"] = sphereRollFriction;
+  sp["restitution"] = sphereRestitution;
+
+  // (2) sub in the new stuff if any exists
+  if (parameters) {
+    for (YAML::const_iterator parameter = parameters.begin(); parameter != parameters.end(); ++parameter) {
+      std::string parameterName = parameter->first.as<std::string>();
+      if (sp.find(parameterName) == sp.end()) {
+	throw std::invalid_argument("Unsupported " + builderClass + " parameter: " + parameterName);
+      }
+      // if defined overwrite default parameter value
+      sp[parameterName] = parameter->second.as<double>();
+    }
+  }
+
+  // (3)
+  // VALIDATION. We need the node locations to be the same.
+  // Can't create a sphere in two different places.
+  // This is a hack. To-do: refactor the YAML parser so that builders can be
+  // created for a single node, or more than two nodes.
+  // ...actually, we can't do this check here. Makes things more difficult...
+
+  // (4) add the builder. HOPEFULLY, this will create a builder that only
+  // looks for nodes, not pairs. To do, check tgSphereInfo to see what happens if
+  // it encounters a pair of spheres.
+  const tgSphere::Config sphereConfig = tgSphere::Config(sp["radius"],
+		 sp["density"], sp["friction"], sp["roll_friction"], sp["restitution"]);
+  if (builderClass == "tgSphereInfo") {
+    // tgBuildSpec takes ownership of the tgSphereInfo object
+    spec.addBuilder(tagMatch, new tgSphereInfo(sphereConfig));
+  }
+
+}
+
 void TensegrityModel::yamlNoDuplicates(const Yam& yam, const std::string structurePath) {
     std::set<std::string> keys;
     for (YAML::const_iterator iter = yam.begin(); iter != yam.end(); ++iter) {

src/yamlbuilder/TensegrityModel.h

@@ -111,6 +111,18 @@ class TensegrityModel : public tgSubject<TensegrityModel>, public tgModel
      */
     const static double boxRestitution = 0.2;
 
+    // Sphere parameters:
+
+    /*
+     * Default sphere radius, density, friction, rolling friction, density, etc.
+     * See tgSphere.h.
+     */
+    const static double sphereRadius = 0.5;
+    const static double sphereDensity = 1.0;
+    const static double sphereFriction = 1.0;
+    const static double sphereRollFriction = 0.0;
+    const static double sphereRestitution = 0.2;
+
     // String parameters:
 
     /*
@@ -389,6 +401,11 @@ class TensegrityModel : public tgSubject<TensegrityModel>, public tgModel
      */
     void addBoxBuilder(const std::string& builderClass, const std::string& tagMatch, const Yam& parameters, tgBuildSpec& spec);
 
+    /*
+     * Responsible for adding a builder that uses the tgSphere config
+     */
+    void addSphereBuilder(const std::string& builderClass, const std::string& tagMatch, const Yam& parameters, tgBuildSpec& spec);
+
     /*
      * Ensures YAML node contains only keys from the supplied vector
      */