WIP: Refactor trajectory #278
Conversation
Feature/point on link
…ich is now derived from ConstraintSpec
…g contact_points_ attribute, still need to Construct WalkCycle from FootContactState
gtdynamics/utils/Trajectory.h
Outdated
| phases_.insert(phases_.end(), phases_i.begin(), phases_i.end()); | ||
| } | ||
|
|
||
| for (auto&& phase : phases_) { |
There was a problem hiding this comment.
document. But, also, should it be only done for walk_cycle.phases() ?
There was a problem hiding this comment.
contact_points_ was a member of WalkCycle and I moved it to trajectory because we took out walk_cycle_ as a member from Trajectory (also did this for other members functions from WalkCycle).
To build the contact_points_ vector now I Take from each phase in the trajectory the unique contact points and add them them to contact_points_. Is there a reason to do this only for the walk_cycle.phases()? These contact_points_ should be unique (that's what was done in WalkCycle as well).
| throw std::runtime_error("Contact Point was not found."); | ||
| } | ||
|
|
||
| NonlinearFactorGraph FootContactState::contactPointObjectives( |
There was a problem hiding this comment.
I think we might want to put this in KinematicsPhase instead?
There was a problem hiding this comment.
I am a bit confused by this. We have a pointGoalObjectives method under KinematicsPhase (which calls a similar method in KinematicsInterval). Both FootContactState::contactPointObjectives and KInematicsPhase::pointGoalObjectives push pointGoalFactors to the factor graph (differently). I am not sure what is the difference and when each of them is used.
There was a problem hiding this comment.
Something to chat about; maybe prepare one slide for meeting on refactored structure?
|
@dellaert ready for review |
There was a problem hiding this comment.
OK, this is getting very close. Biggest issue is the Phase methods that I don't think should exists as Interval already implements them, and you should be able to call those with a Phase (as it is an interval).
Also, phase should be, like trajectory, generic. Added comments below.
|
@danbarla pls re-request review when you want me to take another look. |
There was a problem hiding this comment.
Done !!! I did not see you merge in master, so please do that before merging this PR. Also, there is one issue with static_cast you have to fix, but other than that, done!
Since I am the author of the original PR, I can't actually approve. I'll ask Gerry to do a final review and approve.
|
@gchenfc could you do a final (loose, more FYI) review and approve? |
gtdynamics/utils/WalkCycle.cpp
Outdated
| boost::dynamic_pointer_cast<const FootContactConstraintSpec>( | ||
| constraint_spec); | ||
| if (!foot_contact_spec) { | ||
| throw std::runtime_error( |
There was a problem hiding this comment.
You should not throw an error, though: just ignore specs that are not footholds. Some other class we build later might take care of other constraints, like orientation etc…
There was a problem hiding this comment.
removed the error throw and added checks in all functions that cast.
done.
There was a problem hiding this comment.
Cool
My interpretation of the main changes:
- Lots of functionality from Trajectory moved to WalkCycle
- Phase refactored to inherit from Interval, and lots of functionality moved into FootContactConstraintSpec
And then all the necessary adjustments to support those changes.
Correct me if I'm wrong!
| * @class Phase class stores information about a robot stance | ||
| * and its duration. | ||
| */ | ||
| using ContactPointGoals = std::map<std::string, gtsam::Point3>; | ||
|
|
||
| class Phase { | ||
| public: | ||
| class Phase : public Interval { |
There was a problem hiding this comment.
This confuses me a little bit
So Interval is purely a time-specification, while Phase adds robot constraint/stance information? In that case it doesn't feel like Phase is a type of Interval but rather some motion description that includes an Interval?
|
So close :-) what’s the clang issue? |
|
3 tests are failing, getting the gtsam "Indeterminant linear system exception". Weird. |
|
Merged, bravo! |
This branch is currently prototype code Frank and @danbarla made together to resolve #257
PR comment should be updated as we go.