An assert failure when params.ee_in_contact_at_start_ contains false about towr HOT 3 CLOSED

Hello Alexander W. Winkler,
I recently had read your paper, and have been experimenting with your source code. I am really impressed by your work and am very grateful for you to open-source your code.
I tried all the examples and also tried changing timings, gaits, and so on. They all worked great as long as I give the planner a reasonable input.

But I also noticed that changing the contact status at the start frame always causes an assert failure.
For example, if I change towr_examples/example.cc:93 as follows:

//params.ee_phase_durations_.push_back({0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.2});
//params.ee_in_contact_at_start_.push_back(true);
params.ee_phase_durations_.push_back({0.2, 0.4, 0.2, 0.4, 0.2, 0.4, 0.2});
params.ee_in_contact_at_start_.push_back(false);

An assert fails. For multiple limbs, all "ee_in_contact_at_start" variables seem to be necessarily set true for all end effectors. Is this an intended behavior? If so, if I correctly understood, it wouldn't be easy to directly use this planner for simulating a cyclic running motion in an online manner, where there is no double stance phase for a fixed time horizon. The assert failure occurs at the following codes.
47| ConstraintSet::Jacobian
48| ConstraintSet::GetJacobian () const
49| {
50| Jacobian jacobian(GetRows(), variables_->GetRows());
51|
52| int col = 0;
53| for (const auto& vars : variables_->GetComponents()) {
54|
55| int n = vars->GetRows();for example, if
56| Jacobian jac = Jacobian(GetRows(), n);
57|
58+> FillJacobianBlock(vars->GetName(), jac);

90| SwingConstraint::FillJacobianBlock (std::string var_set,
91| Jacobian& jac) const
92| {
93| if (var_set == ee_motion_->GetName()) {
94|
95| int row = 0;
96| for (int node_id : pure_swing_node_ids_) {
97|
98| for (auto dim : {X,Y}) {
99| // position constraint
100| jac.coeffRef(row, ee_motion_->Index(node_id, kPos, dim)) = 1.0; // curren
101| jac.coeffRef(row, ee_motion_->Index(node_id+1, kPos, dim)) = -0.5; // next n
102+> jac.coeffRef(row, ee_motion_->Index(node_id-1, kPos, dim)) = -0.5; // previo

node_id==-1, and an assert fails below.

79| int
80| Nodes::Index(const IndexInfo& n) const
81| {
82| // could also cache this as map for more efficiency, but adding complexity
83| for (int idx=0; idx<GetRows(); ++idx)
84| for ( IndexInfo node_info : GetNodeInfoAtOptIndex(idx))
85| if ( node_info == n )
86| return idx;
87|
88+> assert(false); // index representing these quantities doesn't exist
1

Thanks much,
Taesoo.

from towr.

Hi Taesoo, thanks for bringing this to our attention. This is not the intended behavior, you should be able to set the initial contact state to arbitrary values, allowing it also to be used in an mpc fashion.

For the single legged hopper, you're trying to initialize it in flight-phase that should last 0.4s. This might be difficult for the solver to find a solution, but shouldn't segfault. Possibly this line as you mentioned causes this:

As soon as I have time, I will investigate this further. For now, one thing you can do apart from investigating this is simply turn off the swing constraint and see if that runs:

In case you find something, i'd be happy to review your PR.

Best
Alex

from towr.

Thanks very much. Turning off the Swing constraint and adjusting the first flight duration did solve my problem. I will see more closely what the Swing constraint supposed to do.

from towr.