-
Notifications
You must be signed in to change notification settings - Fork 30
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
Conversation
There was a problem hiding this 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 |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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
.
src/control/ros/CMakeLists.txt
Outdated
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}) |
There was a problem hiding this comment.
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"; |
There was a problem hiding this comment.
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} |
There was a problem hiding this comment.
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? |
There was a problem hiding this comment.
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); |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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); |
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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()))); |
There was a problem hiding this comment.
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. |
There was a problem hiding this comment.
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(); |
There was a problem hiding this comment.
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 RosTrajectoryExecutor
s and JointStateSubscriber
s, into a single thread.
There was a problem hiding this comment.
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); |
There was a problem hiding this comment.
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"; |
There was a problem hiding this comment.
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)
.
There was a problem hiding this comment.
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:
- The state of the
actionlib
state machine is reflected bygetCommState()
. If the goal handle finishes for any reason - even if communication is lost with the action server - the state machine transitions toCommState::DONE
. - Once in
CommState::DONE
,getTerminalState()
indicates why the goal terminated. This can beTerminalState::SUCCEEDED
or one of several error conditions. - If the goal finished in
TerminalState::SUCCEEDED
, thengetResult()
contains the result message. In the case ofFollowJointTrajectoryResult
, the message contains an additionalerror_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? 😅
There was a problem hiding this comment.
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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
What does this mean?
There was a problem hiding this comment.
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, |
There was a problem hiding this comment.
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
?
There was a problem hiding this comment.
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.
There was a problem hiding this comment.
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()) |
There was a problem hiding this comment.
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); |
There was a problem hiding this comment.
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. |
There was a problem hiding this comment.
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, |
There was a problem hiding this comment.
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 |
There was a problem hiding this comment.
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"; |
There was a problem hiding this comment.
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:
- The state of the
actionlib
state machine is reflected bygetCommState()
. If the goal handle finishes for any reason - even if communication is lost with the action server - the state machine transitions toCommState::DONE
. - Once in
CommState::DONE
,getTerminalState()
indicates why the goal terminated. This can beTerminalState::SUCCEEDED
or one of several error conditions. - If the goal finished in
TerminalState::SUCCEEDED
, thengetResult()
contains the result message. In the case ofFollowJointTrajectoryResult
, the message contains an additionalerror_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? 😅
@mkoval I addressed most of your comments, which resulted in two major changes:
Could you add to this? Should I make an enum type to classify the errors into Unit tests are added for the new conversion method. I still need to write unit tests for @jslee02 could you take a look too? Thanks a lot! |
There was a problem hiding this 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()
.
src/control/ros/Conversions.cpp
Outdated
auto state = space->createState(); | ||
trajectory->evaluate(timeAbsolute, state); | ||
space->logMap(state, tangentVector); | ||
assert(tangentVector.size() == numDofs); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
numDofs
is not declared.
src/control/ros/Conversions.cpp
Outdated
const auto numDerivatives = std::min<int>(trajectory->getNumDerivatives(), 1); | ||
const auto timeAbsolute = trajectory->getStartTime() + timeFromStart; | ||
|
||
Eigen::VectorXd tangentVector, jointTangentVector; |
There was a problem hiding this comment.
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.
src/control/ros/Conversions.cpp
Outdated
assert(0 <= numDerivatives && numDerivatives <= 2); | ||
const std::array<std::vector<double>*, 2> derivatives{ | ||
&waypoint.velocities, &waypoint.accelerations}; | ||
for (int iderivative = 1; iderivative <= numDerivatives; ++iderivative) |
There was a problem hiding this comment.
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
.
There was a problem hiding this comment.
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. |
There was a problem hiding this comment.
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( |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
src/control/ros/Conversions.cpp
Outdated
const auto& waypoints = jointTrajectory.points; | ||
for (size_t iwaypoint = 1; iwaypoint < waypoints.size(); ++iwaypoint) | ||
{ | ||
Eigen::VectorXd nextPosition, nextVelocity, nextAcceleration; |
There was a problem hiding this comment.
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)
src/control/ros/Conversions.cpp
Outdated
currTimeFromStart = nextTimeFromStart; | ||
} | ||
|
||
return std::move(trajectory); |
There was a problem hiding this comment.
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.
src/control/ros/Conversions.cpp
Outdated
} | ||
|
||
// For RnJoint, supports only 1D. | ||
if (rnJoint && rnJoint->getDimension() > 1) |
There was a problem hiding this comment.
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; |
There was a problem hiding this comment.
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()
.
There was a problem hiding this comment.
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( |
There was a problem hiding this comment.
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); |
There was a problem hiding this comment.
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} |
There was a problem hiding this comment.
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 duration
s in member variables, so this would require template
ing 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); |
There was a problem hiding this comment.
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.
src/control/ros/Conversions.cpp
Outdated
assert(0 <= numDerivatives && numDerivatives <= 2); | ||
const std::array<std::vector<double>*, 2> derivatives{ | ||
&waypoint.velocities, &waypoint.accelerations}; | ||
for (int iderivative = 1; iderivative <= numDerivatives; ++iderivative) |
There was a problem hiding this comment.
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... 😞
src/control/ros/Conversions.cpp
Outdated
void reorder(std::map<size_t, size_t> indexMap, | ||
const Eigen::VectorXd& inVector, Eigen::VectorXd* outVector) | ||
{ | ||
*outVector = Eigen::VectorXd::Zero(inVector.size()); |
There was a problem hiding this comment.
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; |
There was a problem hiding this comment.
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.
…obotics/aikido into convertJointTrajectory
…obotics/aikido into RosTrajectoryExecutor
There was a problem hiding this 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.
Conflicts: include/aikido/control/ros/Conversions.hpp src/control/ros/CMakeLists.txt src/control/ros/Conversions.cpp tests/control/ros/CMakeLists.txt
and use find modules to find actionlib and control_msgs
There was a problem hiding this 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.
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.)