std::string YAMLUtils::getVectorStr(const std::string& tag, const Eigen::MatrixXd& m)
{
std::stringstream msg;
msg << tag << ": [";
if(m.rows() == 0)
{
msg << "]" << std::endl;
return msg.str();
}
if(m.cols() > 1)
{
std::stringstream msg;
msg << "Eigen::MatrixXd::cols returned value which is bigger than 1." << std::endl
<< "YAMLUtils::VectorStr assumes a vertical vector.";
throw ahl_utils::Exception("YAMLUtils::getVectorStr", msg.str());
}
msg << m.coeff(0, 0);
for(unsigned int i = 1; i < m.rows(); ++i)
{
msg << ", " << m.coeff(i, 0);
}
msg << "]" << std::endl;
return msg.str();
}
void Scaler::calcMaxMin(const std::vector<Eigen::MatrixXd>& src, Eigen::MatrixXd& max, Eigen::MatrixXd& min)
{
if(src.size() == 0)
{
throw nn::Exception("Scaler::calcMaxMin", "Vector size of src is zero.");
}
if(src[0].rows() == 0)
{
throw nn::Exception("Scaler::calcMaxMin", "src[0].rows() is zero.");
}
max = src[0];
min = src[0];
for(uint32_t i = 0; i < src.size(); ++i)
{
for(uint32_t j = 0; j < src[i].rows(); ++j)
{
if(max.coeff(j) < src[i].coeff(j))
max.coeffRef(j) = src[i].coeff(j);
if(min.coeff(j) > src[i].coeff(j))
min.coeffRef(j) = src[i].coeff(j);
}
}
}
Eigen::MatrixXd Tra_via0( double x0 , double v0 , double a0,
double xf , double vf , double af,
double smp , double tf )
/*
simple minimum jerk trajectory
x0 : position at initial state
v0 : velocity at initial state
a0 : acceleration at initial state
xf : position at final state
vf : velocity at final state
af : acceleration at final state
smp : sampling time
tf : movement time
*/
{
Eigen::MatrixXd A( 3 , 3 );
Eigen::MatrixXd B( 3 , 1 );
A << pow( tf , 3 ) , pow( tf , 4 ) , pow( tf , 5 ),
3 * pow( tf , 2 ) , 4 * pow( tf , 3 ) , 5 * pow( tf , 4 ),
6 * tf , 12 * pow( tf , 2 ) , 20 * pow( tf , 3 );
B << xf - x0 - v0 * tf - a0 * pow( tf , 2 ) / 2,
vf - v0 - a0 * tf,
af - a0 ;
Eigen::Matrix<double,3,1> C = A.inverse() * B;
double N;
N = tf / smp;
int NN = round( N + 1 );
Eigen::MatrixXd Time = Eigen::MatrixXd::Zero( NN , 1 );
Eigen::MatrixXd Tra = Eigen::MatrixXd::Zero( NN , 1 );
int i;
for ( i = 1; i <= NN; i++ )
Time.coeffRef( i - 1 , 0 ) = ( i - 1 ) * smp;
for ( i = 1; i <= NN; i++ )
{
Tra.coeffRef(i-1,0) =
x0 +
v0 * Time.coeff( i - 1 ) +
0.5 * a0 * pow( Time.coeff( i - 1 ) , 2 ) +
C.coeff( 0 , 0 ) * pow( Time.coeff( i - 1 ) , 3 ) +
C.coeff( 1 , 0 ) * pow( Time.coeff( i - 1 ) , 4 ) +
C.coeff( 2 , 0 ) * pow( Time.coeff( i - 1 ) , 5 );
}
return Tra;
}
Eigen::MatrixXd MainWindow::rotation2rpy( Eigen::MatrixXd rotation )
{
Eigen::MatrixXd _rpy = Eigen::MatrixXd::Zero( 3 , 1 );
_rpy.coeffRef( 0 , 0 ) = atan2( rotation.coeff( 2 , 1 ), rotation.coeff( 2 , 2 ) );
_rpy.coeffRef( 1 , 0 ) = atan2( -rotation.coeff( 2 , 0 ), sqrt( pow( rotation.coeff( 2 , 1 ) , 2 ) + pow( rotation.coeff( 2 , 2 ) , 2 ) ) );
_rpy.coeffRef( 2 , 0 ) = atan2 ( rotation.coeff( 1 , 0 ) , rotation.coeff( 0 , 0 ) );
return _rpy;
}
void MainWindow::updateCurrKinematicsPoseSpinbox( manipulator_h_base_module_msgs::KinematicsPose msg )
{
ui.pos_x_spinbox->setValue( msg.pose.position.x );
ui.pos_y_spinbox->setValue( msg.pose.position.y );
ui.pos_z_spinbox->setValue( msg.pose.position.z );
Eigen::Quaterniond QR( msg.pose.orientation.w , msg.pose.orientation.x , msg.pose.orientation.y , msg.pose.orientation.z );
Eigen::MatrixXd rpy = quaternion2rpy( QR );
double roll = rpy.coeff( 0 , 0 ) * 180.0 / M_PI;
double pitch = rpy.coeff( 1 , 0 ) * 180.0 / M_PI;
double yaw = rpy.coeff( 2, 0 ) * 180.0 /M_PI;
ui.ori_roll_spinbox->setValue( roll );
ui.ori_pitch_spinbox->setValue( pitch );
ui.ori_yaw_spinbox->setValue( yaw );
}
void Scaler::denormalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
src = (src - Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_)) / range_;
for(uint32_t i = 0; i < src.rows(); ++i)
{
src.coeffRef(i) *= range.coeff(i);
}
src = src + min;
}
void Scaler::normalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
src = src - min;
for(uint32_t i = 0; i < src.rows(); ++i)
{
if(range.coeff(i) == 0.0)
{
std::stringstream msg;
msg << "range.coeff(" << i << ") is zero." << std::endl
<< "This means " << i << "-th data is always constant and shouldn't be included as a training data." << std::endl
<< "If you'd like to include the data somehow, it is recommended that you use neural network without using scaler.";
throw nn::Exception("Scaler::normalize", msg.str());
}
src.coeffRef(i) /= range.coeff(i);
}
src = src * range_ + Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_);
}
void Scaler::normalize(Eigen::MatrixXd& src, const Eigen::MatrixXd& min, const Eigen::MatrixXd& range)
{
src = src - min;
for(unsigned int i = 0; i < src.rows(); ++i)
{
src.coeffRef(i) /= range.coeff(i);
}
src = src * range_ + Eigen::MatrixXd::Constant(src.rows(), src.cols(), min_);
}
void prettyPrint (Eigen::MatrixXd const & mm, std::ostream & os,
std::string const & title, std::string const & prefix,
bool vecmode, bool nonl)
{
char const * nlornot ("\n");
if (nonl) {
nlornot = "";
}
if ( ! title.empty()) {
os << title << nlornot;
}
if ((mm.rows() <= 0) || (mm.cols() <= 0)) {
os << prefix << " (empty)" << nlornot;
}
else {
if (vecmode) {
if ( ! prefix.empty()) {
os << prefix;
}
for (int ir (0); ir < mm.rows(); ++ir) {
prettyPrint (mm.coeff (ir, 0), os);
}
os << nlornot;
}
else {
for (int ir (0); ir < mm.rows(); ++ir) {
if ( ! prefix.empty()) {
os << prefix;
}
for (int ic (0); ic < mm.cols(); ++ic) {
prettyPrint (mm.coeff (ir, ic), os);
}
os << nlornot;
}
}
}
}
void compute_vertex_area_fast(Eigen::MatrixXd& V, Eigen::MatrixXd& F, double* va)
{
int n1,n2,n3;
Eigen::Vector3d v1,v2,v3;
Eigen::Vector3d e1,e2;
Eigen::Vector3d tmpV;
double tmp;
double farea;
for( int i = 0; i < F.rows(); ++i)
{
n1 = F.coeff(i,0)-1; n2 = F.coeff(i,1)-1; n3 = F.coeff(i,2)-1;
v1 = V.row(n1); v2 = V.row(n2); v3 = V.row(n3);
e1 = v2 - v1; e2 = v3 - v1;
tmpV = e1.cross(e2);
farea = tmpV.norm()*0.5;
tmp = farea / 3.0;
va[n1] += tmp;
va[n2] += tmp;
va[n3] += tmp;
}
}
void ATIForceTorqueSensorTWE::setCurrentVoltageOutput(Eigen::MatrixXd voltage)
{
if((voltage.rows() != 6) || (voltage.cols() != 1)) {
ROS_ERROR("Invalid voltage size");
return;
}
setCurrentVoltageOutput(voltage.coeff(0,0), voltage.coeff(1,0), voltage.coeff(2,0),
voltage.coeff(3,0), voltage.coeff(4,0), voltage.coeff(5,0));
}
void IOUtils::print(const Eigen::MatrixXd& m)
{
std::cout << "[ ";
for(unsigned int i = 0; i < m.rows(); ++i)
{
for(unsigned int j = 0; j < m.cols(); ++j)
{
std::cout << m.coeff(i, j);
if(j < m.cols() - 1)
std::cout << ", ";
}
if(i < m.rows() - 1)
std::cout << std::endl;
}
std::cout << " ]" << std::endl;
}
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;
}
