Feature/vector field planner #246
Conversation
Codecov Report
@@ Coverage Diff @@
## master #246 +/- ##
==========================================
+ Coverage 70.48% 70.91% +0.42%
==========================================
Files 181 173 -8
Lines 5476 5126 -350
Branches 870 811 -59
==========================================
- Hits 3860 3635 -225
+ Misses 1092 1002 -90
+ Partials 524 489 -35
|
CHANGELOG.md
Outdated
|
|
||
| * Planner | ||
|
|
||
| * Added vector field planner [#246](https://github.com/personalrobotics/aikido/pull/246) |
There was a problem hiding this comment.
Could you move this under the below section for 0.2.0. It's duplicated.
| @@ -0,0 +1,87 @@ | |||
| #ifndef AIKIDO_PLANNER_VECTOR_FIELD_PLANNER_HPP_ | |||
| #define AIKIDO_PLANNER_VECTOR_FIELD_PLANNER_HPP_ | |||
There was a problem hiding this comment.
Nit: AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNER_HPP_. The convention is not to put space for a name or a directory name (e.g., not VECTOR_FIELD_PLANNER but VECTORFIELDPLANNER).
| CACHE_AND_CONTINUE, | ||
| CONTINUE | ||
| }; | ||
| }; |
There was a problem hiding this comment.
If you put this enum in a struct is to avoid scope confliction, you could consider using enum class, which is introduced in C++11.
| /// Callback function of joint velocity calculated by a vector field and a | ||
| /// MetaSkeleton. | ||
| /// \param stateSpace MetaSkeleton state space | ||
| /// \param t planned time of a given duration |
There was a problem hiding this comment.
Nit: I think the convention is beginning with a capital letter as Planned time .... There are many docstring begin with a lower letter.
| /// MetaSkeleton. | ||
| /// \param stateSpace MetaSkeleton state space | ||
| /// \param t planned time of a given duration | ||
| /// \param qd joint velocity calculated by a vector field and meta skeleton |
There was a problem hiding this comment.
Nit: \param[out] qt Joint velocity ....
|
|
||
| /* | ||
| * VectorFieldTerminated | ||
| */ |
There was a problem hiding this comment.
Nit: Please remove these redundant comments and add the function definition splitter.
| * DofLimitError | ||
| */ | ||
| DofLimitError::DofLimitError( | ||
| dart::dynamics::DegreeOfFreedom* dof, std::string const& what_arg) |
There was a problem hiding this comment.
Nit: const std::string& whatArg
| @@ -0,0 +1,31 @@ | |||
| #ifndef AIKIDO_PLANNER_VECTORFIELDPLANNEREXCEPTIONS_H_ | |||
There was a problem hiding this comment.
Please move this file to the include directory.
| @@ -0,0 +1,31 @@ | |||
| #ifndef AIKIDO_PLANNER_VECTORFIELDPLANNEREXCEPTIONS_H_ | |||
| #define AIKIDO_PLANNER_VECTORFIELDPLANNEREXCEPTIONS_H_ | |||
| #include <string> | |||
There was a problem hiding this comment.
Nit: Add an empty line after the header guard.
| @@ -0,0 +1,31 @@ | |||
| #ifndef AIKIDO_PLANNER_VECTORFIELDPLANNEREXCEPTIONS_H_ | |||
| #define AIKIDO_PLANNER_VECTORFIELDPLANNEREXCEPTIONS_H_ | |||
There was a problem hiding this comment.
Nit: AIKIDO_PLANNER_VECTORFIELD_VECTORFIELDPLANNEREXCEPTIONS_HPP_
There was a problem hiding this comment.
I don't know how we exactly started to using nested namespace and subdirectory for aikido::planner::ompl and aikido::planner::parabolic, but I guess this is because these are dependent on external libraries. If that's true, then I think we may don't want to the same thing for vector field planner because it doesn't have any external dependencies. @mkoval, @psigen, @brianhou, @gilwoolee: Do we have the convention here?
Minor comments
- Please re-run
make format. - Put the source files the associated header files are under
detaildirectory also todetaildirectory accordingly. - It seems
const std::shared_ptr<T>is used wherestd::shared_ptr<const T>should be used.
| } | ||
|
|
||
| //============================================================================== | ||
|
|
There was a problem hiding this comment.
Nit: Remove empty line between a separator and a function definition.
| } | ||
|
|
||
| //============================================================================== | ||
|
|
There was a problem hiding this comment.
Nit: Remove empty line between a separator and a function definition.
| namespace planner { | ||
| namespace vectorfield { | ||
|
|
||
| static void CheckDofLimits( |
There was a problem hiding this comment.
Nit: Function name should use camelCase.
| } | ||
|
|
||
| //============================================================================== | ||
|
|
There was a problem hiding this comment.
Nit: Remove empty line between a separator and a function definition.
| } | ||
|
|
||
| //============================================================================== | ||
|
|
There was a problem hiding this comment.
Nit: Remove empty line between a separator and a function definition.
| double _optimizationTolerance, | ||
| double _timestep, | ||
| double _padding, | ||
| Eigen::VectorXd* _jointVelocity) |
There was a problem hiding this comment.
This also should be changed to reference as Eigen::VectorXd&.
| bool computeJointVelocityFromTwist( | ||
| const Eigen::Vector6d& _desiredTwist, | ||
| const aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace, | ||
| const dart::dynamics::BodyNodePtr _bodyNode, |
There was a problem hiding this comment.
Nit: const dart::dynamics::ConstBodyNodePtr&
|
|
||
| bool computeJointVelocityFromTwist( | ||
| const Eigen::Vector6d& _desiredTwist, | ||
| const aikido::statespace::dart::MetaSkeletonStateSpacePtr _stateSpace, |
There was a problem hiding this comment.
Let's define const shared_ptr type as using ConstMetaSkeletonStateSpacePtr = std::shared_ptr<const MetaSkeletonStateSpace> in MetaSkeletonStateSpace.hpp and replace this to const aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr&
There was a problem hiding this comment.
This change seems not supporting MetaSkeletonStateSpaceSaver.
|
|
||
| const auto problem = std::make_shared<Problem>(numDofs); | ||
| problem->setLowerBounds(lowerLimits); | ||
| problem->setUpperBounds(upperLimits); |
There was a problem hiding this comment.
I think we could optimize this code a bit by passing velocityLowerLimits and velocityUpperLimits instead of passing the copies of them. This could be simply done by changing the above if-statement as:
if (position
< positionLowerLimit - _timestep * velocityLowerLimit + _padding)
velocityLowerLimits[i] = 0.0;
if (position
> positionUpperLimit - _timestep * velocityUpperLimit - _padding)
velocityUpperLimits[i] = 0.0;|
|
||
| if (position | ||
| < positionLowerLimit - _timestep * velocityLowerLimit + _padding) | ||
| lowerLimits[i] = 0; |
There was a problem hiding this comment.
I think this restriction can be a little bit relaxed. Current logic just sets the velocity to zero when the integrated position with the velocity exceeds the padded position limit. However, if the current position doesn't yet exceed the padded position limit, then it would be fine to integrate the position towards the padded position limit with the proper velocity, which might be less than the velocity limit.
I propose to change the logic as
lowerLimits[i] = std::max(velocityLowerLimits[i], ((velocityLowerLimits[i] + _padding) - position) / _timestep);Similarly, below can be changed as
upperLimits[i] = std::min(velocityUpperLimits[i], ((velocityUpperLimits[i] - _padding) - position) / _timestep);One (positive I think) side effect is that the new logic will compute lower/upperLimits that will fix the current position limit violations if there are.
[A vector field planner that generate trajectories based on defined vector fields. Currently only a vector field for straight hand movement is added.]
Before creating a pull request
make formatBefore merging a pull request
CHANGELOG.md