Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ros_control/RosTrajectoryExecutor #149

Merged
merged 31 commits into from
Apr 14, 2017
Merged

ros_control/RosTrajectoryExecutor #149

merged 31 commits into from
Apr 14, 2017

Conversation

gilwoolee
Copy link
Contributor

@gilwoolee gilwoolee commented Apr 3, 2017

Need to review and write unit-tests.

I have some issue compiling this. (I can only compile it if I comment out test-relevant lines in root CMakeLists.txt.)

@gilwoolee gilwoolee self-assigned this Apr 3, 2017
@mkoval mkoval mentioned this pull request Apr 3, 2017
@gilwoolee gilwoolee requested review from jslee02 and aditya-vk April 3, 2017 23:54
@coveralls
Copy link

Coverage Status

Coverage remained the same at 85.391% when pulling b013bfd on RosTrajectoryExecutor into 4cd2795 on master.

@jslee02 jslee02 added this to the Aikido 0.1.0 milestone Apr 4, 2017
Copy link
Member

@mkoval mkoval left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is missing docstrings and unit tests. I suggest splitting the bulk of the functionality into a function that converts a aikido::Trajectory into a trajectory_msgs/JointTrajectory to simplify writing the unit tests.

For a first pass, it's probably good enough to test that function. Ideally, if you're feeling adventurous, you could also try to test the ROS integration. 😁

// actionlib and DART both #define this macro.
#undef DEPRECATED
#include <actionlib/client/action_client.h>
#undef DEPRECATED
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@jslee02 Is this still necessary? I believe DART switched to the DART_DEPRECATED macro to avoid a conflict just like this.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not necessary. DART switched to the DART_DEPRECATED since version 6.0.1.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@gilwoolee Can you switch from DEPRECATED to DART_GENERATED and remove this #undef.

add_component_targets(${PROJECT_NAME} control_ros "${PROJECT_NAME}_control_ros")
add_component_dependencies(${PROJECT_NAME} control_ros control statespace trajectory)

# coveralls_add_sources(${sources})
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Unless there is a strong reason not to, we should uncomment this.

return "goal tolerance violated";

default:
return "unknown";
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: Include errorCode here, e.g.:

std::stringstream errorString;
errorString << "unknown (" << errorCode << ")";
return errorString.str()

std::chrono::milliseconds connectionTimeout
= std::chrono::milliseconds{1000},
std::chrono::milliseconds connectionPollingRate
= std::chrono::milliseconds{20}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: Rename this to connectionPollingPeriod.

RosTrajectoryExecutor::~RosTrajectoryExecutor()
{
// Do nothing.
// TODO: Should we wait for the current trajectory to finish executing?
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@psigen @ClintLiddick Any thoughts here?

util::StepSequence timeSequence{mTimestep, true, 0., _traj->getDuration()};
const auto numDofs = mSkeleton->getNumDofs();
const auto numWaypoints = timeSequence.getMaxSteps();
const auto numDerivatives = std::min<int>(_traj->getNumDerivatives(), 2);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am not sure if this is correct - it seems like it should be valid for numDerivatives to be 1, so we can execute a piecewise linear trajectory.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why can't it be 0? Later we seem to assert that it's between 0 and 2.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good point. 👍

Eigen::VectorXd tangentVector;
auto state = space->createState();
_traj->evaluate(timeAbsolute, state);
space->logMap(state, tangentVector);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Using logMap here only makes sense on Rn and SO2 joints. We may want to add an explicit check to that effect. This is the compliment to the single-DOF restriction we have in #147.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are we supporting anything other than Rn and SO2 joints? I can add a type-check before the whole conversion happens.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think so. At least, I can't think of any other joints that admit a unique parameterization as a single real number. Anything else is difficult to shoehorn into the JointTrajectory message.

ROS_INFO("Transition: CommState = %s TerminalState = %s",
_handle.getCommState().toString().c_str(),
_handle.getTerminalState().toString().c_str());
#endif
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Delete dead code.

{
mPromise.set_exception(
std::make_exception_ptr(
std::runtime_error(message.str())));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should use a custom std::runtime_error subclass here. Otherwise, it will be difficult to distinguish between "expected" (e.g. trajectory was aborted) and "unexpected" failures (e.g. an invalid parameter).

mInProgress = false;
}

// TODO: Check mGoalTimeTolerance here as a fail-safe.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't know what this means and don't think it is necessary.

trajectory::TrajectoryPtr _traj, const ::ros::Time& _startTime);

