From 787b806edf9a1a9f9cfc693b35af8558f66a2b9c Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Sun, 24 Feb 2019 20:57:00 -0800 Subject: [PATCH 01/12] Terminate with user input --- src/util.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/util.cpp b/src/util.cpp index dbafe08..26dbccc 100644 --- a/src/util.cpp +++ b/src/util.cpp @@ -1,5 +1,5 @@ #include "libada/util.hpp" - +#include #include #include #include @@ -21,8 +21,9 @@ void waitForUser(const std::string& msg, const std::shared_ptr& ada) std::cin >> input; if (input == 'n') { - ROS_INFO_STREAM("Terminate with user input " << input); + ROS_INFO_STREAM("Aborting with user input " << input); ada->stopTrajectoryExecutor(); + exit(1); } } From 16aa30c2b41cfa1b1002e2d0512db3b1f9373cf8 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Sun, 24 Feb 2019 20:58:51 -0800 Subject: [PATCH 02/12] Clang format --- src/util.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/util.cpp b/src/util.cpp index 26dbccc..5389f8c 100644 --- a/src/util.cpp +++ b/src/util.cpp @@ -1,5 +1,4 @@ #include "libada/util.hpp" -#include #include #include #include @@ -8,6 +7,7 @@ #include #include #include +#include namespace ada { namespace util { From 2279fe2f3d188b698f2eb05449296d3eaec78a27 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Tue, 26 Feb 2019 22:41:24 -0800 Subject: [PATCH 03/12] Make switch controllers public --- include/libada/Ada.hpp | 16 ++++++++-------- src/Ada.cpp | 13 ++++++++++++- 2 files changed, 20 insertions(+), 9 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index ba86d6a..4c388fa 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -334,6 +334,13 @@ 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); + private: // Named Configurations are read from a YAML file using ConfigurationMap = std::unordered_map; @@ -363,13 +370,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 +384,7 @@ class Ada final : public aikido::robot::Robot const bool mSimulation; // Names of the trajectory executors - const std::string mArmTrajectoryExecutorName; + std::string mArmTrajectoryExecutorName; // GL:allow this to be changed const std::string mHandTrajectoryExecutorName = "j2n6s200_hand_controller"; // Collision resolution. diff --git a/src/Ada.cpp b/src/Ada.cpp index a500abc..41ecd1b 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) { @@ -622,6 +623,7 @@ Ada::createTrajectoryExecutor() { std::string serverName = mArmTrajectoryExecutorName + "/follow_joint_trajectory"; + std::cout << "Create " << serverName << std::endl; return std::make_shared( *mNode, serverName, @@ -648,7 +650,12 @@ bool Ada::switchControllers( = controller_manager_msgs::SwitchControllerRequest::STRICT; if (mControllerServiceClient->call(srv) && srv.response.ok) + { + std::cout << "Switch controller succeeded " << std::endl; + mArmTrajectoryExecutorName = startControllers[0]; + mTrajectoryExecutor = createTrajectoryExecutor(); return true; + } else throw std::runtime_error("SwitchController failed."); } @@ -695,7 +702,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); @@ -841,6 +851,7 @@ bool Ada::moveArmOnTrajectory( } auto future = executeTrajectory(std::move(timedTrajectory)); + std::cout << "Got a future " << std::endl; try { future.get(); From ee6d6372fefbdeecbda16f2eb586e4acfc189479 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Wed, 24 Apr 2019 16:22:45 -0700 Subject: [PATCH 04/12] Add dart components in CMakeLists --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f96ff1..ef9166c 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 From c0a1891aef82530827eb8e371f77b54220c19f3f Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Wed, 24 Apr 2019 16:23:52 -0700 Subject: [PATCH 05/12] MoveArmOnTrajectory return false when a null trajectory is passed --- src/Ada.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/Ada.cpp b/src/Ada.cpp index 41ecd1b..dbc3701 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -782,9 +782,7 @@ bool Ada::moveArmOnTrajectory( std::vector smoothVelocityLimits) { if (!trajectory) - { - throw std::runtime_error("Trajectory execution failed: Empty trajectory."); - } + return false; std::vector constraints; From a1b5f58776d74cdbf5beb1f5ce4f2591a2bbedfd Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Mon, 29 Apr 2019 15:35:05 -0700 Subject: [PATCH 06/12] Minor cleanup --- include/libada/Ada.hpp | 2 +- src/Ada.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 4c388fa..364ab49 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -384,7 +384,7 @@ class Ada final : public aikido::robot::Robot const bool mSimulation; // Names of the trajectory executors - std::string mArmTrajectoryExecutorName; // GL:allow this to be changed + std::string mArmTrajectoryExecutorName; const std::string mHandTrajectoryExecutorName = "j2n6s200_hand_controller"; // Collision resolution. diff --git a/src/Ada.cpp b/src/Ada.cpp index dbc3701..815ecac 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -623,7 +623,6 @@ Ada::createTrajectoryExecutor() { std::string serverName = mArmTrajectoryExecutorName + "/follow_joint_trajectory"; - std::cout << "Create " << serverName << std::endl; return std::make_shared( *mNode, serverName, @@ -651,9 +650,9 @@ bool Ada::switchControllers( if (mControllerServiceClient->call(srv) && srv.response.ok) { - std::cout << "Switch controller succeeded " << std::endl; mArmTrajectoryExecutorName = startControllers[0]; mTrajectoryExecutor = createTrajectoryExecutor(); + ROS_INFO_STREAM("Controllers switched"); return true; } else @@ -849,7 +848,6 @@ bool Ada::moveArmOnTrajectory( } auto future = executeTrajectory(std::move(timedTrajectory)); - std::cout << "Got a future " << std::endl; try { future.get(); From 2050b9c0181eac0da9c746b36e4844ce6993b3b0 Mon Sep 17 00:00:00 2001 From: Gilwoo Lee Date: Mon, 29 Apr 2019 15:40:28 -0700 Subject: [PATCH 07/12] Clang format --- CMakeLists.txt | 2 +- include/libada/AdaHand.hpp | 7 +------ ...dKinematicSimulationPositionCommandExecutor.hpp | 6 +++--- ...dKinematicSimulationPositionCommandExecutor.cpp | 14 ++++++-------- 4 files changed, 11 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ef9166c..5b14494 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -170,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/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/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:: From c8734caeae41560266806c8686c483b05e296ac2 Mon Sep 17 00:00:00 2001 From: nansongyi Date: Sat, 1 Jun 2019 20:29:34 -0700 Subject: [PATCH 08/12] add twist --- include/libada/Ada.hpp | 29 +++++++++++++++++ src/Ada.cpp | 72 ++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 101 insertions(+) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 364ab49..d528f38 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -291,6 +291,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. @@ -328,6 +340,23 @@ class Ada final : public aikido::robot::Robot double positionTolerance, double angularTolerance); + 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 planWithEndEffectorTwist( + const Eigen::Vector6d& twists, + double durations, + const aikido::constraint::dart::CollisionFreePtr& collisionFree, + double timelimit, + double positionTolerance, + double angularTolerance); + /// Opens Ada's hand void openHand(); diff --git a/src/Ada.cpp b/src/Ada.cpp index 815ecac..756ace1 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -734,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, @@ -756,7 +780,55 @@ 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::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; +} //============================================================================== bool Ada::moveArmToConfiguration( const Eigen::Vector6d& configuration, From 68712e7dbea5a1eef9f22e216a2e22f3be9d9d39 Mon Sep 17 00:00:00 2001 From: nansongyi Date: Sun, 2 Jun 2019 13:26:38 -0700 Subject: [PATCH 09/12] add planEEPose --- include/libada/Ada.hpp | 17 ++++++++--- src/Ada.cpp | 69 ++++++++++++++++++++++++++++-------------- 2 files changed, 59 insertions(+), 27 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index d528f38..95c0d10 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -339,7 +339,7 @@ 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, @@ -349,6 +349,16 @@ class Ada final : public aikido::robot::Robot 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, @@ -369,6 +379,8 @@ class Ada final : public aikido::robot::Robot 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 @@ -453,9 +465,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/src/Ada.cpp b/src/Ada.cpp index 756ace1..eac50bb 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -784,29 +784,52 @@ aikido::trajectory::TrajectoryPtr Ada::planArmToEndEffectorOffset( /* 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::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, From 6ac39a82b11e8d75f734ed7920b8874e1c45ffd3 Mon Sep 17 00:00:00 2001 From: NANSONG YI Date: Mon, 24 Jun 2019 14:33:40 -0700 Subject: [PATCH 10/12] cleaned code --- include/libada/Ada.hpp | 17 ++++++++--- src/Ada.cpp | 69 ++++++++++++++++++++++++++++-------------- 2 files changed, 59 insertions(+), 27 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index d528f38..95c0d10 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -339,7 +339,7 @@ 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, @@ -349,6 +349,16 @@ class Ada final : public aikido::robot::Robot 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, @@ -369,6 +379,8 @@ class Ada final : public aikido::robot::Robot 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 @@ -453,9 +465,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/src/Ada.cpp b/src/Ada.cpp index 756ace1..eac50bb 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -784,29 +784,52 @@ aikido::trajectory::TrajectoryPtr Ada::planArmToEndEffectorOffset( /* 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::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, From 29709e424a5ef4fa77afabbd895b0de470107937 Mon Sep 17 00:00:00 2001 From: NANSONG YI Date: Tue, 6 Aug 2019 15:34:30 -0700 Subject: [PATCH 11/12] integrate perception + scooping --- include/libada/Ada.hpp | 10 ++++++++++ include/libada/util.hpp | 2 ++ src/Ada.cpp | 28 ++++++++++++++++++++++++++++ src/util.cpp | 5 +++++ 4 files changed, 45 insertions(+) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 95c0d10..c1e6a67 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -367,6 +367,16 @@ class Ada final : public aikido::robot::Robot 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(); 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 eac50bb..b30c9f4 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -852,6 +852,34 @@ aikido::trajectory::TrajectoryPtr Ada::planWithEndEffectorTwist( 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( const Eigen::Vector6d& configuration, 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) { From b4f24d34f29862bd76d1c6d802a9814322b02049 Mon Sep 17 00:00:00 2001 From: Nansong Yi Date: Sun, 3 Nov 2019 11:20:05 -0800 Subject: [PATCH 12/12] save --- include/libada/Ada.hpp | 3 ++- src/Ada.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index c1e6a67..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). diff --git a/src/Ada.cpp b/src/Ada.cpp index b30c9f4..cf442fc 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -865,7 +865,7 @@ aikido::trajectory::TrajectoryPtr Ada::planWithEndEffectorTwist( double positionTolerance, double angularTolerance) { - auto trajectory = mArm->planWithEndEffectorTwist( + auto trajectory = mArm->planWithEndEffectorTwist( mArmSpace, startState, mArm->getMetaSkeleton(),