diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f96ff1..5b14494 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,8 @@ endif() include(ExternalProject) find_package(DART 6.6.2 REQUIRED - OPTIONAL_COMPONENTS utils + COMPONENTS utils utils-urdf optimizer-nlopt + ) find_package(aikido 0.3.0 REQUIRED @@ -169,7 +170,7 @@ install(FILES "package.xml" # include(ClangFormat) -clang_format_setup(VERSION 3.8) +clang_format_setup(VERSION 3.9) if(CLANG_FORMAT_EXECUTABLE) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index ba86d6a..961e103 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -66,7 +66,8 @@ class Ada final : public aikido::robot::Robot const aikido::robot::util::VectorFieldPlannerParameters vfParams = aikido::robot::util::VectorFieldPlannerParameters( - 0.2, 0.03, 0.03, 0.001, 1e-3, 1e-3, 1.0, 0.2, 0.01); + // 0.2, 0.03, 0.03, 0.001, 1e-3, 1e-3, 1.0, 0.2, 0.01); + 0.2, 0.001, 0.001, 0.001, 1e-3, 1e-3, 1.0, 0.2, 0.01); /// Construct Ada metaskeleton using a URI. /// \param[in] env World (either for planning, post-processing, or executing). @@ -291,6 +292,18 @@ class Ada final : public aikido::robot::Robot double angularTolerance, const std::vector& velocityLimits = std::vector()); + /// Moves the end effector with a certain twist. ? within a durations? + /// Throws a runtime_error if no trajectory could be found. + /// \return True if the trajectory was completed successfully. + bool moveArmWithEndEffectorTwist( + const Eigen::Vector6d& twists, + double durations, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance, + const std::vector& velocityLimits); + /// Moves the robot to a configuration. /// Throws a runtime_error if no trajectory could be found. /// \return True if the trajectory was completed successfully. @@ -327,6 +340,43 @@ class Ada final : public aikido::robot::Robot double timelimit, double positionTolerance, double angularTolerance); +// designate a startState + aikido::trajectory::TrajectoryPtr planArmToEndEffectorOffset( + const Eigen::Vector3d& direction, + double length, + State* startState, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance); + +//=========================================== + aikido::trajectory::TrajectoryPtr planToEndEffectorPose( + const Eigen::Isometry3d& goalPose, + double conversionRatioInGeodesicDistance, + // State* startState, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double poseErrorTolerance); + + + aikido::trajectory::TrajectoryPtr planWithEndEffectorTwist( + const Eigen::Vector6d& twists, + double durations, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance); + +// designate a startState + aikido::trajectory::TrajectoryPtr planWithEndEffectorTwist( + const Eigen::Vector6d& twists, + double durations, + State* startState, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance); /// Opens Ada's hand void openHand(); @@ -334,6 +384,15 @@ class Ada final : public aikido::robot::Robot /// Closes Ada's hand void closeHand(); + /// Switches between controllers. + /// \param[in] startControllers Controllers to start. + /// \param[in] stopControllesr Controllers to stop. + bool switchControllers( + const std::vector& startControllers, + const std::vector& stopControllers); + // Trajectory executor. + std::shared_ptr mTrajectoryExecutor; + private: // Named Configurations are read from a YAML file using ConfigurationMap = std::unordered_map; @@ -363,13 +422,6 @@ class Ada final : public aikido::robot::Robot std::shared_ptr createTrajectoryExecutor(); - /// Switches between controllers. - /// \param[in] startControllers Controllers to start. - /// \param[in] stopControllesr Controllers to stop. - bool switchControllers( - const std::vector& startControllers, - const std::vector& stopControllers); - /// Plans the end effector to a TSR. /// Throws a runtime_error if no trajectory could be found. /// \return trajectory if the planning is successful. @@ -384,7 +436,7 @@ class Ada final : public aikido::robot::Robot const bool mSimulation; // Names of the trajectory executors - const std::string mArmTrajectoryExecutorName; + std::string mArmTrajectoryExecutorName; const std::string mHandTrajectoryExecutorName = "j2n6s200_hand_controller"; // Collision resolution. @@ -424,9 +476,6 @@ class Ada final : public aikido::robot::Robot // Thread for updating joint state from the Ros joint state client. std::unique_ptr mJointStateThread; - // Trajectory executor. - std::shared_ptr mTrajectoryExecutor; - // Name of the first link of the arm in the URDF std::string mArmBaseName; diff --git a/include/libada/AdaHand.hpp b/include/libada/AdaHand.hpp index 772c059..8f752f7 100644 --- a/include/libada/AdaHand.hpp +++ b/include/libada/AdaHand.hpp @@ -127,12 +127,7 @@ class AdaHand : public aikido::robot::Hand /// /// Maps an end-effector transform name (string) to a transform. using EndEffectorTransformMap = std:: - unordered_map, - std::equal_to, - Eigen::aligned_allocator>>; + unordered_map, std::equal_to, Eigen::aligned_allocator>>; /// Create controllers in real or simulation. /// diff --git a/include/libada/AdaHandKinematicSimulationPositionCommandExecutor.hpp b/include/libada/AdaHandKinematicSimulationPositionCommandExecutor.hpp index 76b2062..0cb00ee 100644 --- a/include/libada/AdaHandKinematicSimulationPositionCommandExecutor.hpp +++ b/include/libada/AdaHandKinematicSimulationPositionCommandExecutor.hpp @@ -81,9 +81,9 @@ class AdaHandKinematicSimulationPositionCommandExecutor = std::array{{1, 1}}; /// Executor for proximal and distal joints - std::array - mPositionCommandExecutors; + std:: + array + mPositionCommandExecutors; /// Duration to wait for futures from executors constexpr static auto kWaitPeriod = std::chrono::milliseconds(0); diff --git a/include/libada/util.hpp b/include/libada/util.hpp index 87d4cf1..e27e0be 100644 --- a/include/libada/util.hpp +++ b/include/libada/util.hpp @@ -12,6 +12,8 @@ namespace ada { namespace util { +void waitForAnyKey(); + /// Displays a message and waits for the user to press the enter key. /// Terminates if 'n' is pressed. /// \param[in] msg The message to display. diff --git a/src/Ada.cpp b/src/Ada.cpp index a500abc..cf442fc 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -295,7 +295,7 @@ std::unique_ptr Ada::retimePathWithKunz( //============================================================================== std::future Ada::executeTrajectory(const TrajectoryPtr& trajectory) const { - return mRobot->executeTrajectory(trajectory); + return mTrajectoryExecutor->execute(trajectory); } //============================================================================== @@ -342,6 +342,7 @@ void Ada::step(const std::chrono::system_clock::time_point& timepoint) std::lock_guard lock(mRobotSkeleton->getMutex()); mRobot->step(timepoint); mArm->step(timepoint); + mTrajectoryExecutor->step(timepoint); if (!mSimulation) { @@ -648,7 +649,12 @@ bool Ada::switchControllers( = controller_manager_msgs::SwitchControllerRequest::STRICT; if (mControllerServiceClient->call(srv) && srv.response.ok) + { + mArmTrajectoryExecutorName = startControllers[0]; + mTrajectoryExecutor = createTrajectoryExecutor(); + ROS_INFO_STREAM("Controllers switched"); return true; + } else throw std::runtime_error("SwitchController failed."); } @@ -695,7 +701,10 @@ bool Ada::moveArmToTSR( = planArmToTSR(tsr, collisionFree, timelimit, maxNumTrials, ranker); if (!trajectory) + { + dtwarn << "Failed to plan to tsr" << std::endl; return false; + } return moveArmOnTrajectory( trajectory, collisionFree, postprocessType, velocityLimits); @@ -725,6 +734,30 @@ bool Ada::moveArmToEndEffectorOffset( return moveArmOnTrajectory(traj, collisionFree, KUNZ, velocityLimits); } +//============================================================================== +bool Ada::moveArmWithEndEffectorTwist( + const Eigen::Vector6d& twists, + double durations, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance, + const std::vector& velocityLimits) +{ + auto traj = planWithEndEffectorTwist( + twists, + durations, + collisionFree, + timelimit, + positionTolerance, + angularTolerance); + + if (!traj) + return false; + + return moveArmOnTrajectory(traj, collisionFree, KUNZ, velocityLimits); +} + //============================================================================== aikido::trajectory::TrajectoryPtr Ada::planArmToEndEffectorOffset( const Eigen::Vector3d& direction, @@ -747,6 +780,105 @@ aikido::trajectory::TrajectoryPtr Ada::planArmToEndEffectorOffset( return trajectory; } +//======================================================================== +/* + designate a startState +*/ +aikido::trajectory::TrajectoryPtr Ada::planArmToEndEffectorOffset( + const Eigen::Vector3d& direction, + double length, + State* startState, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance) +{ + auto trajectory = mArm->planToEndEffectorOffset( + mArmSpace, + startState, + mArm->getMetaSkeleton(), + mHand->getEndEffectorBodyNode(), + collisionFree, + direction, + length, + timelimit, + positionTolerance, + angularTolerance); + + return trajectory; +} + +//======================================================================== +aikido::trajectory::TrajectoryPtr Ada::planToEndEffectorPose( + const Eigen::Isometry3d& goalPose, + double conversionRatioInGeodesicDistance, + // State* startState, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double poseErrorTolerance) +{ + auto trajectory = mArm->planToEndEffectorPose( + mArmSpace, + // State* startState, + mArm->getMetaSkeleton(), + mHand->getEndEffectorBodyNode(), + collisionFree, + goalPose, + conversionRatioInGeodesicDistance, + timelimit, + poseErrorTolerance); + + return trajectory; +} + +aikido::trajectory::TrajectoryPtr Ada::planWithEndEffectorTwist( + const Eigen::Vector6d& twists, + double durations, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance) +{ + auto trajectory = mArm->planWithEndEffectorTwist( + mArmSpace, + mArm->getMetaSkeleton(), + mHand->getEndEffectorBodyNode(), + twists, + durations, + collisionFree, + timelimit, + positionTolerance, + angularTolerance); + + return trajectory; +} + +//============================================================================== +// designate a startState + +aikido::trajectory::TrajectoryPtr Ada::planWithEndEffectorTwist( + const Eigen::Vector6d& twists, + double durations, + State* startState, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance) +{ + auto trajectory = mArm->planWithEndEffectorTwist( + mArmSpace, + startState, + mArm->getMetaSkeleton(), + mHand->getEndEffectorBodyNode(), + twists, + durations, + collisionFree, + timelimit, + positionTolerance, + angularTolerance); + + return trajectory; +} //============================================================================== bool Ada::moveArmToConfiguration( @@ -772,9 +904,7 @@ bool Ada::moveArmOnTrajectory( std::vector smoothVelocityLimits) { if (!trajectory) - { - throw std::runtime_error("Trajectory execution failed: Empty trajectory."); - } + return false; std::vector constraints; diff --git a/src/AdaHandKinematicSimulationPositionCommandExecutor.cpp b/src/AdaHandKinematicSimulationPositionCommandExecutor.cpp index f24eaa1..6dc63ee 100644 --- a/src/AdaHandKinematicSimulationPositionCommandExecutor.cpp +++ b/src/AdaHandKinematicSimulationPositionCommandExecutor.cpp @@ -8,14 +8,12 @@ constexpr std::chrono::milliseconds AdaHandKinematicSimulationPositionCommandExecutor::kWaitPeriod; constexpr int AdaHandKinematicSimulationPositionCommandExecutor::kNumPositionExecutors; -constexpr std::array - AdaHandKinematicSimulationPositionCommandExecutor::kPrimalDofs; -constexpr std::array - AdaHandKinematicSimulationPositionCommandExecutor::kDistalDofs; +constexpr std:: + array + AdaHandKinematicSimulationPositionCommandExecutor::kPrimalDofs; +constexpr std:: + array + AdaHandKinematicSimulationPositionCommandExecutor::kDistalDofs; //============================================================================== AdaHandKinematicSimulationPositionCommandExecutor:: diff --git a/src/util.cpp b/src/util.cpp index 5389f8c..b17679d 100644 --- a/src/util.cpp +++ b/src/util.cpp @@ -12,6 +12,11 @@ namespace ada { namespace util { +//============================================================================== + void waitForAnyKey() { + std::cout << "Press Enter to Continue... \n"; + std::cin.ignore(); + } //============================================================================== void waitForUser(const std::string& msg, const std::shared_ptr& ada) {