Skip to content

Commit

Permalink
Support only specifying some joints in the space. (#217)
Browse files Browse the repository at this point in the history 
* Fix bug with empty JointStateClient buffer.

* Make JointStateClient::spin wait longer.

* Revert "Make JointStateClient::spin wait longer."

This reverts commit 83a4ec7.

* Fix minor typo in RosTrajectoryExecutor.

* Support only specifying some joints in the space.

Resolves personalrobotics/rewd_controllers#13.

* Fix typos and compilation warnings.

* Optimize code for performance improvement
  • Loading branch information
@brianhou
brianhou authored Oct 2, 2017
1 parent aa8b220 commit 944d7b5
Show file tree
Hide file tree
Showing 2 changed files with 137 additions and 66 deletions.
20 changes: 19 additions & 1 deletion include/aikido/control/ros/Conversions.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,14 +14,32 @@ 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<aikido::trajectory::Spline> toSplineJointTrajectory(
const std::shared_ptr<aikido::statespace::dart::MetaSkeletonStateSpace>&
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<aikido::trajectory::Spline> toSplineJointTrajectory(
const std::shared_ptr<aikido::statespace::dart::MetaSkeletonStateSpace>&
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.
Expand Down
183 changes: 118 additions & 65 deletions src/control/ros/Conversions.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
#include <aikido/control/ros/Conversions.hpp>

#include <map>
#include <unordered_set>
#include <sstream>
#include <dart/dynamics/Joint.hpp>
#include <aikido/common/Spline.hpp>
Expand All @@ -20,17 +20,17 @@ namespace ros {
namespace {

void reorder(
std::map<size_t, size_t> indexMap,
const std::vector<std::pair<size_t, size_t>>& indexMap,
const Eigen::VectorXd& inVector,
Eigen::VectorXd* outVector);
Eigen::VectorXd& outVector);

//==============================================================================
void checkVector(
const std::string& _name,
const std::vector<double>& _values,
size_t _expectedLength,
bool _isRequired,
Eigen::VectorXd* _output)
Eigen::VectorXd& _output)
{
if (_values.empty())
{
Expand All @@ -49,9 +49,7 @@ void checkVector(
throw std::invalid_argument{message.str()};
}

if (_output)
*_output
= Eigen::Map<const Eigen::VectorXd>(_values.data(), _values.size());
_output = Eigen::Map<const Eigen::VectorXd>(_values.data(), _values.size());
}

//==============================================================================
Expand Down Expand Up @@ -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<size_t, size_t> indexMap)
const std::vector<std::pair<size_t, size_t>>& indexMap,
const std::vector<size_t>& 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);
Expand Down Expand Up @@ -191,71 +206,84 @@ void extractTrajectoryPoint(
//==============================================================================
// The rows of inVector is reordered in outVector.
void reorder(
std::map<size_t, size_t> indexMap,
const std::vector<std::pair<size_t, size_t>>& indexMap,
const Eigen::VectorXd& inVector,
Eigen::VectorXd* outVector)
Eigen::VectorXd& outVector)
{
assert(outVector != nullptr);
assert(indexMap.size() == static_cast<std::size_t>(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<const dart::dynamics::Joint*> findJointByName(
const dart::dynamics::MetaSkeleton& metaSkeleton,
const std::string& jointName)
std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(
const std::shared_ptr<MetaSkeletonStateSpace>& space,
const trajectory_msgs::JointTrajectory& jointTrajectory)
{
std::vector<const dart::dynamics::Joint*> 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<SplineTrajectory> toSplineJointTrajectory(
const std::shared_ptr<MetaSkeletonStateSpace>& 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<std::string> 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<std::size_t>(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<std::string> 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);
Expand All @@ -280,15 +308,15 @@ std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(
}

// Map joint indices between jointTrajectory and space subspaces.
std::map<size_t, size_t> rosJointToMetaSkeletonJoint;
std::vector<std::pair<size_t, size_t>> rosJointToMetaSkeletonJoint;
std::unordered_set<size_t> 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)
{
Expand All @@ -309,8 +337,29 @@ std::unique_ptr<SplineTrajectory> 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<size_t> 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.
Expand All @@ -319,13 +368,15 @@ std::unique_ptr<SplineTrajectory> 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();
Expand Down Expand Up @@ -361,13 +412,15 @@ std::unique_ptr<SplineTrajectory> 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();
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit 944d7b5

Please sign in to comment.