VectorXd velocityReference(NewQPControllerData *pdata, double t, const Ref<VectorXd> &q, const Ref<VectorXd> &qd, const Ref<VectorXd> &qdd, bool foot_contact[2], VRefIntegratorParams *params, RobotPropertyCache *rpc) {
// Integrate expected accelerations to determine a target feed-forward velocity, which we can pass in to Atlas
int i;
assert(qdd.size() == pdata->r->num_velocities);
double dt = 0;
if (pdata->state.t_prev != 0) {
dt = t - pdata->state.t_prev;
}
pdata->state.vref_integrator_state = (1-params->eta)*pdata->state.vref_integrator_state + params->eta*qd + qdd*dt;
if (params->zero_ankles_on_contact && foot_contact[0] == 1) {
for (i=0; i < rpc->position_indices.l_leg_ak.size(); i++) {
pdata->state.vref_integrator_state(rpc->position_indices.l_leg_ak(i)) = 0;
}
}
if (params->zero_ankles_on_contact && foot_contact[1] == 1) {
for (i=0; i < rpc->position_indices.r_leg_ak.size(); i++) {
pdata->state.vref_integrator_state(rpc->position_indices.r_leg_ak(i)) = 0;
}
}
if (pdata->state.foot_contact_prev[0] != foot_contact[0]) {
// contact state changed, reset integrated velocities
for (i=0; i < rpc->position_indices.l_leg.size(); i++) {
pdata->state.vref_integrator_state(rpc->position_indices.l_leg(i)) = qd(rpc->position_indices.l_leg(i));
}
}
if (pdata->state.foot_contact_prev[1] != foot_contact[1]) {
// contact state changed, reset integrated velocities
for (i=0; i < rpc->position_indices.r_leg.size(); i++) {
pdata->state.vref_integrator_state(rpc->position_indices.r_leg(i)) = qd(rpc->position_indices.r_leg(i));
}
}
pdata->state.foot_contact_prev[0] = foot_contact[0];
pdata->state.foot_contact_prev[1] = foot_contact[1];
VectorXd qd_err = pdata->state.vref_integrator_state - qd;
// do not velocity control ankles when in contact
if (params->zero_ankles_on_contact && foot_contact[0] == 1) {
for (i=0; i < rpc->position_indices.l_leg_ak.size(); i++) {
qd_err(rpc->position_indices.l_leg_ak(i)) = 0;
}
}
if (params->zero_ankles_on_contact && foot_contact[1] == 1) {
for (i=0; i < rpc->position_indices.r_leg_ak.size(); i++) {
qd_err(rpc->position_indices.r_leg_ak(i)) = 0;
}
}
double delta_max = 1.0;
VectorXd v_ref = VectorXd::Zero(rpc->actuated_indices.size());
for (i=0; i < rpc->actuated_indices.size(); i++) {
v_ref(i) = qd_err(rpc->actuated_indices(i));
}
v_ref = v_ref.array().max(-delta_max);
v_ref = v_ref.array().min(delta_max);
return v_ref;
}
VectorXd velocityReference(NewQPControllerData *pdata, double t, const Ref<VectorXd> &q, const Ref<VectorXd> &qd, const Ref<VectorXd> &qdd, bool foot_contact[2], VRefIntegratorParams *params, RobotPropertyCache *rpc) {
// Integrate expected accelerations to determine a target feed-forward velocity, which we can pass in to Atlas
int i;
assert(qdd.size() == pdata->r->num_velocities);
double dt = 0;
if (pdata->state.t_prev != 0) {
dt = t - pdata->state.t_prev;
}
VectorXd qdd_limited = qdd;
// Do not wind the vref integrator up against the joint limits for the legs
for (i=0; i < rpc->position_indices.at("r_leg").size(); i++) {
int pos_ind = rpc->position_indices.at("r_leg")(i);
if (q(pos_ind) <= pdata->r->joint_limit_min(pos_ind) + LEG_INTEGRATOR_DEACTIVATION_MARGIN) {
qdd_limited(pos_ind) = std::max(qdd(pos_ind), 0.0);
} else if (q(pos_ind) >= pdata->r->joint_limit_max(pos_ind) - LEG_INTEGRATOR_DEACTIVATION_MARGIN) {
qdd_limited(pos_ind) = std::min(qdd(pos_ind), 0.0);
}
}
for (i=0; i < rpc->position_indices.at("l_leg").size(); i++) {
int pos_ind = rpc->position_indices.at("l_leg")(i);
if (q(pos_ind) <= pdata->r->joint_limit_min(pos_ind) + LEG_INTEGRATOR_DEACTIVATION_MARGIN) {
qdd_limited(pos_ind) = std::max(qdd(pos_ind), 0.0);
} else if (q(pos_ind) >= pdata->r->joint_limit_max(pos_ind) - LEG_INTEGRATOR_DEACTIVATION_MARGIN) {
qdd_limited(pos_ind) = std::min(qdd(pos_ind), 0.0);
}
}
pdata->state.vref_integrator_state = (1-params->eta)*pdata->state.vref_integrator_state + params->eta*qd + qdd_limited*dt;
if (params->zero_ankles_on_contact && foot_contact[0] == 1) {
for (i=0; i < rpc->position_indices.at("l_leg_ak").size(); i++) {
pdata->state.vref_integrator_state(rpc->position_indices.at("l_leg_ak")(i)) = 0;
}
}
if (params->zero_ankles_on_contact && foot_contact[1] == 1) {
for (i=0; i < rpc->position_indices.at("r_leg_ak").size(); i++) {
pdata->state.vref_integrator_state(rpc->position_indices.at("r_leg_ak")(i)) = 0;
}
}
if (pdata->state.foot_contact_prev[0] != foot_contact[0]) {
// contact state changed, reset integrated velocities
for (i=0; i < rpc->position_indices.at("l_leg").size(); i++) {
pdata->state.vref_integrator_state(rpc->position_indices.at("l_leg")(i)) = qd(rpc->position_indices.at("l_leg")(i));
}
}
if (pdata->state.foot_contact_prev[1] != foot_contact[1]) {
// contact state changed, reset integrated velocities
for (i=0; i < rpc->position_indices.at("r_leg").size(); i++) {
pdata->state.vref_integrator_state(rpc->position_indices.at("r_leg")(i)) = qd(rpc->position_indices.at("r_leg")(i));
}
}
pdata->state.foot_contact_prev[0] = foot_contact[0];
pdata->state.foot_contact_prev[1] = foot_contact[1];
VectorXd qd_err = pdata->state.vref_integrator_state - qd;
// do not velocity control ankles when in contact
if (params->zero_ankles_on_contact && foot_contact[0] == 1) {
for (i=0; i < rpc->position_indices.at("l_leg_ak").size(); i++) {
qd_err(rpc->position_indices.at("l_leg_ak")(i)) = 0;
}
}
if (params->zero_ankles_on_contact && foot_contact[1] == 1) {
for (i=0; i < rpc->position_indices.at("r_leg_ak").size(); i++) {
qd_err(rpc->position_indices.at("r_leg_ak")(i)) = 0;
}
}
VectorXd qd_ref = qd_err.array().max(-params->delta_max);
qd_ref = qd_ref.array().min(params->delta_max);
return qd_ref;
}
