diff --git a/include/libada/Ada.hpp b/include/libada/Ada.hpp index 902925b..24b799a 100644 --- a/include/libada/Ada.hpp +++ b/include/libada/Ada.hpp @@ -30,6 +30,9 @@ #include #include +#include +#include + #include "libada/AdaHand.hpp" namespace ada { @@ -54,6 +57,17 @@ struct KunzParams : aikido::planner::kunzretimer::KunzRetimer::Params } }; +// Enum for Cartesian Velocity Return Result +enum CartVelocityResult +{ + kCVR_SUCCESS, + kCVR_INVALID, + kCVR_FORCE, + kCVR_CANCELLED, + kCVR_TIMEOUT, + kCVR_UNKNOWN +}; + class Ada final : public aikido::robot::Robot { public: @@ -240,6 +254,29 @@ class Ada final : public aikido::robot::Robot /// \return true if all controllers have been successfully switched bool stopTrajectoryExecutor(); + /// Toggle controllers and enable/disable cartesian velocity control. + /// Required before calling \c moveArmCommandVelocity + /// \param[in] useFT whether to use the FT-enabled controller or not. + /// \return true if all controllers have been successfully switched + bool setVelocityControl(bool enabled, bool useFT = false); + + /// Moves the end effector at a cartesian velocity for a specified + /// amount of time. + /// \param[in] linear, 3-D velocity in world space, m/s + /// \param[in] angular, 3-D velocity about world axes, rad/s + /// \param[in] forTime, maximum time to execute velocity + /// \param[in] block, block until completed, cancelled, or aborted + /// \return True if the trajectory was completed successfully. + std::future moveArmCommandVelocity( + const Eigen::Vector3d& linear, + const Eigen::Vector3d& angular = Eigen::Vector3d(0.0, 0.0, 0.0), + ros::Duration forTime = ros::Duration(1.0), + bool block = false); + + /// Stops any movement created by \c moveArmCommandVelocity + /// \return True on successful cancellation + bool cancelCommandVelocity(); + /// Sets VectorFieldPlanner parameters. /// TODO: To be removed with Planner API. /// \param[in] vfParameters VectorField Parameters @@ -336,6 +373,15 @@ class Ada final : public aikido::robot::Robot const std::vector& startControllers, const std::vector& stopControllers); +// Short-cuts for action client types +protected: + using Action = pr_control_msgs::SetCartesianVelocityAction; + using ActionClient = actionlib::SimpleActionClient; + using GoalState = actionlib::SimpleClientGoalState; + + using Result = pr_control_msgs::SetCartesianVelocityResult; + using ResultPtr = pr_control_msgs::SetCartesianVelocityResultConstPtr; + private: // Named Configurations are read from a YAML file using ConfigurationMap = std::unordered_map; @@ -442,6 +488,19 @@ class Ada final : public aikido::robot::Robot // For trajectory executions. std::unique_ptr mThread; + + ////// Cartesian Velocity Variables + // Name of cartesian velocity action server + const std::string mCartesianVelocityName = "move_until_touch_cartvel_controller"; + const std::string mCartesianVelocityNoFTName = "cartvel_controller"; + // Action Client + std::shared_ptr mActionClient; + // Transition callback for velocity goals + void doneCallback(const GoalState& handle, const ResultPtr& result); + // Promise to keep track of current velocity goal + std::shared_ptr> mPromise; + // Track whether using velocity control + bool mVelocityControl; }; } // namespace ada diff --git a/src/Ada.cpp b/src/Ada.cpp index 30a5e4b..02673ae 100644 --- a/src/Ada.cpp +++ b/src/Ada.cpp @@ -117,6 +117,7 @@ Ada::Ada( , mSmootherFeasibilityApproxTolerance(1e-3) , mWorld(std::move(env)) , mEndEffectorName(endEffectorName) + , mVelocityControl(false) { simulation = true; // temporarily set simulation to true @@ -459,6 +460,175 @@ bool Ada::stopTrajectoryExecutor() mHandTrajectoryExecutorName}); } +//============================================================================= +bool Ada::setVelocityControl(bool enabled, bool useFT) { + if(mSimulation) return false; + // Do not re-enable or re-disable + if(mVelocityControl == enabled) return true; + + bool ret; + + controller_manager_msgs::SwitchController srv; + srv.request.strictness + = controller_manager_msgs::SwitchControllerRequest::STRICT; + + const std::string controlName = useFT ? mCartesianVelocityName : mCartesianVelocityNoFTName; + + if(enabled) { + mActionClient = std::make_shared(controlName + "/cart_velocity"); + // Stop joint trajectory control + ret = stopTrajectoryExecutor(); + if (!ret) return false; + + // Switch controllers + srv.request.start_controllers = std::vector{controlName}; + srv.request.stop_controllers = std::vector(); + if (!mControllerServiceClient->call(srv) || !srv.response.ok) { + return false; + } + + // Ensure action client is running + ROS_INFO_STREAM("Starting cartesian velocity action client..."); + if(!mActionClient->waitForServer(ros::Duration(10.0))) { + return false; + } + mVelocityControl = true; + return true; + } else { + // Kill action goals + ret = cancelCommandVelocity(); + if (!ret) return false; + + // Switch controllers + srv.request.start_controllers = std::vector(); + srv.request.stop_controllers = std::vector{controlName}; + if (!mControllerServiceClient->call(srv) || !srv.response.ok) { + return false; + } + + // Start joint trajectory control + if(!startTrajectoryExecutor()) { + return false; + } + mVelocityControl = false; + return true; + } +} + +//============================================================================= +std::future Ada::moveArmCommandVelocity( + const Eigen::Vector3d& linear, + const Eigen::Vector3d& angular, + ros::Duration forTime, + bool block) { + mPromise.reset(new std::promise()); + std::future ret = mPromise->get_future(); + if (mSimulation || !mVelocityControl) { + mPromise->set_value(kCVR_INVALID); + return ret; + } + + // Construct goal + pr_control_msgs::SetCartesianVelocityGoal goal; + goal.exec_time = forTime; + goal.command.linear.x = linear(0); + goal.command.linear.y = linear(1); + goal.command.linear.z = linear(2); + goal.command.angular.x = angular(0); + goal.command.angular.y = angular(1); + goal.command.angular.z = angular(2); + + // Send goal + using std::placeholders::_1; + using std::placeholders::_2; + if (mActionClient->getState() != actionlib::SimpleClientGoalState::LOST) { + mActionClient->stopTrackingGoal(); + } + mActionClient->sendGoal(goal, + std::bind(&Ada::doneCallback, this, _1, _2)); + + // Block until done or time-out + if(block) { + std::future_status status = ret.wait_for(std::chrono::nanoseconds(forTime.toNSec())); + if (status != std::future_status::ready) { + // Time-out, write false + mPromise->set_value(kCVR_TIMEOUT); + } + } + + return ret; +} + +void Ada::doneCallback(const GoalState& handle, const ResultPtr& result) { + // Only handle current goal + if (!mVelocityControl) return; + + if (handle.isDone()) + { + std::stringstream message; + CartVelocityResult isSuccessful = kCVR_SUCCESS; + + // Check the status of execution. + if (result && result->error_code != Result::SUCCESSFUL) + { + message << ": "; + switch(result->error_code) { + case Result::INVALID_CMD: + message << "INVALID_CMD"; + isSuccessful = kCVR_INVALID; + break; + case Result::FORCE_THRESH: + message << "FORCE_THRESH"; + isSuccessful = kCVR_FORCE; + break; + case Result::CANCELLED: + message << "CANCELLED"; + isSuccessful = kCVR_CANCELLED; + break; + default: + message << "UNKNOWN"; + isSuccessful = kCVR_UNKNOWN; + } + + if (!result->error_string.empty()) + message << " (" << result->error_string << ")"; + } + // we can fail without a result for another reason + else if (!result) { + message << "Velocity Command " << handle.toString(); + + const auto terminalMessage = handle.getText(); + if (!terminalMessage.empty()) + message << " (" << terminalMessage << ")"; + + isSuccessful = kCVR_UNKNOWN; + } + + if(isSuccessful != kCVR_SUCCESS) { + ROS_WARN_STREAM(message.str()); + } + + try { + mPromise->set_value(isSuccessful); + } catch(...) { + ROS_WARN_STREAM("Promise already satisfied."); + } + } + +} + +//============================================================================= +bool Ada::cancelCommandVelocity() { + if(mSimulation) return false; + if(!mActionClient->isServerConnected()) return false; + if(!mVelocityControl) return false; + if (mActionClient->getState() != actionlib::SimpleClientGoalState::LOST) { + mActionClient->stopTrackingGoal(); + } + mActionClient->cancelAllGoals(); + return true; +} + //============================================================================= void Ada::setVectorFieldPlannerParameters( const VectorFieldPlannerParameters& vfParameters) @@ -576,8 +746,10 @@ bool Ada::switchControllers( if (mControllerServiceClient->call(srv) && srv.response.ok) { - mArmTrajectoryExecutorName = startControllers[0]; - mTrajectoryExecutor = createTrajectoryExecutor(); + if(startControllers.size() > 0) { + mArmTrajectoryExecutorName = startControllers[0]; + mTrajectoryExecutor = createTrajectoryExecutor(); + } ROS_INFO_STREAM("Controllers switched"); return true; }