There was an error in controller.cpp in the function Controller::_solve(const Eigen::VectorXd& q, const Eigen::VectorXd& dq). q and dq were not used to update vtsid and qtsid.
Before:
vtsid += dt_ dv;
qtsid = pinocchio::integrate(robot_->model(), qtsid, dt_ vtsid);
After correction:
vtsid = dq + dt_ dv;
qtsid = pinocchio::integrate(robot->model(), q, dt vtsid);
dalinel
commented
3 years ago
There was an error in controller.cpp in the function Controller::_solve(const Eigen::VectorXd& q, const Eigen::VectorXd& dq). q and dq were not used to update vtsid and qtsid. Before: vtsid += dt_ dv; qtsid = pinocchio::integrate(robot_->model(), qtsid, dt_ vtsid); After correction: vtsid = dq + dt_ dv; qtsid = pinocchio::integrate(robot->model(), q, dt vtsid);