diff --git a/CMakeLists.txt b/CMakeLists.txt index 2c421f3f8b0eaec68e49bf3f5ee61ef5068955fb..960c9c6848378bfa5dcf0df5e45cfa9ab22a62dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -44,8 +44,8 @@ ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.16.1") ADD_REQUIRED_DEPENDENCY("hrp2-dynamics >= 1.3.0") ADD_REQUIRED_DEPENDENCY("hrp2-10-optimized >= 1.0") -ADD_REQUIRED_DEPENDENCY("hrp2_10 >= 1.0.0") -ADD_REQUIRED_DEPENDENCY("hrp2_14 >= 1.0.0") +ADD_REQUIRED_DEPENDENCY("hrp2-10 >= 1.0.0") +ADD_REQUIRED_DEPENDENCY("hrp2-14 >= 1.0.0") ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 1.0.0") ADD_REQUIRED_DEPENDENCY("sot-core >= 1.0.0") diff --git a/include/sot-dynamic/dynamic-hrp2.h b/include/sot-dynamic/dynamic-hrp2.h index 12f1a0d4118a3d4648f48df6471d506a1c5ed747..416016b900c4c00112c8664300c57e84a5ce9ee0 100644 --- a/include/sot-dynamic/dynamic-hrp2.h +++ b/include/sot-dynamic/dynamic-hrp2.h @@ -65,6 +65,10 @@ class SOTDYNAMICHRP2_EXPORT DynamicHrp2 { public: + virtual const std::string& getClassName () const + { + return CLASS_NAME; + } static const std::string CLASS_NAME; public: /* --- CONSTRUCTION --- */ diff --git a/include/sot-dynamic/dynamic.h b/include/sot-dynamic/dynamic.h index 0f10f2962e6c559cb82bfddeb180d2d90c17d266..8b70743d3028172b8938321969d6b944cc1d828b 100644 --- a/include/sot-dynamic/dynamic.h +++ b/include/sot-dynamic/dynamic.h @@ -93,6 +93,10 @@ class SOTDYNAMIC_EXPORT Dynamic friend class sot::command::InitializeRobot; public: + virtual const std::string& getClassName () const + { + return CLASS_NAME; + } static const std::string CLASS_NAME; virtual const std::string& getClassName( void ) const { return CLASS_NAME; } diff --git a/src/dynamic_graph/sot/dynamics/solver.py b/src/dynamic_graph/sot/dynamics/solver.py index 51b27b183d464277cf2884b8dabc742cb4c0abd7..1be5988b798546111fbaa6c5d8da98ad521cb95b 100644 --- a/src/dynamic_graph/sot/dynamics/solver.py +++ b/src/dynamic_graph/sot/dynamics/solver.py @@ -16,7 +16,7 @@ # dynamic-graph. If not, see <http://www.gnu.org/licenses/>. from dynamic_graph import plug -from dynamic_graph.sot.core import SOT +from dynamic_graph.sot.core import JointLimitator, SOT class Solver: robot = None @@ -24,14 +24,28 @@ class Solver: def __init__(self, robot): self.robot = robot + + # Make sure control does not exceed joint limits. + self.jointLimitator = JointLimitator('joint_limitator') + plug(self.robot.dynamic.position, self.jointLimitator.joint) + plug(self.robot.dynamic.upperJl, self.jointLimitator.upperJl) + plug(self.robot.dynamic.lowerJl, self.jointLimitator.lowerJl) + + # Create the solver. self.sot = SOT('solver') self.sot.signal('damping').value = 1e-6 self.sot.setNumberDofs(self.robot.dimension) + # Plug the solver control into the filter. + plug(self.sot.control, self.jointLimitator.controlIN) + + # Important: always use 'jointLimitator.control' + # and NOT 'sot.control'! + if robot.device: - plug(self.sot.signal('control'), robot.device.signal('control')) - plug(self.robot.device.state, - self.robot.dynamic.position) + plug(self.jointLimitator.control, robot.device.control) + plug(self.robot.device.state, self.robot.dynamic.position) + def push(self, taskName): """ Proxy method to push a task in the sot