diff --git a/include/OpenSoT/constraints/velocity/CollisionAvoidance.h b/include/OpenSoT/constraints/velocity/CollisionAvoidance.h index cb4d15ff..aca184c6 100644 --- a/include/OpenSoT/constraints/velocity/CollisionAvoidance.h +++ b/include/OpenSoT/constraints/velocity/CollisionAvoidance.h @@ -94,24 +94,11 @@ class CollisionAvoidance: public Constraint void update(); - /** - * @brief setCollisionList permits to set a list of collision pairs (links). - * @note when using set CollisionList, the internal list of collision is reset. This entails the resizing of - * internal constraint matrices. For this reason, this method SHOULD NOT be called once the constraint has been - * added into the solver but only BEFORE! - * @param collisionList set of link pairs names - * @return false if links are not founded in the model - */ - bool setCollisionList(std::set> collisionList); + void setMaxPairs(const unsigned int max_pairs); + + void setCollisionList(std::set> collisionList); + - /** - * @brief updateCollisionList permits to update the collision list with different collision pairs - * @note the parameter collisionList in input can not be larger than the maximum number of link pairs (_max_pairs), - * this is to avoid resize of constraint matrix - * @param collisionList set of link pairs names - * @return false if collisionList.size() > _max_pairs or if links are not founded in the model - */ - bool updateCollisionList(std::set> collisionList); /** * @brief collisionModelUpdated must be called after a collision has been added or @@ -119,36 +106,22 @@ class CollisionAvoidance: public Constraint */ void collisionModelUpdated(); - // TODO: waiting for world collision support ! - // - // /** - // * @brief add/remove world collision objects according to the given planning - // * scene world - // * @return true if all requests (additions, deletions) could be performs - // * succesfully, false on (partial) insuccess - // */ - // bool setWorldCollisions(const moveit_msgs::PlanningSceneWorld& wc); - - // /** - // * @brief add single collision to the world - // * @param id is the unique collision id - // * @param fcl_obj is the fcl collision object (geometry + transform) - // * @return true if input is valid - // */ - // bool addWorldCollision(const std::string& id, - // std::shared_ptr fcl_obj); + bool addCollisionShape(const std::string& name, + const std::string& link, + const XBot::Collision::Shape::Variant& shape, + const Eigen::Affine3d& link_T_shape, + const std::vector& disabled_collisions = {}); // /** // * @brief remove world collision with given id // */ // bool removeWorldCollision(const std::string& id); - // /** - // * @brief change the transform w.r.t. the world for the given - // * world collision - // */ - // bool moveWorldCollision(const std::string& id, - // Eigen::Affine3d new_pose); + /** + * @brief change the transform w.r.t. the world for the given + * world collision + */ + bool moveCollisionShape(const std::string& id, const Eigen::Affine3d& new_pose); /** * @brief setBoundScaling sets bound scaling for the capsule constraint @@ -162,7 +135,7 @@ class CollisionAvoidance: public Constraint * @brief setLinksVsEnvironment * @param links */ - void setLinksVsEnvironment(const std::list& links); + void setLinksVsEnvironment(const std::set& links); /** * @brief getter for the internal collision model diff --git a/src/constraints/velocity/CollisionAvoidance.cpp b/src/constraints/velocity/CollisionAvoidance.cpp index f262a0b3..f9aff991 100644 --- a/src/constraints/velocity/CollisionAvoidance.cpp +++ b/src/constraints/velocity/CollisionAvoidance.cpp @@ -97,12 +97,16 @@ void CollisionAvoidance::update() // reset constraint _Aineq.setZero(_max_pairs, getXSize()); _bUpperBound.setConstant(_max_pairs, std::numeric_limits::max()); + _bLowerBound.setConstant(_max_pairs, std::numeric_limits::lowest()); + + + _distance_J.setZero(_dist_calc->getNumCollisionPairs(_include_env), _distance_J.cols()); + _distances.setZero(_distance_J.rows()); // compute distances _dist_calc->computeDistance(_distances, _include_env, _detection_threshold); // compute jacobians - _distance_J.setZero(_dist_calc->getNumCollisionPairs(_include_env), _distance_J.cols()); _dist_calc->getDistanceJacobian(_distance_J, _include_env); // populate Aineq and bUpperBound @@ -138,48 +142,29 @@ void CollisionAvoidance::update() } -bool CollisionAvoidance::setCollisionList(std::set> collisionList) +bool CollisionAvoidance::addCollisionShape(const std::string &name, + const std::string &link, + const XBot::Collision::Shape::Variant &shape, + const Eigen::Affine3d &link_T_shape, + const std::vector &disabled_collisions) { - try - { - _dist_calc->setLinkPairs(collisionList); - _max_pairs = collisionList.size(); - // initialize matrices - _Aineq.setZero(_max_pairs, getXSize()); - _bUpperBound.setZero(_max_pairs); - _bLowerBound.setConstant(_max_pairs, std::numeric_limits::lowest()); - _distance_J.setZero(_max_pairs, _robot.getNv()); - - return true; - } - catch(std::out_of_range& e) - { - Logger::error(e.what()); - return false; - } + return _dist_calc->addCollisionShape(name, link, shape, link_T_shape, disabled_collisions); } -bool CollisionAvoidance::updateCollisionList(std::set> collisionList) +bool CollisionAvoidance::moveCollisionShape(const std::string& id, const Eigen::Affine3d& new_pose) { - if(collisionList.size() > _max_pairs) - { - Logger::error("collisionList.size() > _max_pairs when updating collision list"); - return false; - } + return _dist_calc->moveCollisionShape(id, new_pose); +} - try - { - _dist_calc->setLinkPairs(collisionList); - _Aineq.setZero(); - _bUpperBound.setZero(); - _distance_J.setZero(collisionList.size(), _robot.getNv()); - return true; - } - catch(std::out_of_range& e) - { - Logger::error(e.what()); - return false; - } +void CollisionAvoidance::setMaxPairs(const unsigned int max_pairs) +{ + _max_pairs = max_pairs; + update(); +} + +void CollisionAvoidance::setCollisionList(std::set> collisionList) +{ + _dist_calc->setLinkPairs(collisionList); } void CollisionAvoidance::collisionModelUpdated() @@ -187,41 +172,25 @@ void CollisionAvoidance::collisionModelUpdated() _lpv = _dist_calc->getCollisionPairs(_include_env); } -// bool CollisionAvoidance::setWorldCollisions(const moveit_msgs::PlanningSceneWorld &wc) -// { -// return _dist_calc->setWorldCollisions(wc); -// } -// void CollisionAvoidance::setLinksVsEnvironment(const std::list& links) -// { -// _dist_calc->setLinksVsEnvironment(links); -// } -// bool CollisionAvoidance::addWorldCollision(const std::string &id, -// std::shared_ptr fcl_obj) -// { -// return _dist_calc->addWorldCollision(id, fcl_obj); -// } + // bool CollisionAvoidance::removeWorldCollision(const std::string &id) // { // return _dist_calc->removeWorldCollision(id); // } -// bool CollisionAvoidance::moveWorldCollision(const std::string &id, -// Eigen::Affine3d new_pose) -// { -// return _dist_calc->moveWorldCollision(id, new_pose); -// } + void CollisionAvoidance::setBoundScaling(const double boundScaling) { _bound_scaling = boundScaling; } -void CollisionAvoidance::setLinksVsEnvironment(const std::list &links) +void CollisionAvoidance::setLinksVsEnvironment(const std::set &links) { - _dist_calc->setLinksVsEnvironment(std::set(links.begin(), links.end())); + _dist_calc->setLinksVsEnvironment(links); } Collision::CollisionModel &CollisionAvoidance::getCollisionModel() diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index df9a7361..81430e93 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -212,10 +212,10 @@ if(${FCL_FOUND}) add_dependencies(testCollisionAvoidanceVelocityConstraint OpenSoT) add_test(NAME OpenSoT_constraints_velocity_CollisionAvoidance COMMAND testCollisionAvoidanceVelocityConstraint) -# ADD_EXECUTABLE(testCollisionAvoidanceEnvironmentVelocityConstraint constraints/velocity/TestCollisionAvoidanceEnvironment.cpp) -# TARGET_LINK_LIBRARIES(testCollisionAvoidanceEnvironmentVelocityConstraint ${TestLibs} -lccd) -# add_dependencies(testCollisionAvoidanceEnvironmentVelocityConstraint OpenSoT) -# add_test(NAME OpenSoT_constraints_velocity_CollisionAvoidanceEnvironment COMMAND testCollisionAvoidanceEnvironmentVelocityConstraint) + ADD_EXECUTABLE(testCollisionAvoidanceEnvironmentVelocityConstraint constraints/velocity/TestCollisionAvoidanceEnvironment.cpp) + TARGET_LINK_LIBRARIES(testCollisionAvoidanceEnvironmentVelocityConstraint ${TestLibs} -lccd) + add_dependencies(testCollisionAvoidanceEnvironmentVelocityConstraint OpenSoT) + add_test(NAME OpenSoT_constraints_velocity_CollisionAvoidanceEnvironment COMMAND testCollisionAvoidanceEnvironmentVelocityConstraint) endif() ADD_EXECUTABLE(testCartesianUtils utils/cartesian_utils_test.cpp) diff --git a/tests/constraints/velocity/TestCollisionAvoidance.cpp b/tests/constraints/velocity/TestCollisionAvoidance.cpp index febc7b2b..fa8b762d 100644 --- a/tests/constraints/velocity/TestCollisionAvoidance.cpp +++ b/tests/constraints/velocity/TestCollisionAvoidance.cpp @@ -489,6 +489,7 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testCartesianTaskWithSC){ std::set > whiteList; whiteList.insert(std::pair(linkA,linkB)); this->sc_constraint->setCollisionList(whiteList); + this->sc_constraint->setMaxPairs(whiteList.size()); std::cout << "xxx Whitelist of size " << whiteList.size() << " set. Constraint automatically updated" << std::endl; std::list cartesianTasks; @@ -724,6 +725,7 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testMultipleCapsulePairsSC){ whiteList.insert(std::pair(linkA,linkB)); whiteList.insert(std::pair(linkC,linkD)); this->sc_constraint->setCollisionList(whiteList); + this->sc_constraint->setMaxPairs(whiteList.size()); std::cout << "xxx Whitelist of size " << whiteList.size() << " set. Constraint automatically updated" << std::endl; std::list cartesianTasks; @@ -945,6 +947,7 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testChangeWhitelistOnline){ whiteList.insert(std::pair(linkA,linkB)); whiteList.insert(std::pair(linkC,linkD)); this->sc_constraint->setCollisionList(whiteList); + this->sc_constraint->setMaxPairs(whiteList.size()); std::cout << "xxx Whitelist of size " << whiteList.size() << " set. Constraint automatically updated" << std::endl; std::list cartesianTasks; @@ -1089,7 +1092,7 @@ TEST_F(testSelfCollisionAvoidanceConstraint, testChangeWhitelistOnline){ std::cout << "xxx Setting whitelist" << std::endl; std::set > whiteList2; whiteList2.insert(std::pair(linkA,linkB)); - EXPECT_TRUE(this->sc_constraint->updateCollisionList(whiteList2)); + this->sc_constraint->setCollisionList(whiteList2); std::cout << "xxx Whitelist of size " << whiteList.size() << " set. Constraint automatically updated" << std::endl; dq.setZero(); diff --git a/tests/constraints/velocity/TestCollisionAvoidanceEnvironment.cpp b/tests/constraints/velocity/TestCollisionAvoidanceEnvironment.cpp index b84377da..57ce6ce8 100644 --- a/tests/constraints/velocity/TestCollisionAvoidanceEnvironment.cpp +++ b/tests/constraints/velocity/TestCollisionAvoidanceEnvironment.cpp @@ -9,10 +9,12 @@ #include #include #include -#include +#include #include #include #include +#include +#include "collision_utils.h" #define ENABLE_ROS false #if ENABLE_ROS @@ -40,35 +42,41 @@ class testCollisionAvoidanceConstraint : public ::testing::Test public: - protected: + std::string ReadFile(std::string path) + { + std::ifstream t(path); + std::stringstream buffer; + buffer << t.rdbuf(); + return buffer.str(); + } + testCollisionAvoidanceConstraint() { - std::string relative_path = OPENSOT_TEST_PATH "configs/bigman/configs/config_bigman_capsules.yaml"; - std::string urdf_path = OPENSOT_TEST_PATH "robots/bigman/bigman_capsules.rviz"; - std::ifstream f(urdf_path); + std::string urdf_capsule_path = OPENSOT_TEST_PATH "robots/bigman/bigman_capsules.rviz"; + std::ifstream f(urdf_capsule_path); std::stringstream ss; ss << f.rdbuf(); - urdf = MAKE_SHARED(); - urdf->initFile(urdf_path); + urdf = std::make_shared(); + urdf->initFile(urdf_capsule_path); std::string srdf_capsule_path = OPENSOT_TEST_PATH "robots/bigman/bigman.srdf"; - srdf = MAKE_SHARED(); + srdf = std::make_shared(); srdf->initFile(*urdf, srdf_capsule_path); - _path_to_cfg = relative_path; - _model_ptr = XBot::ModelInterface::getModel(_path_to_cfg); + _model_ptr = XBot::ModelInterface::getModel(ReadFile(urdf_capsule_path), ReadFile(srdf_capsule_path), "pin"); if(_model_ptr) std::cout<<"pointer address: "<<_model_ptr.get()<getJointNum()); - q.setZero(q.size()); + q = _model_ptr->getNeutralQ(); + _model_ptr->setJointPosition(q); + _model_ptr->update(); #if ENABLE_ROS int argc = 0; @@ -78,13 +86,13 @@ class testCollisionAvoidanceConstraint : public ::testing::Test pub = n->advertise("joint_states", 1000,1); KDL::Tree my_tree; - if (!kdl_parser::treeFromFile(urdf_path, my_tree)){ + if (!kdl_parser::treeFromFile(urdf_capsule_path, my_tree)){ ROS_ERROR("Failed to construct kdl tree");} rsp = std::make_shared(my_tree); n->setParam("/robot_description", ss.str()); - for(unsigned int i = 0; i < this->_model_ptr->getEnabledJointNames().size(); ++i){ - joint_state.name.push_back(this->_model_ptr->getEnabledJointNames()[i]); + for(unsigned int i = 0; i < this->_model_ptr->getJointNames().size(); ++i){ + joint_state.name.push_back(this->_model_ptr->getJointNames()[i]); joint_state.position.push_back(0.0);} #endif @@ -102,28 +110,26 @@ class testCollisionAvoidanceConstraint : public ::testing::Test #if ENABLE_ROS public: void publishJointStates(const Eigen::VectorXd& q) -{ - std::map joint_map; - for(unsigned int i = 0; i < q.size(); ++i){ - joint_state.position[i] = q[i]; - joint_map[joint_state.name[i]] = joint_state.position[i]; - } - joint_state.header.stamp = ros::Time::now(); - - pub.publish(joint_state); + { + std::map joint_map; + for(unsigned int i = 0; i < q.size(); ++i){ + joint_state.position[i] = q[i]; + joint_map[joint_state.name[i]] = joint_state.position[i]; + } + joint_state.header.stamp = ros::Time::now(); - rsp->publishTransforms(joint_map, ros::Time::now(), ""); - rsp->publishFixedTransforms(""); + pub.publish(joint_state); + rsp->publishTransforms(joint_map, ros::Time::now(), ""); + rsp->publishFixedTransforms(""); - ros::spinOnce(); -} + ros::spinOnce(); + } #endif XBot::ModelInterface::Ptr _model_ptr; - std::string _path_to_cfg; Eigen::VectorXd q; urdf::ModelSharedPtr urdf; @@ -140,25 +146,24 @@ class testCollisionAvoidanceConstraint : public ::testing::Test }; Eigen::VectorXd getGoodInitialPosition(const XBot::ModelInterface::Ptr _model_ptr) { - Eigen::VectorXd _q(_model_ptr->getJointNum()); - _q.setZero(_q.size()); - _q[_model_ptr->getDofIndex("RHipSag")] = -25.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RKneeSag")] = 50.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RAnkSag")] = -25.0*M_PI/180.0; - - _q[_model_ptr->getDofIndex("LHipSag")] = -25.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LKneeSag")] = 50.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LAnkSag")] = -25.0*M_PI/180.0; - - _q[_model_ptr->getDofIndex("LShSag")] = 20.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LShLat")] = 10.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LShYaw")] = -15.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("LElbj")] = -80.0*M_PI/180.0; - - _q[_model_ptr->getDofIndex("RShSag")] = 20.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RShLat")] = -10.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RShYaw")] = 15.0*M_PI/180.0; - _q[_model_ptr->getDofIndex("RElbj")] = -80.0*M_PI/180.0; + Eigen::VectorXd _q = _model_ptr->getNeutralQ(); + _q[_model_ptr->getQIndex("RHipSag")] = -25.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RKneeSag")] = 50.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RAnkSag")] = -25.0*M_PI/180.0; + + _q[_model_ptr->getQIndex("LHipSag")] = -25.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LKneeSag")] = 50.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LAnkSag")] = -25.0*M_PI/180.0; + + _q[_model_ptr->getQIndex("LShSag")] = 20.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LShLat")] = 10.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LShYaw")] = -15.0*M_PI/180.0; + _q[_model_ptr->getQIndex("LElbj")] = -80.0*M_PI/180.0; + + _q[_model_ptr->getQIndex("RShSag")] = 20.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RShLat")] = -10.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RShYaw")] = 15.0*M_PI/180.0; + _q[_model_ptr->getQIndex("RElbj")] = -80.0*M_PI/180.0; return _q; } @@ -183,7 +188,6 @@ TEST_F(testCollisionAvoidanceConstraint, testEnvironmentCollisionAvoidance){ string left_arm_link = "LSoftHandLink"; auto left_arm_task = std::make_shared ( base_link + "_TO_" + left_arm_link, - q, *_model_ptr, left_arm_link, base_link @@ -195,7 +199,6 @@ TEST_F(testCollisionAvoidanceConstraint, testEnvironmentCollisionAvoidance){ string right_arm_link = "RSoftHandLink"; auto right_arm_task = std::make_shared ( base_link + "_TO_" + right_arm_link, - q, *_model_ptr, right_arm_link, base_link @@ -206,42 +209,43 @@ TEST_F(testCollisionAvoidanceConstraint, testEnvironmentCollisionAvoidance){ Eigen::VectorXd q_min, q_max; _model_ptr->getJointLimits ( q_min, q_max ); - auto joint_limit_constraint = std::make_shared ( q, q_max, q_min ); + auto joint_limit_constraint = std::make_shared ( *_model_ptr, q_max, q_min ); + auto joint_velocity_limit_constraint = std::make_shared ( *_model_ptr, 1., 0.005); - std::list interested_links = {"LShp","LShr","LShy","LElb","LForearm","LSoftHandLink"}; - std::map> envionment_collision_objects; - std::shared_ptr shape = std::make_shared ( 0.1, 0.6, 1.4 ); - std::shared_ptr collision_object ( new fcl::CollisionObjectd ( shape ) ); - fcl::Transform3d shape_origin; - shape_origin.translation() << 0.7, 0, 0.; // in world frame - shape_origin.linear() = Eigen::Matrix3d::Identity(); - collision_object->setTransform ( shape_origin ); - envionment_collision_objects["env"] = collision_object; OpenSoT::constraints::velocity::CollisionAvoidance::Ptr environment_collsion_constraint = std::make_shared ( - q, *_model_ptr, -1, this->urdf, this->srdf); + *_model_ptr, -1, this->urdf, this->srdf); + // we consider only environment collision avoidance + environment_collsion_constraint->setCollisionList(std::set>()); - environment_collsion_constraint->setDetectionThreshold(1.); - environment_collsion_constraint->setLinkPairThreshold(0.0001); - environment_collsion_constraint->setBoundScaling(1.); + Eigen::Affine3d w_T_c; w_T_c.setIdentity(); + w_T_c.translation()<< 0.7, 0, 0.; + XBot::Collision::Shape::Box box; + box.size<<0.1, 0.6, 1.4; + EXPECT_TRUE(environment_collsion_constraint->addCollisionShape("mybox", "world", box, w_T_c)); + std::set interested_links = {"LShp","LShr","LShy","LElb","LForearm","LSoftHandLink"}; + environment_collsion_constraint->setLinksVsEnvironment(interested_links); + + environment_collsion_constraint->setMaxPairs(300); + + + environment_collsion_constraint->setDetectionThreshold(1.); + environment_collsion_constraint->setLinkPairThreshold(0.0001); + environment_collsion_constraint->setBoundScaling(1.); - for(auto pair : envionment_collision_objects) - EXPECT_TRUE(environment_collsion_constraint->addWorldCollision(pair.first, pair.second)); - // we consider only environment collision avoidance - environment_collsion_constraint->setCollisionWhiteList(std::list()); - environment_collsion_constraint->setLinksVsEnvironment(interested_links); auto autostack_ = std::make_shared ( left_arm_task + right_arm_task); // + 0.2*postural_task%indices autostack_ << joint_limit_constraint; autostack_ << environment_collsion_constraint; + autostack_<getNv()); for(unsigned int i = 0; i <= int(T/dt); ++i) { @@ -302,11 +303,9 @@ TEST_F(testCollisionAvoidanceConstraint, testEnvironmentCollisionAvoidance){ right_arm_task->setReference ( desired_pose.matrix() ); - EXPECT_LE(left_arm_task->getActualPose()(0,3), 0.406); //checked empirically... - - autostack_->update ( q ); + autostack_->update (); EXPECT_TRUE(solver->solve ( dq )); - q += dq; + q = _model_ptr->sum(q, dq); #if ENABLE_ROS this->publishJointStates(q); @@ -316,6 +315,15 @@ TEST_F(testCollisionAvoidanceConstraint, testEnvironmentCollisionAvoidance){ } + /** + * Due to environment the final y position of the left arm should be < than the final position of the right arm + */ + EXPECT_NEAR(std::fabs(left_arm_task->getActualPose()(0,3) - right_arm_task->getActualPose()(0,3)), 0.00130826, 1e-8); //checked empirically... + std::cout<<"std::fabs(left_arm_task->getActualPose()(0,3) - right_arm_task->getActualPose()(0,3)): "<getActualPose()(0,3) - right_arm_task->getActualPose()(0,3))<getActualPose()(0,3): "<getActualPose()(0,3)<getActualPose()(0,3): "<getActualPose()(0,3)<