├── resources └── screenshot.png ├── humanoid_constraint_sampler_plugin.xml ├── .gitignore ├── config └── hrp2jsknt_stability.yaml ├── package.xml ├── CMakeLists.txt ├── README.md ├── include └── moveit_humanoid_stability │ ├── humanoid_stability.h │ └── humanoid_constraint_sampler.h └── src ├── humanoid_stability.cpp └── humanoid_constraint_sampler.cpp /resources/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/davetcoleman/moveit_humanoid_stability/HEAD/resources/screenshot.png -------------------------------------------------------------------------------- /humanoid_constraint_sampler_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Samples a whole body humanoid robot, starting with one leg 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | 15 | # Python stuff 16 | *.pyc 17 | 18 | # Data files 19 | *.dat 20 | 21 | # Emacs temp files 22 | .#* 23 | *~ 24 | 25 | # Silly OSX Files 26 | .DS_Store 27 | 28 | # CSV Files of Data 29 | *.csv 30 | 31 | # Catkin Workspace 32 | /CMakeLists.txt 33 | 34 | # Images 35 | *.png 36 | *.jpg 37 | 38 | # Catkin config 39 | CATKIN_IGNORE -------------------------------------------------------------------------------- /config/hrp2jsknt_stability.yaml: -------------------------------------------------------------------------------- 1 | humanoid_stability: 2 | # A heuristic for quickly determining the valid sampled area limits of a torso's virtual joint for stability checking 3 | bounding_box: 4 | min_x: -0.44564 5 | max_x: 0.44564 6 | min_y: -0.223091 7 | max_y: 0.223091 8 | min_z: 0.193524 9 | max_z: 0.732025 10 | padding: 0.1 11 | # Link names for the URDF 12 | left_foot_name: LLEG_LINK5 13 | right_foot_name: RLEG_LINK5 14 | # Planning groups names from the SRDF 15 | left_leg_name: left_leg 16 | right_leg_name: right_leg 17 | whole_body_minus_left_leg_name: whole_body_minus_left_leg 18 | whole_body_minus_right_leg_name: whole_body_minus_right_leg 19 | # this MoveIt planning group does NOT contain any virtual joints but does have all fingers, feet, head, etc 20 | all_joints_group: robot_joints 21 | # convex hull scaling factor for stability checking foot polygon 22 | foot_polygon_scale: 0.9 -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | moveit_humanoid_stability 4 | 0.0.1 5 | Humanoid stability functionality for MoveIt 6 | 7 | Dave Coleman 8 | 9 | Dave Coleman 10 | 11 | BSD 12 | 13 | https://github.com/davetcoleman/moveit_humanoid_stability 14 | https://github.com/davetcoleman/moveit_humanoid_stability/issues 15 | https://github.com/davetcoleman/moveit_humanoid_stability/ 16 | 17 | catkin 18 | 19 | hrl_kinematics 20 | moveit_core 21 | moveit_visual_tools 22 | roscpp 23 | pluginlib 24 | 25 | hrl_kinematics 26 | moveit_core 27 | moveit_visual_tools 28 | roscpp 29 | pluginlib 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(moveit_humanoid_stability) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | hrl_kinematics 6 | moveit_core 7 | moveit_visual_tools 8 | pluginlib 9 | roscpp 10 | ) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | # Catkin 16 | catkin_package( 17 | LIBRARIES 18 | ${PROJECT_NAME} 19 | CATKIN_DEPENDS 20 | hrl_kinematics 21 | moveit_core 22 | moveit_visual_tools 23 | INCLUDE_DIRS 24 | include 25 | ) 26 | 27 | include_directories( 28 | include 29 | ${catkin_INCLUDE_DIRS} 30 | ) 31 | 32 | # Stability Library 33 | add_library(${PROJECT_NAME} 34 | src/humanoid_stability.cpp 35 | ) 36 | target_link_libraries(${PROJECT_NAME} 37 | ${catkin_LIBRARIES} 38 | ${Boost_LIBRARIES} 39 | ) 40 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 41 | 42 | # Sampling Library 43 | add_library(humanoid_moveit_constraint_sampler 44 | src/humanoid_constraint_sampler.cpp 45 | ) 46 | target_link_libraries(humanoid_moveit_constraint_sampler 47 | ${catkin_LIBRARIES} 48 | ${Boost_LIBRARIES} 49 | ) 50 | add_dependencies(humanoid_moveit_constraint_sampler ${catkin_EXPORTED_TARGETS}) 51 | 52 | # Install library 53 | install( 54 | TARGETS 55 | ${PROJECT_NAME} 56 | humanoid_moveit_constraint_sampler 57 | LIBRARY DESTINATION 58 | ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | ) 60 | 61 | # Install header files 62 | install( 63 | DIRECTORY 64 | include 65 | DESTINATION 66 | ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 67 | ) 68 | 69 | # Install plugins 70 | install( 71 | FILES 72 | humanoid_constraint_sampler_plugin.xml 73 | DESTINATION 74 | ${CATKIN_PACKAGE_SHARE_DESTINATION} 75 | ) 76 | 77 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MoveIt! Humanoid Stability 2 | 3 | Libraries for sampling and validating a humanoid biped with one or more fixed feet for stability constraints in MoveIt! including: 4 | 5 | - Fast stability heuristic checking 6 | - Center of mass, static stability, and support polygon computations. 7 | - Smart sampling of whole body positions using random number generation 8 | 9 | Provides a wrapper for [hrl_kinematics](https://github.com/ahornung/hrl_kinematics) that allows the center of mass stability testing for robots based on KDL. 10 | 11 | Developed by [Dave Coleman](http://dav.ee) and Shintaro Noda at JSK, University of Tokyo. 12 | 13 |

