jspace::Status OpspacePlanarController:: init(jspace::Model const & model) { jspace::Status status(jgoal_controller_.init(model)); if ( ! status) { return status; } taoDNode * q1_node(model.getNodeByName(q1_name_)); if ( ! q1_node) { status.ok = false; status.errstr = "invalid q1_name, no node called `" + q1_name_ + "'"; return status; } q1_index_ = q1_node->getID(); taoDNode * q2_node(model.getNodeByName(q2_name_)); if ( ! q2_node) { status.ok = false; status.errstr = "invalid q2_name, no node called `" + q2_name_ + "'"; return status; } q2_index_ = q2_node->getID(); ndof_ = model.getNDOF(); return status; }
static void StepTaskPosture(jspace::Model const & model, jspace::Vector & tau) { static taoDNode * right_hand(0); if ( ! right_hand) { right_hand = model.getNodeByName("right-hand"); if ( ! right_hand) { tau = jspace::Vector::Zero(7); endme(378); return; } } ////////////////////////////////////////////////// // task jspace::Transform eepos; model.computeGlobalFrame(right_hand, 0.0, -0.15, 0.0, eepos); jspace::Matrix Jfull; model.computeJacobian(right_hand, eepos.translation()[0], eepos.translation()[1], eepos.translation()[2], Jfull); jspace::Matrix Jx(Jfull.block(0, 0, 3, 7)); jspace::Matrix invA; model.getInverseMassInertia(invA); jspace::Matrix invLambda_t(Jx * invA * Jx.transpose()); Eigen::SVD<jspace::Matrix> svdLambda_t(invLambda_t); svdLambda_t.sort(); int const nrows_t(svdLambda_t.singularValues().rows()); jspace::Matrix Sinv_t; Sinv_t = jspace::Matrix::Zero(nrows_t, nrows_t); for (int ii(0); ii < nrows_t; ++ii) { if (svdLambda_t.singularValues().coeff(ii) > 1e-3) { Sinv_t.coeffRef(ii, ii) = 1.0 / svdLambda_t.singularValues().coeff(ii); } } jspace::Matrix Lambda_t(svdLambda_t.matrixU() * Sinv_t * svdLambda_t.matrixU().transpose()); dbg_invLambda_t = invLambda_t; dbg_Lambda_t = Lambda_t; static jspace::Vector eegoal0; if (0 == eegoal0.size()) { if (goal.rows() < 3) { tau = jspace::Vector::Zero(7); endme(256); return; } eegoal0 = goal.block(0, 0, 3, 1); } struct timeval now; gettimeofday(&now, 0); double const alpha((1.0 * now.tv_sec + 1.0e-6 * now.tv_usec) * M_PI / 2.0); jspace::Vector eegoal(eegoal0); eegoal.coeffRef(1) += 0.2 * cos(alpha); eegoal.coeffRef(2) += 0.2 * sin(alpha); jspace::Vector poserror(eepos.translation() - eegoal); jspace::Vector g; model.getGravity(g); static jspace::Vector eegain_kp, eegain_kd; if (0 == eegain_kp.size()) { if (3 <= gain_kp.size()) { eegain_kp = gain_kp.block(0, 0, 3, 1); } else { eegain_kp = gain_kp[0] * jspace::Vector::Ones(3); } if (3 <= gain_kd.size()) { eegain_kd = gain_kd.block(0, 0, 3, 1); } else { eegain_kd = gain_kd[0] * jspace::Vector::Ones(3); } } jspace::Vector tau_task(Jx.transpose() * (-Lambda_t) * ( eegain_kp.cwise() * poserror + eegain_kd.cwise() * Jx * model.getState().velocity_)); ////////////////////////////////////////////////// // posture jspace::Matrix Jbar(invA * Jx.transpose() * Lambda_t); jspace::Matrix nullspace(jspace::Matrix::Identity(7, 7) - Jbar * Jx); static jspace::Vector goalposture; if (7 != goalposture.size()) { if (goal.size() >= 10) { goalposture = goal.block(3, 0, 7, 1); goalposture *= M_PI / 180; } else { goalposture = jspace::Vector::Zero(7); goalposture[1] = 30 * M_PI / 180; // shoulder "sideways" 30 degrees goalposture[3] = 60 * M_PI / 180; // ellbow at 60 degrees } } jspace::Matrix invLambda_p(nullspace * invA); Eigen::SVD<jspace::Matrix> svdLambda_p(invLambda_p); svdLambda_p.sort(); int const nrows_p(svdLambda_p.singularValues().rows()); jspace::Matrix Sinv_p; Sinv_p = jspace::Matrix::Zero(nrows_p, nrows_p); for (int ii(0); ii < nrows_p; ++ii) { if (svdLambda_p.singularValues().coeff(ii) > 1e-3) { Sinv_p.coeffRef(ii, ii) = 1.0 / svdLambda_p.singularValues().coeff(ii); } } jspace::Matrix Lambda_p(svdLambda_p.matrixU() * Sinv_p * svdLambda_p.matrixU().transpose()); dbg_invLambda_p = invLambda_p; dbg_Lambda_p = Lambda_p; static jspace::Vector posturegain_kp, posturegain_kd; if (0 == posturegain_kp.size()) { if (10 <= gain_kp.size()) { posturegain_kp = gain_kp.block(3, 0, 7, 1); } else { if (1 == gain_kp.size()) { posturegain_kp = gain_kp[0] * jspace::Vector::Ones(7); } else { posturegain_kp = gain_kp[1] * jspace::Vector::Ones(7); } } if (10 <= gain_kd.size()) { posturegain_kd = gain_kd.block(3, 0, 7, 1); } else { if (1 == gain_kd.size()) { posturegain_kd = gain_kd[0] * jspace::Vector::Ones(7); } else { posturegain_kd = gain_kd[1] * jspace::Vector::Ones(7); } } } jspace::Vector tau_posture(nullspace.transpose() * (-Lambda_p) * (posturegain_kp.cwise() * (model.getState().position_ - goalposture) + posturegain_kd.cwise() * model.getState().velocity_)); ////////////////////////////////////////////////// // sum it up... tau = tau_task + tau_posture + g; }