Add API. Fix scene builder and reparenting frame (#77)

* added moveit <-> dart state functions

* remove dartsim from catkin includes, added rviz helper ptr

* setting limits on joints

* added options struct

* new pick/place demo

* removed goof

* multi-robot tsr goals tested

* update CO, setTF, fix scene builder

* Clean up

* update cmake

* removed other planning functions

* removed remaining master

* robowflex robotpose consistency

* using new functions

* fixing goofs

Co-authored-by: Zachary Kingston <zak@rice.edu>
Co-authored-by: Zachary Kingston <kingston.zak@gmail.com>

Author: tonypan1995

robowflex_dart/CMakeLists.txt

@@ -48,7 +48,6 @@ if (DART_LIBRARIES)
     LIBRARIES ${LIBRARY_NAME}
     CATKIN_DEPENDS
     robowflex_library
-    # dartsim
     DEPENDS
     ompl
     INCLUDE_DIRS ${CMAKE_CURRENT_LIST_DIR}/include

robowflex_dart/include/robowflex_dart/planning.h

@@ -90,7 +90,7 @@ namespace robowflex
             struct
             {
                 bool use_gradient{false};
-                std::size_t max_samples{2};
+                std::size_t max_samples{10};
             } options;
 
         private:

robowflex_dart/include/robowflex_dart/structure.h

@@ -14,6 +14,8 @@
 #include <dart/dynamics/Skeleton.hpp>
 
 #include <robowflex_library/class_forward.h>
+#include <robowflex_library/adapter.h>
+#include <robowflex_library/tf.h>
 
 namespace robowflex
 {
@@ -159,6 +161,20 @@ namespace robowflex
              */
             void reparentFreeFrame(dart::dynamics::BodyNode *child, const std::string &parent = "");
 
+            /** \brief Set the transform from a joint to its parent.
+             *  \param[in] name Name of joint to set transform of.
+             *  \param[in] tf Transform to set.
+             */
+            void setJointParentTransform(const std::string &name, const RobotPose &tf);
+
+            /** \brief Update or add a collision object.
+             *  \param[in] name Name of object to add.
+             *  \param[in] geometry Geometry of object.
+             *  \param[in] pose Pose of object to set.
+             */
+            void updateCollisionObject(const std::string &name, const GeometryPtr &geometry,
+                                       const robowflex::RobotPose &pose);
+
             /** \} */
 
             /** \name Constructing Frames

robowflex_dart/scripts/fetch_ik.cpp

@@ -85,5 +85,6 @@ int main(int argc, char **argv)
             builder.ss->clear();
         }
     });
+
     return 0;
 }

robowflex_dart/scripts/fetch_pick.cpp

@@ -68,8 +68,8 @@ int main(int argc, char **argv)
         start_tsr.useGroup(GROUP);
         start_tsr.initialize();
         start_tsr.solveWorld();
-        builder.setStartConfigurationFromWorld();
 
+        builder.setStartConfigurationFromWorld();
         builder.initialize();
 
         darts::TSR::Specification goal_spec;
@@ -104,9 +104,7 @@ int main(int argc, char **argv)
     const auto &planToPlace = [&]() {
         darts::PlanBuilder builder(world);
         builder.addGroup(fetch_name, GROUP);
-
         builder.setStartConfigurationFromWorld();
-
         builder.initialize();
 
         darts::TSR::Specification goal_spec;
@@ -147,6 +145,5 @@ int main(int argc, char **argv)
 
         planToPlace();
     });
-
     return 0;
 }

robowflex_dart/src/planning.cpp

@@ -90,7 +90,8 @@ bool TSRGoal::sample(const ompl::base::GoalLazySamples * /*gls*/, ompl::base::St
         si_->enforceBounds(state);
     }
 
-    return true;
+    total_samples_++;
+    return total_samples_ < options.max_samples;
 }
 
 double TSRGoal::distanceGoal(const ompl::base::State *state) const

robowflex_dart/src/robot.cpp

@@ -309,12 +309,12 @@ bool Robot::loadSRDF(const std::string &srdf)
             const auto &value = js->Attribute("value");
             const auto &jname = js->Attribute("name");
 
-            auto values = robowflex::IO::tokenize(value, " ");
+            auto values = robowflex::IO::tokenize<double>(value, " ");
             auto joint = getGroupJoint(group, jname);
             if (joint.second != nullptr)
             {
                 for (std::size_t i = 0; i < values.size(); ++i)
-                    state[joint.first + i] = std::stod(values[i]);
+                    state[joint.first + i] = values[i];
             }
 
             js = js->NextSiblingElement("joint");

robowflex_dart/src/structure.cpp

@@ -38,11 +38,12 @@ Structure::Structure(const std::string &name, const ScenePtr &scene) : Structure
 
         dart::dynamics::FreeJoint::Properties joint;
         joint.mName = object;
-        joint.mT_ParentBodyToJoint = pose;
+        joint.mT_ParentBodyToJoint = robowflex::TF::identity();
 
         auto shape = makeGeometry(geometry);
 
         auto pair = addFreeFrame(joint, shape);
+        setJointParentTransform(object, pose);
         setColor(pair.second, dart::Color::Blue(0.2));
     }
 
