Skip to content

Commit

Permalink
Changing constructor to templated, removing reorderMap.
Browse files Browse the repository at this point in the history
  • Loading branch information
@gilwoolee
gilwoolee committed Apr 13, 2017
1 parent 65292c0 commit 6ad1d9c
Show file tree
Hide file tree
Showing 5 changed files with 40 additions and 165 deletions.
4 changes: 1 addition & 3 deletions include/aikido/control/ros/Conversions.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,9 @@ std::unique_ptr<aikido::trajectory::Spline> toSplineJointTrajectory(
/// Converts Aikido Trajectory to ROS JointTrajectory.
/// Supports only 1D RnJoints and SO2Joints.
/// \param[in] trajectory Aikido trajectory to be converted.
/// \param[in] indexMap Mapping between trajectory's joints and ros joints.
/// \param[in] timestep Timestep between two consecutive waypoints.
trajectory_msgs::JointTrajectory toRosJointTrajectory(
const aikido::trajectory::TrajectoryPtr& trajectory,
const std::map<std::string, size_t>& indexMap, double timestep);
const aikido::trajectory::TrajectoryPtr& trajectory, double timestep);

} // namespace ros
} // namespace control
Expand Down
10 changes: 3 additions & 7 deletions include/aikido/control/ros/RosTrajectoryExecutor.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,20 +25,17 @@ class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor
/// \param[in] serverName Name of the server to send traejctory to.
/// \param[in] timestep Step size for interpolating trajectories.
/// \param[in] goalTimeTolerance
/// \param[in] indexMap Joint index mapping from statespace to ros trajectory.
/// \param[in] connectionTimeout Timeout for server connection.
/// \param[in] connectionPollingPeriod Polling period for server connection.
template <typename DurationA, typename DurationB>
RosTrajectoryExecutor(
statespace::dart::MetaSkeletonStateSpacePtr space,
::ros::NodeHandle node,
const std::string& serverName,
double timestep,
double goalTimeTolerance,
const std::map<std::string, size_t>& jointIndexMap,
std::chrono::milliseconds connectionTimeout
= std::chrono::milliseconds{1000},
std::chrono::milliseconds connectionPollingPeriod
= std::chrono::milliseconds{20}
const DurationA& connectionTimeout = std::chrono::milliseconds{1000},
const DurationB& connectionPollingPeriod = std::chrono::milliseconds{20}
);

virtual ~RosTrajectoryExecutor();
Expand Down Expand Up @@ -81,7 +78,6 @@ class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor
bool mInProgress;
std::promise<void> mPromise;
trajectory::TrajectoryPtr mTrajectory;
std::map<std::string, size_t> mJointIndexMap;

std::mutex mMutex;
};
Expand Down
87 changes: 19 additions & 68 deletions src/control/ros/Conversions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,38 +131,33 @@ void extractJointTrajectoryPoint(
void extractTrajectoryPoint(
const std::shared_ptr<MetaSkeletonStateSpace>& space,
const aikido::trajectory::TrajectoryPtr& trajectory,
double timeFromStart, const std::map<size_t, size_t>& indexMap,
trajectory_msgs::JointTrajectoryPoint& waypoint)
double timeFromStart, trajectory_msgs::JointTrajectoryPoint& waypoint)
{
const auto numDerivatives = std::min<int>(trajectory->getNumDerivatives(), 1);
const auto timeAbsolute = trajectory->getStartTime() + timeFromStart;

const int numDof = space->getDimension();

Eigen::VectorXd tangentVector;
Eigen::VectorXd jointTangentVector;
auto state = space->createState();
trajectory->evaluate(timeAbsolute, state);
space->logMap(state, tangentVector);
assert(tangentVector.size() == space->getDimension());

// Reorder tangentVectoraccording to the index mapping
reorder(indexMap, tangentVector, &jointTangentVector);
assert(tangentVector.size() == numDof);

waypoint.time_from_start = ::ros::Duration(timeFromStart);
waypoint.positions.assign(jointTangentVector.data(),
jointTangentVector.data() + jointTangentVector.size());
waypoint.positions.assign(tangentVector.data(),
tangentVector.data() + tangentVector.size());

assert(0 <= numDerivatives && numDerivatives <= 2);
const std::array<std::vector<double>*, 2> derivatives{
&waypoint.velocities, &waypoint.accelerations};
for (int iDerivative = 1; iDerivative <= numDerivatives; ++iDerivative)
{
trajectory->evaluateDerivative(timeAbsolute, iDerivative, tangentVector);
assert(tangentVector.size() == numDofs);
assert(tangentVector.size() == numDof);

// Reorder according to the index mapping
reorder(indexMap, tangentVector, &jointTangentVector);
derivatives[iDerivative - 1]->assign(jointTangentVector.data(),
jointTangentVector.data() + jointTangentVector.size());
derivatives[iDerivative - 1]->assign(tangentVector.data(),
tangentVector.data() + tangentVector.size());
}
}

