C++ VectorXd::block方法代码示例

void GravityCompensation::computeGeneralizedForce(Eigen::VectorXd& tau)
{
  tau = Eigen::VectorXd::Zero(robot_->getDOF());
  uint32_t macro_dof = robot_->getMacroManipulatorDOF();

  mnp_ = robot_->getManipulator().begin()->second;
  for(uint32_t i = 0; i < macro_dof; ++i)
  {
    tau.block(0, 0, macro_dof, 1) -= mnp_->getLink(i)->m * mnp_->getJacobian()[i].block(0, 0, 3, macro_dof).transpose() * param_->getG();
  }

  uint32_t offset = 0;

  for(uint32_t i = 0; i < mnp_name_.size(); ++i)
  {
    mnp_ = robot_->getManipulator(mnp_name_[i]);
    uint32_t mini_dof = mnp_->getDOF() - macro_dof;

    for(uint32_t j = 0; j < mini_dof; ++j)
    {
      tau.block(macro_dof + offset, 0, mini_dof, 1) -= mnp_->getLink(j + macro_dof)->m * mnp_->getJacobian()[j + macro_dof].block(0, macro_dof, 3, mini_dof).transpose() * param_->getG();
    }

    offset += mini_dof;
  }

  tau_ = tau;
}

void YouBot::control(const ros::TimerEvent&)
{
  try
  {
    boost::mutex::scoped_lock lock(mutex_);

    if(gazebo_interface_->subscribed())
    {
      Eigen::VectorXd q = gazebo_interface_->getJointStates();
      robot_->update(q);
      joint_updated_ = true;

      q_base_ = q.block(0, 0, robot_->getMacroManipulatorDOF(), 1);
    }

    if(!model_updated_) return;

    Eigen::VectorXd tau = Eigen::VectorXd::Zero(robot_->getDOF());
    controller_->computeGeneralizedForce(tau);
    gazebo_interface_->applyJointEfforts(tau);

    tau_base_ = tau.block(0, 0, robot_->getMacroManipulatorDOF(), 1);
  }
  catch(ahl_robot::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
  catch(ahl_ctrl::Exception& e)
  {
    ROS_ERROR_STREAM(e.what());
  }
}

Eigen::MatrixXd unflattenSymmetric(Eigen::VectorXd flat)
{
  //todo: other data types?
  //todo: safety/size checking?

  int num_rows = (-1 + sqrt(1 + 4 * 2 * flat.rows())) / 2; //solve for number of rows using quadratic eq
  Eigen::MatrixXd symm(num_rows, num_rows);
  int count = 0;
  for (int i = 0; i < num_rows; i++) {
    int ltcsz = num_rows - i;
    symm.block(i, i, ltcsz, 1) = flat.block(count, 0, ltcsz, 1);
    symm.block(i, i, 1, ltcsz) = flat.block(count, 0, ltcsz, 1).transpose();
    count += ltcsz;
  }
  return symm;
}

void deleteVectorBlock( Eigen::VectorXd &matrix, const int block_start_index, const int block_length ){

  assert( 1==matrix.cols() );
  assert( matrix.rows()>=block_start_index+block_length );

  int size = matrix.rows();
  /*
    R
    D
    M
  */

  matrix.block( block_start_index, 0, size-block_start_index-block_length, 1 ) = matrix.block( block_start_index+block_length, 0,  size-block_start_index-block_length, 1 );
  matrix.conservativeResize( size-block_length, 1 );

}

void addVectorBlock( Eigen::VectorXd &matrix, const int block_length ){
  assert( 1==matrix.cols() );

  int old_size = matrix.rows();
  matrix.conservativeResize( old_size+block_length, 1 );
  matrix.block( old_size, 0, block_length, 1 ) = Eigen::MatrixXd::Zero( block_length, 1);

}

void FrictionCompensation::computeGeneralizedForce(Eigen::VectorXd& tau)
{
  tau = Eigen::VectorXd::Zero(robot_->getDOF());
  unsigned int macro_dof = robot_->getMacroManipulatorDOF();

  unsigned int offset = 0;
  for(unsigned int i = 0; i < mnp_name_.size(); ++i)
  {
    mnp_ = robot_->getManipulator(mnp_name_[i]);
    unsigned int mini_dof = mnp_->getDOF() - macro_dof;

    tau.block(macro_dof + offset, 0, mini_dof, 1) = b_.block(macro_dof + offset, macro_dof + offset, mini_dof, mini_dof) * mnp_->dq().block(macro_dof, 0, mini_dof, 1);

    offset += mini_dof;
  }

  tau_ = tau;
}

  /// \brief evaluate the error term and return the weighted squared error e^T invR e
  double ErrorTermTransformation::evaluateErrorImplementation()
  {
    _errorMatrix = _T.toTransformationMatrix()*_prior.inverse().T();

    Eigen::VectorXd errorVector = Eigen::VectorXd::Zero(6);
    errorVector.block(0,0,3,1) = _errorMatrix.block(0,3,3,1);
    errorVector.block(3,0,3,1) = sm::kinematics::R2AxisAngle(_errorMatrix.block(0,0,3,3));
    setError(errorVector);

    if (_debug == 1)
    {
      std::cout << "\n\n\nPriorEstim:\n" << _T.toTransformationMatrix() << "\n";
      std::cout << "PriorGiven:\n" << _prior.T() << "\n";
      std::cout << "invPrior:\n" << _prior.inverse().T() << "\n";

      std::cout << "errorMatrix:\n" << _errorMatrix << "\n";
      std::cout << "error:\n" << error() <<  "\ninvR:\n" << invR() << "\n\n\n";
    }

    return evaluateChiSquaredError();
  }

        ///  Copy ::Spacy::Vector to flat coefficient vector of %Eigen .
        void copyToFlatVector( const ::Spacy::Vector& x, Eigen::VectorXd& y,
                               unsigned& currentIndex )
        {
            if ( is< Vector >( x ) )
            {
                y.block( currentIndex, 0, cast_ref< Vector >( x ).get().size(), 1 ) =
                    cast_ref< Vector >( x ).get();
                currentIndex += cast_ref< Vector >( x ).get().size();
                return;
            }

            if ( is<::Spacy::ProductSpace::Vector >( x ) )
            {
                for ( unsigned int i = 0;
                      i < cast_ref<::Spacy::ProductSpace::Vector >( x ).numberOfVariables(); ++i )
                {
                    copyToFlatVector( cast_ref<::Spacy::ProductSpace::Vector >( x ).component( i ),
                                      y, currentIndex );
                }
                return;
            }
        }