Merge pull request #153 from resibots/break_sensors

Allow to "break" sensors

Author: costashatz

src/robot_dart/robot.cpp

@@ -69,6 +69,8 @@ namespace robot_dart {
                     return skeleton->getGravityForces();
                 else if (content == 15)
                     return skeleton->getCoriolisAndGravityForces();
+                else if (content == 16)
+                    return skeleton->getConstraintForces();
                 ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
             }
 
@@ -81,6 +83,8 @@ namespace robot_dart {
                 tmp = skeleton->getGravityForces();
             else if (content == 15)
                 tmp = skeleton->getCoriolisAndGravityForces();
+             else if (content == 16)
+                tmp = skeleton->getConstraintForces();
 
             for (size_t i = 0; i < dof_names.size(); i++) {
                 auto it = dof_map.find(dof_names[i]);
@@ -112,7 +116,7 @@ namespace robot_dart {
                     data(i) = dof->getForceLowerLimit();
                 else if (content == 12)
                     data(i) = dof->getForceUpperLimit();
-                else if (content == 13 || content == 14 || content == 15)
+                else if (content == 13 || content == 14 || content == 15 || content == 16)
                     data(i) = tmp(it->second);
                 else
                     ROBOT_DART_EXCEPTION_ASSERT(false, "Unknown type of data!");
@@ -1640,6 +1644,11 @@ namespace robot_dart {
         return detail::dof_data<15>(_skeleton, dof_names, _dof_map);
     }
 
+    Eigen::VectorXd Robot::constraint_forces(const std::vector<std::string>& dof_names) const
+    {
+        return detail::dof_data<16>(_skeleton, dof_names, _dof_map);
+    }
+
     Eigen::VectorXd Robot::vec_dof(const Eigen::VectorXd& vec, const std::vector<std::string>& dof_names) const
     {
         assert(vec.size() == static_cast<int>(_skeleton->getNumDofs()));

src/robot_dart/robot.hpp

@@ -218,6 +218,7 @@ namespace robot_dart {
         Eigen::VectorXd coriolis_forces(const std::vector<std::string>& dof_names = {}) const;
         Eigen::VectorXd gravity_forces(const std::vector<std::string>& dof_names = {}) const;
         Eigen::VectorXd coriolis_gravity_forces(const std::vector<std::string>& dof_names = {}) const;
+        Eigen::VectorXd constraint_forces(const std::vector<std::string>& dof_names = {}) const;
 
         // Get only the part of vector for DOFs in dof_names
         Eigen::VectorXd vec_dof(const Eigen::VectorXd& vec, const std::vector<std::string>& dof_names) const;

src/robot_dart/sensor/sensor.cpp

@@ -43,8 +43,18 @@ namespace robot_dart {
         void Sensor::set_pose(const Eigen::Isometry3d& tf) { _world_pose = tf; }
         const Eigen::Isometry3d& Sensor::pose() const { return _world_pose; }
 
+        void Sensor::detach() {
+            _attached_to_body = false;
+            _attached_to_joint = false;
+            _body_attached = nullptr;
+            _joint_attached = nullptr;
+            _active = false;
+        }
+
         void Sensor::refresh(double t)
         {
+            if (!_active)
+                return;
             if (_attaching_to_body && !_attached_to_body) {
                 attach_to_body(_body_attached, _attached_tf);
             }
@@ -71,7 +81,6 @@ namespace robot_dart {
                 if (body)
                     _world_pose = body->getWorldTransform() * tf * _attached_tf;
             }
-
             calculate(t);
         }
 
@@ -111,5 +120,13 @@ namespace robot_dart {
                 _attached_to_joint = false;
             }
         }
+        const std::string& Sensor::attached_to() const
+         { 
+            ROBOT_DART_EXCEPTION_ASSERT(_attached_to_body || _attached_to_joint, "Joint is not attached to anything");
+            if (_attached_to_body)
+                return _body_attached->getName();
+            // attached to joint
+            return _joint_attached->getName();
+        }
     } // namespace sensor
 } // namespace robot_dart

src/robot_dart/sensor/sensor.hpp

@@ -52,6 +52,8 @@ namespace robot_dart {
             virtual void attach_to_joint(dart::dynamics::Joint* joint, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity());
             void attach_to_joint(const std::shared_ptr<Robot>& robot, const std::string& joint_name, const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity()) { attach_to_joint(robot->joint(joint_name), tf); }
 
+            void detach();
+            const std::string& attached_to() const;
         protected:
             RobotDARTSimu* _simu = nullptr;
             bool _active;