diff --git a/include/aikido/control/TrajectoryExecutor.hpp b/include/aikido/control/TrajectoryExecutor.hpp index 88af71c8be..ff567339c7 100644 --- a/include/aikido/control/TrajectoryExecutor.hpp +++ b/include/aikido/control/TrajectoryExecutor.hpp @@ -1,7 +1,7 @@ #ifndef AIKIDO_CONTROL_TRAJECTORYEXECUTOR_HPP_ #define AIKIDO_CONTROL_TRAJECTORYEXECUTOR_HPP_ #include "TrajectoryResult.hpp" -#include "../trajectory/Trajectory.hpp" +#include #include namespace aikido { diff --git a/include/aikido/control/ros/Conversions.hpp b/include/aikido/control/ros/Conversions.hpp index 49a8514c0f..fd3076a91d 100644 --- a/include/aikido/control/ros/Conversions.hpp +++ b/include/aikido/control/ros/Conversions.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace aikido { namespace control { @@ -11,18 +12,24 @@ namespace ros { /// Converts a ROS JointTrajectory into an aikido's Spline trajectory. /// This method only handles single-DOF joints. -/// \param[in] _space MetaSkeletonStateSpace for Spline trajectory. +/// \param[in] space MetaSkeletonStateSpace for Spline trajectory. // Subspaces must be either 1D RnJoint or SO2Joint. -/// \param[in] _jointTrajectory ROS JointTrajectory to be converted. +/// \param[in] jointTrajectory ROS JointTrajectory to be converted. /// \return Spline trajectory. -std::unique_ptr convertJointTrajectory( +std::unique_ptr toSplineJointTrajectory( const std::shared_ptr< aikido::statespace::dart::MetaSkeletonStateSpace>& space, const trajectory_msgs::JointTrajectory& jointTrajectory); +/// Converts Aikido Trajectory to ROS JointTrajectory. +/// Supports only 1D RnJoints and SO2Joints. +/// \param[in] trajectory Aikido trajectory to be converted. +/// \param[in] timestep Timestep between two consecutive waypoints. +trajectory_msgs::JointTrajectory toRosJointTrajectory( + const aikido::trajectory::TrajectoryPtr& trajectory, double timestep); + } // namespace ros } // namespace control } // namespace aikido #endif // ifndef AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_ - diff --git a/include/aikido/control/ros/RosTrajectoryExecutionException.hpp b/include/aikido/control/ros/RosTrajectoryExecutionException.hpp new file mode 100644 index 0000000000..9f5428764d --- /dev/null +++ b/include/aikido/control/ros/RosTrajectoryExecutionException.hpp @@ -0,0 +1,33 @@ +#ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTIONEXCEPTION_HPP_ +#define AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTIONEXCEPTION_HPP_ +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { + +/// This class wraps various exceptions that may arise during trajectory +/// execution over ROS. +class RosTrajectoryExecutionException: public std::runtime_error +{ +public: + + RosTrajectoryExecutionException( + const std::string& what, + actionlib::TerminalState terminalState); + + RosTrajectoryExecutionException( + const std::string& what, + int result); + + virtual ~RosTrajectoryExecutionException() = default; + +}; + +} // ros +} // control +} // aikido + +#endif diff --git a/include/aikido/control/ros/RosTrajectoryExecutor.hpp b/include/aikido/control/ros/RosTrajectoryExecutor.hpp new file mode 100644 index 0000000000..6ad01cf2ff --- /dev/null +++ b/include/aikido/control/ros/RosTrajectoryExecutor.hpp @@ -0,0 +1,90 @@ +#ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_ +#define AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { + +/// This class sends trajectory commands to ROS server. +class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor +{ +public: + /// Constructor. + /// \param[in] space Space in which trajectries operate. + /// \param[in] node ROS node handle for action client. + /// \param[in] serverName Name of the server to send traejctory to. + /// \param[in] timestep Step size for interpolating trajectories. + /// \param[in] goalTimeTolerance + /// \param[in] connectionTimeout Timeout for server connection. + /// \param[in] connectionPollingPeriod Polling period for server connection. + template + RosTrajectoryExecutor( + statespace::dart::MetaSkeletonStateSpacePtr space, + ::ros::NodeHandle node, + const std::string& serverName, + double timestep, + double goalTimeTolerance, + const DurationA& connectionTimeout = std::chrono::milliseconds{1000}, + const DurationB& connectionPollingPeriod = std::chrono::milliseconds{20} + ); + + virtual ~RosTrajectoryExecutor(); + + /// Sends trajectory to ROS server for execution. + /// \param[in] traj Trajecotry to be executed. + std::future execute(trajectory::TrajectoryPtr traj) override; + + /// Sends trajectory to ROS server for execution. + /// \param[in] traj Trajectrory to be executed. + /// \param[in] startTime Start time for the trajectory. + std::future execute( + trajectory::TrajectoryPtr traj, const ::ros::Time& startTime); + + /// To be executed on a separate thread. + /// Regularly checks for the completion of a sent trajectory. + void spin(); + +private: + using TrajectoryActionClient + = actionlib::ActionClient; + using GoalHandle = TrajectoryActionClient::GoalHandle; + + bool waitForServer(); + + void transitionCallback(GoalHandle handle); + + statespace::dart::MetaSkeletonStateSpacePtr mSpace; + ::ros::NodeHandle mNode; + ::ros::CallbackQueue mCallbackQueue; + TrajectoryActionClient mClient; + TrajectoryActionClient::GoalHandle mGoalHandle; + + double mTimestep; + double mGoalTimeTolerance; + + std::chrono::milliseconds mConnectionTimeout; + std::chrono::milliseconds mConnectionPollingPeriod; + + bool mInProgress; + std::promise mPromise; + trajectory::TrajectoryPtr mTrajectory; + + std::mutex mMutex; +}; + +} // namespace ros +} // namespace control +} // namespace aikido + +#endif // ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_ + diff --git a/src/control/ros/CMakeLists.txt b/src/control/ros/CMakeLists.txt index a6fedd31d9..a88fc56de7 100644 --- a/src/control/ros/CMakeLists.txt +++ b/src/control/ros/CMakeLists.txt @@ -4,10 +4,28 @@ find_package(trajectory_msgs QUIET) aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs") +# TODO: Workaround because find_package(actionlib) imports "tests" and "run_tests" +# targets that conflict with the targets defined by Aikido. +find_path(actionlib_INCLUDE_DIRS actionlib PATHS "/opt/ros/indigo/include") +find_library(actionlib_LIBRARIES actionlib PATHS "/opt/ros/indigo/lib") +evaluate_condition(actionlib_FOUND actionlib_INCLUDE_DIRS AND actionlib_LIBRARIES) +#find_package(actionlib QUIET) +aikido_check_package(actionlib "aikido::control::ros" "actionlib") + +find_path(control_msgs_INCLUDE_DIRS control_msgs PATHS "/opt/ros/indigo/include") +#find_library(control_msgs_LIBRARIES control_msgs PATHS "/opt/ros/indigo/lib") +evaluate_condition(control_msgs_FOUND control_msgs_INCLUDE_DIRS) +aikido_check_package(control_msgs "aikido::control::ros" "control_msgs") + +find_package(roscpp QUIET) +aikido_check_package(roscpp "aikido::control::ros" "roscpp") + #============================================================================== # Libraries # set(sources + RosTrajectoryExecutor.cpp + RosTrajectoryExecutionException.cpp Conversions.cpp ) @@ -15,6 +33,9 @@ add_library("${PROJECT_NAME}_control_ros" SHARED ${sources}) target_include_directories("${PROJECT_NAME}_control_ros" SYSTEM PUBLIC ${trajectory_msgs_INCLUDE_DIRS} + ${actionlib_INCLUDE_DIRS} + ${control_msgs_INCLUDE_DIRS} + ${roscpp_INCLUDE_DIRS} ) target_link_libraries("${PROJECT_NAME}_control_ros" @@ -23,6 +44,9 @@ target_link_libraries("${PROJECT_NAME}_control_ros" "${PROJECT_NAME}_trajectory" ${DART_LIBRARIES} ${trajectory_msgs_LIBRARIES} + ${actionlib_LIBRARIES} + ${control_msgs_LIBRARIES} + ${roscpp_LIBRARIES} ) add_component(${PROJECT_NAME} control_ros) diff --git a/src/control/ros/Conversions.cpp b/src/control/ros/Conversions.cpp index 416fbccea6..0ac5946224 100644 --- a/src/control/ros/Conversions.cpp +++ b/src/control/ros/Conversions.cpp @@ -1,7 +1,8 @@ #include -#include #include #include +#include +#include #include #include #include @@ -126,16 +127,49 @@ void extractJointTrajectoryPoint( } } +//============================================================================= +void extractTrajectoryPoint( + const std::shared_ptr& space, + const aikido::trajectory::TrajectoryPtr& trajectory, + double timeFromStart, trajectory_msgs::JointTrajectoryPoint& waypoint) +{ + const auto numDerivatives = std::min(trajectory->getNumDerivatives(), 1); + const auto timeAbsolute = trajectory->getStartTime() + timeFromStart; + const int numDof = space->getDimension(); + + Eigen::VectorXd tangentVector; + auto state = space->createState(); + trajectory->evaluate(timeAbsolute, state); + space->logMap(state, tangentVector); + + assert(tangentVector.size() == numDof); + + waypoint.time_from_start = ::ros::Duration(timeFromStart); + waypoint.positions.assign(tangentVector.data(), + tangentVector.data() + tangentVector.size()); + + assert(0 <= numDerivatives && numDerivatives <= 2); + const std::array*, 2> derivatives{ + &waypoint.velocities, &waypoint.accelerations}; + for (int iDerivative = 1; iDerivative <= numDerivatives; ++iDerivative) + { + trajectory->evaluateDerivative(timeAbsolute, iDerivative, tangentVector); + assert(tangentVector.size() == numDof); + + derivatives[iDerivative - 1]->assign(tangentVector.data(), + tangentVector.data() + tangentVector.size()); + } +} + //============================================================================= // The rows of inVector is reordered in outVector. void reorder(std::map indexMap, const Eigen::VectorXd& inVector, Eigen::VectorXd* outVector) { - *outVector = Eigen::VectorXd::Zero(inVector.size()); - for (auto it = indexMap.begin(); it != indexMap.end(); ++it) - { - (*outVector)(it->second) = inVector(it->first); - } + assert(outVector != nullptr); + outVector->resize(inVector.size()); + for (auto index : indexMap) + (*outVector)[index.second] = inVector[index.first]; } //============================================================================= @@ -160,7 +194,7 @@ std::vector findJointByName( } // namespace //============================================================================= -std::unique_ptr convertJointTrajectory( +std::unique_ptr toSplineJointTrajectory( const std::shared_ptr& space, const trajectory_msgs::JointTrajectory& jointTrajectory) { @@ -280,17 +314,19 @@ std::unique_ptr convertJointTrajectory( auto currState = space->createState(); const auto& waypoints = jointTrajectory.points; - for (size_t iwaypoint = 1; iwaypoint < waypoints.size(); ++iwaypoint) + for (size_t iWaypoint = 1; iWaypoint < waypoints.size(); ++iWaypoint) { - Eigen::VectorXd nextPosition, nextVelocity, nextAcceleration; - extractJointTrajectoryPoint(jointTrajectory, iwaypoint, numControlledJoints, + Eigen::VectorXd nextPosition; + Eigen::VectorXd nextVelocity; + Eigen::VectorXd nextAcceleration; + extractJointTrajectoryPoint(jointTrajectory, iWaypoint, numControlledJoints, &nextPosition, isPositionRequired, &nextVelocity, isVelocityRequired, &nextAcceleration, isAccelerationRequired, rosJointToMetaSkeletonJoint); // Compute spline coefficients for this polynomial segment. - const auto nextTimeFromStart = waypoints[iwaypoint].time_from_start.toSec(); + const auto nextTimeFromStart = waypoints[iWaypoint].time_from_start.toSec(); const auto segmentDuration = nextTimeFromStart - currTimeFromStart; const auto segmentCoefficients = fitPolynomial( 0., Eigen::VectorXd::Zero(numControlledJoints), currVelocity, currAcceleration, @@ -308,7 +344,86 @@ std::unique_ptr convertJointTrajectory( currTimeFromStart = nextTimeFromStart; } - return std::move(trajectory); + return trajectory; +} + +//============================================================================= +trajectory_msgs::JointTrajectory toRosJointTrajectory( + const aikido::trajectory::TrajectoryPtr& trajectory, double timestep) +{ + using statespace::dart::MetaSkeletonStateSpace; + using statespace::dart::SO2Joint; + using statespace::dart::RnJoint; + + if (!trajectory) + throw std::invalid_argument("Trajectory is null."); + + if (timestep <= 0) + throw std::invalid_argument("Timestep must be positive."); + + const auto space = std::dynamic_pointer_cast< + MetaSkeletonStateSpace>(trajectory->getStateSpace()); + if (!space) + { + throw std::invalid_argument( + "Trajectory is not in a MetaSkeletonStateSpace."); + } + + for (size_t i = 0; i < space->getNumSubspaces(); ++i) + { + // Supports only RnJoints and SO2Joints. + auto rnJoint = std::dynamic_pointer_cast(space->getSubspace(i)); + auto so2Joint = std::dynamic_pointer_cast(space->getSubspace(i)); + if (!rnJoint && !so2Joint) + { + throw std::invalid_argument( + "MetaSkeletonStateSpace must contain only RnJonts and SO2Joints."); + } + + // For RnJoint, supports only 1D. + if (rnJoint && rnJoint->getDimension() != 1) + { + std::stringstream message; + message << "RnJoint must be 1D. Joint " + << i << " has " << rnJoint->getDimension() << " dimensions."; + throw std::invalid_argument{message.str()}; + } + } + + util::StepSequence timeSequence{timestep, true, 0., trajectory->getDuration()}; + const auto numJoints = space->getNumSubspaces(); + const auto numWaypoints = timeSequence.getMaxSteps(); + const auto metaSkeleton = space->getMetaSkeleton(); + trajectory_msgs::JointTrajectory jointTrajectory; + jointTrajectory.joint_names.reserve(numJoints); + + for (size_t i = 0; i < numJoints; ++i) + { + const auto joint = space->getJointSpace(i)->getJoint(); + const auto jointName = joint->getName(); + auto joints = findJointByName(*metaSkeleton, jointName); + if (joints.size() > 1) + { + std::stringstream message; + message << "Metaskeleton has multiple joints with same name [" + << jointName << "]."; + throw std::invalid_argument{message.str()}; + } + jointTrajectory.joint_names.emplace_back(jointName); + } + + // Evaluate trajectory at each timestep and insert it into jointTrajectory + jointTrajectory.points.reserve(numWaypoints); + for (const auto timeFromStart : timeSequence) + { + trajectory_msgs::JointTrajectoryPoint waypoint; + + extractTrajectoryPoint(space, trajectory, timeFromStart, waypoint); + + jointTrajectory.points.emplace_back(waypoint); + } + + return jointTrajectory; } } // namespace ros diff --git a/src/control/ros/RosTrajectoryExecutionException.cpp b/src/control/ros/RosTrajectoryExecutionException.cpp new file mode 100644 index 0000000000..d8d1390b91 --- /dev/null +++ b/src/control/ros/RosTrajectoryExecutionException.cpp @@ -0,0 +1,31 @@ +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { + +//============================================================================= + +RosTrajectoryExecutionException::RosTrajectoryExecutionException( + const std::string& what, + actionlib::TerminalState terminalState) +: std::runtime_error(what) +{ +} + +RosTrajectoryExecutionException::RosTrajectoryExecutionException( + const std::string& what, + int result) +: std::runtime_error(what) +{ +} + +} // namespace ros +} // namespace control +} // namespace aikido + diff --git a/src/control/ros/RosTrajectoryExecutor.cpp b/src/control/ros/RosTrajectoryExecutor.cpp new file mode 100644 index 0000000000..3cfe573751 --- /dev/null +++ b/src/control/ros/RosTrajectoryExecutor.cpp @@ -0,0 +1,252 @@ +#include +#include +#include +#include +#include +#include +#include + +namespace aikido { +namespace control { +namespace ros { +namespace { + +using std::chrono::milliseconds; + +//============================================================================= +std::string getFollowJointTrajectoryErrorMessage(int32_t errorCode) +{ + using Result = control_msgs::FollowJointTrajectoryResult; + + switch (errorCode) + { + case Result::SUCCESSFUL: + return "successful"; + + case Result::INVALID_GOAL: + return "invalid goal"; + + case Result::INVALID_JOINTS: + return "invalid joints"; + + case Result::OLD_HEADER_TIMESTAMP: + return "old header timestamp"; + + case Result::PATH_TOLERANCE_VIOLATED: + return "path tolerance violated"; + + case Result::GOAL_TOLERANCE_VIOLATED: + return "goal tolerance violated"; + + default: + std::stringstream errorString; + errorString << "unknown (" << errorCode << ")"; + return errorString.str(); + } +} + +} // namespace + +//============================================================================= +template +RosTrajectoryExecutor::RosTrajectoryExecutor( + statespace::dart::MetaSkeletonStateSpacePtr space, + ::ros::NodeHandle node, + const std::string& serverName, + double timestep, + double goalTimeTolerance, + const DurationA& connectionTimeout, + const DurationB& connectionPollingPeriod) + : mSpace{std::move(space)} + , mNode{std::move(node)} + , mCallbackQueue{} + , mClient{mNode, serverName, &mCallbackQueue} + , mTimestep{timestep} + , mGoalTimeTolerance{goalTimeTolerance} + , mConnectionTimeout{ + std::chrono::duration_cast(connectionTimeout)} + , mConnectionPollingPeriod{ + std::chrono::duration_cast(connectionPollingPeriod)} + , mInProgress{false} +{ + if (!mSpace) + throw std::invalid_argument("Space is null."); + + if (mTimestep <= 0) + throw std::invalid_argument("Timestep must be positive."); + + if (mGoalTimeTolerance <= 0) + throw std::invalid_argument("Goal time tolerance must be positive."); +} + +//============================================================================= +RosTrajectoryExecutor::~RosTrajectoryExecutor() +{ + // Do nothing. + // TODO: Should we wait for the current trajectory to finish executing? +} + +//============================================================================= +std::future RosTrajectoryExecutor::execute( + trajectory::TrajectoryPtr traj) +{ + static const ::ros::Time invalidTime; + return execute(traj, invalidTime); +} + +//============================================================================= +std::future RosTrajectoryExecutor::execute( + trajectory::TrajectoryPtr traj, const ::ros::Time& startTime) +{ + using aikido::control::ros::toRosJointTrajectory; + using aikido::statespace::dart::MetaSkeletonStateSpace; + + if (!traj) + throw std::invalid_argument("Trajectory is null."); + + const auto space = std::dynamic_pointer_cast< + MetaSkeletonStateSpace>(traj->getStateSpace()); + if (!space) + { + throw std::invalid_argument( + "Trajectory is not in a MetaSkeletonStateSpace."); + } + + if (space != mSpace) + { + throw std::invalid_argument( + "Trajectory is not in the same StateSpace as this RosTrajectoryExecutor."); + } + + // Setup the goal properties. + // TODO: Also set goal_tolerance, path_tolerance. + control_msgs::FollowJointTrajectoryGoal goal; + goal.trajectory.header.stamp = startTime; + goal.goal_time_tolerance = ::ros::Duration(mGoalTimeTolerance); + + // Convert the Aikido trajectory into a ROS JointTrajectory. + goal.trajectory = toRosJointTrajectory( + traj, mTimestep); + + if (!waitForServer()) + throw std::runtime_error("Unable to connect to action server."); + + { + std::lock_guard lock(mMutex); + DART_UNUSED(lock); // Suppress unused variable warning + + if (mInProgress) + throw std::runtime_error("Another trajectory is in progress."); + + mPromise = std::promise(); + mInProgress = true; + mGoalHandle = mClient.sendGoal(goal, + boost::bind(&RosTrajectoryExecutor::transitionCallback, this, _1)); + + return mPromise.get_future(); + } +} + +//============================================================================= +bool RosTrajectoryExecutor::waitForServer() +{ + using Clock = std::chrono::steady_clock; + + const auto startTime = Clock::now(); + const auto endTime = startTime + mConnectionTimeout; + auto currentTime = startTime + mConnectionPollingPeriod; + + while (currentTime < endTime) + { + mCallbackQueue.callAvailable(); + + // TODO: Is this thread safe? + if (mClient.isServerConnected()) + return true; + + currentTime += mConnectionPollingPeriod; + std::this_thread::sleep_until(currentTime); + } + + return false; +} + +//============================================================================= +void RosTrajectoryExecutor::transitionCallback(GoalHandle handle) +{ + // This function assumes that mMutex is locked. + + using actionlib::TerminalState; + using Result = control_msgs::FollowJointTrajectoryResult; + + if (handle.getCommState() == actionlib::CommState::DONE) + { + std::stringstream message; + bool isSuccessful = true; + + // Check the status of the actionlib call. Note that the actionlib call can + // succeed, even if execution failed. + const auto terminalState = handle.getTerminalState(); + if (terminalState != TerminalState::SUCCEEDED) + { + message << "Action " << terminalState.toString(); + + const auto terminalMessage = terminalState.getText(); + if (!terminalMessage.empty()) + message << " (" << terminalMessage << ")"; + + mPromise.set_exception( + std::make_exception_ptr( + RosTrajectoryExecutionException(message.str(), terminalState))); + + isSuccessful = false; + } + else + { + message << "Execution failed."; + } + + // Check the status of execution. This is only possible if the actionlib + // call succeeded. + const auto result = handle.getResult(); + if (result && result->error_code != Result::SUCCESSFUL) + { + message << ": " + << getFollowJointTrajectoryErrorMessage(result->error_code); + + if (!result->error_string.empty()) + message << " (" << result->error_string << ")"; + + mPromise.set_exception(std::make_exception_ptr( + RosTrajectoryExecutionException(message.str(), result->error_code))); + + isSuccessful = false; + } + + if (isSuccessful) + mPromise.set_value(); + + mInProgress = false; + } +} + +//============================================================================= +void RosTrajectoryExecutor::spin() +{ + std::lock_guard lock(mMutex); + DART_UNUSED(lock); // Suppress unused variable warning. + + mCallbackQueue.callAvailable(); + + if (!::ros::ok() && mInProgress) + { + mPromise.set_exception( + std::make_exception_ptr(std::runtime_error("Detected ROS shutdown."))); + mInProgress = false; + } +} + +} // namespace ros +} // namespace control +} // namespace aikido + diff --git a/tests/control/ros/CMakeLists.txt b/tests/control/ros/CMakeLists.txt index 25eeaa8da5..5ad0801a2c 100644 --- a/tests/control/ros/CMakeLists.txt +++ b/tests/control/ros/CMakeLists.txt @@ -1,7 +1,7 @@ if(TARGET "${PROJECT_NAME}_control_ros") - aikido_add_test(test_Conversions - test_Conversions.cpp) - target_link_libraries(test_Conversions + aikido_add_test(test_RosTrajectoryConversions + test_RosTrajectoryConversions.cpp) + target_link_libraries(test_RosTrajectoryConversions "${PROJECT_NAME}_control_ros") endif() diff --git a/tests/control/ros/test_RosTrajectoryConversions.cpp b/tests/control/ros/test_RosTrajectoryConversions.cpp new file mode 100644 index 0000000000..428693dd4a --- /dev/null +++ b/tests/control/ros/test_RosTrajectoryConversions.cpp @@ -0,0 +1,158 @@ +#include +#include +#include +#include +#include +#include "eigen_tests.hpp" + +using dart::dynamics::SkeletonPtr; +using dart::dynamics::Skeleton; +using dart::dynamics::BodyNode; +using dart::dynamics::RevoluteJoint; +using dart::dynamics::BallJoint; +using aikido::trajectory::Spline; +using aikido::control::ros::toRosJointTrajectory; +using aikido::statespace::dart::MetaSkeletonStateSpacePtr; +using aikido::statespace::dart::MetaSkeletonStateSpace; +using aikido::tests::make_vector; + +static const double kTolerance{1e-6}; + +// TODO: We might want to merge this with test_Conversions.cpp +class ToRosJointTrajectoryTests : public testing::Test +{ +protected: + void SetUp() override + { + // Create a single-DOF skeleton. + auto skeleton = Skeleton::create(); + skeleton->createJointAndBodyNodePair(); + skeleton->getJoint(0)->setName("Joint1"); + + mStateSpace = std::make_shared(skeleton); + auto startState = mStateSpace->createState(); + mStateSpace->getState(startState); + + // Spline trajectory + mTrajectory = std::make_shared(mStateSpace, 0.); + Eigen::Matrix coeffs; + coeffs << 0., 1.; + mTrajectory->addSegment(coeffs, 0.1, startState); + + // Timestep + mTimestep = 0.1; + + // Create a 2-DOF skeleton. + auto skeleton2 = Skeleton::create(); + RevoluteJoint::Properties jointProperties; + jointProperties.mName = "Joint1"; + BodyNode::Properties bnProperties; + bnProperties.mName = "BodyNode1"; + + auto bn = skeleton2->createJointAndBodyNodePair< + RevoluteJoint, BodyNode>( + nullptr, jointProperties, bnProperties).second; + skeleton2->createJointAndBodyNodePair(bn); + skeleton2->getJoint(0)->setName("Joint1"); + skeleton2->getJoint(1)->setName("Joint2"); + skeleton2->getJoint(1)->setPosition(0, 1); + + mStateSpace2DOF = std::make_shared(skeleton2); + auto startState2DOF = mStateSpace2DOF->createState(); + mStateSpace2DOF->getState(startState2DOF); + + // Spline trajectory + mTrajectory2DOF = std::make_shared(mStateSpace2DOF, 0.); + Eigen::Matrix2d coeffs2; + coeffs2 << 3., 1., + 1., 1.; + mTrajectory2DOF->addSegment(coeffs2, 0.1, startState2DOF); + } + + std::shared_ptr mTrajectory; + MetaSkeletonStateSpacePtr mStateSpace; + + std::shared_ptr mTrajectory2DOF; + MetaSkeletonStateSpacePtr mStateSpace2DOF; + + double mTimestep; +}; + +TEST_F(ToRosJointTrajectoryTests, TrajectoryIsNull_Throws) +{ + EXPECT_THROW({ + toRosJointTrajectory(nullptr, mTimestep); + }, std::invalid_argument); +} + +TEST_F(ToRosJointTrajectoryTests, TimestepIsZero_Throws) +{ + EXPECT_THROW({ + toRosJointTrajectory(mTrajectory, 0.0); + }, std::invalid_argument); +} + +TEST_F(ToRosJointTrajectoryTests, TimestepIsNegative_Throws) +{ + EXPECT_THROW({ + toRosJointTrajectory(mTrajectory, -0.1); + }, std::invalid_argument); +} + +TEST_F(ToRosJointTrajectoryTests, SkeletonHasUnsupportedJoint_Throws) +{ + auto skeleton = Skeleton::create(); + skeleton->createJointAndBodyNodePair(); + auto space = std::make_shared(skeleton); + + auto trajectory = std::make_shared(space, 0.0); + EXPECT_THROW({ + toRosJointTrajectory(trajectory, mTimestep); + }, std::invalid_argument); +} + +TEST_F(ToRosJointTrajectoryTests, TrajectoryHasCorrectWaypoints) +{ + auto rosTrajectory = toRosJointTrajectory( + mTrajectory, mTimestep); + + EXPECT_EQ(2, rosTrajectory.points.size()); + for (int i = 0; i < 2; ++i) + { + ASSERT_DOUBLE_EQ(mTimestep*i, rosTrajectory.points[i].time_from_start.toSec()); + + auto state = mStateSpace->createState(); + Eigen::VectorXd values; + + mTrajectory->evaluate(mTimestep*i, state); + mStateSpace->convertStateToPositions(state, values); + + EXPECT_EIGEN_EQUAL(values, + make_vector(rosTrajectory.points[i].positions[0]), kTolerance); + } + ASSERT_DOUBLE_EQ(mTimestep*(rosTrajectory.points.size()-1), + mTrajectory->getEndTime()); + + // Finer timesteps + double timestep = 0.01; + auto rosTrajectory2 = toRosJointTrajectory( + mTrajectory, timestep); + + EXPECT_EQ(11, rosTrajectory2.points.size()); + for (int i = 0; i < 11; ++i) + { + ASSERT_DOUBLE_EQ(timestep*i, + rosTrajectory2.points[i].time_from_start.toSec()); + + auto state = mStateSpace->createState(); + Eigen::VectorXd values; + + mTrajectory->evaluate(timestep*i, state); + mStateSpace->convertStateToPositions(state, values); + EXPECT_EIGEN_EQUAL(values, + make_vector(rosTrajectory2.points[i].positions[0]), kTolerance); + } + ASSERT_DOUBLE_EQ(timestep*(rosTrajectory2.points.size()-1), + mTrajectory->getEndTime()); + +}