From 944d7b5c3afc3278fb6f615afee7a2a9b72c2326 Mon Sep 17 00:00:00 2001 From: Brian Hou Date: Mon, 2 Oct 2017 14:30:11 -0700 Subject: [PATCH] Support only specifying some joints in the space. (#217) * Fix bug with empty JointStateClient buffer. * Make JointStateClient::spin wait longer. * Revert "Make JointStateClient::spin wait longer." This reverts commit 83a4ec756ab6549bcbd2a3efb529c60f8df1d12f. * Fix minor typo in RosTrajectoryExecutor. * Support only specifying some joints in the space. Resolves https://github.com/personalrobotics/rewd_controllers/issues/13. * Fix typos and compilation warnings. * Optimize code for performance improvement --- include/aikido/control/ros/Conversions.hpp | 20 ++- src/control/ros/Conversions.cpp | 183 +++++++++++++-------- 2 files changed, 137 insertions(+), 66 deletions(-) diff --git a/include/aikido/control/ros/Conversions.hpp b/include/aikido/control/ros/Conversions.hpp index 85b25beee0..2c9ed778ca 100644 --- a/include/aikido/control/ros/Conversions.hpp +++ b/include/aikido/control/ros/Conversions.hpp @@ -14,7 +14,7 @@ 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. -// Subspaces must be either R1Joint or SO2Joint. +/// Subspaces must be either R1Joint or SO2Joint. /// \param[in] jointTrajectory ROS JointTrajectory to be converted. /// \return Spline trajectory. std::unique_ptr toSplineJointTrajectory( @@ -22,6 +22,24 @@ std::unique_ptr toSplineJointTrajectory( space, const trajectory_msgs::JointTrajectory& jointTrajectory); +/// Converts a ROS JointTrajectory into an aikido's Spline trajectory. +/// This method only handles single-DOF joints. +/// \param[in] space MetaSkeletonStateSpace for Spline trajectory. +/// Subspaces must be either R1Joint or SO2Joint. +/// \param[in] jointTrajectory ROS JointTrajectory to be converted. +/// \param[in] startPositions If empty, jointTrajectory must specify all joints +/// in the space. If non-empty, jointTrajectory only needs to +/// specify a subset of the joints in the space. The remaining +/// unspecified joint positions are filled with the corresponding +/// positions in startPositions, which must have the same size as +/// the space. Velocities and accelerations are filled with zero. +/// \return Spline trajectory. +std::unique_ptr toSplineJointTrajectory( + const std::shared_ptr& + space, + const trajectory_msgs::JointTrajectory& jointTrajectory, + const Eigen::VectorXd& startPositions); + /// Converts Aikido Trajectory to ROS JointTrajectory. /// Supports only 1D RnJoints and SO2Joints. /// \param[in] trajectory Aikido trajectory to be converted. diff --git a/src/control/ros/Conversions.cpp b/src/control/ros/Conversions.cpp index 10f9825f64..81f86e2b5b 100644 --- a/src/control/ros/Conversions.cpp +++ b/src/control/ros/Conversions.cpp @@ -1,6 +1,6 @@ #include -#include +#include #include #include #include @@ -20,9 +20,9 @@ namespace ros { namespace { void reorder( - std::map indexMap, + const std::vector>& indexMap, const Eigen::VectorXd& inVector, - Eigen::VectorXd* outVector); + Eigen::VectorXd& outVector); //============================================================================== void checkVector( @@ -30,7 +30,7 @@ void checkVector( const std::vector& _values, size_t _expectedLength, bool _isRequired, - Eigen::VectorXd* _output) + Eigen::VectorXd& _output) { if (_values.empty()) { @@ -49,9 +49,7 @@ void checkVector( throw std::invalid_argument{message.str()}; } - if (_output) - *_output - = Eigen::Map(_values.data(), _values.size()); + _output = Eigen::Map(_values.data(), _values.size()); } //============================================================================== @@ -105,37 +103,54 @@ void extractJointTrajectoryPoint( const trajectory_msgs::JointTrajectory& _trajectory, size_t _index, size_t _numDofs, - Eigen::VectorXd* _positions, + Eigen::VectorXd& _positions, bool _positionsRequired, - Eigen::VectorXd* _velocities, + Eigen::VectorXd& _velocities, bool _velocitiesRequired, - Eigen::VectorXd* _accelerations, + Eigen::VectorXd& _accelerations, bool _accelerationsRequired, - std::map indexMap) + const std::vector>& indexMap, + const std::vector& unspecifiedJoints, // joints to get from startPositions + const Eigen::VectorXd& startPositions) { const auto& waypoint = _trajectory.points[_index]; + // TODO: are these / should these be copies? + auto positions = waypoint.positions; + auto velocities = waypoint.velocities; + auto accelerations = waypoint.accelerations; + + positions.reserve(positions.size() + unspecifiedJoints.size()); + for (auto unspecifiedJoint : unspecifiedJoints) + positions.emplace_back(startPositions[unspecifiedJoint]); + + if (velocities.size() > 0) + velocities.resize(velocities.size() + unspecifiedJoints.size(), 0.0); + + if (accelerations.size() > 0) + accelerations.resize(accelerations.size() + unspecifiedJoints.size(), 0.0); + try { Eigen::VectorXd trajPos, trajVel, trajAccel; checkVector( "positions", - waypoint.positions, + positions, _numDofs, _positionsRequired, - &trajPos); + trajPos); checkVector( "velocities", - waypoint.velocities, + velocities, _numDofs, _velocitiesRequired, - &trajVel); + trajVel); checkVector( "accelerations", - waypoint.accelerations, + accelerations, _numDofs, _accelerationsRequired, - &trajAccel); + trajAccel); if (trajPos.size() > 0) reorder(indexMap, trajPos, _positions); @@ -191,71 +206,84 @@ void extractTrajectoryPoint( //============================================================================== // The rows of inVector is reordered in outVector. void reorder( - std::map indexMap, + const std::vector>& indexMap, const Eigen::VectorXd& inVector, - Eigen::VectorXd* outVector) + Eigen::VectorXd& outVector) { - assert(outVector != nullptr); assert(indexMap.size() == static_cast(inVector.size())); - outVector->resize(inVector.size()); + outVector.resize(inVector.size()); for (auto index : indexMap) - (*outVector)[index.second] = inVector[index.first]; + outVector[index.second] = inVector[index.first]; } +} // namespace + //============================================================================== -std::vector findJointByName( - const dart::dynamics::MetaSkeleton& metaSkeleton, - const std::string& jointName) +std::unique_ptr toSplineJointTrajectory( + const std::shared_ptr& space, + const trajectory_msgs::JointTrajectory& jointTrajectory) { - std::vector joints; - - for (size_t i = 0; i < metaSkeleton.getNumJoints(); ++i) - { - if (metaSkeleton.getJoint(i)->getName() == jointName) - joints.emplace_back(metaSkeleton.getJoint(i)); - } - - return joints; + return toSplineJointTrajectory(space, jointTrajectory, Eigen::VectorXd()); } -} // namespace - //============================================================================== std::unique_ptr toSplineJointTrajectory( const std::shared_ptr& space, - const trajectory_msgs::JointTrajectory& jointTrajectory) + const trajectory_msgs::JointTrajectory& jointTrajectory, + const Eigen::VectorXd& startPositions) { if (!space) throw std::invalid_argument{"StateSpace must be non-null."}; + bool paddingMode = false; + + // Check that the number of joints specified in JointTrajectory or start + // position match the state space. + // 1. A trajectory must either specify as many joints as are in the space (no + // padding) or fewer joints than are in the space (padding). + // 2. A start position must either have a size of 0 (no padding) or a size + // matching the number of joints in the space (padding). const auto numControlledJoints = space->getNumSubspaces(); - if (jointTrajectory.joint_names.size() != numControlledJoints) + const auto numTrajectoryJoints = jointTrajectory.joint_names.size(); + if (numTrajectoryJoints > numControlledJoints) { std::stringstream message; message << "Incorrect number of joints: expected " << numControlledJoints - << ", got " << jointTrajectory.joint_names.size() << "."; + << " or a non-negative number less than that, got " + << numTrajectoryJoints << "."; throw std::invalid_argument{message.str()}; } - - // Check that the names in jointTrajectory are unique. - std::vector joint_names; - joint_names.reserve(numControlledJoints); - for (size_t i = 0; i < numControlledJoints; ++i) - joint_names.emplace_back(jointTrajectory.joint_names[i]); - std::sort(joint_names.begin(), joint_names.end()); - for (size_t i = 0; i < numControlledJoints - 1; ++i) + else if (numTrajectoryJoints < numControlledJoints) { - if (joint_names[i] == joint_names[i + 1]) + paddingMode = true; + + if (static_cast(startPositions.size()) != numControlledJoints) { std::stringstream message; - message << "JointTrajectory has multiple joints with same name [" - << joint_names[i] << "]."; + message << "Incorrect number of joints in configuration: expected " + << numControlledJoints << ", got " << startPositions.size() + << "."; throw std::invalid_argument{message.str()}; } } + // When numTrajectoryJoints == numControlledJoints, don't care the size of + // startPositions + + // Check that the names in jointTrajectory are unique. + std::vector joint_names( + jointTrajectory.joint_names.begin(), jointTrajectory.joint_names.end()); + std::sort(joint_names.begin(), joint_names.end()); + auto duplicate = std::adjacent_find(joint_names.begin(), joint_names.end()); + if (duplicate != joint_names.end()) + { + std::stringstream message; + message << "JointTrajectory has multiple joints with same name [" + << *duplicate << "]."; + throw std::invalid_argument{message.str()}; + } // Check that all joints are R1Joint or SO2JOint state spaces. - for (size_t i = 0; i < space->getNumSubspaces(); ++i) + for (size_t i = 0; i < numControlledJoints; ++i) { auto joint = space->getJointSpace(i)->getJoint(); auto jointSpace = space->getSubspace(i); @@ -280,15 +308,15 @@ std::unique_ptr toSplineJointTrajectory( } // Map joint indices between jointTrajectory and space subspaces. - std::map rosJointToMetaSkeletonJoint; + std::vector> rosJointToMetaSkeletonJoint; + std::unordered_set specifiedMetaSkeletonJoints; auto metaSkeleton = space->getMetaSkeleton(); - auto nJoints = jointTrajectory.joint_names.size(); - for (size_t trajIndex = 0; trajIndex < nJoints; ++trajIndex) + for (size_t trajIndex = 0; trajIndex < numTrajectoryJoints; ++trajIndex) { const auto& jointName = jointTrajectory.joint_names[trajIndex]; - auto joints = findJointByName(*metaSkeleton, jointName); + auto joints = metaSkeleton->getJoints(jointName); if (joints.size() == 0) { @@ -309,8 +337,29 @@ std::unique_ptr toSplineJointTrajectory( auto metaSkeletonIndex = metaSkeleton->getIndexOf(joint); assert(metaSkeletonIndex != dart::dynamics::INVALID_INDEX); - rosJointToMetaSkeletonJoint.emplace( + rosJointToMetaSkeletonJoint.emplace_back( std::make_pair(trajIndex, metaSkeletonIndex)); + specifiedMetaSkeletonJoints.insert(metaSkeletonIndex); + } + + // Add unspecified joint mappings to rosJointToMetaSkeletonJoint + std::vector unspecifiedMetaSkeletonJoints; + unspecifiedMetaSkeletonJoints.reserve(numControlledJoints - numTrajectoryJoints); + if (paddingMode) + { + for (size_t metaSkeletonIndex = 0; metaSkeletonIndex < numControlledJoints; + ++metaSkeletonIndex) + { + if (specifiedMetaSkeletonJoints.find(metaSkeletonIndex) + == specifiedMetaSkeletonJoints.end()) + { + // Unspecified joints will be added to the end of the vector + unspecifiedMetaSkeletonJoints.emplace_back(metaSkeletonIndex); + rosJointToMetaSkeletonJoint.emplace_back( + std::make_pair( + rosJointToMetaSkeletonJoint.size(), metaSkeletonIndex)); + } + } } // Extract the first waypoint to infer the dimensionality of the trajectory. @@ -319,13 +368,15 @@ std::unique_ptr toSplineJointTrajectory( jointTrajectory, 0, numControlledJoints, - &currPosition, + currPosition, true, - &currVelocity, + currVelocity, false, - &currAcceleration, + currAcceleration, false, - rosJointToMetaSkeletonJoint); + rosJointToMetaSkeletonJoint, + unspecifiedMetaSkeletonJoints, + startPositions); const auto& firstWaypoint = jointTrajectory.points.front(); auto currTimeFromStart = firstWaypoint.time_from_start.toSec(); @@ -361,13 +412,15 @@ std::unique_ptr toSplineJointTrajectory( jointTrajectory, iWaypoint, numControlledJoints, - &nextPosition, + nextPosition, isPositionRequired, - &nextVelocity, + nextVelocity, isVelocityRequired, - &nextAcceleration, + nextAcceleration, isAccelerationRequired, - rosJointToMetaSkeletonJoint); + rosJointToMetaSkeletonJoint, + unspecifiedMetaSkeletonJoints, + startPositions); // Compute spline coefficients for this polynomial segment. const auto nextTimeFromStart = waypoints[iWaypoint].time_from_start.toSec(); @@ -452,7 +505,7 @@ trajectory_msgs::JointTrajectory toRosJointTrajectory( { const auto joint = space->getJointSpace(i)->getJoint(); const auto jointName = joint->getName(); - auto joints = findJointByName(*metaSkeleton, jointName); + auto joints = metaSkeleton->getJoints(jointName); if (joints.size() > 1) { std::stringstream message;