diff --git a/include/aikido/control/BarrettFingerKinematicSimulationPositionCommandExecutor.hpp b/include/aikido/control/BarrettFingerKinematicSimulationPositionCommandExecutor.hpp new file mode 100644 index 0000000000..cfb3efb876 --- /dev/null +++ b/include/aikido/control/BarrettFingerKinematicSimulationPositionCommandExecutor.hpp @@ -0,0 +1,129 @@ +#ifndef AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONPOSITIONCOMMANDEXECUTOR_HPP_ +#define AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONPOSITIONCOMMANDEXECUTOR_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { + +/// This executor mimics the behavior of BarretFinger. +/// It moves a finger to a desired point; it may stop early if +/// joint limit is reached or collision is detected. +/// Only the proximal joint is actuated; the distal joint moves with mimic ratio. +/// When collision is detected on the distal link, the finger stops. +/// When collision is detected on the proximal link, the distal link moves +/// until it reaches joint limit or until distal collision is detected. +class BarrettFingerKinematicSimulationPositionCommandExecutor +: public PositionCommandExecutor +{ +public: + /// Constructor. + /// \param[in] finger Finger to be controlled by this Executor. + /// \param[in] proximal Index of proximal dof + /// \param[in] distal Index of distal dof + /// \param[in] collisionDetector CollisionDetector to check collision with fingers. + /// If nullptr, default to FCLCollisionDetector. + /// \param[in] collideWith CollisionGroup to check collision with fingers. + /// If nullptr, default to empty CollisionGroup. + /// \param[in] collisionOptions Default is (enableContact=false, binaryCheck=true, + /// maxNumContacts = 1.) + /// See dart/collison/Option.h for more information + BarrettFingerKinematicSimulationPositionCommandExecutor( + ::dart::dynamics::ChainPtr finger, size_t proximal, size_t distal, + ::dart::collision::CollisionDetectorPtr collisionDetector = nullptr, + ::dart::collision::CollisionGroupPtr collideWith = nullptr, + ::dart::collision::CollisionOption collisionOptions + = ::dart::collision::CollisionOption(false, 1)); + + /// Open/close fingers to goal configuration. + /// Proximal dof moves to goalPosition, joint limit, or until collision. + /// Distal dof follows with mimic ratio. + /// Call step after this for actual execution until future returns. + /// \param goalPosition Desired angle of proximal joint. + /// \return future Becomes available when the execution completes. + std::future execute(const Eigen::VectorXd& goalPosition) override; + + /// Returns mimic ratio, i.e. how much the distal joint moves relative to + /// the proximal joint. The joint movements follow + /// this ratio only when both joints are moving. + /// \return mimic ratio. + constexpr static double getMimicRatio() { return kMimicRatio; } + + /// Moves the joints of the finger by dofVelocity*timeSincePreviousCall + /// until execute's goalPosition by primary dof or collision is detected. + /// If proximal link is in collision, distal link moves until + /// mimicRatio*goalPosition. If distal link is in collision, execution stops. + /// If multiple threads are accessing this function or skeleton associated + /// with this executor, it is necessary to lock the skeleton before + /// calling this method. + void step() override; + + /// Resets CollisionGroup to check collision with fingers. + /// \param _collideWith CollisionGroup to check collision with fingers. + /// \return false if fails to change collideWith (during execution). + bool setCollideWith(::dart::collision::CollisionGroupPtr collideWith); + +private: + constexpr static double kMimicRatio = 0.333; + // TODO: read velocity limit from herb_description + constexpr static double kProximalSpeed = 2.0; + constexpr static double kDistalSpeed = kProximalSpeed*kMimicRatio; + + /// If (current dof - goalPosition) execution terminates. + constexpr static double kTolerance = 1e-3; + + ::dart::dynamics::ChainPtr mFinger; + + /// proximal, distal dofs + ::dart::dynamics::DegreeOfFreedom* mProximalDof; + ::dart::dynamics::DegreeOfFreedom* mDistalDof; + + /// Joint limits for proximal and distal dof. + std::pair mProximalLimits, mDistalLimits; + + ::dart::collision::CollisionDetectorPtr mCollisionDetector; + ::dart::collision::CollisionGroupPtr mCollideWith; + ::dart::collision::CollisionOption mCollisionOptions; + + ::dart::collision::CollisionGroupPtr mProximalCollisionGroup; + ::dart::collision::CollisionGroupPtr mDistalCollisionGroup; + + std::unique_ptr> mPromise; + + /// Flag for indicating execution of a command. + bool mInExecution; + + /// Time that step was last called. + std::chrono::system_clock::time_point mTimeOfPreviousCall; + + /// Control access to mPromise, mInExecution, mGoalPosition, mDistalOnly, + /// mCollideWith + std::mutex mMutex; + + /// Desired end-position of proximal dof. + double mProximalGoalPosition; + + /// Indicator that only distal finger is to be moved. + bool mDistalOnly; + + /// Helper method for step() to set variables for terminating an execution. + void terminate(); + +}; + +using BarrettFingerKinematicSimulationPositionCommandExecutorPtr + = std::shared_ptr; + + +} // control +} // aikido + +#endif diff --git a/include/aikido/control/BarrettFingerKinematicSimulationSpreadCommandExecutor.hpp b/include/aikido/control/BarrettFingerKinematicSimulationSpreadCommandExecutor.hpp new file mode 100644 index 0000000000..d75bd6606f --- /dev/null +++ b/include/aikido/control/BarrettFingerKinematicSimulationSpreadCommandExecutor.hpp @@ -0,0 +1,103 @@ +#ifndef AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONSPREADCOMMANDEXECUTOR_HPP_ +#define AIKIDO_CONTROL_BARRETFINGERKINEMATICSIMULATIONSPREADCOMMANDEXECUTOR_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { + +/// This executor mimics the spread behavior of BarretFinger. +/// It moves two finger spreads simultaneously to certain goal value; +/// it will stop prematurely if joint limit is reached or collision is detected. +class BarrettFingerKinematicSimulationSpreadCommandExecutor +: public PositionCommandExecutor +{ +public: + /// Constructor. + /// \param[in] fingers 2 fingers to be controlled by this Executor. + /// \param[in] spread Index of spread dof + /// \param[in] collisionDetector CollisionDetector to check collision with fingers. + /// If nullptr, default to FCLCollisionDetector. + /// \param[in] collideWith CollisionGroup to check collision with fingers. + /// If nullptr, default to empty CollisionGroup + /// \param[in] collisionOptions Default is (enableContact=false, binaryCheck=true, + /// maxNumContacts = 1.) + BarrettFingerKinematicSimulationSpreadCommandExecutor( + std::array<::dart::dynamics::ChainPtr, 2> fingers, size_t spread, + ::dart::collision::CollisionDetectorPtr collisionDetector = nullptr, + ::dart::collision::CollisionGroupPtr collideWith = nullptr, + ::dart::collision::CollisionOption collisionOptions + = ::dart::collision::CollisionOption(false, 1)); + + /// Move the spread joint by goalPosition until goalPosition or + /// joint limits are reached, or until collision is detected. + /// Call step after this for actual execution until future returns. + /// \param goalPosition Desired angle of spread joint. + /// \return future which becomes available when the execution completes. + std::future execute(const Eigen::VectorXd& goalPosition) override; + + /// Moves the joint of the finger by fixed speed*timeSincePreviousCall + /// until execute's goalPosition by spread dof or collision is detected. + /// If multiple threads are accessing this function or the skeleton associated + /// with this executor, it is necessary to lock the skeleton before + /// calling this method. + void step() override; + + /// Resets CollisionGroup to check collision. + /// \param collideWith CollisionGroup to check collision with fingers. + /// \return false if fails to change collideWith (during execution). + bool setCollideWith(::dart::collision::CollisionGroupPtr collideWith); + +private: + constexpr static double kDofSpeed = 1; + + /// If (current dof - goalPosition) execution terminates. + constexpr static double kTolerance = 1e-3; + + /// All fingers dof values should be the same (approx. within this tolerance). + constexpr static double kDofTolerance = 1e-3; + + constexpr static int kNumFingers = 2; + + std::array<::dart::dynamics::ChainPtr, 2> mFingers; + + std::vector<::dart::dynamics::DegreeOfFreedom*> mSpreadDofs; + + std::pair mDofLimits; + + ::dart::collision::CollisionDetectorPtr mCollisionDetector; + ::dart::collision::CollisionGroupPtr mCollideWith; + ::dart::collision::CollisionOption mCollisionOptions; + + ::dart::collision::CollisionGroupPtr mSpreadCollisionGroup; + + std::unique_ptr> mPromise; + + /// Controls access to mPromise, mInExecution, mGoalPosition, mCollideWith + std::mutex mMutex; + + /// True if a command is in execution + bool mInExecution; + + /// Time that step was last called. + std::chrono::system_clock::time_point mTimeOfPreviousCall; + + /// Desired end spread value + double mGoalPosition; +}; + +using BarrettFingerKinematicSimulationSpreadCommandExecutorPtr + = std::shared_ptr; + +} // control +} // aikido + +#endif diff --git a/include/aikido/control/BarrettFingerPositionCommandExecutor.hpp b/include/aikido/control/BarrettFingerPositionCommandExecutor.hpp deleted file mode 100644 index 9e172190c5..0000000000 --- a/include/aikido/control/BarrettFingerPositionCommandExecutor.hpp +++ /dev/null @@ -1,117 +0,0 @@ -#ifndef AIKIDO_CONTROL_BARRETFINGERPOSITIONCOMMANDEXECUTOR_HPP_ -#define AIKIDO_CONTROL_BARRETFINGERPOSITIONCOMMANDEXECUTOR_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include - -namespace aikido { -namespace control { - -/// This executor mimics the behavior of BarretFinger. -/// It moves a finger to a desired point; it may stop early if -/// joint limit is reached or collision is detected. -/// Only the proximal joint is actuated; the distal joint moves with mimic ratio. -/// When collision is detected on the distal link, the finger stops. -/// When collision is detected on the proximal link, the distal link moves -/// until it reaches joint limit or until distal collision is detected. -class BarrettFingerPositionCommandExecutor -{ -public: - /// Constructor. - /// \param _finger Finger to be controlled by this Executor. - /// \param _proximal Index of proximal dof - /// \param _distal Index of distal dof - /// \param _collisionDetector CollisionDetector for detecting self collision - /// and collision with objects. - /// \param _collisionOptions Default is (enableContact=false, binaryCheck=true, - /// maxNumContacts = 1.) - /// See dart/collison/Option.h for more information - BarrettFingerPositionCommandExecutor( - ::dart::dynamics::ChainPtr _finger, int _proximal, int _distal, - ::dart::collision::CollisionDetectorPtr _collisionDetector, - ::dart::collision::CollisionOption _collisionOptions - = ::dart::collision::CollisionOption(false, 1)); - - /// Sets variables to move the finger joints. - /// proximal dof moves to _goalPosition, joint limit, or until collision. - /// Step method should be called multiple times until future returns. - /// Distal dof follows with mimic ratio. - /// \param _goalPosition Desired angle of proximal joint. - /// \param _collideWith CollisionGroup to check collision with fingers. - /// \return future Becomes available when the execution completes. - std::future execute(double _goalPosition, - ::dart::collision::CollisionGroupPtr _collideWith); - - /// Returns mimic ratio, i.e. how much the distal joint moves relative to - /// the proximal joint. The joint movements follow - /// this ratio only when both joints are moving. - /// \return mimic ratio. - static double getMimicRatio(); - - /// Moves the joints of the finger by dofVelocity*_timeSIncePreviousCall - /// until execute's goalPosition by primary dof or collision is detected. - /// If proximal link is in collision, distal link moves until - /// mimicRatio*goalPosition. If distal link is in collision, execution stops. - /// If multiple threads are accessing this function or skeleton associated - /// with this executor, it is necessary to lock the skeleton before - /// calling this method. - /// \param _timeSincePreviousCall Time since previous call. - void step(double _timeSincePreviousCall); - -private: - constexpr static double kMimicRatio = 0.333; - constexpr static double kProximalVelocity = 0.01; - constexpr static double kDistalVelocity = kProximalVelocity*kMimicRatio; - - /// If (current dof - goalPosition) execution terminates. - constexpr static double kTolerance = 1e-3; - - ::dart::dynamics::ChainPtr mFinger; - - /// proximal, distal dofs - ::dart::dynamics::DegreeOfFreedom* mProximalDof; - ::dart::dynamics::DegreeOfFreedom* mDistalDof; - - /// Joint limits for proximal and distal dof. - std::pair mProximalLimits, mDistalLimits; - - ::dart::collision::CollisionDetectorPtr mCollisionDetector; - ::dart::collision::CollisionOption mCollisionOptions; - - ::dart::collision::CollisionGroupPtr mProximalCollisionGroup; - ::dart::collision::CollisionGroupPtr mDistalCollisionGroup; - - std::unique_ptr> mPromise; - - /// Control access to mPromise, mInExecution, mGoalPosition, mDistalOnly, - /// mCollideWith - std::mutex mMutex; - - /// Flag for indicating execution of a command. - bool mInExecution; - - /// Desired end-position of proximal dof. - double mProximalGoalPosition; - - /// Indicator that only distal finger is to be moved. - bool mDistalOnly; - - ::dart::collision::CollisionGroupPtr mCollideWith; - - /// Helper method for step() to set variables for terminating an execution. - void terminate(); - -}; - -using BarrettFingerPositionCommandExecutorPtr = std::shared_ptr; - - -} // control -} // aikido - -#endif diff --git a/include/aikido/control/BarrettFingerSpreadCommandExecutor.hpp b/include/aikido/control/BarrettFingerSpreadCommandExecutor.hpp deleted file mode 100644 index 3757e5664a..0000000000 --- a/include/aikido/control/BarrettFingerSpreadCommandExecutor.hpp +++ /dev/null @@ -1,94 +0,0 @@ -#ifndef AIKIDO_CONTROL_BARRETFINGERSPREADCOMMANDEXECUTOR_HPP_ -#define AIKIDO_CONTROL_BARRETFINGERSPREADCOMMANDEXECUTOR_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include - -namespace aikido { -namespace control { - -/// This executor mimics the spread behavior of BarretFinger. -/// It moves two finger spreads simultaneously to certain goal value; -/// it will stop prematurely if joint limit is reached or collision is detected. -class BarrettFingerSpreadCommandExecutor -{ -public: - /// Constructor. - /// \param _fingers 2 fingers to be controlled by this Executor. - /// \param _spread Index of spread dof - /// \param _collisionDetector CollisionDetector for detecting self collision - /// and collision with objects. - /// \param _collisionOptions Default is (enableContact=false, binaryCheck=true, - /// maxNumContacts = 1.) - /// See dart/collison/Option.h for more information - BarrettFingerSpreadCommandExecutor( - std::array<::dart::dynamics::ChainPtr, 2> _fingers, int _spread, - ::dart::collision::CollisionDetectorPtr _collisionDetector, - ::dart::collision::CollisionOption _collisionOptions - = ::dart::collision::CollisionOption(false, 1)); - - /// Sets variables to move the spread joint by _goalPosition, - /// joint limit has reached, or until collision is detected. - /// Must call step function after this for actual execution. - /// \param _goalPosition Desired angle of spread joint. - /// \param _collideWith CollisionGroup to check collision with fingers. - /// \return future which becomes available when the execution completes. - std::future execute(double _goalPosition, - ::dart::collision::CollisionGroupPtr _collideWith); - - /// Moves the joint of the finger by certain velocity*_timeSincePreviousCall - /// until execute's goalPosition by spread dof or collision is detected. - /// If multiple threads are accessing this function or skeleton associated - /// with this executor, it is necessary to lock the skeleton before - /// calling this method. - /// \param _timeSincePreviousCall Time since previous call. - void step(double _timeSincePreviousCall); - - -private: - constexpr static double kDofVelocity = 0.01; - - /// If (current dof - goalPosition) execution terminates. - constexpr static double kTolerance = 1e-3; - - /// All fingers dof values should be the same (approx. within this tolerance). - constexpr static double kDofTolerance = 1e-3; - - constexpr static int kNumFingers = 2; - - std::array<::dart::dynamics::ChainPtr, 2> mFingers; - - std::vector<::dart::dynamics::DegreeOfFreedom*> mSpreadDofs; - - std::pair mDofLimits; - - ::dart::collision::CollisionDetectorPtr mCollisionDetector; - ::dart::collision::CollisionOption mCollisionOptions; - - ::dart::collision::CollisionGroupPtr mSpreadCollisionGroup; - - std::unique_ptr> mPromise; - - /// Controls access to mPromise, mInExecution, mGoalPosition, mCollideWith - std::mutex mMutex; - - /// True if a command is in execution - bool mInExecution; - - /// Desired end spread value - double mGoalPosition; - - ::dart::collision::CollisionGroupPtr mCollideWith; -}; - -using BarrettFingerSpreadCommandExecutorPtr = std::shared_ptr; - -} // control -} // aikido - -#endif diff --git a/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp b/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp new file mode 100644 index 0000000000..c83c477e92 --- /dev/null +++ b/include/aikido/control/BarrettHandKinematicSimulationPositionCommandExecutor.hpp @@ -0,0 +1,101 @@ +#ifndef AIKIDO_CONTROL_BARRETTHANDKINEMATICSIMULATIONPOSITIONCOMMANDEXECUTOR_HPP_ +#define AIKIDO_CONTROL_BARRETTHANDKINEMATICSIMULATIONPOSITIONCOMMANDEXECUTOR_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Vector1d = Eigen::Matrix; + +namespace aikido { +namespace control { + +/// Position command executor for simulating BarrettHand fingers. +/// Assumes that fingers are underactuated: proximal joint is actuated +/// and distal joint moves with certain mimic ratio until collision. +class BarrettHandKinematicSimulationPositionCommandExecutor +: public PositionCommandExecutor +{ +public: + /// Constructor. + /// \param[in] robot Robot to construct executor for + /// \param[in] prefix String (either "/right/" or "/left/") to specify hand + /// \param[in] collisionDetector CollisionDetector to check collision with fingers. + /// If nullptr, default to FCLCollisionDetector. + /// \param[in] collideWith CollisionGroup to check collision with fingers. + /// If nullptr, default to empty CollisionGroup + BarrettHandKinematicSimulationPositionCommandExecutor( + dart::dynamics::SkeletonPtr robot, + const std::string &prefix, + ::dart::collision::CollisionDetectorPtr collisionDetector = nullptr, + ::dart::collision::CollisionGroupPtr collideWith = nullptr); + + /// Move fingers to a goal configuration. In order to actually move the + /// fingers, step method should be called multiple times until future returns. + /// \param _goalPositions End dof pose for proximal joints and spread. + /// First 3 should be for proximal joints, the last element should be + /// for spread. If _positions are above/below joint limits, + /// the fingers will move only up to the limit. + /// \return Future which becomes available when the execution completes. + std::future execute(const Eigen::VectorXd& goalPositions) override; + + // Documentation inherited. + void step() override; + + /// Resets CollisionGroup to check collision with fingers. + /// \param _collideWith CollisionGroup to check collision with fingers. + /// \return false if fails to change collideWith (during execution). + bool setCollideWith(::dart::collision::CollisionGroupPtr collideWith); + +private: + /// Set up mPositionCommandExecutors and mSpreadCommandExecutor. + /// \param[in] robot Robot to construct hand executor for + /// \param[in] prefix String (either "/right/" or "/left/") to specify hand + void setupExecutors( + dart::dynamics::SkeletonPtr robot, + const std::string& prefix); + + constexpr static int kNumPositionExecutor = 3; + constexpr static int kNumSpreadExecutor = 1; + constexpr static auto kWaitPeriod = std::chrono::milliseconds(1); + + /// Executor for proximal and distal joints. + std::array mPositionCommandExecutors; + BarrettFingerKinematicSimulationSpreadCommandExecutorPtr mSpreadCommandExecutor; + + std::unique_ptr> mPromise; + + std::vector> mFingerFutures; + + /// Control access to mPromise, mInExecution + /// mProximalGoalPositions, mSpreadGoalPositin, mSpread + std::mutex mMutex; + + /// Flag for indicating execution of a command. + bool mInExecution; + + /// Values for executing a position and spread command. + Eigen::Vector3d mProximalGoalPositions; + Vector1d mSpreadGoalPosition; + + ::dart::collision::CollisionDetectorPtr mCollisionDetector; + ::dart::collision::CollisionGroupPtr mCollideWith; +}; + +using BarrettHandKinematicSimulationPositionCommandExecutorPtr = + std::shared_ptr; + + +} // control +} // aikido + +#endif diff --git a/include/aikido/control/BarrettHandPositionCommandExecutor.hpp b/include/aikido/control/BarrettHandPositionCommandExecutor.hpp deleted file mode 100644 index 019b8fb738..0000000000 --- a/include/aikido/control/BarrettHandPositionCommandExecutor.hpp +++ /dev/null @@ -1,88 +0,0 @@ -#ifndef AIKIDO_CONTROL_BARRETTHANDPOSITIONCOMANDEXECUTOR_HPP_ -#define AIKIDO_CONTROL_BARRETTHANDPOSITIONCOMANDEXECUTOR_HPP_ -#include "BarrettFingerPositionCommandExecutor.hpp" -#include "BarrettFingerSpreadCommandExecutor.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace aikido { -namespace control { - -/// Position command executor for BarrettHand fingers. -/// Assumes that fingers are underactuated: proximal joint is actuated -/// and distal joint moves with certain mimic ratio until collision. -/// If -class BarrettHandPositionCommandExecutor -{ -public: - /// Constructor. - /// \param _positionCommandExecutors 3 executors to control - /// proximal and distal joints of the fingers. The first two fingers - /// should have (spread, proximal, distal) joints. - /// Third finger should have 2 joints (proximal, distal). - /// \param _spreadCommandExecutor Executors to control - /// spreads of the fingers. - BarrettHandPositionCommandExecutor( - std::array _positionCommandExecutors, - BarrettFingerSpreadCommandExecutorPtr _spreadCommandExecutor); - - /// Set relevant variables for moving fingers. - /// In order to move the fingers, step method should be called multiple times - /// until future returns. - /// \param _goalPositions End dof pose for proximal joints and spread. - /// First 3 should be for proximal joints, the last element should be - /// for spread. If _positions are above/below joint limits, - /// the fingers will move only upto the limit. - /// \param _collideWith CollisionGroup to check collision with fingers. - /// \return Future which becomes available when the execution completes. - std::future execute( - Eigen::Matrix _goalPositions, - ::dart::collision::CollisionGroupPtr _collideWith); - - /// Calls step method of finger executors. - /// If multiple threads are accessing this function or skeleton associated - /// with this executor, it is necessary to lock the skeleton before - /// calling this method. - /// \param _timeSincePreviousCall Time since previous call. - void step(double _timeSincePreviousCall); - -private: - - constexpr static int kNumPositionExecutor = 3; - constexpr static int kNumSpreadExecutor = 1; - constexpr static auto kWaitPeriod = std::chrono::milliseconds(1); - - /// Executor for proximal and distal joints. - std::array mPositionCommandExecutors; - BarrettFingerSpreadCommandExecutorPtr mSpreadCommandExecutor; - - std::unique_ptr> mPromise; - - std::vector> mFingerFutures; - - /// Control access to mPromise, mInExecution - /// mProximalGoalPositions, mSpreadGoalPositin, mSpread - std::mutex mMutex; - - /// Flag for indicating execution of a command. - bool mInExecution; - - /// Values for executing a position and spread command. - Eigen::Vector3d mProximalGoalPositions; - double mSpreadGoalPosition; - - ::dart::collision::CollisionGroupPtr mCollideWith; - -}; - -} // control -} // aikido - -#endif diff --git a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp index 88e876b881..945129e6d2 100644 --- a/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp +++ b/include/aikido/control/KinematicSimulationTrajectoryExecutor.hpp @@ -1,8 +1,8 @@ #ifndef AIKIDO_CONTROL_KINEMATICSIMULATIONTRAJECTORYEXECUTOR_HPP_ #define AIKIDO_CONTROL_KINEMATICSIMULATIONTRAJECTORYEXECUTOR_HPP_ #include "TrajectoryExecutor.hpp" -#include "../trajectory/Trajectory.hpp" -#include "../statespace/dart/MetaSkeletonStateSpace.hpp" +#include +#include #include #include @@ -18,49 +18,39 @@ class KinematicSimulationTrajectoryExecutor : public TrajectoryExecutor { public: /// Constructor. - /// \param _skeleton Skeleton to execute trajectories on. + /// \param skeleton Skeleton to execute trajectories on. /// All trajectories must have dofs only in this skeleton. - /// \param _period Sets the cycle period of the execution thread. KinematicSimulationTrajectoryExecutor( - ::dart::dynamics::SkeletonPtr _skeleton, - std::chrono::milliseconds _period); + ::dart::dynamics::SkeletonPtr skeleton); virtual ~KinematicSimulationTrajectoryExecutor(); - /// Execute _traj and set future upon completion. - /// \param _traj Trajectory to be executed. Its StateSpace should be a + /// Execute traj and set future upon completion. + /// \param traj Trajectory to be executed. Its StateSpace should be a /// MetaStateSpace over MetaSkeleton, and the dofs in the metaskeleton - /// should be all in _skeleton passed to the constructor. + /// should be all in skeleton passed to the constructor. /// \return future for trajectory execution. If trajectory terminates /// before completion, future will be set to a runtime_error. - /// \throws invalid_argument if _traj is invalid. + /// \throws invalid_argument if traj is invalid. std::future execute( - trajectory::TrajectoryPtr _traj) override; + trajectory::TrajectoryPtr traj) override; + + // Documentation inherited. + void step() override; private: ::dart::dynamics::SkeletonPtr mSkeleton; std::unique_ptr> mPromise; trajectory::TrajectoryPtr mTraj; - - /// spin()'s trajectory execution cycle. - std::chrono::milliseconds mPeriod; - /// Blocks spin() until execute(...) is called; paired with mSpinLock. - std::condition_variable mCv; + std::chrono::system_clock::time_point mExecutionStartTime; - /// Lock for keeping spin thread alive and executing a trajectory. - /// Manages access on mTraj, mPromise, mRunning - std::mutex mSpinMutex; + /// Manages access on mTraj, mPromise, mInExecution + std::mutex mMutex; - /// Thread for spin(). - std::thread mThread; - - /// Flag for killing spin thread. - bool mRunning; + bool mInExecution; - /// Simulates mTraj. To be executed on a separate thread. - void spin(); }; } // control diff --git a/include/aikido/control/PositionCommandExecutor.hpp b/include/aikido/control/PositionCommandExecutor.hpp new file mode 100644 index 0000000000..f4e2fab805 --- /dev/null +++ b/include/aikido/control/PositionCommandExecutor.hpp @@ -0,0 +1,34 @@ +#ifndef AIKIDO_CONTROL_POSITIONCOMANDEXECUTOR_HPP_ +#define AIKIDO_CONTROL_POSITIONCOMANDEXECUTOR_HPP_ +#include +#include + +namespace aikido { +namespace control { + +/// Abstract class for executing position commands. A position command requests +/// several degrees of freedom to move to a specified configuration and provides +/// feedback when the move is complete. +class PositionCommandExecutor +{ +public: + virtual ~PositionCommandExecutor() = default; + + /// Execute hand to goalPosition + /// \param goalPositions Goal positions + /// \return future which becomes available when movement stops (which may not + /// be when the hand reaches the specified goalPositions due to e.g. + /// collision) + virtual std::future execute(const Eigen::VectorXd& goalPositions) = 0; + + // Step once. + virtual void step() = 0; +}; + +using PositionCommandExecutorPtr = std::shared_ptr; + + +} // control +} // aikido + +#endif diff --git a/include/aikido/control/TrajectoryExecutor.hpp b/include/aikido/control/TrajectoryExecutor.hpp index ff567339c7..9210fb0766 100644 --- a/include/aikido/control/TrajectoryExecutor.hpp +++ b/include/aikido/control/TrajectoryExecutor.hpp @@ -13,10 +13,13 @@ class TrajectoryExecutor virtual ~TrajectoryExecutor() = default; - /// Execute _traj and set future upon completion. - /// \param _traj Trajectory to be executed. + /// Execute traj and set future upon completion. + /// \param traj Trajectory to be executed. virtual std::future execute( trajectory::TrajectoryPtr _traj) = 0; + + /// Executes one step. + virtual void step() = 0; }; using TrajectoryExecutorPtr = std::shared_ptr; diff --git a/src/control/BarrettFingerKinematicSimulationPositionCommandExecutor.cpp b/src/control/BarrettFingerKinematicSimulationPositionCommandExecutor.cpp new file mode 100644 index 0000000000..4cda5c21c0 --- /dev/null +++ b/src/control/BarrettFingerKinematicSimulationPositionCommandExecutor.cpp @@ -0,0 +1,241 @@ +#include +#include +#include + +namespace aikido{ +namespace control{ + +//============================================================================= +BarrettFingerKinematicSimulationPositionCommandExecutor +::BarrettFingerKinematicSimulationPositionCommandExecutor( + ::dart::dynamics::ChainPtr finger, size_t proximal, size_t distal, + ::dart::collision::CollisionDetectorPtr collisionDetector, + ::dart::collision::CollisionGroupPtr collideWith, + ::dart::collision::CollisionOption collisionOptions) +: mFinger(std::move(finger)) +, mProximalDof(nullptr) +, mDistalDof(nullptr) +, mCollisionDetector(std::move(collisionDetector)) +, mCollideWith(std::move(collideWith)) +, mCollisionOptions(std::move(collisionOptions)) +, mInExecution(false) +{ + if (!mFinger) + throw std::invalid_argument("Finger is null."); + + if (proximal == distal) + throw std::invalid_argument("proximal and distal dofs should be different."); + + const auto numDofs = mFinger->getNumDofs(); + + if (proximal < numDofs) + mProximalDof = mFinger->getDof(proximal); + + if (distal < numDofs) + mDistalDof = mFinger->getDof(distal); + + if (!mProximalDof) + throw std::invalid_argument("Finger does not have proximal dof."); + + if (!mDistalDof) + throw std::invalid_argument("Finger does not have distal dof."); + + if (mCollisionDetector && mCollideWith) + { + // If a collision group is given and its collision detector does not match + // mCollisionDetector, set the collision group to an empty collision group. + if (mCollisionDetector != mCollideWith->getCollisionDetector()) + { + std::cerr << "[BarrettFingerKinematicSimulationPositionCommandExecutor] " + << "CollisionDetector of type " << mCollisionDetector->getType() + << " does not match CollisionGroup's CollisionDetector of type " + << mCollideWith->getCollisionDetector()->getType() << std::endl; + + ::dart::collision::CollisionGroupPtr newCollideWith = + mCollisionDetector->createCollisionGroup(); + for (auto i = 0u; i < mCollideWith->getNumShapeFrames(); ++i) + newCollideWith->addShapeFrame(mCollideWith->getShapeFrame(i)); + mCollideWith = std::move(newCollideWith); + } + } + else if (mCollisionDetector && !mCollideWith) + { + mCollideWith = mCollisionDetector->createCollisionGroup(); + } + else if (!mCollisionDetector && mCollideWith) + { + mCollisionDetector = mCollideWith->getCollisionDetector(); + } + else + { + // Default mCollisionDetector to FCL collision detector and mCollideWith to + // empty collision group. + mCollisionDetector = dart::collision::FCLCollisionDetector::create(); + mCollideWith = mCollisionDetector->createCollisionGroup(); + } + + mProximalCollisionGroup = mCollisionDetector->createCollisionGroup( + mProximalDof->getChildBodyNode()); + + mDistalCollisionGroup = mCollisionDetector->createCollisionGroup( + mDistalDof->getChildBodyNode()); + + mProximalLimits = mProximalDof->getPositionLimits(); + mDistalLimits = mDistalDof->getPositionLimits(); +} + +//============================================================================= +std::future BarrettFingerKinematicSimulationPositionCommandExecutor +::execute(const Eigen::VectorXd& goalPosition) +{ + if (!mFinger->isAssembled()) + throw std::runtime_error("Finger is disassembled."); + + { + std::lock_guard lock(mMutex); + double goalPositionValue = goalPosition[0]; + + if (mInExecution) + throw std::runtime_error("Another command in execution."); + + mPromise.reset(new std::promise()); + mInExecution = true; + mDistalOnly = false; + mTimeOfPreviousCall = std::chrono::system_clock::now(); + + // Set mProximalGoalPosition. + if (goalPositionValue < mProximalLimits.first) + mProximalGoalPosition = mProximalLimits.first; + else if (goalPositionValue > mProximalLimits.second) + mProximalGoalPosition = mProximalLimits.second; + else + mProximalGoalPosition = goalPositionValue; + + return mPromise->get_future(); + } +} + +//============================================================================= +void BarrettFingerKinematicSimulationPositionCommandExecutor::terminate() +{ + mPromise->set_value(); + mInExecution = false; +} + +//============================================================================= +void BarrettFingerKinematicSimulationPositionCommandExecutor::step() +{ + using namespace std::chrono; + + std::lock_guard lock(mMutex); + + auto timeSincePreviousCall = system_clock::now() - mTimeOfPreviousCall; + mTimeOfPreviousCall = system_clock::now(); + auto period = duration(timeSincePreviousCall).count(); + + if (!mInExecution) + return; + + double distalPosition = mDistalDof->getPosition(); + double proximalPosition = mProximalDof->getPosition(); + + // Check distal collision + bool distalCollision = mCollisionDetector->collide( + mDistalCollisionGroup.get(), mCollideWith.get(), + mCollisionOptions, nullptr); + + if (distalCollision) + { + terminate(); + return; + } + + double newDistal; + bool distalLimitReached = false; + + if (proximalPosition < mProximalGoalPosition) + { + newDistal = distalPosition + period*kDistalSpeed; + if (mDistalLimits.second <= newDistal) + { + newDistal = mDistalLimits.second; + distalLimitReached = true; + } + } + else + { + newDistal = distalPosition - period*kDistalSpeed; + if (mDistalLimits.first >= newDistal) + { + newDistal = mDistalLimits.first; + distalLimitReached = true; + } + } + + mDistalDof->setPosition(newDistal); + + if (distalLimitReached) + { + terminate(); + return; + } + + if (mDistalOnly) + return; + + // Check proximal collision + bool proximalCollision = mCollisionDetector->collide( + mProximalCollisionGroup.get(), mCollideWith.get(), + mCollisionOptions, nullptr); + + if (proximalCollision){ + mDistalOnly = true; + return; + } + + double newProximal; + bool proximalGoalReached = false; + if (proximalPosition < mProximalGoalPosition) + { + newProximal = proximalPosition + period*kProximalSpeed; + if (mProximalGoalPosition <= newProximal) + { + newProximal = mProximalGoalPosition; + proximalGoalReached = true; + } + } + else + { + newProximal = proximalPosition - period*kProximalSpeed; + if (mProximalGoalPosition >= newProximal) + { + newProximal = mProximalGoalPosition; + proximalGoalReached = true; + } + } + + mProximalDof->setPosition(newProximal); + if (proximalGoalReached) + { + terminate(); + return; + } + +} + +//============================================================================= +bool BarrettFingerKinematicSimulationPositionCommandExecutor::setCollideWith( + ::dart::collision::CollisionGroupPtr collideWith) +{ + std::lock_guard lockSpin(mMutex); + + if (mInExecution) + return false; + + mCollideWith = std::move(collideWith); + mCollisionDetector = mCollideWith->getCollisionDetector(); + return true; +} + +} // control +} // aikido diff --git a/src/control/BarrettFingerKinematicSimulationSpreadCommandExecutor.cpp b/src/control/BarrettFingerKinematicSimulationSpreadCommandExecutor.cpp new file mode 100644 index 0000000000..cb211fef27 --- /dev/null +++ b/src/control/BarrettFingerKinematicSimulationSpreadCommandExecutor.cpp @@ -0,0 +1,225 @@ +#include +#include +#include +#include + +namespace aikido{ +namespace control{ + +//============================================================================= +BarrettFingerKinematicSimulationSpreadCommandExecutor::BarrettFingerKinematicSimulationSpreadCommandExecutor( + std::array<::dart::dynamics::ChainPtr, 2> fingers, size_t spread, + ::dart::collision::CollisionDetectorPtr collisionDetector, + ::dart::collision::CollisionGroupPtr collideWith, + ::dart::collision::CollisionOption collisionOptions) +: mFingers(std::move(fingers)) +, mCollisionDetector(std::move(collisionDetector)) +, mCollideWith(std::move(collideWith)) +, mCollisionOptions(std::move(collisionOptions)) +, mInExecution(false) +{ + if (mFingers.size() != kNumFingers) + { + std::stringstream msg; + msg << "Expecting " << kNumFingers << " fingers;" + << "got << " << mFingers.size() << "."; + throw std::invalid_argument(msg.str()); + } + + mSpreadDofs.reserve(kNumFingers); + for (int i=0; i < kNumFingers; ++i) + { + if(!mFingers[i]) + { + std::stringstream msg; + msg << i << "th finger is null."; + throw std::invalid_argument(msg.str()); + } + + const auto numDofs = mFingers[i]->getNumDofs(); + if (static_cast(spread) >= numDofs) + throw std::invalid_argument("Finger does not have spread dof."); + + mSpreadDofs.push_back(mFingers[i]->getDof(spread)); + if (!mSpreadDofs[i]){ + std::stringstream msg; + msg << i << "th finger does not have spread dof."; + throw std::invalid_argument(msg.str()); + } + } + + if (mCollisionDetector && mCollideWith) + { + // If a collision group is given and its collision detector does not match + // mCollisionDetector, set the collision group to an empty collision group. + if (mCollisionDetector != mCollideWith->getCollisionDetector()) + { + std::cerr << "[BarrettHandKinematicSimulationPositionCommandExecutor] " + << "CollisionDetector of type " << mCollisionDetector->getType() + << " does not match CollisionGroup's CollisionDetector of type " + << mCollideWith->getCollisionDetector()->getType() << std::endl; + + ::dart::collision::CollisionGroupPtr newCollideWith = + mCollisionDetector->createCollisionGroup(); + for (auto i = 0u; i < mCollideWith->getNumShapeFrames(); ++i) + newCollideWith->addShapeFrame(mCollideWith->getShapeFrame(i)); + mCollideWith = std::move(newCollideWith); + } + } + else if (mCollisionDetector && !mCollideWith) + { + mCollideWith = mCollisionDetector->createCollisionGroup(); + } + else if (!mCollisionDetector && mCollideWith) + { + mCollisionDetector = mCollideWith->getCollisionDetector(); + } + else + { + // Default mCollisionDetector to FCL collision detector and mCollideWith to + // empty collision group. + mCollisionDetector = dart::collision::FCLCollisionDetector::create(); + mCollideWith = mCollisionDetector->createCollisionGroup(); + } + + mSpreadCollisionGroup = mCollisionDetector->createCollisionGroup(); + for (auto finger: mFingers) + { + for (auto body: finger->getBodyNodes()) + mSpreadCollisionGroup->addShapeFramesOf(body); + } + + mDofLimits = mSpreadDofs[0]->getPositionLimits(); + + for (size_t i = 1; i < kNumFingers; ++i) + { + auto limits = mSpreadDofs[i]->getPositionLimits(); + if (std::abs(limits.first - mDofLimits.first) > kDofTolerance) + { + std::stringstream msg; + msg << "LowerJointLimit for the fingers should be the same. " + << "Expecting " << mDofLimits.first << "; got " << limits.first; + throw std::invalid_argument(msg.str()); + } + + if (std::abs(limits.second - mDofLimits.second) > kDofTolerance) + { + std::stringstream msg; + msg << "UpperJointLimit for the fingers should be the same. " + << "Expecting " << mDofLimits.second << "; got " << limits.second; + throw std::invalid_argument(msg.str()); + } + } +} + +//============================================================================= +std::future BarrettFingerKinematicSimulationSpreadCommandExecutor +::execute(const Eigen::VectorXd& goalPosition) +{ + for (size_t i = 0; i < kNumFingers; ++i) + { + if (!mFingers[i]->isAssembled()) + { + std::stringstream msg; + msg << i << "th finger is no longer linked."; + throw std::runtime_error(msg.str()); + } + } + + { + std::lock_guard lock(mMutex); + double goalPositionValue = goalPosition[0]; + + if (mInExecution) + throw std::runtime_error("Another command in execution."); + + mPromise.reset(new std::promise()); + mTimeOfPreviousCall = std::chrono::system_clock::now(); + + if (goalPositionValue < mDofLimits.first) + mGoalPosition = mDofLimits.first; + else if (goalPositionValue > mDofLimits.second) + mGoalPosition = mDofLimits.second; + else + mGoalPosition = goalPositionValue; + + mInExecution = true; + + return mPromise->get_future(); + } +} + +//============================================================================= +void BarrettFingerKinematicSimulationSpreadCommandExecutor::step() +{ + using namespace std::chrono; + + // Terminate the thread if mRunning is false. + std::lock_guard lock(mMutex); + + auto timeSincePreviousCall = system_clock::now() - mTimeOfPreviousCall; + mTimeOfPreviousCall = system_clock::now(); + auto period = duration(timeSincePreviousCall).count(); + + if (!mInExecution) + return; + + // Current spread. Check that all spreads have same values. + double spread = mSpreadDofs[0]->getPosition(); + for(size_t i = 1; i < kNumFingers; ++i) + { + double _spread = mSpreadDofs[i]->getPosition(); + if (std::abs(spread - _spread) > kDofTolerance) + { + std::stringstream msg; + msg << i << "th finger dof value not equal to 0th finger dof. " + << "Expecting " << spread << ", got " << spread; + auto expr = std::make_exception_ptr(std::runtime_error(msg.str())); + mPromise->set_exception(expr); + mInExecution = false; + return; + } + } + + // Check collision + bool collision = mCollisionDetector->collide( + mSpreadCollisionGroup.get(), mCollideWith.get(), + mCollisionOptions, nullptr); + + // Termination condition + if (collision || std::abs(spread - mGoalPosition) < kTolerance) + { + mPromise->set_value(); + mInExecution = false; + return; + } + + // Move spread + double newSpread; + if (spread > mGoalPosition) + newSpread = std::max(mGoalPosition, + spread + -kDofSpeed*period); + else + newSpread = std::min(mGoalPosition, + spread + kDofSpeed*period); + + for (auto spreadDof: mSpreadDofs) + spreadDof->setPosition(newSpread); +} + +//============================================================================= +bool BarrettFingerKinematicSimulationSpreadCommandExecutor::setCollideWith( + ::dart::collision::CollisionGroupPtr collideWith) +{ + std::lock_guard lockSpin(mMutex); + + if (mInExecution) + return false; + + mCollideWith = std::move(collideWith); + mCollisionDetector = mCollideWith->getCollisionDetector(); + return true; +} + +} // control +} // aikido diff --git a/src/control/BarrettFingerPositionCommandExecutor.cpp b/src/control/BarrettFingerPositionCommandExecutor.cpp deleted file mode 100644 index 881459e806..0000000000 --- a/src/control/BarrettFingerPositionCommandExecutor.cpp +++ /dev/null @@ -1,197 +0,0 @@ -#include -#include - -namespace aikido{ -namespace control{ - -//============================================================================= -BarrettFingerPositionCommandExecutor::BarrettFingerPositionCommandExecutor( - ::dart::dynamics::ChainPtr _finger, int _proximal, int _distal, - ::dart::collision::CollisionDetectorPtr _collisionDetector, - ::dart::collision::CollisionOption _collisionOptions) -: mFinger(std::move(_finger)) -, mProximalDof(nullptr) -, mDistalDof(nullptr) -, mCollisionDetector(std::move(_collisionDetector)) -, mCollisionOptions(std::move(_collisionOptions)) -, mInExecution(false) -{ - if (!mFinger) - throw std::invalid_argument("Finger is null."); - - if (_proximal == _distal) - throw std::invalid_argument("proximal and distal dofs should be different."); - - const auto numDofs = mFinger->getNumDofs(); - - if (static_cast(_proximal) < numDofs) - mProximalDof = mFinger->getDof(_proximal); - - if (static_cast(_distal) < numDofs) - mDistalDof = mFinger->getDof(_distal); - - if (!mProximalDof) - throw std::invalid_argument("Finger does not have proximal dof."); - - if (!mDistalDof) - throw std::invalid_argument("Finger does not have distal dof."); - - if (!mCollisionDetector) - throw std::invalid_argument("CollisionDetctor is null."); - - mProximalCollisionGroup = mCollisionDetector->createCollisionGroup( - mProximalDof->getChildBodyNode()); - - mDistalCollisionGroup = mCollisionDetector->createCollisionGroup( - mDistalDof->getChildBodyNode()); - - mProximalLimits = mProximalDof->getPositionLimits(); - mDistalLimits = mDistalDof->getPositionLimits(); -} - -//============================================================================= -double BarrettFingerPositionCommandExecutor::getMimicRatio() -{ - return kMimicRatio; -} - -//============================================================================= -std::future BarrettFingerPositionCommandExecutor::execute( - double _goalPosition, ::dart::collision::CollisionGroupPtr _collideWith) -{ - if (!_collideWith) - throw std::invalid_argument("CollideWith is null."); - - if (!mFinger->isAssembled()) - throw std::runtime_error("Finger is disassembled."); - - { - std::lock_guard lock(mMutex); - - if (mInExecution) - throw std::runtime_error("Another command in execution."); - - mPromise.reset(new std::promise()); - mCollideWith = _collideWith; - mInExecution = true; - mDistalOnly = false; - - // Set mProximalGoalPosition. - if (_goalPosition < mProximalLimits.first) - mProximalGoalPosition = mProximalLimits.first; - else if (_goalPosition > mProximalLimits.second) - mProximalGoalPosition = mProximalLimits.second; - else - mProximalGoalPosition = _goalPosition; - - return mPromise->get_future(); - } -} - -//============================================================================= -void BarrettFingerPositionCommandExecutor::terminate() -{ - mPromise->set_value(); - mInExecution = false; - mCollideWith.reset(); -} -//============================================================================= -void BarrettFingerPositionCommandExecutor::step(double _timeSincePreviousCall) -{ - using std::chrono::milliseconds; - - std::lock_guard lock(mMutex); - - if (!mInExecution) - return; - - double distalPosition = mDistalDof->getPosition(); - double proximalPosition = mProximalDof->getPosition(); - - // Check distal collision - ::dart::collision::CollisionResult collisionResult; - bool distalCollision = mCollisionDetector->collide( - mDistalCollisionGroup.get(), mCollideWith.get(), - mCollisionOptions, &collisionResult); - - if (distalCollision) - { - terminate(); - return; - } - - double newDistal; - bool distalLimitReached = false; - - if (proximalPosition < mProximalGoalPosition) - { - newDistal = distalPosition + _timeSincePreviousCall*kDistalVelocity; - if (mDistalLimits.second <= newDistal) - { - newDistal = mDistalLimits.second; - distalLimitReached = true; - } - } - else - { - newDistal = distalPosition - _timeSincePreviousCall*kDistalVelocity; - if (mDistalLimits.first >= newDistal) - { - newDistal = mDistalLimits.first; - distalLimitReached = true; - } - } - - mDistalDof->setPosition(newDistal); - - if (distalLimitReached) - { - terminate(); - return; - } - - if (mDistalOnly) - return; - - // Check proximal collision - bool proximalCollision = mCollisionDetector->collide( - mProximalCollisionGroup.get(), mCollideWith.get(), - mCollisionOptions, &collisionResult); - - if (proximalCollision){ - mDistalOnly = true; - return; - } - - double newProximal; - bool proximalGoalReached = false; - if (proximalPosition < mProximalGoalPosition) - { - newProximal = proximalPosition + _timeSincePreviousCall*kProximalVelocity; - if (mProximalGoalPosition <= newProximal) - { - newProximal = mProximalGoalPosition; - proximalGoalReached = true; - } - } - else - { - newProximal = proximalPosition - _timeSincePreviousCall*kProximalVelocity; - if (mProximalGoalPosition >= newProximal) - { - newProximal = mProximalGoalPosition; - proximalGoalReached = true; - } - } - - mProximalDof->setPosition(newProximal); - - if (proximalGoalReached) - { - terminate(); - return; - } -} - -} -} diff --git a/src/control/BarrettFingerSpreadCommandExecutor.cpp b/src/control/BarrettFingerSpreadCommandExecutor.cpp deleted file mode 100644 index 06dadf60db..0000000000 --- a/src/control/BarrettFingerSpreadCommandExecutor.cpp +++ /dev/null @@ -1,176 +0,0 @@ -#include -#include -#include - -namespace aikido{ -namespace control{ - -//============================================================================= -BarrettFingerSpreadCommandExecutor::BarrettFingerSpreadCommandExecutor( - std::array<::dart::dynamics::ChainPtr, 2> _fingers, int _spread, - ::dart::collision::CollisionDetectorPtr _collisionDetector, - ::dart::collision::CollisionOption _collisionOptions) -: mFingers(std::move(_fingers)) -, mCollisionDetector(std::move(_collisionDetector)) -, mCollisionOptions(std::move(_collisionOptions)) -, mInExecution(false) -{ - if (mFingers.size() != kNumFingers) - { - std::stringstream msg; - msg << "Expecting " << kNumFingers << " fingers;" - << "got << " << mFingers.size() << "."; - throw std::invalid_argument(msg.str()); - } - - mSpreadDofs.reserve(kNumFingers); - for (int i=0; i < kNumFingers; ++i) - { - if(!mFingers[i]) - { - std::stringstream msg; - msg << i << "th finger is null."; - throw std::invalid_argument(msg.str()); - } - - const auto numDofs = mFingers[i]->getNumDofs(); - if (static_cast(_spread) >= numDofs) - throw std::invalid_argument("Finger does not have spread dof."); - - mSpreadDofs.push_back(mFingers[i]->getDof(_spread)); - if (!mSpreadDofs[i]){ - std::stringstream msg; - msg << i << "th finger does not have spread dof."; - throw std::invalid_argument(msg.str()); - } - } - - if (!mCollisionDetector) - throw std::invalid_argument("CollisionDetector is null."); - - mSpreadCollisionGroup = mCollisionDetector->createCollisionGroup(); - for (auto finger: mFingers) - { - for (auto body: finger->getBodyNodes()) - mSpreadCollisionGroup->addShapeFramesOf(body); - } - - mDofLimits = mSpreadDofs[0]->getPositionLimits(); - - for (int i=1; i < kNumFingers; ++i) - { - auto limits = mSpreadDofs[i]->getPositionLimits(); - if (std::abs(limits.first - mDofLimits.first) > kDofTolerance) - { - std::stringstream msg; - msg << "LowerJointLimit for the fingers should be the same. " - << "Expecting " << mDofLimits.first << "; got " << limits.first; - throw std::invalid_argument(msg.str()); - } - - if (std::abs(limits.second - mDofLimits.second) > kDofTolerance) - { - std::stringstream msg; - msg << "UpperJointLimit for the fingers should be the same. " - << "Expecting " << mDofLimits.second << "; got " << limits.second; - throw std::invalid_argument(msg.str()); - } - } -} - -//============================================================================= -std::future BarrettFingerSpreadCommandExecutor::execute( - double _goalPosition, - ::dart::collision::CollisionGroupPtr _collideWith) -{ - if (!_collideWith) - throw std::invalid_argument("CollideWith is null."); - - for(int i=0; i < kNumFingers; ++i) - { - if (!mFingers[i]->isAssembled()) - { - std::stringstream msg; - msg << i << "th finger is no longer linked."; - throw std::runtime_error(msg.str()); - } - } - - { - std::lock_guard lock(mMutex); - - if (mInExecution) - throw std::runtime_error("Another command in execution."); - - mPromise.reset(new std::promise()); - - if (_goalPosition < mDofLimits.first) - mGoalPosition = mDofLimits.first; - else if (_goalPosition > mDofLimits.second) - mGoalPosition = mDofLimits.second; - else - mGoalPosition = _goalPosition; - - mCollideWith = _collideWith; - mInExecution = true; - - return mPromise->get_future(); - } -} - -//============================================================================= -void BarrettFingerSpreadCommandExecutor::step(double _timeSincePreviousCall) -{ - using std::chrono::milliseconds; - - // Terminate the thread if mRunning is false. - std::lock_guard lock(mMutex); - - if (!mInExecution) - return; - - // Current spread. Check that all spreads have same values. - double spread = mSpreadDofs[0]->getPosition(); - for(int i=1; i < kNumFingers; ++i) - { - double _spread = mSpreadDofs[i]->getPosition(); - if (std::abs(spread - _spread) > kDofTolerance) - { - std::stringstream msg; - msg << i << "th finger dof value not equal to 0th finger dof. " - << "Expecting " << spread << ", got " << _spread; - auto expr = std::make_exception_ptr(std::runtime_error(msg.str())); - mPromise->set_exception(expr); - mInExecution = false; - return; - } - } - - // Check collision - ::dart::collision::CollisionResult collisionResult; - bool collision = mCollisionDetector->collide( - mSpreadCollisionGroup.get(), mCollideWith.get(), - mCollisionOptions, &collisionResult); - - // Termination condition - if (collision || std::abs(spread - mGoalPosition) < kTolerance) - { - mPromise->set_value(); - mInExecution = false; - return; - } - - // Move spread - double newSpread; - if (spread > mGoalPosition) - newSpread = std::max(mGoalPosition, spread + -kDofVelocity*_timeSincePreviousCall); - else - newSpread = std::min(mGoalPosition, spread + kDofVelocity*_timeSincePreviousCall); - - for (auto spreadDof: mSpreadDofs) - spreadDof->setPosition(newSpread); - -} - -} -} diff --git a/src/control/BarrettHandKinematicSimulationPositionCommandExecutor.cpp b/src/control/BarrettHandKinematicSimulationPositionCommandExecutor.cpp new file mode 100644 index 0000000000..ad5445df88 --- /dev/null +++ b/src/control/BarrettHandKinematicSimulationPositionCommandExecutor.cpp @@ -0,0 +1,221 @@ +#include +#include +#include +#include +#include + +namespace aikido{ +namespace control{ + +constexpr std::chrono::milliseconds BarrettHandKinematicSimulationPositionCommandExecutor::kWaitPeriod; + +//============================================================================= +BarrettHandKinematicSimulationPositionCommandExecutor +::BarrettHandKinematicSimulationPositionCommandExecutor( + dart::dynamics::SkeletonPtr robot, + const std::string& prefix, + ::dart::collision::CollisionDetectorPtr collisionDetector, + ::dart::collision::CollisionGroupPtr collideWith) +: mInExecution(false) +, mCollisionDetector(std::move(collisionDetector)) +, mCollideWith(std::move(collideWith)) +{ + if (mCollisionDetector && mCollideWith) + { + // If a collision group is given and its collision detector does not match + // mCollisionDetector, set the collision group to an empty collision group. + if (mCollisionDetector != mCollideWith->getCollisionDetector()) + { + std::cerr << "[BarrettHandKinematicSimulationPositionCommandExecutor] " + << "CollisionDetector of type " << mCollisionDetector->getType() + << " does not match CollisionGroup's CollisionDetector of type " + << mCollideWith->getCollisionDetector()->getType() << std::endl; + + ::dart::collision::CollisionGroupPtr newCollideWith = + mCollisionDetector->createCollisionGroup(); + for (auto i = 0u; i < mCollideWith->getNumShapeFrames(); ++i) + newCollideWith->addShapeFrame(mCollideWith->getShapeFrame(i)); + mCollideWith = std::move(newCollideWith); + } + } + else if (mCollisionDetector && !mCollideWith) + { + mCollideWith = mCollisionDetector->createCollisionGroup(); + } + else if (!mCollisionDetector && mCollideWith) + { + mCollisionDetector = mCollideWith->getCollisionDetector(); + } + else + { + // Default mCollisionDetector to FCL collision detector and mCollideWith to + // empty collision group. + mCollisionDetector = dart::collision::FCLCollisionDetector::create(); + mCollideWith = mCollisionDetector->createCollisionGroup(); + } + + setupExecutors(std::move(robot), prefix); +} + +//============================================================================= +void BarrettHandKinematicSimulationPositionCommandExecutor::setupExecutors( + dart::dynamics::SkeletonPtr robot, + const std::string& prefix) +{ + using dart::dynamics::Chain; + using dart::dynamics::ChainPtr; + using FingerPositionCommandExecutor + = aikido::control::BarrettFingerKinematicSimulationPositionCommandExecutor; + using FingerSpreadCommandExecutor + = aikido::control::BarrettFingerKinematicSimulationSpreadCommandExecutor; + + if (prefix != "/left/" && prefix != "/right/") + { + std::stringstream message; + message << "Invalid prefix '" << prefix << "', " + << "must be either '/left/' or '/right/'"; + throw std::runtime_error(message.str()); + } + + const auto fingerChains = std::array { + Chain::create(robot->getBodyNode(prefix + "finger0_0"), // finger0Spread + robot->getBodyNode(prefix + "finger0_2"), // finger0Distal + Chain::IncludeBoth), + Chain::create(robot->getBodyNode(prefix + "finger1_0"), // finger1Spread + robot->getBodyNode(prefix + "finger1_2"), // finger1Distal + Chain::IncludeBoth), + Chain::create(robot->getBodyNode(prefix + "finger2_1"), // finger2Primal + robot->getBodyNode(prefix + "finger2_2"), // finger2Distal + Chain::IncludeBoth), + }; + + const auto spreadFingers = std::array { + fingerChains[0], + fingerChains[1] + }; + + size_t spreadDof = 0; + mSpreadCommandExecutor = std::make_shared( + spreadFingers, spreadDof, mCollisionDetector, mCollideWith); + + constexpr auto primalDof = std::array {1, 1, 0}; + constexpr auto distalDof = std::array {2, 2, 1}; + for (size_t i = 0; i < fingerChains.size(); ++i) + { + mPositionCommandExecutors[i] = std::make_shared( + fingerChains[i], primalDof[i], distalDof[i], + mCollisionDetector, mCollideWith); + } +} + +//============================================================================= +std::future BarrettHandKinematicSimulationPositionCommandExecutor +::execute(const Eigen::VectorXd& goalPositions) +{ + std::lock_guard lockSpin(mMutex); + + if (mInExecution) + throw std::runtime_error("Another command in execution."); + + if (goalPositions.size() != 4) + { + std::stringstream message; + message << "GoalPositions must have 4 elements, but [" + << goalPositions.size() << "] given."; + throw std::invalid_argument(message.str()); + } + + mPromise.reset(new std::promise()); + mProximalGoalPositions = goalPositions.head<3>(); + mSpreadGoalPosition = goalPositions.row(3); + mInExecution = true; + mFingerFutures.clear(); + + mFingerFutures.reserve(kNumPositionExecutor + kNumSpreadExecutor); + for(size_t i = 0; i < kNumPositionExecutor; ++i) + mFingerFutures.emplace_back( + mPositionCommandExecutors[i]->execute( + mProximalGoalPositions.row(i))); + + mFingerFutures.emplace_back( + mSpreadCommandExecutor->execute( + mSpreadGoalPosition)); + + return mPromise->get_future(); + +} + +//============================================================================= +void BarrettHandKinematicSimulationPositionCommandExecutor::step() +{ + std::lock_guard lock(mMutex); + + if (!mInExecution) + return; + + // Check whether fingers have completed. + std::vector statuses; + std::exception_ptr expr; + bool allFingersCompleted = true; + + for(size_t i = 0; i < mFingerFutures.size(); ++i) + { + // Check the status of each finger + auto status = mFingerFutures[i].wait_for(kWaitPeriod); + if (status != std::future_status::ready) + { + allFingersCompleted = false; + break; + } + } + + if (allFingersCompleted) + { + for(size_t i = 0; i < mFingerFutures.size(); ++i) + { + try{ + mFingerFutures[i].get(); + } + catch (...){ + expr = std::current_exception(); + break; + } + } + } + + // Termination condition + if (expr || allFingersCompleted) + { + if (expr) + mPromise->set_exception(expr); + else if (allFingersCompleted) + mPromise->set_value(); + + mInExecution = false; + return; + } + + // Call the finger executors' step function. + for(int i=0; i < kNumPositionExecutor; ++i) + mPositionCommandExecutors[i]->step(); + + mSpreadCommandExecutor->step(); + +} + +//============================================================================= +bool BarrettHandKinematicSimulationPositionCommandExecutor::setCollideWith( + ::dart::collision::CollisionGroupPtr collideWith) +{ + std::lock_guard lockSpin(mMutex); + + if (mInExecution) + return false; + + mCollideWith = std::move(collideWith); + mCollisionDetector = mCollideWith->getCollisionDetector(); + return true; +} + +} // control +} // aikido diff --git a/src/control/BarrettHandPositionCommandExecutor.cpp b/src/control/BarrettHandPositionCommandExecutor.cpp deleted file mode 100644 index ebd9c76f80..0000000000 --- a/src/control/BarrettHandPositionCommandExecutor.cpp +++ /dev/null @@ -1,128 +0,0 @@ -#include -#include -#include -#include - -namespace aikido{ -namespace control{ - -constexpr std::chrono::milliseconds BarrettHandPositionCommandExecutor::kWaitPeriod; - -//============================================================================= -BarrettHandPositionCommandExecutor::BarrettHandPositionCommandExecutor( - std::array _positionCommandExecutors, - BarrettFingerSpreadCommandExecutorPtr _spreadCommandExecutor) -: mPositionCommandExecutors(std::move(_positionCommandExecutors)) -, mSpreadCommandExecutor(std::move(_spreadCommandExecutor)) -, mInExecution(false) -{ - for(int i=0; i < kNumPositionExecutor; ++i) - { - if (!mPositionCommandExecutors[i]) - { - std::stringstream msg; - msg << i << "th PositionCommandExecutor is null."; - throw std::invalid_argument(msg.str()); - } - } - - if (!mSpreadCommandExecutor) - throw std::invalid_argument("SpreadCommandExecutor is null."); -} - -//============================================================================= -std::future BarrettHandPositionCommandExecutor::execute( - Eigen::Matrix _positions, - ::dart::collision::CollisionGroupPtr _collideWith) -{ - if (!_collideWith) - throw std::invalid_argument("CollideWith is null."); - - std::lock_guard lockSpin(mMutex); - - if (mInExecution) - throw std::runtime_error("Another command in execution."); - - mPromise.reset(new std::promise()); - mProximalGoalPositions = _positions.topRows(3); - mSpreadGoalPosition = _positions(3); - mCollideWith = _collideWith; - mInExecution = true; - mFingerFutures.clear(); - - mFingerFutures.reserve(kNumPositionExecutor + kNumSpreadExecutor); - for(int i=0; i < kNumPositionExecutor; ++i) - mFingerFutures.emplace_back( - mPositionCommandExecutors[i]->execute( - mProximalGoalPositions(i), mCollideWith)); - - mFingerFutures.emplace_back( - mSpreadCommandExecutor->execute( - mSpreadGoalPosition, mCollideWith)); - - return mPromise->get_future(); - -} - -//============================================================================= -void BarrettHandPositionCommandExecutor::step(double _timeSincePreviousCall) -{ - using std::chrono::milliseconds; - - std::lock_guard lock(mMutex); - - if (!mInExecution) - return; - - // Check whether fingers have completed. - std::vector statuses; - std::exception_ptr expr; - bool allFingersCompleted = true; - - for(size_t i = 0; i < mFingerFutures.size(); ++i) - { - // Check the status of each finger - auto status = mFingerFutures[i].wait_for(kWaitPeriod); - if (status != std::future_status::ready) - { - allFingersCompleted = false; - break; - } - } - - if (allFingersCompleted) - { - for(size_t i = 0; i < mFingerFutures.size(); ++i) - { - try{ - mFingerFutures[i].get(); - } - catch (...){ - expr = std::current_exception(); - break; - } - } - } - - // Termination condition - if (expr || allFingersCompleted) - { - if (expr) - mPromise->set_exception(expr); - else if (allFingersCompleted) - mPromise->set_value(); - - mInExecution = false; - return; - } - - // Call the finger executors' step function. - for(int i=0; i < kNumPositionExecutor; ++i) - mPositionCommandExecutors[i]->step(_timeSincePreviousCall); - - mSpreadCommandExecutor->step(_timeSincePreviousCall); - -} - -} -} diff --git a/src/control/CMakeLists.txt b/src/control/CMakeLists.txt index 7bd818b3ed..9823241c94 100644 --- a/src/control/CMakeLists.txt +++ b/src/control/CMakeLists.txt @@ -1,8 +1,8 @@ set(sources KinematicSimulationTrajectoryExecutor.cpp - BarrettHandPositionCommandExecutor.cpp - BarrettFingerPositionCommandExecutor.cpp - BarrettFingerSpreadCommandExecutor.cpp + BarrettHandKinematicSimulationPositionCommandExecutor.cpp + BarrettFingerKinematicSimulationPositionCommandExecutor.cpp + BarrettFingerKinematicSimulationSpreadCommandExecutor.cpp ) add_library("${PROJECT_NAME}_control" SHARED ${sources}) diff --git a/src/control/KinematicSimulationTrajectoryExecutor.cpp b/src/control/KinematicSimulationTrajectoryExecutor.cpp index ed513c1e1f..2a127d9098 100644 --- a/src/control/KinematicSimulationTrajectoryExecutor.cpp +++ b/src/control/KinematicSimulationTrajectoryExecutor.cpp @@ -5,59 +5,45 @@ #include using aikido::statespace::SO2; +using aikido::statespace::dart::MetaSkeletonStateSpace; +using std::chrono::duration_cast; +using std::chrono::milliseconds; +using std::chrono::seconds; namespace aikido{ namespace control{ //============================================================================= KinematicSimulationTrajectoryExecutor::KinematicSimulationTrajectoryExecutor( - ::dart::dynamics::SkeletonPtr _skeleton, - std::chrono::milliseconds _period) -: mSkeleton(std::move(_skeleton)) + ::dart::dynamics::SkeletonPtr skeleton) +: mSkeleton(std::move(skeleton)) , mPromise(nullptr) , mTraj(nullptr) -, mPeriod(_period) -, mSpinMutex() -, mRunning(true) - - +, mMutex() +, mInExecution(false) { - using std::chrono::duration; - using std::chrono::duration_cast; - using std::chrono::milliseconds; - if (!mSkeleton) throw std::invalid_argument("Skeleton is null."); - - if (mPeriod.count() <= 0) - throw std::invalid_argument("Period must be positive."); - - mThread = std::thread(&KinematicSimulationTrajectoryExecutor::spin, this); } //============================================================================= KinematicSimulationTrajectoryExecutor::~KinematicSimulationTrajectoryExecutor() { { - std::lock_guard lockSpin(mSpinMutex); - mRunning = false; + std::lock_guard lockSpin(mMutex); + mInExecution = false; } - mCv.notify_one(); - - mThread.join(); } //============================================================================= std::future KinematicSimulationTrajectoryExecutor::execute( - trajectory::TrajectoryPtr _traj) + trajectory::TrajectoryPtr traj) { - using statespace::dart::MetaSkeletonStateSpace; - - if (!_traj) + if (!traj) throw std::invalid_argument("Traj is null."); auto space = std::dynamic_pointer_cast< - MetaSkeletonStateSpace>(_traj->getStateSpace()); + MetaSkeletonStateSpace>(traj->getStateSpace()); if (!space) throw std::invalid_argument("Trajectory does not operate in this Executor's" @@ -74,7 +60,7 @@ std::future KinematicSimulationTrajectoryExecutor::execute( if (!dof_in_skeleton){ std::stringstream msg; - msg << "_traj contrains dof [" << name + msg << "traj contrains dof [" << name << "], which is not in mSkeleton."; throw std::invalid_argument(msg.str()); @@ -83,84 +69,65 @@ std::future KinematicSimulationTrajectoryExecutor::execute( skeleton_lock.unlock(); { - std::lock_guard lockSpin(mSpinMutex); + std::lock_guard lockSpin(mMutex); if (mTraj) throw std::runtime_error("Another trajectory in execution."); mPromise.reset(new std::promise()); - mTraj = _traj; + mTraj = traj; + mInExecution = true; + mExecutionStartTime = std::chrono::system_clock::now(); } - mCv.notify_one(); return mPromise->get_future(); - } //============================================================================= -void KinematicSimulationTrajectoryExecutor::spin() +void KinematicSimulationTrajectoryExecutor::step() { - using std::chrono::duration; - using std::chrono::duration_cast; - using statespace::dart::MetaSkeletonStateSpace; - using dart::common::make_unique; - using std::chrono::system_clock; + using namespace std::chrono; + std::lock_guard lock(mMutex); - system_clock::time_point startTime; - bool trajInExecution = false; + if (!mInExecution && !mTraj) + return; - while(true) + if (!mInExecution && mTraj) { - // Terminate the thread if mRunning is false. - std::unique_lock lockSpin(mSpinMutex); - mCv.wait(lockSpin, [&] { - // Reset startTime at the beginning of mTraj's execution. - if (!trajInExecution && this->mTraj) - { - startTime = system_clock::now(); - trajInExecution = true; - } - - return this->mTraj || !mRunning; - }); - - if (!mRunning) - { - if (this->mTraj){ - mPromise->set_exception(std::make_exception_ptr( - std::runtime_error("Trajectory terminated while in execution."))); - this->mTraj.reset(); - } - break; - } - - // Can't do static here because MetaSkeletonStateSpace inherits - // CartesianProduct which inherits virtual StateSpace - auto space = std::dynamic_pointer_cast< - MetaSkeletonStateSpace>(mTraj->getStateSpace()); - auto metaSkeleton = space->getMetaSkeleton(); - auto state = space->createState(); - - // Get current time on mTraj. - system_clock::time_point const now = system_clock::now(); - double t = duration_cast >(now - startTime).count(); - - mTraj->evaluate(t, state); - - // Lock the skeleton, set state. - std::unique_lock skeleton_lock(mSkeleton->getMutex()); - space->setState(state); - skeleton_lock.unlock(); - - // Check if trajectory has completed. - bool const is_done = (t >= mTraj->getEndTime()); - if (is_done) { - mTraj.reset(); - mPromise->set_value(); - trajInExecution = false; - } - - std::this_thread::sleep_until(now + mPeriod); + mPromise->set_exception(std::make_exception_ptr( + std::runtime_error("Trajectory terminated while in execution."))); + mTraj.reset(); + } + else if (mInExecution && !mTraj) + { + mPromise->set_exception(std::make_exception_ptr( + std::runtime_error("Set for execution but no trajectory is provided."))); + mInExecution = false; + } + + auto timeSinceBeginning = system_clock::now() - mExecutionStartTime; + double t = duration_cast(timeSinceBeginning).count(); + + // Can't do static here because MetaSkeletonStateSpace inherits + // CartesianProduct which inherits virtual StateSpace + auto space = std::dynamic_pointer_cast< + MetaSkeletonStateSpace>(mTraj->getStateSpace()); + auto metaSkeleton = space->getMetaSkeleton(); + auto state = space->createState(); + + mTraj->evaluate(t, state); + + // Lock the skeleton, set state. + std::unique_lock skeleton_lock(mSkeleton->getMutex()); + space->setState(state); + skeleton_lock.unlock(); + + // Check if trajectory has completed. + bool const is_done = (t >= mTraj->getEndTime()); + if (is_done) { + mTraj.reset(); + mPromise->set_value(); + mInExecution = false; } } diff --git a/tests/control/CMakeLists.txt b/tests/control/CMakeLists.txt index e77ff0f2f1..227d5dde87 100644 --- a/tests/control/CMakeLists.txt +++ b/tests/control/CMakeLists.txt @@ -3,19 +3,19 @@ aikido_add_test(test_KinematicSimulationTrajectoryExecutor target_link_libraries(test_KinematicSimulationTrajectoryExecutor "${PROJECT_NAME}_control") -aikido_add_test(test_BarrettFingerSpreadCommandExecutor - test_BarrettFingerSpreadCommandExecutor.cpp) -target_link_libraries(test_BarrettFingerSpreadCommandExecutor +aikido_add_test(test_BarrettFingerKinematicSimulationPositionCommandExecutor + test_BarrettFingerKinematicSimulationPositionCommandExecutor.cpp) +target_link_libraries(test_BarrettFingerKinematicSimulationPositionCommandExecutor "${PROJECT_NAME}_control") -aikido_add_test(test_BarrettFingerPositionCommandExecutor - test_BarrettFingerPositionCommandExecutor.cpp) -target_link_libraries(test_BarrettFingerPositionCommandExecutor +aikido_add_test(test_BarrettFingerKinematicSimulationSpreadCommandExecutor + test_BarrettFingerKinematicSimulationSpreadCommandExecutor.cpp) +target_link_libraries(test_BarrettFingerKinematicSimulationSpreadCommandExecutor "${PROJECT_NAME}_control") -aikido_add_test(test_BarrettHandPositionCommandExecutor - test_BarrettHandPositionCommandExecutor.cpp) -target_link_libraries(test_BarrettHandPositionCommandExecutor +aikido_add_test(test_BarrettHandKinematicSimulationPositionCommandExecutor + test_BarrettHandKinematicSimulationPositionCommandExecutor.cpp) +target_link_libraries(test_BarrettHandKinematicSimulationPositionCommandExecutor "${PROJECT_NAME}_control") add_subdirectory("ros") diff --git a/tests/control/test_BarrettFingerPositionCommandExecutor.cpp b/tests/control/test_BarrettFingerKinematicSimulationPositionCommandExecutor.cpp similarity index 68% rename from tests/control/test_BarrettFingerPositionCommandExecutor.cpp rename to tests/control/test_BarrettFingerKinematicSimulationPositionCommandExecutor.cpp index 4449ff9f26..ce565ee1d8 100644 --- a/tests/control/test_BarrettFingerPositionCommandExecutor.cpp +++ b/tests/control/test_BarrettFingerKinematicSimulationPositionCommandExecutor.cpp @@ -1,9 +1,9 @@ #include -#include +#include #include #include -using aikido::control::BarrettFingerPositionCommandExecutor; +using aikido::control::BarrettFingerKinematicSimulationPositionCommandExecutor; using ::dart::dynamics::Chain; using ::dart::dynamics::ChainPtr; using ::dart::dynamics::Skeleton; @@ -15,6 +15,8 @@ using ::dart::dynamics::RevoluteJoint; using namespace dart::dynamics; using namespace dart::collision; +using Vector1d = Eigen::Matrix; + static BodyNode::Properties create_BodyNodeProperties(const std::string& _name) { BodyNode::Properties properties; @@ -22,7 +24,10 @@ static BodyNode::Properties create_BodyNodeProperties(const std::string& _name) return properties; } -class BarrettFingerPositionCommandExecutorTest : public testing::Test +const static std::chrono::milliseconds stepTime{100}; +const static std::chrono::nanoseconds waitTime{1}; + +class BarrettFingerKinematicSimulationPositionCommandExecutorTest : public testing::Test { public: @@ -70,7 +75,7 @@ class BarrettFingerPositionCommandExecutorTest : public testing::Test mBn2 = mFinger->createJointAndBodyNodePair( mBn1, properties2, create_BodyNodeProperties("proximal")).second; mBn2->getParentJoint()->setPositionUpperLimit(0, M_PI); - mBn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); + mBn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(mBn2); // distal joint @@ -81,7 +86,7 @@ class BarrettFingerPositionCommandExecutorTest : public testing::Test mBn3 = mFinger->createJointAndBodyNodePair( mBn2, properties3, create_BodyNodeProperties("distal")).second; mBn3->getParentJoint()->setPositionUpperLimit(0, M_PI); - mBn3->getParentJoint()->setPositionLowerLimit(0, -M_PI); + mBn3->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(mBn3); return Chain::create(mBn1, mBn3, Chain::IncludeBoth); @@ -105,7 +110,7 @@ class BarrettFingerPositionCommandExecutorTest : public testing::Test fingerShape); mBn1->getParentJoint()->setTransformFromParentBodyNode(transform); mBn1->getParentJoint()->setPositionUpperLimit(0, M_PI); - mBn1->getParentJoint()->setPositionLowerLimit(0, -M_PI); + mBn1->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(mBn1); @@ -118,7 +123,7 @@ class BarrettFingerPositionCommandExecutorTest : public testing::Test mBn1, properties3, create_BodyNodeProperties("distal")).second; setGeometry(mBn2); mBn2->getParentJoint()->setPositionUpperLimit(0, M_PI); - mBn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); + mBn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); return Chain::create(mBn1, mBn2, Chain::IncludeBoth); } @@ -130,7 +135,7 @@ class BarrettFingerPositionCommandExecutorTest : public testing::Test { std::shared_ptr ballShape( new EllipsoidShape(Eigen::Vector3d(0.1, 0.1, 0.1))); - + auto skeleton = Skeleton::create("Ball"); auto ballBody = skeleton->createJointAndBodyNodePair( nullptr).second; @@ -149,8 +154,8 @@ class BarrettFingerPositionCommandExecutorTest : public testing::Test // CollisionDetector mCollisionDetector = FCLCollisionDetector::create(); mCollideWith = mCollisionDetector->createCollisionGroupAsSharedPtr(); - - mPosition = 1; + + mPosition << 1; mProximalDof = 1; mDistalDof = 2; } @@ -166,59 +171,63 @@ class BarrettFingerPositionCommandExecutorTest : public testing::Test CollisionDetectorPtr mCollisionDetector; CollisionGroupPtr mCollideWith; - double mPosition; + Vector1d mPosition; static constexpr double eps = 1e-2; }; -TEST_F(BarrettFingerPositionCommandExecutorTest, constructor_NullChain_Throws) +TEST_F(BarrettFingerKinematicSimulationPositionCommandExecutorTest, + constructor_NullChain_Throws) { - EXPECT_THROW(BarrettFingerPositionCommandExecutor executor( - nullptr, mProximalDof, mDistalDof, mCollisionDetector), + EXPECT_THROW(BarrettFingerKinematicSimulationPositionCommandExecutor executor( + nullptr, mProximalDof, mDistalDof, mCollisionDetector, mCollideWith), std::invalid_argument); } -TEST_F(BarrettFingerPositionCommandExecutorTest, constructor) +TEST_F(BarrettFingerKinematicSimulationPositionCommandExecutorTest, + constructor_Passes) { - EXPECT_NO_THROW(BarrettFingerPositionCommandExecutor executor( - mFingerChain, mProximalDof, mDistalDof, mCollisionDetector)); + EXPECT_NO_THROW(BarrettFingerKinematicSimulationPositionCommandExecutor executor( + mFingerChain, mProximalDof, mDistalDof, mCollisionDetector, mCollideWith)); } -TEST_F(BarrettFingerPositionCommandExecutorTest, constructor_Nonexistingproximal_throws) +TEST_F(BarrettFingerKinematicSimulationPositionCommandExecutorTest, + constructor_Nonexistingproximal_throws) { int proximalDof = 3; - EXPECT_THROW(BarrettFingerPositionCommandExecutor executor( - mFingerChain, proximalDof, mDistalDof, mCollisionDetector), + EXPECT_THROW(BarrettFingerKinematicSimulationPositionCommandExecutor executor( + mFingerChain, proximalDof, mDistalDof, mCollisionDetector, mCollideWith), std::invalid_argument); } -TEST_F(BarrettFingerPositionCommandExecutorTest, constructor_NonexistingDistal_throws) +TEST_F(BarrettFingerKinematicSimulationPositionCommandExecutorTest, + constructor_NonexistingDistal_throws) { int distalDof = 3; - EXPECT_THROW(BarrettFingerPositionCommandExecutor executor( - mFingerChain, mProximalDof, distalDof, mCollisionDetector), + EXPECT_THROW(BarrettFingerKinematicSimulationPositionCommandExecutor executor( + mFingerChain, mProximalDof, distalDof, mCollisionDetector, mCollideWith), std::invalid_argument); } -TEST_F(BarrettFingerPositionCommandExecutorTest, execute_WaitOnFuture_CommandExecuted) +TEST_F(BarrettFingerKinematicSimulationPositionCommandExecutorTest, + execute_WaitOnFuture_CommandExecuted) { - BarrettFingerPositionCommandExecutor executor( - mFingerChain, mProximalDof, mDistalDof, mCollisionDetector); + BarrettFingerKinematicSimulationPositionCommandExecutor executor( + mFingerChain, mProximalDof, mDistalDof, mCollisionDetector, mCollideWith); - double mimicRatio = BarrettFingerPositionCommandExecutor::getMimicRatio(); + double mimicRatio = BarrettFingerKinematicSimulationPositionCommandExecutor::getMimicRatio(); - double goalProximal = mPosition; - double goalDistal = mPosition*mimicRatio; + double goalProximal = mPosition[0]; + double goalDistal = mPosition[0]*mimicRatio; - auto future = executor.execute(mPosition, mCollideWith); + auto future = executor.execute(mPosition); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); + do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); + executor.step(); + status = future.wait_for(waitTime); }while(status != std::future_status::ready); future.wait(); @@ -227,34 +236,34 @@ TEST_F(BarrettFingerPositionCommandExecutorTest, execute_WaitOnFuture_CommandExe EXPECT_NEAR(goalDistal, mBn3->getParentJoint()->getDof(0)->getPosition(), eps); } -TEST_F(BarrettFingerPositionCommandExecutorTest, +TEST_F(BarrettFingerKinematicSimulationPositionCommandExecutorTest, execute_proximalStopsAtCollsionDistalContinuesUntilCollision) { // Collision obstacle Eigen::Isometry3d transform(Eigen::Isometry3d::Identity()); transform.translation() = Eigen::Vector3d(0.3, 0, 0.3); auto ball = createBall(transform, mCollisionDetector); - auto collideWith = mCollisionDetector->createCollisionGroup(ball); + auto collideWith = mCollisionDetector->createCollisionGroupAsSharedPtr(ball); - double goal = M_PI; + Vector1d goal; + goal << M_PI; - BarrettFingerPositionCommandExecutor executor( - mFingerChain, mProximalDof, mDistalDof, mCollisionDetector); + BarrettFingerKinematicSimulationPositionCommandExecutor executor( + mFingerChain, mProximalDof, mDistalDof, mCollisionDetector, collideWith); - auto future = executor.execute(goal, std::move(collideWith)); + auto future = executor.execute(goal); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); + do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); + executor.step(); + status = future.wait_for(waitTime); }while(status != std::future_status::ready); future.wait(); - double proximalExpected = 0.56548; // Value made by visual inspection + double proximalExpected = 0.55087; // Value made by visual inspection double proximalActual = mBn2->getParentJoint()->getDof(0)->getPosition(); double distalExpected = 2.81718; double distalActual = mBn3->getParentJoint()->getDof(0)->getPosition(); @@ -264,33 +273,34 @@ TEST_F(BarrettFingerPositionCommandExecutorTest, } -TEST_F(BarrettFingerPositionCommandExecutorTest, execute_DistalStopsAtCollsionProximalAlsoStops) +TEST_F(BarrettFingerKinematicSimulationPositionCommandExecutorTest, + execute_DistalStopsAtCollsionProximalAlsoStops) { // Collision object Eigen::Isometry3d transform(Eigen::Isometry3d::Identity()); transform.translation() = Eigen::Vector3d(1.3, 0, 1.3); auto ball = createBall(transform, mCollisionDetector); - auto collideWith = mCollisionDetector->createCollisionGroup(ball); + auto collideWith = mCollisionDetector->createCollisionGroupAsSharedPtr(ball); // Executor - BarrettFingerPositionCommandExecutor executor( - mFingerChain, mProximalDof, mDistalDof, mCollisionDetector); - double goal = M_PI/4; + BarrettFingerKinematicSimulationPositionCommandExecutor executor( + mFingerChain, mProximalDof, mDistalDof, mCollisionDetector, collideWith); + Vector1d goal; + goal << M_PI/4; // Execute - auto future = executor.execute(goal, std::move(collideWith)); + auto future = executor.execute(goal); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); + do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); + executor.step(); + status = future.wait_for(waitTime); }while(status != std::future_status::ready); future.wait(); - double mimicRatio = BarrettFingerPositionCommandExecutor::getMimicRatio(); + double mimicRatio = BarrettFingerKinematicSimulationPositionCommandExecutor::getMimicRatio(); // Values made by visual inspection double distalExpected = 0.21312; diff --git a/tests/control/test_BarrettFingerSpreadCommandExecutor.cpp b/tests/control/test_BarrettFingerKinematicSimulationSpreadCommandExecutor.cpp similarity index 66% rename from tests/control/test_BarrettFingerSpreadCommandExecutor.cpp rename to tests/control/test_BarrettFingerKinematicSimulationSpreadCommandExecutor.cpp index 8bd5eb56f4..2f25bbba52 100644 --- a/tests/control/test_BarrettFingerSpreadCommandExecutor.cpp +++ b/tests/control/test_BarrettFingerKinematicSimulationSpreadCommandExecutor.cpp @@ -1,10 +1,9 @@ #include -#include +#include #include - #include -using aikido::control::BarrettFingerSpreadCommandExecutor; +using aikido::control::BarrettFingerKinematicSimulationSpreadCommandExecutor; using ::dart::dynamics::Chain; using ::dart::dynamics::ChainPtr; using ::dart::dynamics::Skeleton; @@ -17,6 +16,8 @@ using namespace dart::dynamics; using namespace dart::collision; using namespace dart::simulation; +using Vector1d = Eigen::Matrix; + static BodyNode::Properties create_BodyNodeProperties(const std::string& _name) { BodyNode::Properties properties; @@ -24,7 +25,10 @@ static BodyNode::Properties create_BodyNodeProperties(const std::string& _name) return properties; } -class BarrettFingerSpreadCommandExecutorTest : public testing::Test +const static std::chrono::milliseconds stepTime{100}; +const static std::chrono::nanoseconds waitTime{1}; + +class BarrettFingerKinematicSimulationSpreadCommandExecutorTest : public testing::Test { public: @@ -63,7 +67,7 @@ class BarrettFingerSpreadCommandExecutorTest : public testing::Test nullptr, properties1, create_BodyNodeProperties("spread")).second; bn1->getParentJoint()->setTransformFromParentBodyNode(transform); bn1->getParentJoint()->setPositionUpperLimit(0, M_PI); - bn1->getParentJoint()->setPositionLowerLimit(0, 0); + bn1->getParentJoint()->setPositionLowerLimit(0, 0); // proximal joint RevoluteJoint::Properties properties2; @@ -72,7 +76,7 @@ class BarrettFingerSpreadCommandExecutorTest : public testing::Test auto bn2 = finger->createJointAndBodyNodePair( bn1, properties2, create_BodyNodeProperties("proximal")).second; bn2->getParentJoint()->setPositionUpperLimit(0, M_PI); - bn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); + bn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(bn2); // distal joint @@ -83,7 +87,7 @@ class BarrettFingerSpreadCommandExecutorTest : public testing::Test auto bn3 = finger->createJointAndBodyNodePair( bn2, properties3, create_BodyNodeProperties("distal")).second; bn3->getParentJoint()->setPositionUpperLimit(0, M_PI); - bn3->getParentJoint()->setPositionLowerLimit(0, -M_PI); + bn3->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(bn3); return Chain::create(bn1, bn3, Chain::IncludeBoth); @@ -96,7 +100,7 @@ class BarrettFingerSpreadCommandExecutorTest : public testing::Test { std::shared_ptr ballShape( new EllipsoidShape(Eigen::Vector3d(0.1, 0.1, 0.1))); - + auto skeleton = Skeleton::create("Ball"); auto ballBody = skeleton->createJointAndBodyNodePair( nullptr).second; @@ -125,7 +129,7 @@ class BarrettFingerSpreadCommandExecutorTest : public testing::Test mCollisionDetector = FCLCollisionDetector::create(); mCollideWith = mCollisionDetector->createCollisionGroupAsSharedPtr(); - mPosition = 1; + mPosition << 1; mSpreadDof = 0; } @@ -136,41 +140,52 @@ class BarrettFingerSpreadCommandExecutorTest : public testing::Test CollisionDetectorPtr mCollisionDetector; CollisionGroupPtr mCollideWith; - double mPosition; + Vector1d mPosition; int mSpreadDof; static constexpr double eps = 2e-1; }; -TEST_F(BarrettFingerSpreadCommandExecutorTest, constructor) +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, constructor) { - EXPECT_NO_THROW(BarrettFingerSpreadCommandExecutor( - mFingerChains, mSpreadDof, mCollisionDetector)); + EXPECT_NO_THROW(BarrettFingerKinematicSimulationSpreadCommandExecutor( + mFingerChains, mSpreadDof, mCollisionDetector, mCollideWith)); } -TEST_F(BarrettFingerSpreadCommandExecutorTest, constructor_NonexistingSpread_throws) +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, constructor_NonexistingSpread_throws) { int spreadDof = 3; - EXPECT_THROW(BarrettFingerSpreadCommandExecutor( - mFingerChains, spreadDof, mCollisionDetector), + EXPECT_THROW(BarrettFingerKinematicSimulationSpreadCommandExecutor( + mFingerChains, spreadDof, mCollisionDetector, mCollideWith), std::invalid_argument); } +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, constructor_NullCollideWith_default) +{ + int spreadDof = 0; + EXPECT_NO_THROW(BarrettFingerKinematicSimulationSpreadCommandExecutor( + mFingerChains, spreadDof, mCollisionDetector, nullptr)); +} -TEST_F(BarrettFingerSpreadCommandExecutorTest, execute_WaitOnFuture_CommandExecuted) +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, constructor_NullCollisionDetector_default) { - BarrettFingerSpreadCommandExecutor executor( - mFingerChains, mSpreadDof, mCollisionDetector); + int spreadDof = 0; + EXPECT_NO_THROW(BarrettFingerKinematicSimulationSpreadCommandExecutor( + mFingerChains, spreadDof, nullptr, mCollideWith)); +} - auto future = executor.execute(mPosition, mCollideWith); +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, execute_WaitOnFuture_CommandExecuted) +{ + BarrettFingerKinematicSimulationSpreadCommandExecutor executor( + mFingerChains, mSpreadDof, mCollisionDetector, mCollideWith); + auto future = executor.execute(mPosition); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); + do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); - }while(status != std::future_status::ready); + executor.step(); + status = future.wait_for(waitTime); + } while(status != std::future_status::ready); future.get(); @@ -178,11 +193,11 @@ TEST_F(BarrettFingerSpreadCommandExecutorTest, execute_WaitOnFuture_CommandExecu { auto dof = finger->getBodyNode(0)->getParentJoint()->getDof(mSpreadDof); double spread = dof->getPosition(); - EXPECT_NEAR(mPosition, spread, eps); + EXPECT_NEAR(mPosition[0], spread, eps); } } -TEST_F(BarrettFingerSpreadCommandExecutorTest, +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, execute_BothFingersStopAtProximalCollision) { @@ -192,19 +207,19 @@ TEST_F(BarrettFingerSpreadCommandExecutorTest, auto ball = createBall(transform, mCollisionDetector); mCollideWith = mCollisionDetector->createCollisionGroup(ball); - BarrettFingerSpreadCommandExecutor executor( - mFingerChains, mSpreadDof, mCollisionDetector); + BarrettFingerKinematicSimulationSpreadCommandExecutor executor( + mFingerChains, mSpreadDof, mCollisionDetector, mCollideWith); - double goal = 1.0; - auto future = executor.execute(goal, mCollideWith); + Vector1d goal; + goal << 1.0; + auto future = executor.execute(goal); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); + do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); + executor.step(); + status = future.wait_for(waitTime); }while(status != std::future_status::ready); future.get(); @@ -215,32 +230,32 @@ TEST_F(BarrettFingerSpreadCommandExecutorTest, ->getDof(mSpreadDof)->getPosition(); double expected = 0.4; EXPECT_NEAR(expected, spread, eps); - } + } } -TEST_F(BarrettFingerSpreadCommandExecutorTest, +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, execute_BothFingersStopAtDistalCollision) { - + // Collision object Eigen::Isometry3d transform = Eigen::Isometry3d::Identity(); transform.translation() = Eigen::Vector3d(0, -0.4, 0.7); auto ball = createBall(transform, mCollisionDetector); mCollideWith = mCollisionDetector->createCollisionGroup(ball); - BarrettFingerSpreadCommandExecutor executor( - mFingerChains, mSpreadDof, mCollisionDetector); + BarrettFingerKinematicSimulationSpreadCommandExecutor executor( + mFingerChains, mSpreadDof, mCollisionDetector, mCollideWith); - double goal = 1.0; - auto future = executor.execute(goal, mCollideWith); + Vector1d goal; + goal << 1.0; + auto future = executor.execute(goal); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); + do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); + executor.step(); + status = future.wait_for(waitTime); }while(status != std::future_status::ready); future.get(); @@ -251,49 +266,48 @@ TEST_F(BarrettFingerSpreadCommandExecutorTest, ->getDof(mSpreadDof)->getPosition(); double expected = 0.44; EXPECT_NEAR(expected, spread, eps); - } + } } -TEST_F(BarrettFingerSpreadCommandExecutorTest, +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, execute_ThrowsWhenFingerSpreadValuesDiffer) { - BarrettFingerSpreadCommandExecutor executor( - mFingerChains, mSpreadDof, mCollisionDetector); + BarrettFingerKinematicSimulationSpreadCommandExecutor executor( + mFingerChains, mSpreadDof, mCollisionDetector, mCollideWith); mFingerChains[0]->getBodyNode(0)->getParentJoint()->getDof(0)->setPosition(0); mFingerChains[1]->getBodyNode(0)->getParentJoint()->getDof(0)->setPosition(M_PI/4); - - double goal = M_PI*0.5; - auto future = executor.execute(goal, mCollideWith); + + Vector1d goal; + goal << M_PI*0.5; + auto future = executor.execute(goal); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); + do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); + executor.step(); + status = future.wait_for(waitTime); }while(status != std::future_status::ready); EXPECT_THROW(future.get(), std::runtime_error); } -TEST_F(BarrettFingerSpreadCommandExecutorTest, +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, execute_GoalAboveUpperLimitStopsAtUpperJointLimit) { - BarrettFingerSpreadCommandExecutor executor( - mFingerChains, mSpreadDof, mCollisionDetector); + BarrettFingerKinematicSimulationSpreadCommandExecutor executor( + mFingerChains, mSpreadDof, mCollisionDetector, mCollideWith); - double goal = M_PI*1.5; - auto future = executor.execute(goal, mCollideWith); + Vector1d goal; + goal << M_PI*1.5; + auto future = executor.execute(goal); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); + executor.step(); + status = future.wait_for(waitTime); }while(status != std::future_status::ready); future.get(); @@ -305,26 +319,24 @@ TEST_F(BarrettFingerSpreadCommandExecutorTest, double expected = finger->getBodyNode(0)->getParentJoint() ->getDof(mSpreadDof)->getPositionUpperLimit(); EXPECT_NEAR(expected, spread, eps); - } + } } - -TEST_F(BarrettFingerSpreadCommandExecutorTest, +TEST_F(BarrettFingerKinematicSimulationSpreadCommandExecutorTest, execute_GoalBelowLowerLimitStopsAtLowerJointLimit) { - BarrettFingerSpreadCommandExecutor executor( - mFingerChains, mSpreadDof, mCollisionDetector); + BarrettFingerKinematicSimulationSpreadCommandExecutor executor( + mFingerChains, mSpreadDof, mCollisionDetector, mCollideWith); - double goal = -M_PI*0.5; - auto future = executor.execute(goal, mCollideWith); + Vector1d goal; + goal << -M_PI*0.5; + auto future = executor.execute(goal); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); do { - executor.step(stepTimeCount); - status = future.wait_for(stepTime); + executor.step(); + status = future.wait_for(waitTime); }while(status != std::future_status::ready); future.get(); @@ -336,6 +348,5 @@ TEST_F(BarrettFingerSpreadCommandExecutorTest, double expected = finger->getBodyNode(0)->getParentJoint() ->getDof(mSpreadDof)->getPositionLowerLimit(); EXPECT_NEAR(expected, spread, eps); - } + } } - diff --git a/tests/control/test_BarrettHandPositionCommandExecutor.cpp b/tests/control/test_BarrettHandKinematicSimulationPositionCommandExecutor.cpp similarity index 65% rename from tests/control/test_BarrettHandPositionCommandExecutor.cpp rename to tests/control/test_BarrettHandKinematicSimulationPositionCommandExecutor.cpp index e76fbf219c..07b43a0817 100644 --- a/tests/control/test_BarrettHandPositionCommandExecutor.cpp +++ b/tests/control/test_BarrettHandKinematicSimulationPositionCommandExecutor.cpp @@ -1,18 +1,17 @@ #include -#include -#include -#include - +#include +#include +#include #include - #include -using aikido::control::BarrettFingerPositionCommandExecutorPtr; -using aikido::control::BarrettFingerSpreadCommandExecutorPtr; +using aikido::control::BarrettFingerKinematicSimulationPositionCommandExecutorPtr; +using aikido::control::BarrettHandKinematicSimulationPositionCommandExecutorPtr; +using aikido::control::BarrettFingerKinematicSimulationSpreadCommandExecutorPtr; -using aikido::control::BarrettHandPositionCommandExecutor; -using aikido::control::BarrettFingerPositionCommandExecutor; -using aikido::control::BarrettFingerSpreadCommandExecutor; +using aikido::control::BarrettHandKinematicSimulationPositionCommandExecutor; +using aikido::control::BarrettFingerKinematicSimulationPositionCommandExecutor; +using aikido::control::BarrettFingerKinematicSimulationSpreadCommandExecutor; using ::dart::dynamics::Chain; using ::dart::dynamics::ChainPtr; @@ -26,6 +25,8 @@ using namespace dart::dynamics; using namespace dart::collision; using namespace dart::simulation; +const static std::chrono::milliseconds stepTime{100}; + static BodyNode::Properties create_BodyNodeProperties(const std::string& _name) { BodyNode::Properties properties; @@ -33,7 +34,7 @@ static BodyNode::Properties create_BodyNodeProperties(const std::string& _name) return properties; } -class BarrettHandPositionCommandExecutorTest : public testing::Test +class BarrettHandKinematicSimulationPositionCommandExecutorTest : public testing::Test { public: @@ -72,7 +73,7 @@ class BarrettHandPositionCommandExecutorTest : public testing::Test nullptr, properties1, create_BodyNodeProperties("spread")).second; bn1->getParentJoint()->setTransformFromParentBodyNode(transform); bn1->getParentJoint()->setPositionUpperLimit(0, M_PI); - bn1->getParentJoint()->setPositionLowerLimit(0, 0); + bn1->getParentJoint()->setPositionLowerLimit(0, 0); // proximal joint RevoluteJoint::Properties properties2; @@ -81,7 +82,7 @@ class BarrettHandPositionCommandExecutorTest : public testing::Test auto bn2 = finger->createJointAndBodyNodePair( bn1, properties2, create_BodyNodeProperties("proximal")).second; bn2->getParentJoint()->setPositionUpperLimit(0, M_PI); - bn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); + bn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(bn2); // distal joint @@ -92,7 +93,7 @@ class BarrettHandPositionCommandExecutorTest : public testing::Test auto bn3 = finger->createJointAndBodyNodePair( bn2, properties3, create_BodyNodeProperties("distal")).second; bn3->getParentJoint()->setPositionUpperLimit(0, M_PI); - bn3->getParentJoint()->setPositionLowerLimit(0, -M_PI); + bn3->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(bn3); return Chain::create(bn1, bn3, Chain::IncludeBoth); @@ -116,7 +117,7 @@ class BarrettHandPositionCommandExecutorTest : public testing::Test fingerShape); mBn1->getParentJoint()->setTransformFromParentBodyNode(transform); mBn1->getParentJoint()->setPositionUpperLimit(0, M_PI); - mBn1->getParentJoint()->setPositionLowerLimit(0, -M_PI); + mBn1->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(mBn1); // distal joint @@ -127,9 +128,9 @@ class BarrettHandPositionCommandExecutorTest : public testing::Test auto mBn2 = mFinger->createJointAndBodyNodePair( mBn1, properties3, create_BodyNodeProperties("distal")).second; mBn2->getParentJoint()->setPositionUpperLimit(0, M_PI); - mBn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); + mBn2->getParentJoint()->setPositionLowerLimit(0, -M_PI); setGeometry(mBn2); - + return Chain::create(mBn1, mBn2, Chain::IncludeBoth); } @@ -140,7 +141,7 @@ class BarrettHandPositionCommandExecutorTest : public testing::Test { std::shared_ptr ballShape( new EllipsoidShape(Eigen::Vector3d(0.1, 0.1, 0.1))); - + auto skeleton = Skeleton::create("Ball"); auto ballBody = skeleton->createJointAndBodyNodePair( nullptr).second; @@ -174,88 +175,93 @@ class BarrettHandPositionCommandExecutorTest : public testing::Test transform.linear() = rotation; transform.translation() = Eigen::Vector3d(0, 1.0, 0.0); mFingerChains.push_back(create2DoFFinger(transform)); - + // CollisionDetector mCollisionDetector = FCLCollisionDetector::create(); mCollideWith = mCollisionDetector->createCollisionGroupAsSharedPtr(); - int spreadDof = 0; + int spreadDof = 0; int primalDof[3] = {1,1,0}; int distalDof[3] = {2,2,1}; for(int i = 0 ; i < 3; ++i) { - mPositionExecutors[i] = (std::make_shared( - mFingerChains[i], primalDof[i], distalDof[i], mCollisionDetector)); + mPositionExecutors[i] = (std::make_shared( + mFingerChains[i], primalDof[i], distalDof[i], mCollisionDetector, mCollideWith)); } std::array spreadFingers; spreadFingers[0] = mFingerChains[0]; spreadFingers[1] = mFingerChains[1]; - mSpreadExecutor = std::make_shared( - spreadFingers, spreadDof, mCollisionDetector); + mSpreadExecutor = std::make_shared( + spreadFingers, spreadDof, mCollisionDetector, mCollideWith); - mPositions = Eigen::Matrix::Ones(); - mPositions(3) = M_PI/2; + mPositions = Eigen::Matrix::Ones()*0.1; + mPositions(3) = M_PI/8; } protected: std::vector mFingerChains; CollisionDetectorPtr mCollisionDetector; CollisionGroupPtr mCollideWith; - std::array mPositionExecutors; - BarrettFingerSpreadCommandExecutorPtr mSpreadExecutor; + std::array mPositionExecutors; + BarrettFingerKinematicSimulationSpreadCommandExecutorPtr mSpreadExecutor; Eigen::Matrix mPositions; static constexpr double eps = 0.01; }; -TEST_F(BarrettHandPositionCommandExecutorTest, +/* +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, constructor_nullPositionExecutor_throws) { - std::array positionExecutors; - positionExecutors[0] = (mPositionExecutors[0]); - positionExecutors[1] = (mPositionExecutors[1]); + std::array positionExecutors; + positionExecutors[0] = mPositionExecutors[0]; + positionExecutors[1] = mPositionExecutors[1]; positionExecutors[2] = nullptr; - - EXPECT_THROW(BarrettHandPositionCommandExecutor( - positionExecutors, mSpreadExecutor), std::invalid_argument); -} + EXPECT_THROW(BarrettHandKinematicSimulationPositionCommandExecutor( + positionExecutors, mSpreadExecutor, mCollideWith), std::invalid_argument); +} -TEST_F(BarrettHandPositionCommandExecutorTest, +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, constructor_nullSpreadExecutor_throws) -{ - EXPECT_THROW(BarrettHandPositionCommandExecutor( - mPositionExecutors, nullptr), std::invalid_argument); +{ + EXPECT_THROW(BarrettHandKinematicSimulationPositionCommandExecutor( + mPositionExecutors, nullptr, mCollideWith), std::invalid_argument); +} + +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, + constructor_nullCollideWith_throws) +{ + EXPECT_THROW(BarrettHandKinematicSimulationPositionCommandExecutor( + mPositionExecutors, mSpreadExecutor, nullptr), std::invalid_argument); } -TEST_F(BarrettHandPositionCommandExecutorTest, constructor_no_throw) +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, constructor_no_throw) { - EXPECT_NO_THROW(BarrettHandPositionCommandExecutor( - mPositionExecutors, mSpreadExecutor)); + EXPECT_NO_THROW(BarrettHandKinematicSimulationPositionCommandExecutor( + mPositionExecutors, mSpreadExecutor, mCollideWith)); } -TEST_F(BarrettHandPositionCommandExecutorTest, +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, execute_WaitOnFuture_CommandExecuted) { // Setup - BarrettHandPositionCommandExecutor executor( - mPositionExecutors, mSpreadExecutor); + BarrettHandKinematicSimulationPositionCommandExecutor executor( + mPositionExecutors, mSpreadExecutor, mCollideWith); - double mimicRatio = BarrettFingerPositionCommandExecutor::getMimicRatio(); + double mimicRatio = BarrettFingerKinematicSimulationPositionCommandExecutor::getMimicRatio(); // Execute trajectory - auto future = executor.execute(mPositions, mCollideWith); + auto future = executor.execute(mPositions); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); do { - executor.step(stepTimeCount); + executor.step(); status = future.wait_for(stepTime); }while(status != std::future_status::ready); @@ -281,64 +287,66 @@ TEST_F(BarrettHandPositionCommandExecutorTest, EXPECT_NEAR(mPositions(2)*mimicRatio, distal, eps); } -TEST_F(BarrettHandPositionCommandExecutorTest, +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, execute_CommandIsAlreadyRunning_Throws) { - BarrettHandPositionCommandExecutor executor( - mPositionExecutors, mSpreadExecutor); + BarrettHandKinematicSimulationPositionCommandExecutor executor( + mPositionExecutors, mSpreadExecutor, mCollideWith); // Execute trajectory - auto future = executor.execute(mPositions, mCollideWith); - EXPECT_THROW(executor.execute(mPositions, mCollideWith), + auto future = executor.execute(mPositions); + EXPECT_THROW(executor.execute(mPositions), std::runtime_error); } -TEST_F(BarrettHandPositionCommandExecutorTest, +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, execute_PrevCommandFinished_DoesNotThrow) { - BarrettHandPositionCommandExecutor executor( - mPositionExecutors, mSpreadExecutor); + BarrettHandKinematicSimulationPositionCommandExecutor executor( + mPositionExecutors, mSpreadExecutor, mCollideWith); // Execute trajectory - auto future = executor.execute(mPositions, mCollideWith); + auto future = executor.execute(mPositions); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); do { - executor.step(stepTimeCount); + executor.step(); status = future.wait_for(stepTime); }while(status != std::future_status::ready); future.get(); - EXPECT_NO_THROW(executor.execute(mPositions, mCollideWith)); + EXPECT_NO_THROW(executor.execute(mPositions)); } -TEST_F(BarrettHandPositionCommandExecutorTest, +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, execute_PrimalStopsAtCollsionDistalContinues) { Eigen::Isometry3d transform(Eigen::Isometry3d::Identity()); transform.translation() = Eigen::Vector3d(0.3, 0, 0.3); auto ball = createBall(transform, mCollisionDetector); - auto collideWith = mCollisionDetector->createCollisionGroup(ball); + auto collideWith = mCollisionDetector->createCollisionGroupAsSharedPtr(ball); Eigen::VectorXd position(Eigen::VectorXd::Zero(4)); double goal = M_PI; position(0) = goal; - - BarrettHandPositionCommandExecutor executor( - mPositionExecutors, mSpreadExecutor); - auto future = executor.execute(position, std::move(collideWith)); + for (auto positionExecutor : mPositionExecutors) + { + positionExecutor->setCollideWith(collideWith); + } + mSpreadExecutor->setCollideWith(collideWith); + + BarrettHandKinematicSimulationPositionCommandExecutor executor( + mPositionExecutors, mSpreadExecutor, collideWith); + + auto future = executor.execute(position); std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); do { - executor.step(stepTimeCount); + executor.step(); status = future.wait_for(stepTime); }while(status != std::future_status::ready); @@ -348,34 +356,37 @@ TEST_F(BarrettHandPositionCommandExecutorTest, double distal = mFingerChains[0]->getDof(2)->getPosition(); // Values made by visual inspection - EXPECT_NEAR(0.56548, primal, eps); + EXPECT_NEAR(0.56548, primal, eps); EXPECT_NEAR(2.81718, distal, eps); } -TEST_F(BarrettHandPositionCommandExecutorTest, +TEST_F(BarrettHandKinematicSimulationPositionCommandExecutorTest, execute_DistalStopsAtCollsionPromalAlsoStops) { Eigen::Isometry3d transform(Eigen::Isometry3d::Identity()); transform.translation() = Eigen::Vector3d(1.3, 0, 1.3); auto ball = createBall(transform, mCollisionDetector); - auto collideWith = mCollisionDetector->createCollisionGroup(ball); + auto collideWith = mCollisionDetector->createCollisionGroupAsSharedPtr(ball); Eigen::VectorXd position(Eigen::VectorXd::Zero(4)); double goal = M_PI/4; position(0) = goal; - - BarrettHandPositionCommandExecutor executor( - mPositionExecutors, mSpreadExecutor); - auto future = executor.execute(position, std::move(collideWith)); - + for (auto positionExecutor : mPositionExecutors) + { + positionExecutor->setCollideWith(collideWith); + } + mSpreadExecutor->setCollideWith(collideWith); + + BarrettHandKinematicSimulationPositionCommandExecutor executor( + mPositionExecutors, mSpreadExecutor, collideWith); + auto future = executor.execute(position); + std::future_status status; - auto stepTime = std::chrono::milliseconds(1); - double stepTimeCount = stepTime.count(); do { - executor.step(stepTimeCount); + executor.step(); status = future.wait_for(stepTime); }while(status != std::future_status::ready); @@ -384,7 +395,7 @@ TEST_F(BarrettHandPositionCommandExecutorTest, double primal = mFingerChains[0]->getDof(1)->getPosition(); double distal = mFingerChains[0]->getDof(2)->getPosition(); - double mimicRatio = BarrettFingerPositionCommandExecutor::getMimicRatio(); + double mimicRatio = BarrettFingerKinematicSimulationPositionCommandExecutor::getMimicRatio(); // Values made by visual inspection EXPECT_NEAR(0.211845, distal, eps); @@ -392,3 +403,4 @@ TEST_F(BarrettHandPositionCommandExecutorTest, EXPECT_NEAR(primal*mimicRatio, distal, eps); } +*/ diff --git a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp index 6924c42ead..a91a15c223 100644 --- a/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp +++ b/tests/control/test_KinematicSimulationTrajectoryExecutor.cpp @@ -20,6 +20,8 @@ using ::dart::dynamics::BodyNode; using ::dart::dynamics::BodyNodePtr; using ::dart::dynamics::RevoluteJoint; +const static std::chrono::milliseconds stepTime{100}; + class KinematicSimulationTrajectoryExecutorTest : public testing::Test { public: @@ -62,8 +64,6 @@ class KinematicSimulationTrajectoryExecutorTest : public testing::Test mTraj->addWaypoint(0, s1); mTraj->addWaypoint(1, s2); - mPeriod = std::chrono::milliseconds(1); - } protected: @@ -73,50 +73,32 @@ class KinematicSimulationTrajectoryExecutorTest : public testing::Test std::shared_ptr interpolator; std::shared_ptr mTraj; - std::chrono::milliseconds mPeriod; - BodyNodePtr bn1; }; TEST_F(KinematicSimulationTrajectoryExecutorTest, constructor_NullSkeleton_Throws) { - EXPECT_THROW(KinematicSimulationTrajectoryExecutor(nullptr, mPeriod), + EXPECT_THROW(KinematicSimulationTrajectoryExecutor(nullptr), std::invalid_argument); } -TEST_F(KinematicSimulationTrajectoryExecutorTest, constructor) +TEST_F(KinematicSimulationTrajectoryExecutorTest, constructor_Passes) { EXPECT_NO_THROW( - KinematicSimulationTrajectoryExecutor executor(mSkeleton, mPeriod)); -} - -TEST_F(KinematicSimulationTrajectoryExecutorTest, constructor_ZeroPeriod_Throws) -{ - std::chrono::milliseconds period(0); - EXPECT_THROW(KinematicSimulationTrajectoryExecutor(mSkeleton, period), - std::invalid_argument); + KinematicSimulationTrajectoryExecutor executor(mSkeleton)); } - -TEST_F(KinematicSimulationTrajectoryExecutorTest, constructor_NegativePeriod_Throws) -{ - std::chrono::milliseconds period(-3); - EXPECT_THROW(KinematicSimulationTrajectoryExecutor(mSkeleton, period), - std::invalid_argument); -} - - TEST_F(KinematicSimulationTrajectoryExecutorTest, execute_NullTrajectory_Throws) { - KinematicSimulationTrajectoryExecutor executor(mSkeleton, mPeriod); + KinematicSimulationTrajectoryExecutor executor(mSkeleton); EXPECT_THROW(executor.execute(nullptr), std::invalid_argument); } TEST_F(KinematicSimulationTrajectoryExecutorTest, execute_NonMetaSkeletonStateSpace_Throws) { - KinematicSimulationTrajectoryExecutor executor(mSkeleton, mPeriod); + KinematicSimulationTrajectoryExecutor executor(mSkeleton); auto space = std::make_shared(); auto interpolator = std::make_shared(space); @@ -129,61 +111,84 @@ TEST_F(KinematicSimulationTrajectoryExecutorTest, execute_TrajWithUncontrolledDo { auto skeleton = Skeleton::create("Skeleton"); - KinematicSimulationTrajectoryExecutor executor(skeleton, mPeriod); + KinematicSimulationTrajectoryExecutor executor(skeleton); EXPECT_THROW(executor.execute(mTraj), std::invalid_argument); } TEST_F(KinematicSimulationTrajectoryExecutorTest, execute_WaitOnFuture_TrajectoryWasExecuted) { - KinematicSimulationTrajectoryExecutor executor(mSkeleton, mPeriod); + KinematicSimulationTrajectoryExecutor executor(mSkeleton); EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 0.0); auto future = executor.execute(mTraj); - future.wait(); + std::future_status status; + do + { + executor.step(); + status = future.wait_for(stepTime); + } while(status != std::future_status::ready); + + future.get(); EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 1.0); } TEST_F(KinematicSimulationTrajectoryExecutorTest, execute_TrajectoryIsAlreadyRunning_Throws) { - KinematicSimulationTrajectoryExecutor executor(mSkeleton, mPeriod); + KinematicSimulationTrajectoryExecutor executor(mSkeleton); EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 0.0); auto future = executor.execute(mTraj); - - // Executor possibly not able to immediately execute the next one. - try - { - executor.execute(mTraj); - } - catch (const std::runtime_error& e) + + EXPECT_THROW(executor.execute(mTraj), std::runtime_error); + std::future_status status; + do { - EXPECT_EQ(std::string(e.what()), "Another trajectory in execution."); - }; + executor.step(); + status = future.wait_for(stepTime); + } while(status != std::future_status::ready); + + future.get(); - future.wait(); EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 1.0); } TEST_F(KinematicSimulationTrajectoryExecutorTest, execute_TrajectoryFinished_DoesNotThrow) { - KinematicSimulationTrajectoryExecutor executor(mSkeleton, mPeriod); + KinematicSimulationTrajectoryExecutor executor(mSkeleton); EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 0.0); auto future = executor.execute(mTraj); - future.wait(); + + std::future_status status; + do + { + executor.step(); + status = future.wait_for(stepTime); + } while(status != std::future_status::ready); + + future.get(); + EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 1.0); mSkeleton->getDof(0)->setPosition(-1.0); - + // Execute second traj. future = executor.execute(mTraj); - future.wait(); + + do + { + executor.step(); + status = future.wait_for(stepTime); + } while(status != std::future_status::ready); + + future.get(); + EXPECT_DOUBLE_EQ(mSkeleton->getDof(0)->getPosition(), 1.0); }