Merge pull request #96 from resibots/robot_reset

Robot reset

Author: costashatz

src/python/robot.cpp

@@ -61,6 +61,9 @@ namespace robot_dart {
                 .def("free", &Robot::free)
 
                 .def("reset", &Robot::reset)
+                .def("clear_external_forces", &Robot::clear_external_forces)
+                .def("clear_internal_forces", &Robot::clear_internal_forces)
+                .def("reset_commands", &Robot::reset_commands)
 
                 .def("set_actuator_types", &Robot::set_actuator_types,
                     py::arg("type"),
@@ -237,8 +240,6 @@ namespace robot_dart {
                     py::arg("torque"),
                     py::arg("local") = false)
 
-                .def("clear_external_forces", &Robot::clear_external_forces)
-
                 .def("external_forces", (Eigen::Vector6d(Robot::*)(const std::string& body_name) const) & Robot::external_forces)
                 .def("external_forces", (Eigen::Vector6d(Robot::*)(size_t body_index) const) & Robot::external_forces)
 

src/robot_dart/robot.cpp

@@ -288,6 +288,7 @@ namespace robot_dart {
         ROBOT_DART_EXCEPTION_INTERNAL_ASSERT(_skeleton != nullptr);
         _skeleton->setName(robot_name);
         _set_damages(damages);
+        reset();
     }
 
     std::shared_ptr<Robot> Robot::clone() const
@@ -488,9 +489,22 @@ namespace robot_dart {
         _skeleton->resetPositions();
         _skeleton->resetVelocities();
         _skeleton->resetAccelerations();
-        this->clear_external_forces();
+
+        clear_internal_forces();
+        reset_commands();
+        clear_external_forces();
     }
 
+    void Robot::clear_external_forces() { _skeleton->clearExternalForces(); }
+
+    void Robot::clear_internal_forces()
+    {
+        _skeleton->clearInternalForces();
+        _skeleton->clearConstraintImpulses();
+    }
+
+    void Robot::reset_commands() { _skeleton->resetCommands(); }
+
     void Robot::set_actuator_types(const std::string& type, const std::vector<std::string>& joint_names, bool override_mimic, bool override_base)
     {
         // Set all dofs to same actuator type
@@ -1003,8 +1017,6 @@ namespace robot_dart {
         bd->addExtTorque(torque, local);
     }
 
-    void Robot::clear_external_forces() { _skeleton->clearExternalForces(); }
-
     Eigen::Vector6d Robot::external_forces(const std::string& body_name) const
     {
         auto bd = _skeleton->getBodyNode(body_name);
@@ -1718,8 +1730,10 @@ namespace robot_dart {
         body->setInertia(inertia);
 
         // Put the body into position
+        auto robot = std::make_shared<Robot>(box_skel, box_name);
+
         if (type == "free") // free floating
-            box_skel->setPositions(pose);
+            robot->set_positions(pose);
         else // fixed
         {
             Eigen::Isometry3d T;
@@ -1728,7 +1742,7 @@ namespace robot_dart {
             body->getParentJoint()->setTransformFromParentBodyNode(T);
         }
 
-        return std::make_shared<Robot>(box_skel, box_name);
+        return robot;
     }
 
     std::shared_ptr<Robot> Robot::create_ellipsoid(const Eigen::Vector3d& dims,
@@ -1759,9 +1773,11 @@ namespace robot_dart {
         inertia.setMoment(ellipsoid->computeInertia(mass));
         body->setInertia(inertia);
 
+        auto robot = std::make_shared<Robot>(ellipsoid_skel, ellipsoid_name);
+
         // Put the body into position
         if (type == "free") // free floating
-            ellipsoid_skel->setPositions(pose);
+            robot->set_positions(pose);
         else // fixed
         {
             Eigen::Isometry3d T;
@@ -1770,6 +1786,6 @@ namespace robot_dart {
             body->getParentJoint()->setTransformFromParentBodyNode(T);
         }
 
-        return std::make_shared<Robot>(ellipsoid_skel, ellipsoid_name);
+        return robot;
     }
 } // namespace robot_dart

src/robot_dart/robot.hpp

@@ -65,6 +65,10 @@ namespace robot_dart {
 
         void reset();
 
+        void clear_external_forces();
+        void clear_internal_forces();
+        void reset_commands();
+
         // actuator type can be: torque, servo, velocity, passive, locked, mimic (only for completeness, use set_mimic to use this)
         // Be careful that actuator types are per joint and not per DoF
         void set_actuator_types(const std::string& type, const std::vector<std::string>& joint_names = {}, bool override_mimic = false, bool override_base = false);
@@ -155,8 +159,6 @@ namespace robot_dart {
         void add_external_torque(const std::string& body_name, const Eigen::Vector3d& torque, bool local = false);
         void add_external_torque(size_t body_index, const Eigen::Vector3d& torque, bool local = false);
 
-        void clear_external_forces();
-
         Eigen::Vector6d external_forces(const std::string& body_name) const;
         Eigen::Vector6d external_forces(size_t body_index) const;