Ejemplo n.º 1
0
    virtual jspace::Status update(jspace::Model const & model)
    {
      //////////////////////////////////////////////////
      // Update the state of our task. Again, this is not critical
      // here, but important later when we want to integrate several
      // operational space tasks into a hierarchy.
      
      actual_ = model.getState().position_;
      
      //////////////////////////////////////////////////
      // Compute PD control torques and store them in command_ for
      // later retrieval. If enabled, add the estimated effect of
      // gravity in order to make the robot behave as if was
      // weightless.
      
      command_ = kp_ * (goal_ - actual_) - kd_ * model.getState().velocity_;
      if (enable_gravity_compensation_) {
	jspace::Vector gg;
	if ( ! model.getGravity(gg)) {
	  return jspace::Status(false, "failed to retrieve gravity torque");
	}
	command_ += gg;
      }
      
      jspace::Status ok;
      return ok;
    }
Ejemplo n.º 2
0
// used as fall-back controller
static void StepFloat(jspace::Model const & model, jspace::Vector & tau)
{
  // this simplistic float mode can only handle a signle scalar kd
  // value...
  if (gain_kd.size() != 1) {
    tau = jspace::Vector::Zero(7);
    endme(269);
    return;
  }
  
  jspace::Vector gravity_torque;
  model.getGravity(gravity_torque);
  jspace::Matrix massInertia;
  model.getMassInertia(massInertia);
  tau = gravity_torque - gain_kd[0] * massInertia * model.getState().velocity_;
}
Ejemplo n.º 3
0
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;
}