/// Simulates mTraj. To be executed on a separate thread.
void spin();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should refactor this function into a spinOnce-style callback that we can call from the Executor implemented in #151. That is potentially important because it lets us consolidate all of our various callbacks, e.g. multiple RosTrajectoryExecutors and JointStateSubscribers, into a single thread.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nevermind! It turns out that spin() is already implemented that way. 😅

Maybe we should rename this to something more obvious? Maybe step() or tick()?

util::StepSequence timeSequence{mTimestep, true, 0., _traj->getDuration()};
const auto numDofs = mSkeleton->getNumDofs();
const auto numWaypoints = timeSequence.getMaxSteps();
const auto numDerivatives = std::min<int>(_traj->getNumDerivatives(), 2);
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why can't it be 0? Later we seem to assert that it's between 0 and 2.

}
else
{
message << "Execution failed";
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Is this correct? It seems like you'd get here if (_handle.getCommState() == actionlib::CommState::DONE) && (terminalState == TerminalState::SUCCEEDED) .

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Believe it or not, this is actually correct! 😈

There are two classes of errors in this function:

  1. The state of the actionlib state machine is reflected by getCommState(). If the goal handle finishes for any reason - even if communication is lost with the action server - the state machine transitions to CommState::DONE.
  2. Once in CommState::DONE, getTerminalState() indicates why the goal terminated. This can be TerminalState::SUCCEEDED or one of several error conditions.
  3. If the goal finished in TerminalState::SUCCEEDED, then getResult() contains the result message. In the case of FollowJointTrajectoryResult, the message contains an additional error_code.

To summarize: (1) and (2) are actionlib errors and (3) is a FollowJointTrajectory error. It's possible for the actionlib call to succeed, but for executing the trajectory to fail (in fact: this is the most common case of failure we observe in practice).

The else clause here populates message with "Execution failed" to differentiate actionlib errors from controller errors. Note that isSuccessful is not set to false, so the message will only be used if the result->error_code != Result::SUCCESSFUL condition fails below.

Does that make sense? Maybe add a comment or two to explain this? 😅

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thanks!

}

// Setup the goal properties.
// TODO: Also set the goal properties.
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What does this mean?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should eventually add support for populating the goal_tolerance, goal_time_tolerance, and path_tolerance fields in the control_msgs/FollowJointTrajectory message.

{
public:
RosTrajectoryExecutor(
::dart::dynamics::MetaSkeletonPtr skeleton,
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why do we need to tie each RosTrajectoryExecutor instance to a specific skeleton?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Great point! It seems like we could replace mSkeleton with a list of DOFs.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This probably comes from my lack of knowledge in ROS. My question was, can't a RosTrajectoryExecutor be very flexible and take trajectories covering different set of dofs? Will serverName refer to something specific like arm-controller-server, etc.?

}

const auto trajectorySkeleton = space->getMetaSkeleton();
for (const auto dof : mSkeleton->getDofs())
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If this Executor instance is not tied to a specific mSkeleton, this check will not be necessary.

util::StepSequence timeSequence{mTimestep, true, 0., _traj->getDuration()};
const auto numDofs = mSkeleton->getNumDofs();
const auto numWaypoints = timeSequence.getMaxSteps();
const auto numDerivatives = std::min<int>(_traj->getNumDerivatives(), 2);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good point. 👍

}

// Setup the goal properties.
// TODO: Also set the goal properties.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We should eventually add support for populating the goal_tolerance, goal_time_tolerance, and path_tolerance fields in the control_msgs/FollowJointTrajectory message.

