1 /* 2 * Copyright (c) 2011-2021, The DART development contributors 3 * All rights reserved. 4 * 5 * The list of contributors can be found at: 6 * https://github.com/dartsim/dart/blob/master/LICENSE 7 * 8 * This file is provided under the following "BSD-style" License: 9 * Redistribution and use in source and binary forms, with or 10 * without modification, are permitted provided that the following 11 * conditions are met: 12 * * Redistributions of source code must retain the above copyright 13 * notice, this list of conditions and the following disclaimer. 14 * * Redistributions in binary form must reproduce the above 15 * copyright notice, this list of conditions and the following 16 * disclaimer in the documentation and/or other materials provided 17 * with the distribution. 18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF 26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 * POSSIBILITY OF SUCH DAMAGE. 31 */ 32 33 #ifndef EXAMPLES_ATLASSIMBICON_CONTROLLER_HPP_ 34 #define EXAMPLES_ATLASSIMBICON_CONTROLLER_HPP_ 35 36 #include <vector> 37 38 #include <Eigen/Dense> 39 40 #include <dart/dart.hpp> 41 42 class StateMachine; 43 44 /// \brief Implementation of Simbicon (Simple biped locomotion control) for 45 /// Atlas robot 46 /// 47 /// Reference: http://dl.acm.org/citation.cfm?id=1276509 48 class Controller 49 { 50 public: 51 /// \brief Constructor 52 Controller( 53 dart::dynamics::SkeletonPtr _atlasRobot, 54 dart::constraint::ConstraintSolver* _collisionSolver); 55 56 /// \brief Destructor 57 virtual ~Controller(); 58 59 /// \brief Called before every simulation time step in MyWindow class. 60 /// Compute control force and apply it to Atlas robot 61 virtual void update(); 62 63 /// \brief 64 dart::dynamics::SkeletonPtr getAtlasRobot(); 65 66 /// \brief Get current state machine 67 StateMachine* getCurrentState(); 68 69 /// \brief Change state to _stateMachine 70 void changeStateMachine(StateMachine* _stateMachine, double _currentTime); 71 72 /// \brief Change state machine to a state machine whose names is _name 73 void changeStateMachine(const std::string& _name, double _currentTime); 74 75 /// \brief Change state machine to a state machine whose index is _idx 76 void changeStateMachine(std::size_t _idx, double _currentTime); 77 78 /// \brief Get true iff this controller is currently allowing to control the 79 /// Atlas robot 80 bool isAllowingControl() const; 81 82 /// \brief Keyboard control 83 void keyboard(unsigned char _key, int _x, int _y, double _currentTime); 84 85 /// \brief Print debug information 86 void printDebugInfo() const; 87 88 /// \brief Harness the robot 89 void harnessPelvis(); 90 91 /// \brief Unharness the robot 92 void unharnessPelvis(); 93 94 /// \brief Harness the robot 95 void harnessLeftFoot(); 96 97 /// \brief Harness the robot 98 void unharnessLeftFoot(); 99 100 /// \brief Harness the robot 101 void harnessRightFoot(); 102 103 /// \brief Harness the robot 104 void unharnessRightFoot(); 105 106 /// \brief Reset the robot 107 void resetRobot(); 108 109 /// \brief Set the verbosity 110 void setVerbosity(bool verbosity); 111 112 protected: 113 /// \brief Atlas robot skeleton 114 dart::dynamics::SkeletonPtr mAtlasRobot; 115 116 /// \brief Conllision detector 117 dart::constraint::ConstraintSolver* mConstratinSolver; 118 119 /// \brief List of state machines 120 std::vector<StateMachine*> mStateMachines; 121 122 /// \brief Current state machine 123 StateMachine* mCurrentStateMachine; 124 125 /// \brief Flag for pelvis harnessing 126 bool mPelvisHarnessOn; 127 128 /// \brief Flag for left foot harnessing 129 bool mLeftFootHarnessOn; 130 131 /// \brief Flag for right foot harnessing 132 bool mRightFootHarnessOn; 133 134 /// \brief Index for coronal left hip 135 std::size_t mCoronalLeftHip; 136 137 /// \brief Index for coronal right hip 138 std::size_t mCoronalRightHip; 139 140 /// \brief Index for sagital left hip 141 std::size_t mSagitalLeftHip; 142 143 /// \brief Index for sagital right hip 144 std::size_t mSagitalRightHip; 145 146 /// \brief Lower bound for emergency stop 147 double mMinPelvisHeight; 148 149 /// \brief Upper bound for emergency stop 150 double mMaxPelvisHeight; 151 152 private: 153 /// \brief Check if this controller contains _stateMachine 154 bool _containStateMachine(const StateMachine* _stateMachine) const; 155 156 /// \brief Check if this controller contains a state machine whose name is 157 /// _name 158 bool _containStateMachine(const std::string& _name) const; 159 160 /// \brief Find a state machine whose name is _name 161 StateMachine* _findStateMachine(const std::string& _name) const; 162 163 /// \brief Build state machines 164 void _buildStateMachines(); 165 166 /// \brief Create standing controller 167 StateMachine* _createStandingStateMachine(); 168 169 /// \brief Create standing controller 170 StateMachine* _createWalkingInPlaceStateMachine(); 171 172 /// \brief Create standing controller 173 StateMachine* _createWalkingStateMachine(); 174 175 /// \brief Create running controller 176 StateMachine* _createRunningStateMachine(); 177 178 /// \brief Set joint damping 179 void _setJointDamping(); 180 181 /// \brief Get left foot 182 dart::dynamics::BodyNode* _getLeftFoot() const; 183 184 /// \brief Get right foot 185 dart::dynamics::BodyNode* _getRightFoot() const; 186 187 /// \brief Weld joint constraint for pelvis harnessing 188 dart::constraint::WeldJointConstraintPtr mWeldJointConstraintPelvis; 189 190 /// \brief Weld joint constraint for left foot harnessing 191 dart::constraint::WeldJointConstraintPtr mWeldJointConstraintLeftFoot; 192 193 /// \brief Weld joint constraint for right foot harnessing 194 dart::constraint::WeldJointConstraintPtr mWeldJointConstraintRightFoot; 195 196 /// \brief Initial state of the robot 197 dart::dynamics::Skeleton::Configuration mInitialState; 198 199 /// \brief Whether to print messages about the internal state 200 bool mVerbosity; 201 }; 202 203 #endif // EXAMPLES_ATLASSIMBICON_CONTROLLER_HPP_ 204