void AdmittanceController::update(const Eigen::VectorXd& tau, Eigen::VectorXd& qdot_reference, const KDL::JntArray& q_current, Eigen::VectorXd& q_reference) {

    // Update filters --> as a result desired velocities are known
    for (int i = 0; i<tau.size(); i++) {
/// Check input values (and create segfault to debug)
        if (!(fabs(tau(i)) < 10000.0 && fabs(qdot_reference_previous_(i)) < 10000.0) ) {
            std::cout << "tauin: " << tau(i) << ", qdot: " << qdot_reference_previous_(i) << std::endl;
            assert(false);
        } else {
        qdot_reference(i)  = b_(0,i) * tau(i);
        qdot_reference(i) += b_(1,i) * tau_previous_(i);
        qdot_reference(i) -= a_(1,i) * qdot_reference_previous_(i);
        qdot_reference(i) *= k_(i);

        // Integrate desired velocities and limit outputs
        q_reference(i) = std::min(q_max_(i),std::max(q_min_(i),q_current(i)+Ts_*qdot_reference(i)));
}
    }
    tau_previous_ = tau;
    qdot_reference_previous_ = qdot_reference;

    //ROS_INFO("qdr = %f %f %f %f", qdot_reference(0), qdot_reference(1), qdot_reference(2), qdot_reference(3));
    ///ROS_INFO("qdrspindle = %f", qdot_reference(7));

}
double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
  assert(q_out.rows() == q_in.rows());
  assert(q_dot_.rows() == q_out.rows());

  q_out = q_in;

  // First check, if all elements in p_in are available
  for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
    if(frames_.find(f_des_it->first)==frames_.end())
      return -2;

  for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
  {
    // Get all iterators for this endpoint
    Frames::iterator f_it = frames_.find(f_des_it->first);
    Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);

    fksolver_.JntToCart(q_out, f_it->second, f_it->first);
    twist_ = diff(f_it->second, f_des_it->second);

    // Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
    // And scales them, if necessary
    enforceCartVelLimits();

    delta_twists_it->second = twist_;
  }

  double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);

  // Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
  enforceJointVelLimits();

  // Integrate
  Add(q_out, q_dot_, q_out);

  // Limit joint positions
  for (unsigned int j = 0; j < q_min_.rows(); j++)
  {
    if (q_out(j) < q_min_(j))
      q_out(j) = q_min_(j);
    else if (q_out(j) > q_max_(j))
      q_out(j) = q_max_(j);
  }

  return res;
}
Exemplo n.º 3
0
bool JacoIKSolver::initFromURDF(const std::string urdf, const std::string root_name,
                            const std::string tip_name, unsigned int max_iter, std::string error)
{
    urdf::Model robot_model;
    KDL::Tree tree;

    if (!robot_model.initFile(urdf))
    {
        error += "Could not initialize robot model";
        return false;
    }

    if (!kdl_parser::treeFromFile(urdf, tree))
    {
        error += "Could not initialize tree object";
        return false;
    }

    if (tree.getSegment(root_name) == tree.getSegments().end())
    {
        error += "Could not find root link '" + root_name + "'.";
        return false;
    }

    if (tree.getSegment(tip_name) == tree.getSegments().end())
    {
        error += "Could not find tip link '" + tip_name + "'.";
        return false;
    }

    if (!tree.getChain(root_name, tip_name, chain_))
    {
        error += "Could not initialize chain object";
        return false;
    }

    // Get the joint limits from the robot model

    q_min_.resize(chain_.getNrOfJoints());
    q_max_.resize(chain_.getNrOfJoints());
    q_seed_.resize(chain_.getNrOfJoints());

    joint_names_.resize(chain_.getNrOfJoints());

    unsigned int j = 0;
    for(unsigned int i = 0; i < chain_.getNrOfSegments(); ++i)
    {
        const KDL::Joint& kdl_joint = chain_.getSegment(i).getJoint();
        if (kdl_joint.getType() != KDL::Joint::None)
        {
//            std::cout << chain_.getSegment(i).getName() << " -> " << kdl_joint.getName() << " -> " << chain_.getSegment(i + 1).getName() << std::endl;

            boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(kdl_joint.getName());
            if (joint && joint->limits)
            {
                q_min_(j) = joint->limits->lower;
                q_max_(j) = joint->limits->upper;
                q_seed_(j) = (q_min_(j) + q_max_(j)) / 2;
            }
            else
            {
                q_min_(j) = -1e9;
                q_max_(j) = 1e9;
                q_seed_(j) = 0;

            }

            joint_names_[j] = kdl_joint.getName();

            ++j;
        }
    }
;
    
    fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_)); 
    ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(chain_));
    ik_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(chain_, q_min_, q_max_, *fk_solver_, *ik_vel_solver_, max_iter));
    std::cout << "Using normal solver" << std::endl;
    
    jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));

    return true;
}
void AdmittanceController::initialize(const double Ts, const KDL::JntArray& q_min, const KDL::JntArray& q_max, const std::vector<double>& mass, const std::vector<double>& damping) {

    // Resize relevant parameters
    uint num_joints = 15;
    Ts_ = Ts;
    // ToDo: Make variables (use M and D as inputs?)
    m_.resize(num_joints);
    d_.resize(num_joints);
    k_.resize(num_joints);
    om_.resize(num_joints);
    a_.resize(2,num_joints);
    b_.resize(2,num_joints);
    tau_previous_.resize(num_joints);
    qdot_reference_previous_.resize(num_joints);
    q_min_.resize(num_joints);
    q_max_.resize(num_joints);

    // Set parameters
    /*(hardcoded)
    for (uint i = 0; i<num_joints; i++) {
        m_(i) = 0.1;
        d_(i) = 1;
    }
    m_(7) = 1; // Torso
    d_(7) = 10;   // Torso
    */
    for (uint i = 0; i < num_joints; i++) {
        m_(i) = mass[i];
        d_(i) = damping[i];
    }

    for (uint i = 0; i<num_joints; i++) {

        k_(i) = 1/d_(i);
        om_(i) = d_(i)/m_(i);

        double wp = om_(i) + eps;
        double alpha = wp/(tan(wp*Ts_/2));

        double x1 = alpha/om_(i)+1;
        double x2 = -alpha/om_(i)+1;

        // Numerator and denominator of the filter
        a_(0,i) = 1;
        a_(1,i) = x2 / x1;
        b_(0,i) = 1  / x1;
        b_(1,i) = 1  / x1;

        ///ROS_INFO("a %i = %f, %f, b %i = %f, %f", i, a_(0,i), a_(1,i), i, b_(0,1), b_(1,i));

        // Set previous in- and outputs to zero
        tau_previous_(i) = 0;
        qdot_reference_previous_(i) = 0;

        // Set joint limits
        q_min_(i) = q_min(i);
        q_max_(i) = q_max(i);

    }

    // Set inputs, states, outputs to zero
    ROS_INFO("Admittance controller initialized");

}
double TreeIkSolverPos_Online::CartToJnt_it(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
  assert(q_out.rows() == q_in.rows());
  assert(q_out_old_.rows() == q_in.rows());
  assert(q_dot_.rows() == q_in.rows());
  assert(q_dot_old_.rows() == q_in.rows());
  assert(q_dot_new_.rows() == q_in.rows());



  q_out = q_in;
  SetToZero(q_dot_);
  SetToZero(q_dot_old_);
  SetToZero(q_dot_new_);
  twist_ = Twist::Zero();

  // First check, if all elements in p_in are available
  unsigned int nr_of_endeffectors = 0;
  nr_of_still_endeffectors_ = 0;
  low_pass_adj_factor_ = 0.0;

  for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
  {
    if(frames_.find(f_des_it->first)==frames_.end())
      return -2;
    else
    {
      /*
      Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
      old_twists_it->second = Twist::Zero();
      Frames::iterator f_0_it = frames_0_.find(f_des_it->first);
      */
      /*
      Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
      old_twists_it->second = diff(f_old_it->second, f_des_it->second);
      if (){};
      */
      Frames::iterator f_old_it = p_in_old_.find(f_des_it->first);
      Frames::iterator f_des_pos_l_it = frames_pos_lim_.find(f_des_it->first);
      twist_ = diff(f_old_it->second, f_des_it->second);
      /*
      if(sqrt(pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2)) < x_dot_trans_min_)
      {
        f_des_pos_l_it->second.p = addDelta(f_old_it->second.p, (0.1 * x_dot_trans_max_ * twist_.vel));
        std::cout << "old position is used." << std::endl;
      }
      else
        f_des_pos_l_it->second.p = f_des_it->second.p;

      if(sqrt(pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2)) < x_dot_rot_min_)
      {
        f_des_pos_l_it->second.M = addDelta(f_old_it->second.M, (0.1 * x_dot_rot_max_ * twist_.rot));
        std::cout << "old orientation is used." << std::endl;
      }
      else
        f_des_pos_l_it->second.M = f_des_it->second.M;
      */
      f_des_pos_l_it->second.p = f_des_it->second.p;
      f_des_pos_l_it->second.M = f_des_it->second.M;

      Frames::iterator f_des_vel_l_it = frames_vel_lim_.find(f_des_it->first);
      fksolver_.JntToCart(q_in, f_des_vel_l_it->second, f_des_it->first);
      twist_ = diff(f_des_vel_l_it->second, f_des_pos_l_it->second);



      f_des_vel_l_it->second = addDelta(f_des_vel_l_it->second, twist_);
      nr_of_endeffectors++;
    }
  }
  if(nr_of_still_endeffectors_ == nr_of_endeffectors)
  {
    small_task_space_movement_ = true;

  }
  else
    small_task_space_movement_ = false;

  unsigned int k=0;
  double res = 0.0;
  while(++k <= maxiter_)
  {
    //for (Frames::const_iterator f_des_it=p_in.begin(); f_des_it!=p_in.end(); ++f_des_it)
    for (Frames::const_iterator f_des_it=frames_vel_lim_.begin(); f_des_it!=frames_vel_lim_.end(); ++f_des_it)
    {
      // Get all iterators for this endpoint
      Frames::iterator f_it = frames_.find(f_des_it->first);
      Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
      Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
      Frames::iterator f_0_it = frames_vel_lim_.find(f_des_it->first);

      fksolver_.JntToCart(q_out, f_it->second, f_it->first);

      // Checks, if the current overall twist exceeds the maximum translational and/or rotational velocity.
      // If so, the velocities of the overall twist get scaled and a new current twist is calculated.
      delta_twists_it->second = diff(f_it->second, f_des_it->second);


      old_twists_it->second = diff(f_0_it->second, f_it->second);

      enforceCartVelLimits_it(old_twists_it->second, delta_twists_it->second);


    }

    res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);

    if (res < eps_)
    {
      break;
      //return res;
    }

    // Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
    //Subtract(q_out, q_in, q_dot_old_);
    //enforceJointVelLimits_it(q_dot_old_, q_dot_);

    Subtract(q_out, q_in, q_dot_old_);

    Add(q_dot_old_, q_dot_, q_dot_);

    enforceJointVelLimits();

    Subtract(q_dot_, q_dot_old_, q_dot_);

    // Integrate
    Add(q_out, q_dot_, q_out);

    // Limit joint positions
    for (unsigned int j = 0; j < q_min_.rows(); ++j)
    {
      if (q_out(j) < q_min_(j))
        q_out(j) = q_min_(j);
      else if (q_out(j) > q_max_(j))
        q_out(j) = q_max_(j);
    }
  }

  Subtract(q_out, q_in, q_dot_);


  Add(q_in, q_dot_, q_out);
  filter(q_dot_, q_out, q_out_old_);



  q_out_old_ = q_out;
  p_in_old_ = p_in;


  if (k <= maxiter_)
    return res;
  else
    return -3;
}