{
public:
RosTrajectoryExecutor(
::dart::dynamics::MetaSkeletonPtr skeleton,
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Great point! It seems like we could replace mSkeleton with a list of DOFs.

// actionlib and DART both #define this macro.
#undef DEPRECATED
#include <actionlib/client/action_client.h>
#undef DEPRECATED
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@gilwoolee Can you switch from DEPRECATED to DART_GENERATED and remove this #undef.

}
else
{
message << "Execution failed";
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Believe it or not, this is actually correct! 😈

There are two classes of errors in this function:

  1. The state of the actionlib state machine is reflected by getCommState(). If the goal handle finishes for any reason - even if communication is lost with the action server - the state machine transitions to CommState::DONE.
  2. Once in CommState::DONE, getTerminalState() indicates why the goal terminated. This can be TerminalState::SUCCEEDED or one of several error conditions.
  3. If the goal finished in TerminalState::SUCCEEDED, then getResult() contains the result message. In the case of FollowJointTrajectoryResult, the message contains an additional error_code.

To summarize: (1) and (2) are actionlib errors and (3) is a FollowJointTrajectory error. It's possible for the actionlib call to succeed, but for executing the trajectory to fail (in fact: this is the most common case of failure we observe in practice).

The else clause here populates message with "Execution failed" to differentiate actionlib errors from controller errors. Note that isSuccessful is not set to false, so the message will only be used if the result->error_code != Result::SUCCESSFUL condition fails below.

Does that make sense? Maybe add a comment or two to explain this? 😅

@gilwoolee
Copy link
Contributor Author

@mkoval I addressed most of your comments, which resulted in two major changes:

  • a new conversion function from Trajectory to trajectory_msgs::JointTrajectory in Conversions.hpp
  • a new Exception class that inherits from std::runtime_error. I am actually a bit not sure what exactly how this class will be used and what methods we need here. Your comment was:

We should use a custom std::runtime_error subclass here. Otherwise, it will be difficult to distinguish between "expected" (e.g. trajectory was aborted) and "unexpected" failures (e.g. an invalid parameter).

Could you add to this? Should I make an enum type to classify the errors into expected and unexpected?

Unit tests are added for the new conversion method. I still need to write unit tests for RosTrajectoryExecutor, but since I've already made some big changes, it'd be great if you could comment on them while I work on the tests.

@jslee02 could you take a look too? Thanks a lot!

@coveralls
Copy link

Coverage Status

Coverage remained the same at 85.208% when pulling f750b32 on RosTrajectoryExecutor into a9b50ae on master.

Copy link
Member

@jslee02 jslee02 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I made some comments mostly on styles. The only blocker is the undeclared variable numDofs. I guess it's space->getDimension().

auto state = space->createState();
trajectory->evaluate(timeAbsolute, state);
space->logMap(state, tangentVector);
assert(tangentVector.size() == numDofs);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

numDofs is not declared.

const auto numDerivatives = std::min<int>(trajectory->getNumDerivatives(), 1);
const auto timeAbsolute = trajectory->getStartTime() + timeFromStart;

Eigen::VectorXd tangentVector, jointTangentVector;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: Declaring multiple variables on one line is not our convention.

assert(0 <= numDerivatives && numDerivatives <= 2);
const std::array<std::vector<double>*, 2> derivatives{
&waypoint.velocities, &waypoint.accelerations};
for (int iderivative = 1; iderivative <= numDerivatives; ++iderivative)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: It took a bit to understand what iderivative means. I think it would be nicer if we change the name to be easier to distinguish the index segment and derivative for the readability like iDerivative, derivativeIndex, or derivIndex.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is the convention I typically use for naming loop variables, so I suspect you're going to have a lot more comments like this on the other ros_control pull requests. 😅

I generally like this convention, but I am fine with @gilwoolee renaming the variables if you insist... 😞

/// 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.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: Please remove the leading underscores.

/// \_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.
const trajectory_msgs::JointTrajectory convertTrajectoryToRosTrajectory(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: Returning const qualifier for non-reference rvalue is meaningless. Related SO posts: 1, 2

const auto& waypoints = jointTrajectory.points;
for (size_t iwaypoint = 1; iwaypoint < waypoints.size(); ++iwaypoint)
{
Eigen::VectorXd nextPosition, nextVelocity, nextAcceleration;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nit: One variable declaration per one line. (see above)

currTimeFromStart = nextTimeFromStart;
}

return std::move(trajectory);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

std::move( ) is unnecessary. Related SO answer.

}

// For RnJoint, supports only 1D.
if (rnJoint && rnJoint->getDimension() > 1)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we need to explicitly check whether the dimension is not one because it can be zero.

std::future<void> RosTrajectoryExecutor::execute(
trajectory::TrajectoryPtr traj)
{
static const ::ros::Time invalidTime;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Passing invalid time is intentional? I think we could simply pass the current time using ros::Time::now().

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Passing an invalid time is intentional. Passing ros::Time::now() computes the timestamp in this process, whereas passing ros::Time() causes the controller to compute the timestamp when it receives the message.

This difference can be significant if the client and server are running on different computers.

/// \_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.
const trajectory_msgs::JointTrajectory convertTrajectoryToRosTrajectory(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would like to suggest patterning the two convert function names. convertJointTrajectory() doesn't convey the original and target type in the function name unlike to the latter function name, and convertTrajectoryToRosTrajectory() uses only Trajectory to refer JointTrajectory unlike to the former function name.

We could put both of original and target type name to the function name like convertJointTrajectoryToRosJointTrajectory() but this might be too long. So my preference is including only the goal type to the function name so that we can call the correct conversion functions depending on the target type. We don't need to worry about the original type since functions can overload for the argument types, which is the original type, but not for the return type.

TL;DR: I suggest renaming the two function names to something like:

convertToSplineJointTrajectory()
convertToRosJointTrajectory()

// or
toSplineJointTrajectory()
toRosJointTrajectory()


RosTrajectoryExecutionException(
const std::string& what,
actionlib::TerminalState terminalState);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

TerminalState is an enum, so there is no point in avoiding a copy here.

std::chrono::milliseconds connectionTimeout
= std::chrono::milliseconds{1000},
std::chrono::milliseconds connectionPollingPeriod
= std::chrono::milliseconds{20}
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am generally supportive of this, but I am not sure how to implement it. We store these durations in member variables, so this would require templateing the entire class on DurationA and DurationB. It's possible we could do some type erasure shenaniganz to avoid this, but I don't think it's worth the effort at this point.


bool waitForServer();

void transitionCallback(GoalHandle handle);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Nope. This function is passed as a callback to ActionClient::sendGoal, which expects this signature.

assert(0 <= numDerivatives && numDerivatives <= 2);
const std::array<std::vector<double>*, 2> derivatives{
&waypoint.velocities, &waypoint.accelerations};
for (int iderivative = 1; iderivative <= numDerivatives; ++iderivative)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is the convention I typically use for naming loop variables, so I suspect you're going to have a lot more comments like this on the other ros_control pull requests. 😅

I generally like this convention, but I am fine with @gilwoolee renaming the variables if you insist... 😞

void reorder(std::map<size_t, size_t> indexMap,
const Eigen::VectorXd& inVector, Eigen::VectorXd* outVector)
{
*outVector = Eigen::VectorXd::Zero(inVector.size());
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a helper function in an anonymous namespace, so I suggest just adding an assert.

std::future<void> RosTrajectoryExecutor::execute(
trajectory::TrajectoryPtr traj)
{
static const ::ros::Time invalidTime;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Passing an invalid time is intentional. Passing ros::Time::now() computes the timestamp in this process, whereas passing ros::Time() causes the controller to compute the timestamp when it receives the message.

This difference can be significant if the client and server are running on different computers.

@coveralls
Copy link

Coverage Status

Coverage remained the same at 85.208% when pulling 9b9ba1e on RosTrajectoryExecutor into a59975f on master.

@gilwoolee gilwoolee changed the base branch from master to convertJointTrajectory April 12, 2017 01:37
@coveralls
Copy link

Coverage Status

Coverage decreased (-0.01%) to 85.208% when pulling 65292c0 on RosTrajectoryExecutor into 8f1a94b on convertJointTrajectory.

@coveralls
Copy link

Coverage Status

Coverage decreased (-0.01%) to 85.208% when pulling 6ad1d9c on RosTrajectoryExecutor into 8f1a94b on convertJointTrajectory.

Copy link
Member

@jslee02 jslee02 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This recent change looks good.

However, the ROS control code is not being tested by CI since actionlib is not found.

jslee02 added 3 commits April 13, 2017 14:15
Conflicts:
	include/aikido/control/ros/Conversions.hpp
	src/control/ros/CMakeLists.txt
	src/control/ros/Conversions.cpp
	tests/control/ros/CMakeLists.txt
@jslee02 jslee02 changed the base branch from convertJointTrajectory to master April 13, 2017 18:38
jslee02 added 2 commits April 13, 2017 22:37
and use find modules to find actionlib and control_msgs
@coveralls
Copy link

Coverage Status

Coverage decreased (-2.3%) to 82.204% when pulling 2537bfd on RosTrajectoryExecutor into 2b21ddd on master.

Copy link
Member

@jslee02 jslee02 left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay! CI now builds and tests all the Aikido components (except for rviz testing) without zero warnings.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants