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;
}
}