@@ -117,6 +118,10 @@ void Structure::createShapeNode(dart::dynamics::BodyNode *body, const dart::dyna
     double mass = magic::DEFAULT_DENSITY * shape->getVolume();
     inertia.setMass(mass);
     inertia.setMoment(shape->computeInertia(mass));
+
+    if (not inertia.verify(false))
+        inertia = dart::dynamics::Inertia();
+
     body->setInertia(inertia);
     body->setRestitutionCoeff(magic::DEFAULT_RESTITUTION);
 
@@ -233,16 +238,11 @@ dart::dynamics::BodyNode *Structure::getFrame(const std::string &name) const
     return skeleton_->getBodyNode(name);
 }
 
-dart::dynamics::BodyNode *Structure::getRootFrame() const
-{
-    return skeleton_->getRootBodyNode();
-}
-
 void Structure::reparentFreeFrame(dart::dynamics::BodyNode *child, const std::string &parent)
 {
     auto frame = getFrame(parent);
 
-    Eigen::Isometry3d tf;
+    RobotPose tf;
     if (frame)
         tf = child->getTransform(frame);
     else
@@ -253,6 +253,37 @@ void Structure::reparentFreeFrame(dart::dynamics::BodyNode *child, const std::st
     jt->setRelativeTransform(tf);
 }
 
+void Structure::setJointParentTransform(const std::string &name, const RobotPose &tf)
+{
+    auto joint = skeleton_->getJoint(name);
+    joint->setTransformFromParentBodyNode(tf);
+}
+
+void Structure::updateCollisionObject(const std::string &name, const GeometryPtr &geometry,
+                                      const robowflex::RobotPose &pose)
+{
+    auto nodes = skeleton_->getBodyNodes(name);  // Get all nodes with this name
+    if (nodes.size() != 0)
+        setJointParentTransform(name, pose);
+    else
+    {
+        dart::dynamics::FreeJoint::Properties joint;
+        joint.mName = name;
+        joint.mT_ParentBodyToJoint = robowflex::TF::identity();
+
+        auto shape = makeGeometry(geometry);
+
+        auto pair = addFreeFrame(joint, shape);
+        setJointParentTransform(name, pose);
+        setColor(pair.second, dart::Color::Blue(0.2));
+    }
+}
+
+dart::dynamics::BodyNode *Structure::getRootFrame() const
+{
+    return skeleton_->getRootBodyNode();
+}
+
 dart::dynamics::ShapePtr robowflex::darts::makeGeometry(const GeometryPtr &geometry)
 {
     const auto &dimensions = geometry->getDimensions();
@@ -297,16 +328,27 @@ std::shared_ptr<dart::dynamics::SphereShape> robowflex::darts::makeSphere(double
 
 std::shared_ptr<dart::dynamics::MeshShape> robowflex::darts::makeMesh(const GeometryPtr &geometry)
 {
-    static Assimp::Importer importer_;
+    auto uri = geometry->getResource();
+    if (uri.empty())
+    {
+        static Assimp::Importer importer_;
 
-    auto shape = std::dynamic_pointer_cast<shapes::Mesh>(geometry->getShape());
-    std::vector<char> buffer;
-    shapes::writeSTLBinary(shape.get(), buffer);
+        auto shape = std::dynamic_pointer_cast<shapes::Mesh>(geometry->getShape());
+        std::vector<char> buffer;
+        shapes::writeSTLBinary(shape.get(), buffer);
 
-    auto aiscene = importer_.ReadFileFromMemory(buffer.data(), buffer.size(),
-                                                aiProcessPreset_TargetRealtime_MaxQuality, "STOL");
+        auto aiscene = importer_.ReadFileFromMemory(buffer.data(), buffer.size(),
+                                                    aiProcessPreset_TargetRealtime_MaxQuality, "STOL");
 
-    return std::make_shared<dart::dynamics::MeshShape>(geometry->getDimensions(), aiscene);
+        auto mesh = std::make_shared<dart::dynamics::MeshShape>(geometry->getDimensions(), aiscene);
+        return mesh;
+    }
+    else
+    {
+        auto aiscene = dart::dynamics::MeshShape::loadMesh(uri);
+        auto mesh = std::make_shared<dart::dynamics::MeshShape>(geometry->getDimensions(), aiscene);
+        return mesh;
+    }
 }
 
 std::shared_ptr<dart::dynamics::MeshShape> robowflex::darts::makeArcsegment(double low, double high,

robowflex_library/include/robowflex_library/tf.h

@@ -28,6 +28,11 @@ namespace robowflex
      */
     namespace TF
     {
+        /** \brief Creates the Identity pose.
+         *  \return A new identity robot pose.
+         */
+        RobotPose identity();
+
         /** \brief Creates a robot pose from a linear component and XYZ convention Euler angles
          *  \param[in] x X-axis translation
          *  \param[in] y Y-ayis translation

robowflex_library/src/tf.cpp

@@ -8,6 +8,11 @@
 
 using namespace robowflex;
 
+RobotPose TF::identity()
+{
+    return RobotPose::Identity();
+}
+
 RobotPose TF::createPoseXYZ(double x, double y, double z, double X, double Y, double Z)
 {
     return createPoseQ(Eigen::Vector3d{x, y, z},  //