14 | . 15 | 16 | ## Install 17 | 18 | This package requires an experimental branch of moveit_core be installed. It might be a while until it is merged into the main distribution. See [experimental_fake_base](https://github.com/davetcoleman/moveit_core/tree/experimental_fake_base) 19 | 20 | Overall, making this work on your machine will be fairly complicated until a long list of pending MoveIt! pull requests are accepted. See Dave Coleman for more details. 21 | 22 | ## Usage and Explanation 23 | 24 | ### Balance Constraint Validator 25 | 26 | Performs two course-grain checks and, if they pass, finally does full COM check: 27 | 28 | 1. Checks for a course-grain stability check by seeing if the torso is within a bounding box. This bound box is set in the yaml file and should be found through many iterations. A 0.1m expansion is added to the bounding box so that it over-accepts poses. 29 | 2. Checks if the other foot is above ground (z >= 0) 30 | 3. Checks if the COM is within the foot projection using hrl_kinematics 31 | 32 | It does not check for self collision or collision with environment, because that is accomplished by other components in MoveIt! automatically. 33 | 34 | You can use the validator within a MoveIt! planning scene as a custom stability checker - see the function ``getStateFeasibilityFn()`` for use with a planning scene by calling: 35 | 36 | ``` 37 | planning_scene_->setStateFeasibilityPredicate(humanoid_stability_->getStateFeasibilityFn()); 38 | ``` 39 | 40 | ### State Sampling 41 | 42 | The sampler first samples the fixed leg, performs some quick checks, then samples the rest of the body and performs exact checks. 43 | 44 | You can use a custom constraint sampler plugin for MoveIt! by adding a "constraint_samplers" param to your move_group launch file, for example: 45 | 46 | ``` 47 | 48 | 49 | 50 | 51 | ``` 52 | 53 | ## Configuration 54 | 55 | A yaml configuration file is required to load the settings for your robot. See [config/hrp2jsknt_stability.yaml](https://github.com/davetcoleman/moveit_humanoid_stability/blob/hydro-devel/config/hrp2jsknt_stability.yaml) 56 | 57 | ## Debug 58 | 59 | ### Balance Constraint Validator 60 | 61 | N/A 62 | 63 | ### State Sampling 64 | 65 | Rviz markers are available on topic ``/humanoid_constraint_sample_markers`` and the robot state is published on ``/humanoid_constraint_sample_robots`` 66 | 67 | ## Limitations / Future Work 68 | 69 | ### Stability Validator 70 | 71 | - Only supports standing on left or right foot - needs dual feet support 72 | - Only supports fixed link remaining the same (never changes location or link) 73 | - Improve conversion of joint states from MoveIt! to KDL 74 | - Smartly check which foot/feet to check for stability 75 | - Improve other foot check for above ground to include all of mesh, not just the origin of the tip link of the leg 76 | 77 | ### Constraint Sampler 78 | 79 | - Cleanup code (currently research code) 80 | - Sample for dual legs, which is a closed chain, and probably requires an IK solver -------------------------------------------------------------------------------- /include/moveit_humanoid_stability/humanoid_stability.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, JSK, The University of Tokyo. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the JSK, The University of Tokyo nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Wrapper for hrl_kinematics for calculing COM 37 | */ 38 | 39 | #ifndef MOVEIT_HUMANOID_STABILITY__HUMANOID_STABILITY 40 | #define MOVEIT_HUMANOID_STABILITY__HUMANOID_STABILITY 41 | 42 | // ROS 43 | #include 44 | 45 | // Helper for Rviz 46 | #include 47 | 48 | // Humanoid balance constraint tester 49 | #include 50 | 51 | namespace moveit_humanoid_stability 52 | { 53 | 54 | class HumanoidStability 55 | { 56 | private: 57 | 58 | // Show more visual and console output, with general slower run time. 59 | bool verbose_; 60 | 61 | // For visualizing things in rviz 62 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 63 | 64 | // Bounds for estimating virtual joint contraint 65 | double min_x_; 66 | double max_x_; 67 | double min_y_; 68 | double max_y_; 69 | double min_z_; 70 | double max_z_; 71 | 72 | // Model semantics 73 | const robot_model::LinkModel* left_foot_; 74 | const robot_model::LinkModel* right_foot_; 75 | const robot_model::LinkModel* free_foot_; // opposite of fixed_foot, this is the foot that is in the air 76 | const robot_model::JointModelGroup* all_joints_group_; 77 | 78 | // Stability checker 79 | hrl_kinematics::TestStabilityPtr test_stability_; 80 | std::map joint_positions_map_; 81 | hrl_kinematics::Kinematics::FootSupport support_mode_; 82 | tf::Vector3 normal_vector_; 83 | 84 | public: 85 | 86 | /** 87 | * \brief Constructor 88 | * \param verbose - run in debug mode 89 | * \param robot_state 90 | * \param visual_tools - shared visual tools ptr for debugging (optional) 91 | */ 92 | HumanoidStability(bool verbose, const moveit::core::RobotState &robot_state, 93 | const moveit_visual_tools::MoveItVisualToolsPtr &visual_tools = moveit_visual_tools::MoveItVisualToolsPtr()); 94 | 95 | /** 96 | * \brief Destructor 97 | */ 98 | ~HumanoidStability() {}; 99 | 100 | /** 101 | * \brief Helper for setting a planning scene state validator function 102 | * \return boost function pointer for use in PlanningScene 103 | */ 104 | planning_scene::StateFeasibilityFn getStateFeasibilityFn() 105 | { 106 | return boost::bind(&HumanoidStability::isValid, this, _1, _2); 107 | } 108 | 109 | /** 110 | * \brief Check if a robot state is stable/balanced based on all checks including heuristics 111 | * NOTE: this function can be used as a planning_scene StateFeasibilityFn 112 | * \param robot_state - input state to check 113 | * \return true if stable 114 | */ 115 | bool isValid(const robot_state::RobotState &robot_state, bool verbose); 116 | 117 | /** 118 | * \brief Run quick/low-cost virtual joint bounding box test on state to see if vjoint is within rough bounds 119 | * \param robot_state - input state to check 120 | * \return true if within bounds 121 | */ 122 | bool isApproximateValidBase(const robot_state::RobotState &robot_state); 123 | 124 | /** 125 | * \brief Run quick/low-cost other leg test to see if its above the z 0 value, e.g. above ground 126 | * \param robot_state - input state to check 127 | * \return true if within bounds 128 | */ 129 | bool isApproximateValidFoot(const robot_state::RobotState &robot_state); 130 | 131 | /** 132 | * \brief Run HRL kinematics COM calculation 133 | * \param robot_state - input state to check 134 | * \return true if stable 135 | */ 136 | bool isValidCOM(const robot_state::RobotState &robot_state); 137 | 138 | /** 139 | * \brief Debug function for showing the course-grain constraint enforcement of torso 140 | * \return true on success 141 | */ 142 | bool displayBoundingBox(const Eigen::Affine3d &translation = Eigen::Affine3d::Identity()) const; 143 | 144 | /** 145 | * \brief For debugging 146 | */ 147 | void printVirtualJointExtremes() const; 148 | 149 | /** 150 | * \brief Getter for Verbose 151 | */ 152 | bool getVerbose() 153 | { 154 | return verbose_; 155 | } 156 | 157 | /** 158 | * \brief Setter for Verbose 159 | */ 160 | void setVerbose(bool verbose) 161 | { 162 | verbose_ = verbose; 163 | } 164 | 165 | 166 | }; // end class 167 | 168 | // Create boost pointers for this class 169 | typedef boost::shared_ptr HumanoidStabilityPtr; 170 | typedef boost::shared_ptr HumanoidStabilityConstPtr; 171 | 172 | } // end namespace 173 | 174 | #endif 175 | -------------------------------------------------------------------------------- /include/moveit_humanoid_stability/humanoid_constraint_sampler.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement ("Modified BSD License") 3 | * 4 | * Copyright (c) 2014, JSK, The University of Tokyo. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the JSK, The University of Tokyo nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Samples 1 leg joint random, checks for partial validity, 37 | then samples rest of joints randomly adn checks for validity again 38 | */ 39 | 40 | #ifndef MOVEIT_HUMANOID_STABILITY__HUMANOID_CONSTRAINT_SAMPLER_ 41 | #define MOVEIT_HUMANOID_STABILITY__HUMANOID_CONSTRAINT_SAMPLER_ 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | // Stability checker 51 | #include 52 | 53 | // Helper for Rviz 54 | #include 55 | 56 | namespace moveit_humanoid_stability 57 | { 58 | 59 | /** 60 | * \brief HumanoidConstraintSampler is a class that allows the sampling 61 | * of joints in a particular group of the robot, subject to a set of individual joint constraints. 62 | * 63 | * The set of individual joint constraint reduce the allowable bounds 64 | * used in the sampling. Unconstrained values will be sampled within 65 | * their limits. 66 | * 67 | */ 68 | class HumanoidConstraintSampler : public constraint_samplers::ConstraintSampler 69 | { 70 | public: 71 | 72 | /** 73 | * Constructor 74 | * 75 | * @param [in] scene The planning scene used to check the constraint 76 | * 77 | * @param [in] group_name The group name associated with the 78 | * constraint. Will be invalid if no group name is passed in or the 79 | * joint model group cannot be found in the kinematic model 80 | * 81 | */ 82 | HumanoidConstraintSampler(const planning_scene::PlanningSceneConstPtr &scene, 83 | const std::string &group_name) 84 | : ConstraintSampler(scene, group_name) 85 | { 86 | } 87 | 88 | /** 89 | * \brief Configures a joint constraint given a Constraints message. 90 | * 91 | * If more than one constraint for a particular joint is specified, 92 | * the most restrictive set of bounds will be used (highest minimum 93 | * value, lowest maximum value). For the configuration to be 94 | * successful, the following condition must be met, in addition to 95 | * the conditions specified in \ref configure(const std::vector &jc) : 96 | 97 | * \li The Constraints message must contain one or more valid joint 98 | * constraints (where validity is judged by the ability to configure 99 | * a \ref JointConstraint) 100 | * 101 | * @param [in] constr The message containing the constraints 102 | * 103 | * @return True if the conditions are met, otherwise false 104 | */ 105 | virtual bool configure(const moveit_msgs::Constraints &constr); 106 | 107 | /** 108 | * \brief Helper for setting up the visualizer in Rviz 109 | */ 110 | void loadVisualTools(); 111 | 112 | /** 113 | * \brief Configures a joint constraint given a vector of constraints. 114 | * 115 | * If more than one constraint for a particular joint is specified, 116 | * the most restrictive set of bounds will be used (highest minimum 117 | * value, lowest_maximum value. For the configuration to be 118 | * successful, the following conditions must be met: 119 | 120 | * \li The vector must contain one or more valid, enabled joint 121 | * constraints 122 | * 123 | * \li At least one constraint must reference a joint in the 124 | * indicated group. If no additional bounds exist for this group, 125 | * then RobotState::setToRandomPositions() can be 126 | * used to generate a sample independently from the 127 | * constraint_samplers infrastructure. 128 | * 129 | * \li The constraints must allow a sampleable region for all 130 | * joints, where the most restrictive minimum bound is less than the 131 | * most restrictive maximum bound 132 | * 133 | * @param [in] jc The vector of joint constraints 134 | * 135 | * @return True if the conditions are met, otherwise false 136 | */ 137 | bool configureJoint(const std::vector &jc); 138 | 139 | virtual bool sample(robot_state::RobotState &robot_state, const robot_state::RobotState &ks, 140 | unsigned int max_attempts); 141 | 142 | bool sampleJoints(robot_state::RobotState &robot_state); 143 | 144 | virtual bool project(robot_state::RobotState &robot_state, 145 | unsigned int max_attempts); 146 | 147 | /** 148 | * \brief Gets the number of constrained joints - joints that have an 149 | * additional bound beyond the joint limits. 150 | * 151 | * 152 | * @return The number of constrained joints. 153 | */ 154 | std::size_t getConstrainedJointCount() const 155 | { 156 | return bounds_.size(); 157 | } 158 | 159 | /** 160 | * \brief Gets the number of unconstrained joints - joint that have 161 | * no additional bound beyond the joint limits. 162 | * 163 | * @return The number of unconstrained joints. 164 | */ 165 | std::size_t getUnconstrainedJointCount() const 166 | { 167 | return unbounded_.size(); 168 | } 169 | 170 | /** 171 | * \brief Get the name of the constraint sampler, for debugging purposes 172 | * should be in CamelCase format. 173 | * \return string of name 174 | */ 175 | virtual const std::string& getName() const 176 | { 177 | //printVirtualJointExtremes(); 178 | static const std::string SAMPLER_NAME = "HumanoidConstraintSampler"; 179 | return SAMPLER_NAME; 180 | } 181 | 182 | /** 183 | * \brief Override so we can set verbose flag in sub components 184 | */ 185 | virtual void setVerbose(bool verbose); 186 | 187 | private: 188 | 189 | 190 | protected: 191 | 192 | /// \brief An internal structure used for maintaining constraints on a particular joint 193 | struct JointInfo 194 | { 195 | /** 196 | * \brief Constructor 197 | * 198 | * @return 199 | */ 200 | JointInfo() 201 | { 202 | min_bound_ = -std::numeric_limits::max(); 203 | max_bound_ = std::numeric_limits::max(); 204 | } 205 | 206 | /** 207 | * \brief Function that adjusts the joints only if they are more 208 | * restrictive. This means that the min limit is higher than the 209 | * current limit, or the max limit is lower than the current max 210 | * limit. 211 | * 212 | * @param min The min limit for potential adjustment 213 | * @param max The max limit for potential adjustment 214 | */ 215 | void potentiallyAdjustMinMaxBounds(double min, double max) 216 | { 217 | min_bound_ = std::max(min, min_bound_); 218 | max_bound_ = std::min(max, max_bound_); 219 | } 220 | 221 | double min_bound_; /**< The most restrictive min value of those set */ 222 | double max_bound_; /**< The most restrictive max value of those set */ 223 | std::size_t index_; /**< The index within the joint state vector for this joint */ 224 | }; 225 | 226 | virtual void clear(); 227 | 228 | random_numbers::RandomNumberGenerator random_number_generator_; /**< \brief Random number generator used to sample */ 229 | std::vector bounds_; /**< \brief The bounds for any joint with bounds that are more restrictive than the joint limits */ 230 | 231 | std::vector unbounded_; /**< \brief The joints that are not bounded except by joint limits */ 232 | std::vector uindex_; /**< \brief The index of the unbounded joints in the joint state vector */ 233 | std::vector values_; /**< \brief Values associated with this group to avoid continuously reallocating */ 234 | 235 | std::string sampler_name_; // used for debugging 236 | 237 | // Leg IK solvers 238 | //const robot_model::JointModelGroup* left_leg_; 239 | //const robot_model::JointModelGroup* right_leg_; 240 | 241 | // Store desired feet positions 242 | //Eigen::Affine3d left_foot_position_; 243 | //Eigen::Affine3d right_foot_position_; 244 | 245 | // Store foot to torso translation 246 | //Eigen::Affine3d left_foot_to_torso_; 247 | 248 | // Allocate memory for storing transforms of feet 249 | //Eigen::Affine3d left_foot_position_new_; 250 | //Eigen::Affine3d right_foot_position_new_; 251 | 252 | // Verbose mode 253 | geometry_msgs::Pose text_pose_; 254 | 255 | // For visualizing things in rviz 256 | moveit_visual_tools::MoveItVisualToolsPtr visual_tools_; 257 | 258 | // Tool for checking balance 259 | moveit_humanoid_stability::HumanoidStabilityPtr humanoid_stability_; 260 | 261 | // Properties of robot's standing stance 262 | const robot_model::JointModelGroup *whole_body_minus_fixed_leg_; 263 | const robot_model::JointModelGroup *fixed_leg_; 264 | const robot_model::LinkModel* left_foot_; 265 | const robot_model::LinkModel* right_foot_; 266 | // Robot model properties 267 | const robot_model::JointModelGroup *whole_body_minus_left_leg_; 268 | const robot_model::JointModelGroup *whole_body_minus_right_leg_; 269 | const robot_model::JointModelGroup *left_leg_; 270 | const robot_model::JointModelGroup *right_leg_; 271 | 272 | }; 273 | 274 | // define the sampler allocator plugin interface 275 | class HumanoidConstraintSamplerAllocator : public constraint_samplers::ConstraintSamplerAllocator 276 | { 277 | public: 278 | 279 | virtual constraint_samplers::ConstraintSamplerPtr alloc(const planning_scene::PlanningSceneConstPtr &scene, 280 | const std::string &group_name, const moveit_msgs::Constraints &constr) 281 | { 282 | constraint_samplers::ConstraintSamplerPtr cs(new HumanoidConstraintSampler(scene, group_name)); 283 | cs->configure(constr); 284 | return cs; 285 | } 286 | 287 | virtual bool canService(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, 288 | const moveit_msgs::Constraints &constr) const 289 | { 290 | // override: always use 291 | return true; 292 | 293 | // do not use this sampler if there are any joint constraints, because then we are in the goal sampling stage 294 | if ( 295 | constr.joint_constraints.size() == 0 && 296 | group_name == "whole_body") 297 | { 298 | logInform("humanoid_constraint_sampler: Using custom constraint sampler"); 299 | return true; 300 | } 301 | 302 | logInform("humanoid_constraint_sampler: NOT using custom constraint sampler"); 303 | return false; 304 | } 305 | 306 | }; 307 | 308 | } // namespace 309 | 310 | PLUGINLIB_EXPORT_CLASS(moveit_humanoid_stability::HumanoidConstraintSamplerAllocator, 311 | constraint_samplers::ConstraintSamplerAllocator); 312 | 313 | 314 | #endif 315 | -------------------------------------------------------------------------------- /src/humanoid_stability.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, JSK, The University of Tokyo. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the JSK, The University of Tokyo nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | Desc: Wrapper for hrl_kinematics for calculing COM 37 | */ 38 | 39 | #include 40 | 41 | namespace moveit_humanoid_stability 42 | { 43 | 44 | HumanoidStability::HumanoidStability(bool verbose, const moveit::core::RobotState &robot_state, 45 | const moveit_visual_tools::MoveItVisualToolsPtr &visual_tools) 46 | : verbose_(verbose) 47 | , visual_tools_(visual_tools) 48 | , normal_vector_(0.0, 0.0, 1.0) 49 | { 50 | // Configurations 51 | static const std::string ROOT_NAME = "humanoid_stability"; 52 | 53 | normal_vector_.normalize(); // TODO is this necessary? 54 | 55 | ros::NodeHandle nh("~"); 56 | 57 | // Temporary vars for loading from param server 58 | double bounding_box_padding; 59 | double foot_polygon_scale; 60 | std::string left_foot_name = "left_foot"; 61 | std::string right_foot_name = "right_foot"; 62 | std::string all_joints_group = "robot_joints"; 63 | std::string base_link = robot_state.getRobotModel()->getRootLink()->getName(); 64 | 65 | // Load the torso (vjoint) bounds from yaml file 66 | nh.param(ROOT_NAME + "/bounding_box/min_x", min_x_, 0.0); 67 | nh.param(ROOT_NAME + "/bounding_box/max_x", max_x_, 0.0); 68 | nh.param(ROOT_NAME + "/bounding_box/min_y", min_y_, 0.0); 69 | nh.param(ROOT_NAME + "/bounding_box/max_y", max_y_, 0.0); 70 | nh.param(ROOT_NAME + "/bounding_box/min_z", min_z_, 0.0); 71 | nh.param(ROOT_NAME + "/bounding_box/max_z", max_z_, 0.0); 72 | nh.param(ROOT_NAME + "/bounding_box/padding", bounding_box_padding, 0.1); 73 | 74 | // Load other settings from yaml file 75 | nh.getParam(ROOT_NAME + "/left_foot_name", left_foot_name); 76 | nh.getParam(ROOT_NAME + "/right_foot_name", right_foot_name); 77 | nh.getParam(ROOT_NAME + "/all_joints_group", all_joints_group); 78 | nh.param(ROOT_NAME + "/foot_polygon_scale", foot_polygon_scale, 1.0); 79 | 80 | // Add padding to all limits just in case 81 | min_x_ += bounding_box_padding; 82 | max_x_ += bounding_box_padding; 83 | min_y_ += bounding_box_padding; 84 | max_y_ += bounding_box_padding; 85 | min_z_ += bounding_box_padding; 86 | max_z_ += bounding_box_padding; 87 | 88 | // Useful links to remember 89 | left_foot_ = robot_state.getRobotModel()->getLinkModel(left_foot_name); 90 | right_foot_ = robot_state.getRobotModel()->getLinkModel(right_foot_name); 91 | all_joints_group_ = robot_state.getRobotModel()->getJointModelGroup(all_joints_group); 92 | 93 | // Get the offset between the foot and the torso to allow correct positioning of bounding box 94 | // TODO: this assumes the fixed link never changes, which will eventually be a bad assumption 95 | moveit::core::RobotState temp_state(robot_state); 96 | temp_state.setToDefaultValues(); 97 | Eigen::Affine3d left_foot_to_torso = temp_state.getGlobalLinkTransform(left_foot_); 98 | 99 | // Move the x & y bounds over from foot frame of reference to torso frame of reference. z is always w.r.t. ground (0) 100 | max_x_ -= left_foot_to_torso.translation().x(); 101 | max_y_ -= left_foot_to_torso.translation().y(); 102 | min_x_ -= left_foot_to_torso.translation().x(); 103 | min_y_ -= left_foot_to_torso.translation().y(); 104 | 105 | // Setup HRL Kinematics Balance Constraint 106 | 107 | // Choose which foot is fixed 108 | if (robot_state.getFixedFoot() == left_foot_) 109 | { 110 | support_mode_ = hrl_kinematics::Kinematics::SUPPORT_SINGLE_LEFT; 111 | free_foot_ = right_foot_; 112 | } 113 | else if (robot_state.getFixedFoot() == right_foot_) 114 | { 115 | support_mode_ = hrl_kinematics::Kinematics::SUPPORT_SINGLE_RIGHT; 116 | free_foot_ = left_foot_; 117 | } 118 | else 119 | { 120 | ROS_ERROR_STREAM_NAMED("temp","Could not match a foot link with the chosen fixed on in RobotState. rosparam may be misconfigured to your URDF"); 121 | } 122 | // TODO: allow both 123 | //hrl_kinematics::Kinematics::FootSupport support_mode_ = hrl_kinematics::Kinematics::SUPPORT_DOUBLE; 124 | 125 | // Preload joint positions map for less memory usage 126 | for (std::size_t i = 0; i < all_joints_group_->getVariableCount(); ++i) 127 | { 128 | // Intitialize empty 129 | joint_positions_map_.insert(std::make_pair(all_joints_group_->getJointModels()[i]->getName(), 0)); 130 | } 131 | 132 | // Error check 133 | if (!visual_tools_ && verbose_) 134 | { 135 | ROS_ERROR_STREAM_NAMED("stability","No visual_tools passed in when in verbose mode, turning off verbose"); 136 | verbose_ = false; 137 | } 138 | 139 | // Show bounding box 140 | if (verbose_) 141 | printVirtualJointExtremes(); 142 | 143 | // Load hrl_kinematics 144 | test_stability_.reset(new hrl_kinematics::TestStability(right_foot_name, // rfoot_mesh_link_name 145 | base_link, // root_link_name 146 | right_foot_name, left_foot_name, // rfoot, lfoot link name 147 | robot_state.getRobotModel()->getURDF(), 148 | foot_polygon_scale)); 149 | 150 | 151 | ROS_INFO_STREAM_NAMED("stability","Humanoid stability validator initialized."); 152 | } 153 | 154 | bool HumanoidStability::isValid(const robot_state::RobotState &robot_state, bool verbose) 155 | { 156 | // Turn on verbose mode if planner requests it 157 | if (verbose == true && verbose_ == false) 158 | { 159 | if (!visual_tools_) 160 | ROS_ERROR_STREAM_NAMED("stability","No visual_tools passed in when in verbose mode, turning off verbose"); 161 | else 162 | { 163 | ROS_WARN_STREAM_NAMED("temp","Verbose mode forced on but IGNORED"); 164 | //verbose_ = true; // TODO 165 | } 166 | } 167 | 168 | // Publish state 169 | if (verbose_) 170 | { 171 | visual_tools_->publishRobotState(robot_state); 172 | ros::Duration(0.25).sleep(); 173 | } 174 | 175 | // Check torso 176 | if (!isApproximateValidBase(robot_state)) 177 | { 178 | if (verbose_) 179 | ROS_WARN_STREAM_NAMED("stability","Invalid because of approximate base location"); 180 | return false; 181 | } 182 | 183 | // Check COM 184 | if (!isApproximateValidFoot(robot_state)) 185 | { 186 | if (verbose_) 187 | ROS_WARN_STREAM_NAMED("stability","Invalid because of approximate foot location"); 188 | return false; 189 | } 190 | 191 | // Check COM 192 | if (!isValidCOM(robot_state)) 193 | { 194 | if (verbose_) 195 | ROS_WARN_STREAM_NAMED("stability","Invalid because of COM"); 196 | return false; 197 | } 198 | 199 | return true; 200 | } 201 | 202 | bool HumanoidStability::isApproximateValidBase(const robot_state::RobotState &robot_state) 203 | { 204 | // Check if vjoint (torso) is within reasonable limits 205 | const robot_model::JointModel *vjoint = robot_state.getJointModel("virtual_joint"); // TODO unhard code 206 | const double* vjoint_positions = robot_state.getJointPositions(vjoint); 207 | if (verbose_) 208 | { 209 | if (false) // disabled 210 | { 211 | std::cout << "Vjoint: " << std::endl; 212 | std::cout << " X: " << boost::format("%8.4f") % min_x_ << boost::format("%8.4f") % vjoint_positions[0] 213 | << boost::format("%8.4f") % max_x_ << std::endl; 214 | std::cout << " Y: " << boost::format("%8.4f") % min_y_ << boost::format("%8.4f") % vjoint_positions[1] 215 | << boost::format("%8.4f") % max_y_ << std::endl; 216 | std::cout << " Z: " << boost::format("%8.4f") % min_z_ << boost::format("%8.4f") % vjoint_positions[2] 217 | << boost::format("%8.4f") % max_z_ << std::endl; 218 | } 219 | 220 | if (!visual_tools_) 221 | ROS_ERROR_STREAM_NAMED("temp","Caught that error"); 222 | visual_tools_->deleteAllMarkers(); 223 | displayBoundingBox(robot_state.getFakeBaseTransform()); 224 | } 225 | 226 | if (vjoint_positions[0] < min_x_ + robot_state.getFakeBaseTransform().translation().x() || 227 | vjoint_positions[0] > max_x_ + robot_state.getFakeBaseTransform().translation().x() || 228 | vjoint_positions[1] < min_y_ + robot_state.getFakeBaseTransform().translation().y() || 229 | vjoint_positions[1] > max_y_ + robot_state.getFakeBaseTransform().translation().y() || 230 | vjoint_positions[2] < min_z_ + robot_state.getFakeBaseTransform().translation().z() || 231 | vjoint_positions[2] > max_z_ + robot_state.getFakeBaseTransform().translation().z() ) 232 | { 233 | return false; 234 | } 235 | return true; 236 | } 237 | 238 | bool HumanoidStability::isApproximateValidFoot(const robot_state::RobotState &robot_state) 239 | { 240 | if (robot_state.getGlobalLinkTransform(free_foot_).translation().z() < 0) 241 | return false; 242 | 243 | return true; 244 | } 245 | 246 | bool HumanoidStability::isValidCOM(const robot_state::RobotState &robot_state) 247 | { 248 | // Copy robot state to map 249 | for (std::size_t i = 0; i < all_joints_group_->getVariableCount(); ++i) 250 | { 251 | joint_positions_map_[all_joints_group_->getJointModels()[i]->getName()] = 252 | robot_state.getJointPositions(all_joints_group_->getJointModels()[i])[0]; 253 | } 254 | 255 | // Run test 256 | bool stable = test_stability_->isPoseStable(joint_positions_map_, support_mode_, normal_vector_); 257 | 258 | bool show_com_makers = false; 259 | 260 | // Publish COM marker 261 | if (verbose_ && show_com_makers) 262 | { 263 | visualization_msgs::Marker com_marker = test_stability_->getCOMMarker(); 264 | // Translate to world frame 265 | com_marker.pose = 266 | moveit_visual_tools::MoveItVisualTools::convertPose(moveit_visual_tools::MoveItVisualTools::convertPose(com_marker.pose) * 267 | robot_state.getGlobalLinkTransform(com_marker.header.frame_id)); 268 | 269 | // Change frame name to world fame 270 | com_marker.header.frame_id = robot_state.getRobotModel()->getModelFrame(); // odom 271 | com_marker.ns = "COM"; 272 | visual_tools_->publishMarker(com_marker); 273 | } 274 | 275 | // Publish footprint polygon 276 | if (verbose_ && show_com_makers) 277 | { 278 | const geometry_msgs::PolygonStamped polygon_msg = test_stability_->getSupportPolygon(); 279 | 280 | // Change polygon points to world frame 281 | std::vector points; 282 | for (std::size_t i = 0; i < polygon_msg.polygon.points.size(); ++i) 283 | { 284 | Eigen::Affine3d temp_pose = moveit_visual_tools::MoveItVisualTools::convertPoint32ToPose(polygon_msg.polygon.points[i]); 285 | // TODO make this hard coded, but also fix this whole transforms thing cause it doesn't work right 286 | temp_pose = temp_pose * robot_state.getGlobalLinkTransform("BODY"); 287 | 288 | points.push_back( visual_tools_->convertPose(temp_pose).position ); 289 | } 290 | points.push_back(points.front()); // connect first and last points for last line 291 | 292 | visual_tools_->publishPath(points); 293 | } 294 | 295 | return stable; 296 | } 297 | 298 | bool HumanoidStability::displayBoundingBox(const Eigen::Affine3d &translation) const 299 | { 300 | if (!visual_tools_) 301 | { 302 | ROS_WARN_STREAM_NAMED("stability","visual tools not loaded"); 303 | return false; 304 | } 305 | 306 | geometry_msgs::Point point1; 307 | geometry_msgs::Point point2; 308 | point1.x = max_x_ + translation.translation().x(); 309 | point1.y = max_y_ + translation.translation().y(); 310 | point1.z = max_z_ + translation.translation().z(); 311 | point2.x = min_x_ + translation.translation().x(); 312 | point2.y = min_y_ + translation.translation().y(); 313 | point2.z = min_z_ + translation.translation().z(); 314 | 315 | return visual_tools_->publishRectangle(point1, point2, rviz_visual_tools::TRANSLUCENT); 316 | } 317 | 318 | void HumanoidStability::printVirtualJointExtremes() const 319 | { 320 | std::cout << "Virtual Joint Extremes: " << std::endl; 321 | std::cout << " min_x: " << min_x_ << std::endl; 322 | std::cout << " max_x: " << max_x_ << std::endl; 323 | std::cout << " min_y: " << min_y_ << std::endl; 324 | std::cout << " max_y: " << max_y_ << std::endl; 325 | std::cout << " min_z: " << min_z_ << std::endl; 326 | std::cout << " max_z: " << max_z_ << std::endl; 327 | } 328 | 329 | } // end namespace 330 | -------------------------------------------------------------------------------- /src/humanoid_constraint_sampler.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2014, JSK, The University of Tokyo. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the JSK, The University of Tokyo nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | /* Author: Dave Coleman 36 | */ 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | namespace moveit_humanoid_stability 45 | { 46 | bool HumanoidConstraintSampler::configure(const moveit_msgs::Constraints &constr) 47 | { 48 | clear(); 49 | 50 | //moveit_msgs::Constraints constraints = constr; // copy to non-const 51 | 52 | ROS_DEBUG_STREAM_NAMED("sampler","Configuring humanoid constraint sampler"); 53 | //std::cout << "message:\n " << constr << std::endl; 54 | 55 | // Humanoid custom constraints: define here -------------------- 56 | moveit_msgs::JointConstraint jc1; 57 | 58 | // construct the joint constraints 59 | std::vector jc; 60 | for (std::size_t i = 0 ; i < constr.joint_constraints.size() ; ++i) 61 | { 62 | kinematic_constraints::JointConstraint j(scene_->getRobotModel()); 63 | if (j.configure(constr.joint_constraints[i])) 64 | jc.push_back(j); 65 | } 66 | 67 | // Load visual tools 68 | if (verbose_ || true) // i don't know how to make this any better :-/ 69 | { 70 | loadVisualTools(); 71 | } 72 | 73 | // Configure stability checker 74 | if (!humanoid_stability_) 75 | humanoid_stability_.reset(new moveit_humanoid_stability::HumanoidStability(verbose_, scene_->getCurrentState(), visual_tools_)); 76 | 77 | // If joint constriaints are provided, configure them. otherwise we will use regular random joint sampling 78 | if (jc.empty()) 79 | { 80 | sampler_name_ = "No_Joint_Constraints"; 81 | logDebug("No joint constraints passed to humanoid constraint sampler"); 82 | is_valid_ = true; // set as configured 83 | } 84 | else 85 | { 86 | sampler_name_ = "Has_Joint_Constraints"; 87 | 88 | if (!configureJoint(jc)) 89 | return false; 90 | 91 | is_valid_ = true; // set as configured 92 | } 93 | 94 | // Temporary vars for loading from param server 95 | std::string whole_body_minus_left_leg = "whole_body_minus_left_leg"; 96 | std::string whole_body_minus_right_leg = "whole_body_minus_right_leg"; 97 | std::string left_leg = "left_leg"; 98 | std::string right_leg = "right_leg"; 99 | std::string left_foot = "left_foot"; 100 | std::string right_foot = "right_foot"; 101 | 102 | // Load other settings from yaml file 103 | static const std::string ROOT_NAME = "humanoid_stability"; 104 | ros::NodeHandle nh("~"); 105 | nh.getParam(ROOT_NAME + "/whole_body_minus_left_leg_name", whole_body_minus_left_leg); 106 | nh.getParam(ROOT_NAME + "/whole_body_minus_right_leg_name", whole_body_minus_right_leg); 107 | nh.getParam(ROOT_NAME + "/left_leg_name", left_leg); 108 | nh.getParam(ROOT_NAME + "/right_leg_name", right_leg); 109 | nh.getParam(ROOT_NAME + "/left_foot_name", left_foot); 110 | nh.getParam(ROOT_NAME + "/right_foot_name", right_foot); 111 | 112 | // Load the joint model groups and links 113 | whole_body_minus_left_leg_ = scene_->getCurrentState().getRobotModel()->getJointModelGroup(whole_body_minus_left_leg); 114 | whole_body_minus_right_leg_ = scene_->getCurrentState().getRobotModel()->getJointModelGroup(whole_body_minus_right_leg); 115 | left_leg_ = scene_->getCurrentState().getRobotModel()->getJointModelGroup(left_leg); 116 | right_leg_ = scene_->getCurrentState().getRobotModel()->getJointModelGroup(right_leg); 117 | left_foot_ = scene_->getCurrentState().getRobotModel()->getLinkModel(left_foot); 118 | right_foot_ = scene_->getCurrentState().getRobotModel()->getLinkModel(right_foot); 119 | 120 | logInform("%s: Humanoid Constraint Sampler initialized. Bounded: %d, Unbounded: %d", sampler_name_.c_str(), bounds_.size(), unbounded_.size()); 121 | return true; 122 | } 123 | 124 | void HumanoidConstraintSampler::loadVisualTools() 125 | { 126 | // Don't load twice 127 | if (visual_tools_ != NULL) 128 | return; 129 | 130 | visual_tools_.reset(new moveit_visual_tools::MoveItVisualTools("/odom", "/humanoid_constraint_sample_markers", scene_->getRobotModel())); 131 | visual_tools_->loadRobotStatePub("/humanoid_constraint_sample_robots"); 132 | 133 | // Verbose mode text display setting 134 | text_pose_.position.x = scene_->getCurrentState().getFakeBaseTransform().translation().x(); 135 | text_pose_.position.y = scene_->getCurrentState().getFakeBaseTransform().translation().y(); 136 | text_pose_.position.z = scene_->getCurrentState().getFakeBaseTransform().translation().z() + 2; 137 | } 138 | 139 | 140 | bool HumanoidConstraintSampler::configureJoint(const std::vector &jc) 141 | { 142 | clear(); 143 | 144 | if (!jmg_) 145 | { 146 | logError("NULL planning group specified for constraint sampler"); 147 | return false; 148 | } 149 | 150 | // find and keep the constraints that operate on the group we sample 151 | // also keep bounds for joints for convenience 152 | std::map bound_data; 153 | for (std::size_t i = 0 ; i < jc.size() ; ++i) 154 | { 155 | // Check that joint constraint is enabled 156 | if (!jc[i].enabled()) 157 | continue; 158 | 159 | // Check that joint constraint has valid joint model 160 | const robot_model::JointModel *jm = jc[i].getJointModel(); 161 | if (!jmg_->hasJointModel(jm->getName())) 162 | continue; 163 | 164 | // Get the bounds of this variable 165 | const robot_model::VariableBounds& joint_bounds = jm->getVariableBounds(jc[i].getJointVariableName()); 166 | 167 | // Populate the joint info object 168 | JointInfo ji; 169 | // Check if this variable already has bounds set (for some reason_ 170 | std::map::iterator it = bound_data.find(jc[i].getJointVariableName()); 171 | if (it != bound_data.end()) 172 | ji = it->second; 173 | else 174 | ji.index_ = jmg_->getVariableGroupIndex(jc[i].getJointVariableName()); // copy the index of the variable with respect to the joint model group 175 | 176 | // Attempt to tighten the variables bounds if applicable from the constraint 177 | ji.potentiallyAdjustMinMaxBounds(std::max(joint_bounds.min_position_, jc[i].getDesiredJointPosition() - jc[i].getJointToleranceBelow()), 178 | std::min(joint_bounds.max_position_, jc[i].getDesiredJointPosition() + jc[i].getJointToleranceAbove())); 179 | 180 | // Debug 181 | logDebug("Bounds for %s JointConstraint are %g %g", jc[i].getJointVariableName().c_str(), ji.min_bound_, ji.max_bound_); 182 | 183 | // Error check 184 | if (ji.min_bound_ > ji.max_bound_ + std::numeric_limits::epsilon()) 185 | { 186 | std::stringstream cs; jc[i].print(cs); 187 | logError("The constraints for joint '%s' have no possible values for the joint: min_bound: %g, max_bound: %g. Failing.\n", 188 | jm->getName().c_str(), ji.min_bound_, ji.max_bound_); 189 | clear(); 190 | return false; 191 | } 192 | 193 | // Save this new joint info 194 | bound_data[jc[i].getJointVariableName()] = ji; 195 | } 196 | 197 | // Copy our larger bound data structure into a more compact one 198 | for (std::map::iterator it = bound_data.begin(); it != bound_data.end(); ++it) 199 | bounds_.push_back(it->second); 200 | 201 | // get a separate list of joints that are not bounded; we will sample these randomly 202 | const std::vector &joints = jmg_->getJointModels(); 203 | for (std::size_t i = 0 ; i < joints.size() ; ++i) 204 | if (bound_data.find(joints[i]->getName()) == bound_data.end() && 205 | joints[i]->getVariableCount() > 0 && 206 | joints[i]->getMimic() == NULL) 207 | { 208 | // check if all the vars of the joint are found in bound_data instead 209 | const std::vector &vars = joints[i]->getVariableNames(); 210 | if (vars.size() > 1) 211 | { 212 | bool all_found = true; 213 | for (std::size_t j = 0 ; j < vars.size() ; ++j) 214 | if (bound_data.find(vars[j]) == bound_data.end()) 215 | { 216 | all_found = false; 217 | break; 218 | } 219 | if (all_found) 220 | continue; 221 | } 222 | unbounded_.push_back(joints[i]); 223 | // Get the first variable name of this joint and find its index position in the planning group 224 | uindex_.push_back(jmg_->getVariableGroupIndex(vars[0])); 225 | //logInform("Adding variable index %d for joint index %d",jmg_->getVariableGroupIndex(vars[0]), i); 226 | } 227 | 228 | values_.resize(jmg_->getVariableCount()); 229 | 230 | return true; 231 | } 232 | 233 | bool HumanoidConstraintSampler::sample(robot_state::RobotState &robot_state, const robot_state::RobotState & /* reference_state */, 234 | unsigned int max_attempts) 235 | { 236 | if (!jmg_) 237 | logError("no joint model group loaded"); 238 | 239 | if (!is_valid_) 240 | { 241 | logWarn("HumanoidConstraintSampler not configured, won't sample"); 242 | return false; 243 | } 244 | 245 | //logWarn("%s: HumanoidConstraintSampler SAMPLING -----------------------------",sampler_name_.c_str()); 246 | 247 | 248 | 249 | if (robot_state.getFixedFoot() == left_foot_) 250 | { 251 | whole_body_minus_fixed_leg_ = whole_body_minus_left_leg_; 252 | fixed_leg_ = left_leg_; 253 | } 254 | else if (robot_state.getFixedFoot() == right_foot_) 255 | { 256 | whole_body_minus_fixed_leg_ = whole_body_minus_right_leg_; 257 | fixed_leg_ = right_leg_; 258 | } 259 | else 260 | { 261 | ROS_ERROR_STREAM_NAMED("temp","Could not match a foot link with the chosen fixed on in RobotState. rosparam may be misconfigured to your URDF"); 262 | } 263 | 264 | max_attempts = 100000; // TODO this might be a bad hack 265 | 266 | for (std::size_t attempt = 0; attempt < max_attempts; ++attempt) 267 | { 268 | if (verbose_) 269 | logInform("Sampling attempt number %d for group %s", attempt, jmg_->getName().c_str() ); 270 | 271 | if (!ros::ok()) 272 | return false; 273 | 274 | if (verbose_) 275 | visual_tools_->deleteAllMarkers(); 276 | 277 | // Decide how to sample the joints 278 | bool use_constraint_sampling = false; 279 | // Generate simple movement states 280 | if (false) 281 | { 282 | double lleg_position[1]; 283 | lleg_position[0] = robot_state.getJointPositions("LLEG_JOINT1")[0] + 0.01; 284 | robot_state.setJointPositions("LLEG_JOINT1", lleg_position); 285 | 286 | // Update with leg fixed 287 | robot_state.updateStateWithFakeBase(); 288 | use_constraint_sampling = true; 289 | } 290 | else if (bounds_.size() > 0) 291 | { 292 | if (verbose_) 293 | ROS_INFO_STREAM_NAMED("sampler","Sampling joints using joint constraints"); 294 | use_constraint_sampling = true; 295 | 296 | // Calculate random position of robot 297 | // \todo: don't sample virtual joint orientation and the legs to save time 298 | if (!sampleJoints(robot_state)) 299 | { 300 | logError("Unable to sample joints"); 301 | return false; 302 | } 303 | 304 | // Update with leg fixed 305 | robot_state.updateStateWithFakeBase(); 306 | } 307 | else 308 | { 309 | //ROS_INFO_STREAM_NAMED("temp","Sampling joints using robot state random variables"); 310 | // Generate random state for leg only 311 | robot_state.setToRandomPositions(fixed_leg_); 312 | 313 | // Update only the virtual joint and the leg we just updated 314 | robot_state.updateSingleChainWithFakeBase(); 315 | } 316 | 317 | if (!humanoid_stability_->isApproximateValidBase(robot_state)) 318 | { 319 | if (verbose_) 320 | { 321 | ROS_WARN_STREAM_NAMED("sampler","Sample outside VIRTUAL JOINT constraints"); 322 | visual_tools_->publishText(text_pose_, "OUTSIDE virtual joint bounding box", rviz_visual_tools::RED, rviz_visual_tools::LARGE); 323 | ros::Duration(1.1).sleep(); 324 | } 325 | 326 | continue; // stop checking 327 | } 328 | 329 | // Is Valid 330 | if (verbose_) 331 | { 332 | visual_tools_->publishText(text_pose_, "Within virtual joint bounding box", rviz_visual_tools::WHITE, rviz_visual_tools::LARGE); 333 | visual_tools_->publishRobotState(robot_state); 334 | ros::Duration(1.1).sleep(); 335 | } 336 | 337 | if (!use_constraint_sampling) 338 | { 339 | // Generate remainder of body random state 340 | robot_state.setToRandomPositions(whole_body_minus_fixed_leg_); 341 | robot_state.update(true); // update entire robot using previously computed virtual joint tranform 342 | } 343 | 344 | if (verbose_) 345 | { 346 | visual_tools_->publishRobotState(robot_state); 347 | } 348 | 349 | // Check if the free foot is above ground 350 | if (!humanoid_stability_->isApproximateValidFoot(robot_state)) 351 | { 352 | if (verbose_) 353 | { 354 | ROS_WARN_STREAM_NAMED("sampler","Sample outside FREE FOOT ground constraint "); 355 | visual_tools_->publishText(text_pose_, "OUTSIDE free foot ground constraint", rviz_visual_tools::RED, rviz_visual_tools::LARGE); 356 | ros::Duration(1.1).sleep(); 357 | } 358 | 359 | continue; // stop checking 360 | } 361 | 362 | // Check COM balance constraints -------------------------------------------------------------- 363 | if (!humanoid_stability_->isValidCOM(robot_state)) 364 | { 365 | if (verbose_) 366 | { 367 | ROS_WARN("Pose is NOT stable"); 368 | visual_tools_->publishText(text_pose_, "NOT stable from center of mass", rviz_visual_tools::RED, rviz_visual_tools::LARGE); 369 | ros::Duration(1.1).sleep(); 370 | } 371 | continue; 372 | } 373 | 374 | if (verbose_) 375 | ROS_INFO("Pose is stable"); 376 | 377 | bool check_verbose = false; 378 | if (!scene_->isStateValid(robot_state, "", check_verbose)) // second argument is what planning group to collision check, "" is everything 379 | { 380 | if (verbose_) 381 | { 382 | ROS_ERROR_STREAM_NAMED("sampler","Pose not valid (self or enviornment collision)"); 383 | visual_tools_->publishText(text_pose_, "NOT valid from self or env collision", rviz_visual_tools::RED, rviz_visual_tools::LARGE); 384 | ros::Duration(1.1).sleep(); 385 | } 386 | continue; 387 | } 388 | 389 | // Find min/max 390 | /*min_x_ = std::min(vjoint_positions[0], min_x_); 391 | max_x_ = std::max(vjoint_positions[0], max_x_); 392 | 393 | min_y_ = std::min(vjoint_positions[1], min_y_); 394 | max_y_ = std::max(vjoint_positions[1], max_y_); 395 | 396 | min_z_ = std::min(vjoint_positions[2], min_z_); 397 | max_z_ = std::max(vjoint_positions[2], max_z_);*/ 398 | 399 | if (verbose_) 400 | { 401 | visual_tools_->publishText(text_pose_, "Sample VALID!", rviz_visual_tools::WHITE, rviz_visual_tools::LARGE); 402 | ros::Duration(3.0).sleep(); 403 | } 404 | 405 | ROS_DEBUG_STREAM_NAMED("sampler","Passed - valid sample found on attempts " << attempt); 406 | 407 | return true; 408 | } // for attempts 409 | 410 | ROS_DEBUG_STREAM_NAMED("sampler","Aborted - ran out of attempts (" << max_attempts << ")"); 411 | 412 | return false; 413 | } 414 | 415 | bool HumanoidConstraintSampler::sampleJoints(robot_state::RobotState &robot_state) 416 | { 417 | // sample the unbounded joints first (in case some joint varipables are bounded) 418 | std::vector v; 419 | for (std::size_t i = 0 ; i < unbounded_.size() ; ++i) 420 | { 421 | v.resize(unbounded_[i]->getVariableCount()); 422 | 423 | if (false) 424 | logInform("%s: UNCONSTRAINED: Joint number %d named %s with variables %d", sampler_name_.c_str(), 425 | i, unbounded_[i]->getName().c_str(),v.size()); 426 | 427 | unbounded_[i]->getVariableRandomPositions(random_number_generator_, &v[0]); 428 | 429 | for (std::size_t j = 0 ; j < v.size() ; ++j) 430 | { 431 | values_[uindex_[i] + j] = v[j]; 432 | } 433 | } 434 | 435 | // enforce the constraints for the constrained components (could be all of them) 436 | for (std::size_t i = 0 ; i < bounds_.size() ; ++i) 437 | { 438 | if (false) 439 | logInform("%s: CONSTRAINED: Joint number %d named %s bounds [%f,%f]", sampler_name_.c_str(), bounds_[i].index_, 440 | jmg_->getVariableNames()[ bounds_[i].index_ ].c_str(), 441 | bounds_[i].min_bound_, bounds_[i].max_bound_); 442 | 443 | values_[bounds_[i].index_] = random_number_generator_.uniformReal(bounds_[i].min_bound_, bounds_[i].max_bound_); 444 | } 445 | 446 | robot_state.setJointGroupPositions(jmg_, values_); 447 | 448 | return true; 449 | } 450 | 451 | bool HumanoidConstraintSampler::project(robot_state::RobotState &robot_state, 452 | unsigned int max_attempts) 453 | { 454 | return sample(robot_state, robot_state, max_attempts); 455 | } 456 | 457 | void HumanoidConstraintSampler::clear() 458 | { 459 | ConstraintSampler::clear(); 460 | bounds_.clear(); 461 | unbounded_.clear(); 462 | uindex_.clear(); 463 | values_.clear(); 464 | } 465 | 466 | void HumanoidConstraintSampler::setVerbose(bool verbose) 467 | { 468 | // Load visual tools 469 | if (verbose) 470 | { 471 | loadVisualTools(); 472 | } 473 | 474 | humanoid_stability_->setVerbose(verbose); 475 | } 476 | 477 | 478 | } //namespace 479 | --------------------------------------------------------------------------------