Expand Down Expand Up @@ -359,8 +354,7 @@ std::unique_ptr<SplineTrajectory> toSplineJointTrajectory(

//=============================================================================
trajectory_msgs::JointTrajectory toRosJointTrajectory(
const aikido::trajectory::TrajectoryPtr& trajectory,
const std::map<std::string, size_t>& indexMap, double timestep)
const aikido::trajectory::TrajectoryPtr& trajectory, double timestep)
{
using statespace::dart::MetaSkeletonStateSpace;
using statespace::dart::SO2Joint;
Expand Down Expand Up @@ -406,63 +400,21 @@ trajectory_msgs::JointTrajectory toRosJointTrajectory(
const auto numWaypoints = timeSequence.getMaxSteps();
const auto metaSkeleton = space->getMetaSkeleton();
trajectory_msgs::JointTrajectory jointTrajectory;
jointTrajectory.joint_names.resize(numJoints);

if (indexMap.size() != numJoints)
{
std::stringstream message;
message << "Trajectory's skeleton has " << numJoints << " joints, but "
<< "indexMap has " << indexMap.size() << " elements.";
throw std::invalid_argument{message.str()};
}

// Assign joint names
std::map<size_t, size_t> orderMap;
jointTrajectory.joint_names.reserve(numJoints);

for (auto trajToRosTrajPair : indexMap)
for (size_t i = 0; i < numJoints; ++i)
{
auto joints = findJointByName(*metaSkeleton, trajToRosTrajPair.first);
if (joints.size() == 0)
{
std::stringstream message;
message << "Metaskeleton does not have joint[" << trajToRosTrajPair.first
<< "], given by indexMap.";
throw std::invalid_argument{message.str()};
}
else if (joints.size() > 1)
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 ["
<< trajToRosTrajPair.first << "].";
<< jointName << "].";
throw std::invalid_argument{message.str()};
}
auto joint = joints[0];

if (trajToRosTrajPair.second > numJoints)
{
std::stringstream message;
message << "Metaskeleton has " << numJoints << " joints, but "
<< "indexMap maps " << trajToRosTrajPair.first
<< " to joint " << trajToRosTrajPair.second;
throw std::invalid_argument{message.str()};
}

if (jointTrajectory.joint_names[trajToRosTrajPair.second] != "")
{
std::stringstream message;
message << jointTrajectory.joint_names[trajToRosTrajPair.second]
<< " and " << trajToRosTrajPair.first << " map to the same "
<< trajToRosTrajPair.second << " joint.";
throw std::invalid_argument{message.str()};
}

jointTrajectory.joint_names[trajToRosTrajPair.second]
= trajToRosTrajPair.first;

auto metaSkeletonIndex = metaSkeleton->getIndexOf(joint);
assert(metaSkeletonIndex != dart::dynamics::INVALID_INDEX);
orderMap.emplace(std::make_pair(metaSkeletonIndex,
trajToRosTrajPair.second));
jointTrajectory.joint_names.emplace_back(jointName);
}

// Evaluate trajectory at each timestep and insert it into jointTrajectory
Expand All @@ -471,8 +423,7 @@ trajectory_msgs::JointTrajectory toRosJointTrajectory(
{
trajectory_msgs::JointTrajectoryPoint waypoint;

extractTrajectoryPoint(
space, trajectory, timeFromStart, orderMap, waypoint);
extractTrajectoryPoint(space, trajectory, timeFromStart, waypoint);

jointTrajectory.points.emplace_back(waypoint);
}
Expand Down
17 changes: 10 additions & 7 deletions src/control/ros/RosTrajectoryExecutor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@ namespace control {
namespace ros {
namespace {

using std::chrono::milliseconds;

//=============================================================================
std::string getFollowJointTrajectoryErrorMessage(int32_t errorCode)
{
Expand Down Expand Up @@ -46,25 +48,26 @@ std::string getFollowJointTrajectoryErrorMessage(int32_t errorCode)
} // namespace

//=============================================================================
template <typename DurationA, typename DurationB>
RosTrajectoryExecutor::RosTrajectoryExecutor(
statespace::dart::MetaSkeletonStateSpacePtr space,
::ros::NodeHandle node,
const std::string& serverName,
double timestep,
double goalTimeTolerance,
const std::map<std::string, size_t>& jointIndexMap,
std::chrono::milliseconds connectionTimeout,
std::chrono::milliseconds connectionPollingPeriod)
const DurationA& connectionTimeout,
const DurationB& connectionPollingPeriod)
: mSpace{std::move(space)}
, mNode{std::move(node)}
, mCallbackQueue{}
, mClient{mNode, serverName, &mCallbackQueue}
, mTimestep{timestep}
, mGoalTimeTolerance{goalTimeTolerance}
, mConnectionTimeout{connectionTimeout}
, mConnectionPollingPeriod{connectionPollingPeriod}
, mConnectionTimeout{
std::chrono::duration_cast<milliseconds>(connectionTimeout)}
, mConnectionPollingPeriod{
std::chrono::duration_cast<milliseconds>(connectionPollingPeriod)}
, mInProgress{false}
, mJointIndexMap(jointIndexMap)
{
if (!mSpace)
throw std::invalid_argument("Space is null.");
Expand Down Expand Up @@ -123,7 +126,7 @@ std::future<void> RosTrajectoryExecutor::execute(

// Convert the Aikido trajectory into a ROS JointTrajectory.
goal.trajectory = toRosJointTrajectory(
traj, mJointIndexMap, mTimestep);
traj, mTimestep);

if (!waitForServer())
throw std::runtime_error("Unable to connect to action server.");
Expand Down
87 changes: 7 additions & 80 deletions tests/control/ros/test_RosTrajectoryConversions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,6 @@ class ToRosJointTrajectoryTests : public testing::Test
// Timestep
mTimestep = 0.1;

// indexMap
mIndexMap.insert(std::make_pair("Joint1", 0));

// Create a 2-DOF skeleton.
auto skeleton2 = Skeleton::create();
RevoluteJoint::Properties jointProperties;
Expand All @@ -70,9 +67,6 @@ class ToRosJointTrajectoryTests : public testing::Test
coeffs2 << 3., 1.,
1., 1.;
mTrajectory2DOF->addSegment(coeffs2, 0.1, startState2DOF);
mIndexMap2DOF.insert(std::make_pair("Joint1", 0));
mIndexMap2DOF.insert(std::make_pair("Joint2", 1));

}

std::shared_ptr<Spline> mTrajectory;
Expand All @@ -82,58 +76,26 @@ class ToRosJointTrajectoryTests : public testing::Test
MetaSkeletonStateSpacePtr mStateSpace2DOF;

double mTimestep;
std::map<std::string, size_t> mIndexMap;
std::map<std::string, size_t> mIndexMap2DOF;
};

TEST_F(ToRosJointTrajectoryTests, TrajectoryIsNull_Throws)
{
EXPECT_THROW({
toRosJointTrajectory(nullptr, mIndexMap, mTimestep);
toRosJointTrajectory(nullptr, mTimestep);
}, std::invalid_argument);
}

TEST_F(ToRosJointTrajectoryTests, TimestepIsZero_Throws)
{
EXPECT_THROW({
toRosJointTrajectory(mTrajectory, mIndexMap, 0.0);
toRosJointTrajectory(mTrajectory, 0.0);
}, std::invalid_argument);
}

TEST_F(ToRosJointTrajectoryTests, TimestepIsNegative_Throws)
{
EXPECT_THROW({
toRosJointTrajectory(mTrajectory, mIndexMap, -0.1);
}, std::invalid_argument);
}

TEST_F(ToRosJointTrajectoryTests,
NonExistingJointInIndexMap_Throws)
{
std::map<std::string, size_t> indexMap;
indexMap.insert(std::make_pair("Joint0", 0));
EXPECT_THROW({
toRosJointTrajectory(mTrajectory, indexMap, mTimestep);
}, std::invalid_argument);
}

TEST_F(ToRosJointTrajectoryTests,
IndexMapHasDuplicateElements_Throws)
{
std::map<std::string, size_t> indexMap;
indexMap.insert(std::make_pair("Joint1", 0));
indexMap.insert(std::make_pair("Joint2", 0));

EXPECT_THROW({
toRosJointTrajectory(mTrajectory2DOF, indexMap, mTimestep);
}, std::invalid_argument);
}

TEST_F(ToRosJointTrajectoryTests, EmptyIndexMap_Throws)
{
std::map<std::string, size_t> indexMap;
EXPECT_THROW({
toRosJointTrajectory(mTrajectory, indexMap, mTimestep);
toRosJointTrajectory(mTrajectory, -0.1);
}, std::invalid_argument);
}

Expand All @@ -145,17 +107,14 @@ TEST_F(ToRosJointTrajectoryTests, SkeletonHasUnsupportedJoint_Throws)

auto trajectory = std::make_shared<Spline>(space, 0.0);
EXPECT_THROW({
toRosJointTrajectory(trajectory, mIndexMap, mTimestep);
toRosJointTrajectory(trajectory, mTimestep);
}, std::invalid_argument);
}

TEST_F(ToRosJointTrajectoryTests, TrajectoryHasCorrectWaypoints)
{
auto rosTrajectory = toRosJointTrajectory(
mTrajectory, mIndexMap, mTimestep);

for (auto it = mIndexMap.begin(); it != mIndexMap.end(); ++it)
EXPECT_EQ(it->first, rosTrajectory.joint_names[it->second]);
mTrajectory, mTimestep);

EXPECT_EQ(2, rosTrajectory.points.size());
for (int i = 0; i < 2; ++i)
Expand All @@ -174,9 +133,10 @@ TEST_F(ToRosJointTrajectoryTests, TrajectoryHasCorrectWaypoints)
ASSERT_DOUBLE_EQ(mTimestep*(rosTrajectory.points.size()-1),
mTrajectory->getEndTime());

// Finer timesteps
double timestep = 0.01;
auto rosTrajectory2 = toRosJointTrajectory(
mTrajectory, mIndexMap, timestep);
mTrajectory, timestep);

EXPECT_EQ(11, rosTrajectory2.points.size());
for (int i = 0; i < 11; ++i)
Expand All @@ -191,41 +151,8 @@ TEST_F(ToRosJointTrajectoryTests, TrajectoryHasCorrectWaypoints)
mStateSpace->convertStateToPositions(state, values);
EXPECT_EIGEN_EQUAL(values,
make_vector(rosTrajectory2.points[i].positions[0]), kTolerance);

for (auto it = mIndexMap.begin(); it != mIndexMap.end(); ++it)
EXPECT_EQ(it->first, rosTrajectory2.joint_names[it->second]);
}
ASSERT_DOUBLE_EQ(timestep*(rosTrajectory2.points.size()-1),
mTrajectory->getEndTime());

}

TEST_F(ToRosJointTrajectoryTests, DifferentOrdering)
{
auto rosTrajectory = toRosJointTrajectory(
mTrajectory2DOF, mIndexMap2DOF, mTimestep);

for (auto it = mIndexMap.begin(); it != mIndexMap.end(); ++it)
EXPECT_EQ(it->first, rosTrajectory.joint_names[it->second]);

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 = mStateSpace2DOF->createState();
Eigen::VectorXd values;

mTrajectory2DOF->evaluate(mTimestep*i, state);
mStateSpace2DOF->convertStateToPositions(state, values);

EXPECT_EIGEN_EQUAL(values,
make_vector(rosTrajectory.points[i].positions[0],
rosTrajectory.points[i].positions[1]), kTolerance);
}

ASSERT_DOUBLE_EQ(mTimestep*(rosTrajectory.points.size()-1),
mTrajectory->getEndTime());
}

0 comments on commit 6ad1d9c

Please sign in to comment.