//==============================================================================
void Controller::update(const Eigen::Vector3d& _targetPosition)
{
using namespace dart;
// Get equation of motions
Eigen::Vector3d x = mEndEffector->getTransform().translation();
Eigen::Vector3d dx = mEndEffector->getLinearVelocity();
Eigen::MatrixXd invM = mRobot->getInvMassMatrix(); // n x n
Eigen::VectorXd Cg = mRobot->getCoriolisAndGravityForces(); // n x 1
math::LinearJacobian Jv = mEndEffector->getLinearJacobian(); // 3 x n
math::LinearJacobian dJv = mEndEffector->getLinearJacobianDeriv(); // 3 x n
Eigen::VectorXd dq = mRobot->getVelocities(); // n x 1
// Compute operational space values
Eigen::MatrixXd A = Jv*invM; // 3 x n
Eigen::Vector3d b = /*-(A*Cg) + */dJv*dq; // 3 x 1
Eigen::MatrixXd M2 = Jv*invM*Jv.transpose(); // 3 x 3
// Compute virtual operational space spring force at the end effector
Eigen::Vector3d f = -mKp*(x - _targetPosition) - mKv*dx;
// Compute desired operational space acceleration given f
Eigen::Vector3d desired_ddx = b + M2*f;
// Gravity compensation
mForces = Cg;
// Compute joint space forces to acheive the desired acceleration by solving:
// A tau + b = desired_ddx
mForces += A.colPivHouseholderQr().solve(desired_ddx - b);
// Apply the joint space forces to the robot
mRobot->setForces(mForces);
}
void MLRModel::train()
{
if (descriptor_matrix_.cols() == 0)
{
throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!");
}
if (Y_.cols() == 0)
{
throw Exception::InconsistentUsage(__FILE__, __LINE__, "No response values have been read! Model can not be trained!");
}
if (descriptor_matrix_.cols() >= descriptor_matrix_.rows())
{
throw Exception::SingularMatrixError(__FILE__, __LINE__, "For MLR model, matrix must have more rows than columns in order to be invertible!!");
//training_result_.ReSize(0, 0);
//return;
}
Eigen::MatrixXd m = descriptor_matrix_.transpose()*descriptor_matrix_;
try
{
training_result_ = m.colPivHouseholderQr().solve(descriptor_matrix_.transpose()*Y_);
}
catch(BALL::Exception::GeneralException e)
{
training_result_.resize(0, 0);
throw Exception::SingularMatrixError(__FILE__, __LINE__, "Matrix for MLR training is singular!! Check that descriptor_matrix_ does not contain empty columns!");
return;
}
calculateOffsets();
}
void PointSolver::solveForCorrection ()
{
Eigen::MatrixXd jat = Jac.transpose() * Jac;
Eigen::VectorXd jae = Jac.transpose() * pointErrs;
Pcorrect = jat.colPivHouseholderQr().solve (jae);
std::cout << Pcorrect << std::endl;
}
void PointSolver2::solveForCorrection ()
{
Eigen::MatrixXd jat = Jac.transpose() * Jac;
Eigen::VectorXd jae = Jac.transpose() * pointErrs;
Pcorrect = jat.colPivHouseholderQr().solve (jae);
position0.x() -= Pcorrect[0];
position0.y() -= Pcorrect[1];
position0.z() -= Pcorrect[2];
orientation0.x() -= Pcorrect[3];
orientation0.y() -= Pcorrect[4];
orientation0.z() -= Pcorrect[5];
orientation0.w() -= Pcorrect[6];
// orientation0.normalize();
// std::cout << Jac << std::endl;
// exit (1);
// std::cout << Pcorrect << std::endl;
}
Eigen::VectorXd ALLModel::predict(const vector<double> & substance, bool transform)
{
if (descriptor_matrix_.cols() == 0)
{
throw Exception::InconsistentUsage(__FILE__, __LINE__, "Training data must be read into the ALL-model before the activity of a substance can be predicted!");
}
Eigen::VectorXd v0 = getSubstanceVector(substance, transform);
Eigen::MatrixXd v(1, v0.rows());
v.row(0) = v0;
Eigen::MatrixXd dist;
// calculate distances between the given substance and the substances of X
// dimension of dist: 1xn
calculateEuclDistanceMatrix(v, descriptor_matrix_, dist);
Eigen::VectorXd w;
calculateWeights(dist, w);
Eigen::MatrixXd XX;
// calculate X.t()*X as cross-products weighted by the similarity of the given substance to the respective row of X
calculateXX(w, XX);
Eigen::MatrixXd XY;
// calculate X.t()*Y_ as cross-products weighted by the similarity of the given substance to the respective row of X
calculateXY(w, XY);
// rigde regression in order to be able to cope with linearly dependent columns, i.e. singular matrices
Eigen::MatrixXd im(XX.rows(), XX.rows());
im.setIdentity();
im *= lambda_;
XX += im;
Eigen::MatrixXd train = XX.colPivHouseholderQr().solve(XY);
Eigen::VectorXd res(Y_.cols());
res = v0.transpose()*train;
if (transform && y_transformations_.cols() != 0)
{
backTransformPrediction(res);
}
return res;
}
void KPLSModel::train()
{
if (descriptor_matrix_.cols() == 0)
{
throw Exception::InconsistentUsage(__FILE__, __LINE__, "Data must be read into the model before training!");
}
// if (descriptor_matrix_.cols() < no_components_)
// {
// throw Exception::TooManyPLSComponents(__FILE__, __LINE__, no_components_, descriptor_matrix_.cols());
// }
kernel->calculateKernelMatrix(descriptor_matrix_, K_);
Eigen::MatrixXd P; // Matrix P saves all vectors p
int cols = K_.cols();
// determine the number of components that are to be created.
// no_components_ contains the number of components desired by the user,
// but obviously we cannot calculate more PLS components than features
int features = descriptor_matrix_.cols();
unsigned int components_to_create = no_components_;
if (features < no_components_) components_to_create = features;
U_.resize(K_.rows(), components_to_create);
loadings_.resize(cols, components_to_create);
weights_.resize(Y_.cols(), components_to_create);
latent_variables_.resize(K_.rows(), components_to_create);
P.resize(cols, components_to_create);
Eigen::VectorXd w;
Eigen::VectorXd t;
Eigen::VectorXd c;
Eigen::VectorXd u = Y_.col(0);
Eigen::VectorXd u_old;
for (unsigned int j = 0; j < components_to_create; j++)
{
for (int i = 0; i < 10000 ; i++)
{
w = K_.transpose()*u / Statistics::scalarProduct(u);
w = w / Statistics::euclNorm(w);
t = K_*w;
c = Y_.transpose()*t / Statistics::scalarProduct(t);
u_old = u;
u = Y_*c / Statistics::scalarProduct(c);
if (Statistics::euclDistance(u, u_old)/Statistics::euclNorm(u) > 0.0000001)
{
continue;
}
else // if u has converged
{
break;
}
}
Eigen::VectorXd p = K_.transpose() * t / Statistics::scalarProduct(t);
K_ -= t * p.transpose();
U_.col(j) = u;
loadings_.col(j) = w;
weights_.col(j) = c;
P.col(j) = p;
latent_variables_.col(j) = t;
}
// try // p's are not orthogonal to each other, so that in rare cases P.t()*loadings_ is not invertible
// {
// loadings_ = loadings_*(P.t()*loadings_).i();
// }
// catch(BALL::Exception::MatrixIsSingular e)
// {
// Eigen::MatrixXd I; I.setToIdentity(P.cols());
// I *= 0.001;
// loadings_ = loadings_*(P.t()*loadings_+I).i();
// }
Eigen::MatrixXd m = P.transpose()*loadings_;
training_result_ = loadings_*m.colPivHouseholderQr().solve(weights_.transpose());
calculateOffsets();
}
int main(int argc, char** argv) {
ros::init(argc, argv, "eigen_test");
ros::NodeHandle nh_jntPub; // node handle for joint command publisher
// ros::Publisher pub_joint_commands; //
// pub_joint_commands = nh_jntPub.advertise<atlas_msgs::AtlasCommand>("/atlas/atlas_command", 1, true);
ROS_INFO("test eigen program");
Eigen::Matrix3f A;
Eigen::Vector3f b;
A << 1,2,3, 4,5,6, 7,8,10;
A(1,2)=0; // how to access one element of matrix; start from 0; no warning out of range...
b << 3,3,4;
std::cout <<"b = "<<b <<std::endl;
// column operaton: replace first column of A with vector b:
A.col(0)= b; // could copy columns of matrices w/ A.col(0) = B.col(0);
std::cout <<"A = "<<A <<std::endl;
Eigen::MatrixXd mat1 = Eigen::MatrixXd::Zero(6, 6); //6x6 matrix full of zeros
Eigen::MatrixXd mat2 = Eigen::MatrixXd::Identity(6, 6); //6x6 identity matrix
std::cout<<mat1<<std::endl;
std::cout<<mat2<<std::endl;
Eigen::Vector3f xtest = A.colPivHouseholderQr().solve(b);
std::cout<<"soln xtest = "<<xtest<<std::endl;
Eigen::Vector3f x = A.partialPivLu().solve(b); //dec.solve(b); //A.colPivHouseholderQr().solve(b);
std::cout<<"soln x = "<<x<<std::endl;
Eigen::Vector3f btest = A*x;
std::cout<<"test soln: A*x = " <<btest<<std::endl;
//extend to 6x6 test: v = M*z, find z using 2 methods
// use double-precision matrices/vectors
Eigen::MatrixXd M = Eigen::MatrixXd::Random(6,6);
std::cout<<"test 6x6: M = "<<M<<std::endl;
Eigen::VectorXd v(6);
v << 1,2,3,4,5,6;
std::cout<<"v = "<<v<<std::endl;
Eigen::VectorXd z(6);
Eigen::VectorXd ztest(6);
ztest = M.colPivHouseholderQr().solve(v);
std::cout<<"soln ztest = "<<ztest<<std::endl;
z = M.partialPivLu().solve(v);
std::cout<<"soln 6x6: z = "<<z<<std::endl;
Eigen::VectorXd vtest(6);
vtest = M*z;
std::cout<<"test soln: M*z = "<<vtest<<std::endl;
// .norm() operator...
double relative_error = (M*z - v).norm() / v.norm(); // norm() is L2 norm
std::cout << "The relative error is:\n" << relative_error << std::endl;
std::cout<<"dot prod, v, z: "<< v.dot(z)<<std::endl;
std::cout<<"cross prod, b-cross-x: " << b.cross(x)<<std::endl;
return 0;
}
Eigen::MatrixXd Tra_vian_q( int n ,
double x0 , double v0 , double a0 ,
Eigen::MatrixXd x ,
double xf , double vf , double af ,
double smp , Eigen::MatrixXd t , double tf)
/*
minimum jerk trajectory with via-points
(via-point constraints: position at each point)
n : the number of via-points
x0 : position at initial state
v0 : velocity at initial state
a0 : acceleration at initial state
x : position matrix at via-points state ( size : n x 1 )
xf : position at final state
vf : velocity at final state
af : acceleration at final state
smp : sampling time
t : time matrix passing through via-points state ( size : n x 1 )
tf : movement time
*/
{
int i , j , k ;
/* Calculation Matrix B */
Eigen::MatrixXd B = Eigen::MatrixXd::Zero( n + 3 , 1 );
for ( i = 1; i <= n; i++ )
{
B.coeffRef( i - 1 , 0 ) =
x.coeff( i - 1 , 0 ) -
x0 -
v0 * t.coeff( i - 1 , 0 ) -
( a0 / 2 ) * pow( t.coeff( i - 1 , 0 ) , 2 ) ;
}
B.coeffRef( n , 0 ) =
xf -
x0 -
v0 * tf -
( a0 / 2 ) * pow( tf , 2 ) ;
B.coeffRef( n + 1 , 0 ) =
vf -
v0 -
a0 * tf ;
B.coeffRef( n + 2 , 0 ) =
af -
a0 ;
/* Calculation Matrix A */
Eigen::MatrixXd A1 = Eigen::MatrixXd::Zero( n , 3 );
for ( i = 1; i <= n; i++ )
{
A1.coeffRef( i - 1 , 0 ) = pow( t.coeff( i - 1 , 0 ) , 3 );
A1.coeffRef( i - 1 , 1 ) = pow( t.coeff( i - 1 , 0 ) , 4 );
A1.coeffRef( i - 1 , 2 ) = pow( t.coeff( i - 1 , 0 ) , 5 );
}
Eigen::MatrixXd A2 = Eigen::MatrixXd::Zero( n , n );
for ( i = 1; i <= n; i++ )
{
for ( j = 1; j <= n; j++ )
{
if ( i > j )
k = i;
else
k = j;
A2.coeffRef( j - 1 , i - 1 ) =
pow( ( t.coeff( k - 1 , 0 ) - t.coeff( i - 1 , 0 ) ) , 5 ) / 120;
}
}
Eigen::MatrixXd A3 = Eigen::MatrixXd::Zero( 3 , n + 3 );
A3.coeffRef( 0 , 0 ) = pow( tf , 3 );
A3.coeffRef( 0 , 1 ) = pow( tf , 4 );
A3.coeffRef( 0 , 2 ) = pow( tf , 5 );
A3.coeffRef( 1 , 0 ) = 3 * pow( tf , 2 );
A3.coeffRef( 1 , 1 ) = 4 * pow( tf , 3 );
A3.coeffRef( 1 , 2 ) = 5 * pow( tf , 4 );
A3.coeffRef( 2 , 0 ) = 6 * tf;
A3.coeffRef( 2 , 1 ) = 12 * pow( tf , 2 );
A3.coeffRef( 2 , 2 ) = 20 * pow( tf , 3 );
for ( i = 1; i <= n; i++ )
{
A3.coeffRef( 0 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 5 ) / 120;
A3.coeffRef( 1 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 4 ) / 24;
A3.coeffRef( 2 , i + 2 ) = pow( tf - t.coeff( i - 1 , 0 ) , 3 ) / 6;
}
Eigen::MatrixXd A = Eigen::MatrixXd::Zero( n + 3 , n + 3 );
A.block( 0 , 0 , n , 3 ) = A1;
A.block( 0 , 3 , n , n ) = A2;
A.block( n , 0 , 3 , n + 3 ) = A3;
/* Calculation Matrix C (coefficient of polynomial function) */
Eigen::MatrixXd C( 2 * n + 3 , 1 );
//C = A.inverse() * B;
C = A.colPivHouseholderQr().solve(B);
/* Time */
int NN;
double N;
N = tf / smp ;
NN = round( N ) ;
Eigen::MatrixXd Time = Eigen::MatrixXd::Zero( NN + 1 , 1 );
for ( i = 1; i <= NN + 1; i++ )
Time.coeffRef( i - 1 , 0 ) = ( i - 1 ) * smp;
/* Time_via */
Eigen::MatrixXd Time_via = Eigen::MatrixXd::Zero( n , 1 );
for ( i = 1; i <= n; i++ )
Time_via.coeffRef( i - 1 , 0 ) = round( t.coeff( i - 1 , 0 ) / smp ) + 2;
/* Minimum Jerk Trajectory with Via-points */
Eigen::MatrixXd Tra_jerk_via = Eigen::MatrixXd::Zero( NN + 1 , 1 );
for ( i = 1; i <= NN + 1; i++ )
{
Tra_jerk_via.coeffRef( i - 1 , 0 ) =
x0 +
v0 * Time.coeff( i - 1 , 0 ) +
0.5 * a0 * pow( Time.coeff( i - 1 , 0 ) , 2 ) +
C.coeff( 0 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 3 ) +
C.coeff( 1 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 4 ) +
C.coeff( 2 , 0 ) * pow( Time.coeff( i - 1 , 0 ) , 5 ) ;
}
for ( i = 1 ; i <= n; i++ )
{
for ( j = Time_via.coeff( i - 1 , 0 ); j <= NN + 1; j++ )
{
Tra_jerk_via.coeffRef( j - 1 , 0 ) =
Tra_jerk_via.coeff( j - 1 , 0 ) +
C.coeff( i + 2 , 0 ) * pow( ( Time.coeff( j - 1 , 0 ) - t.coeff( i - 1 , 0 ) ) , 5 ) / 120 ;
}
}
return Tra_jerk_via;
}
