Cwbollinger/mobile api wip #69
Conversation
Specifically, behavior is bad when waypoint velocities are not constrained. Need to fix `buildTrajectory`
Now uses piecewise cartesian trajectories and maps to joint space as you go. More similar to old approach
…hebi-cpp-examples into cwbollinger/mobile_api_wip
| mobile -> clearText(); | ||
|
|
||
| // Display instructions on screen | ||
| mobile -> sendText(instructions); |
There was a problem hiding this comment.
I think these have bool return values; I'm not sure about the balance between making the example more complex vs. more robust by handling them
kits/base/omni_mobile_io_control.cpp
Outdated
|
|
||
| std::cout << "Creating Omni Base" << std::endl; | ||
|
|
||
| OmniBase base(p); |
There was a problem hiding this comment.
Just looking at the example here vs. the code below, it isn't clear what happens if it can't find these modules on the network. I'd probably match the arm and mobile IO and have a static create method that returns a unique_ptr, so we can have a invalid option.
(actually...if we are OK bumping to C++17, returning an optional might be a good way to go...but we'd want to update the MobileIO and Arm APIs too. Worth further discussion...)
There was a problem hiding this comment.
This should be addressed now, and be c++11 compatible
|
|
||
| while(base.update()) | ||
| { | ||
| auto state = mobile->getState(); |
There was a problem hiding this comment.
This reminds me...I think we still need to fix the MobileIO::getState call to not block in C++, or accept a timeout. I'll track this as a separate issue.
util/omni_base.hpp
Outdated
| struct Params: public MobileBase::Params { | ||
| Params() : wheel_radius(0.0762), base_radius(0.220), sample_density(0.1) {} | ||
| double wheel_radius; // m | ||
| double base_radius; // m (center of omni to origin of base) | ||
| double sample_density; // time between sampled waypoints on cartesian trajectory | ||
| }; |
There was a problem hiding this comment.
I like the basic idea of the inherited parameter structures here.
I want to try to think of something clean for initialization -- you can always set each value after creating a default instance, or add a constructor whose first argument is an instance of the base class. I feel like both of those work, but are sort of clunky...I'm not sure.
util/base.hpp
Outdated
| const Eigen::VectorXd times_{0}; | ||
| const Eigen::Matrix<double, 3, Eigen::Dynamic> positions_{0, 0}; | ||
| const Eigen::Matrix<double, 3, Eigen::Dynamic> velocities_{0, 0}; | ||
| const Eigen::Matrix<double, 3, Eigen::Dynamic> accelerations_{0, 0}; |
There was a problem hiding this comment.
I wonder about just storing a list of Waypoint objects instead, and just converting to the Eigen types as needed in the getters? Probably just moves the complexity rather than reduces it
util/omni_base.hpp
Outdated
|
|
||
| std::unique_ptr<queue<shared_ptr<Trajectory>>> OmniBase::buildTrajectory(const CartesianGoal& g) { | ||
| auto num_waypoints = g.times().size(); | ||
| auto num_wheels = g.positions().row(0).size(); |
There was a problem hiding this comment.
I think we need to add an initial waypoint like:
This is hard.
There was a problem hiding this comment.
maybe:
if (_past_trajectory && trajectory time is valid)
{
// these are both in old trajectory frame.
auto pose, vel = _past_trajectory.getState(time);
auto local_vel = vel * pose.theta.inverse();
// now we have a starting waypoint velocity.
}
else
{
// first try last command velocity, then last feedback if it doesn't exist.
auto v = group_manager.getLastCommand().getVelocity();
auto cartesian_local_vel = WheelToSE2(nil, v);
}
Minor refactoring to remove duplicated transform math
util/group_trajectory_manager.hpp
Outdated
|
|
||
| using arm::Goal; | ||
|
|
||
| class GroupTrajectoryManager: GroupMananger { |
util/group_trajectory_manager.hpp
Outdated
| double t = get_current_time_s_(); | ||
|
|
||
| // Time must be monotonically increasing! | ||
| if (t < last_time_) | ||
| return false; | ||
|
|
||
| dt_ = t - last_time_; | ||
| last_time_ = t; | ||
|
|
||
| if (!group_->getNextFeedback(feedback_)) | ||
| return false; |
No description provided.