void DiagonalBlockSparseMatrix::Solve(const Eigen::VectorXd& rhs, Eigen::VectorXd& sol)
{
int numRows = GetNumRows();
int numCols = GetNumCols();
LOG_IF(FATAL, numRows != numCols) << "DiagonalBlockSparseMatrix is not a square matrix and noninvertible.";
int vecLength = rhs.size();
LOG_IF(FATAL, numCols != vecLength) << "DiagonalBlockSparseMatrix and right hand side vector are of different dimensions.";
sol.resize(numCols);
int numUntilLast = 0;
int numDiagElements = static_cast<int>(mDiagElements.size());
for (int i = 0; i < numDiagElements; ++i)
{
int numColsElement = mDiagElements[i].GetNumCols();
Eigen::VectorXd partSol(numColsElement);
Eigen::VectorXd partRhs = rhs.segment(numUntilLast, numColsElement);
#if LLT_SOLVE
mDiagElements[i].LltSolve(partRhs, partSol);
#else
mDiagElements[i].ConjugateGradientSolve(partRhs, partSol);
#endif
sol.segment(numUntilLast, numColsElement) = partSol;
numUntilLast += numColsElement;
}
}
// Compute the average contour, by calling compute_contour_vals repeatedly
//
// [[Rcpp::export]]
Eigen::MatrixXd rcppstandardize_rates(const Eigen::VectorXd &tiles, const Eigen::VectorXd &rates,
const Eigen::VectorXd &xseed, const Eigen::VectorXd &yseed,
const Eigen::MatrixXd &marks, const Eigen::VectorXd &nmrks,
const std::string &distm) {
bool use_weighted_mean = true;
int nxmrks = nmrks(0);
int nymrks = nmrks(1);
Eigen::MatrixXd Zvals = Eigen::MatrixXd::Zero(nymrks,nxmrks);
Eigen::MatrixXd zvals = Eigen::MatrixXd::Zero(nymrks,nxmrks);
int niters = tiles.size();
for ( int i = 0, pos = 0 ; i < niters ; i++ ) {
int now_tiles = (int)tiles(i);
Eigen::VectorXd now_rates = rates.segment(pos,now_tiles);
Eigen::VectorXd now_xseed = xseed.segment(pos,now_tiles);
Eigen::VectorXd now_yseed = yseed.segment(pos,now_tiles);
Eigen::MatrixXd now_seeds(now_tiles, 2);
now_seeds << now_xseed,now_yseed;
if (use_weighted_mean) {
compute_contour_vals(zvals,marks,now_rates,now_seeds,distm);
zvals = zvals.array() - zvals.mean();
} else {
now_rates = now_rates.array() - now_rates.mean();
compute_contour_vals(zvals,marks,now_rates,now_seeds,distm);
}
Zvals += zvals;
pos += now_tiles;
}
// Do not divide by niters here but in 'average.eems.contours' instead
// Zvals = Zvals.array() / niters;
return Zvals.transpose();
}
void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
_J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->get_dq();
_C.segment(_rowIndex, 3) = worldP1 - mOffset2;
_CDot.segment(_rowIndex, 3) = mJ1 * qDot1;
}
//! Convert Cartesian to spherical state.
Eigen::VectorXd convertCartesianToSphericalState( const Eigen::VectorXd& cartesianState )
{
// Create spherical state vector, initialized with zero entries.
Eigen::VectorXd convertedSphericalState = Eigen::VectorXd::Zero( 6 );
// Compute radius.
convertedSphericalState( 0 ) = cartesianState.segment( 0, 3 ).norm( );
// Check if radius is nonzero.
/*
* If r > 0, the elevation and azimuth angles are computed using trigonometric relationships.
* If r = 0, the coordinates are at the origin, the elevation and azimuth angles equal to zero.
* Since the state vector was initialized with zeroes, this is already the case.
*/
if ( convertedSphericalState( 0 ) > std::numeric_limits< double >::epsilon( ) )
{
// Compute elevation and azimuth angles using trigonometric relationships.
// Azimuth angle.
convertedSphericalState( 1 ) = std::atan2( cartesianState( 1 ), cartesianState( 0 ) );
// Elevation angle.
convertedSphericalState( 2 ) = std::asin( cartesianState( 2 )
/ convertedSphericalState( 0 ) );
}
// Precompute sine/cosine of angles, which has multiple usages, to save computation time.
const double cosineOfElevationAngle = std::cos( convertedSphericalState( 2 ) );
const double sineOfElevationAngle = std::sin( convertedSphericalState( 2 ) );
const double cosineOfAzimuthAngle = std::cos( convertedSphericalState( 1 ) );
const double sineOfAzimuthAngle = std::sin( convertedSphericalState( 1 ) );
// Set up transformation matrix for cylindrical to spherical conversion.
Eigen::MatrixXd transformationMatrixCylindricalToSpherical = Eigen::MatrixXd::Zero( 3, 3 );
transformationMatrixCylindricalToSpherical( 0, 0 ) = cosineOfElevationAngle;
transformationMatrixCylindricalToSpherical( 0, 2 ) = sineOfElevationAngle;
transformationMatrixCylindricalToSpherical( 1, 1 ) = 1.0;
transformationMatrixCylindricalToSpherical( 2, 0 ) = -sineOfElevationAngle;
transformationMatrixCylindricalToSpherical( 2, 2 ) = cosineOfElevationAngle;
// Set up transformation matrix for Cartesian to cylindrical conversion.
Eigen::MatrixXd transformationMatrixCartesianToCylindrical = Eigen::MatrixXd::Zero( 3, 3 );
transformationMatrixCartesianToCylindrical( 0, 0 ) = cosineOfAzimuthAngle;
transformationMatrixCartesianToCylindrical( 0, 1 ) = sineOfAzimuthAngle;
transformationMatrixCartesianToCylindrical( 1, 0 ) = -sineOfAzimuthAngle;
transformationMatrixCartesianToCylindrical( 1, 1 ) = cosineOfAzimuthAngle;
transformationMatrixCartesianToCylindrical( 2, 2 ) = 1.0;
// Compute transformation matrix for Cartesian to spherical conversion.
const Eigen::MatrixXd transformationMatrixCartesianToSpherical
= transformationMatrixCylindricalToSpherical
* transformationMatrixCartesianToCylindrical;
// Perform transformation of velocity vector.
convertedSphericalState.segment( 3, 3 )
= transformationMatrixCartesianToSpherical * cartesianState.segment( 3, 3 );
// Return spherical state vector.
return convertedSphericalState;
}
void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
SkeletonDynamics *skel = (SkeletonDynamics*)mBody->getSkel();
_J[mSkelIndex].block(_rowIndex, 0, 3, skel->getNumDofs()) = mJ;
Vector3d worldP = xformHom(mBody->getWorldTransform(), mOffset);
VectorXd qDot = skel->getQDotVector();
_C.segment(_rowIndex, 3) = worldP - mTarget;
_CDot.segment(_rowIndex, 3) = mJ * qDot;
}
void PointConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
dynamics::Skeleton *skel = mBody->getSkeleton();
_J[mSkelIndex].block(_rowIndex, 0, 3, skel->getDOF()) = mJ;
Eigen::Vector3d worldP = mBody->getWorldTransform() * mOffset;
Eigen::VectorXd qDot = skel->get_dq();
_C.segment(_rowIndex, 3) = worldP - mTarget;
_CDot.segment(_rowIndex, 3) = mJ * qDot;
}
Eigen::VectorXd IEFSolver::computeCharge_impl(const Eigen::VectorXd & potential, int irrep) const
{
// The potential and charge vector are of dimension equal to the
// full dimension of the cavity. We have to select just the part
// relative to the irrep needed.
int fullDim = fullPCMMatrix_.rows();
Eigen::VectorXd charge = Eigen::VectorXd::Zero(fullDim);
int nrBlocks = blockPCMMatrix_.size();
int irrDim = fullDim/nrBlocks;
charge.segment(irrep*irrDim, irrDim) =
- blockPCMMatrix_[irrep] * potential.segment(irrep*irrDim, irrDim);
return charge;
}
void ClosedLoopConstraint::updateDynamics(std::vector<Eigen::MatrixXd> & _J, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
_J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()).setZero();
_J[mSkelIndex1].block(_rowIndex, 0, 3, mBody1->getSkel()->getNumDofs()) = mJ1;
_J[mSkelIndex2].block(_rowIndex, 0, 3, mBody2->getSkel()->getNumDofs()) += mJ2;
Vector3d worldP1 = xformHom(mBody1->getWorldTransform(), mOffset1);
Vector3d worldP2 = xformHom(mBody2->getWorldTransform(), mOffset2);
VectorXd qDot1 = ((SkeletonDynamics*)mBody1->getSkel())->getPoseVelocity();
VectorXd qDot2 = ((SkeletonDynamics*)mBody2->getSkel())->getPoseVelocity();
_C.segment(_rowIndex, 3) = worldP1 - worldP2;
_CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
}
void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::MatrixXd & _J2, Eigen::VectorXd & _C, Eigen::VectorXd & _CDot, int _rowIndex) {
getJacobian();
_J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()).setZero();
_J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1;
_J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()) += mJ2;
Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1;
Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels();
Eigen::VectorXd worldP2 = mBodyNode2->getWorldTransform() * mOffset2;
Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->getGenVels();
_C.segment(_rowIndex, 3) = worldP1 - worldP2;
_CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2;
}
//! Convert spherical to Cartesian state.
Eigen::VectorXd convertSphericalToCartesianState( const Eigen::VectorXd& sphericalState )
{
// Create Cartesian state vector, initialized with zero entries.
Eigen::VectorXd convertedCartesianState = Eigen::VectorXd::Zero( 6 );
// Create local variables.
const double radius = sphericalState( 0 );
const double azimuthAngle = sphericalState( 1 );
const double elevationAngle = sphericalState( 2 );
// Precompute sine/cosine of angles, which has multiple usages, to save computation time.
const double cosineOfElevationAngle = std::cos( elevationAngle );
const double sineOfElevationAngle = std::sin( elevationAngle );
const double cosineOfAzimuthAngle = std::cos( azimuthAngle );
const double sineOfAzimuthAngle = std::sin( azimuthAngle );
// Set up transformation matrix for spherical to cylindrical conversion.
Eigen::MatrixXd transformationMatrixSphericalToCylindrical = Eigen::MatrixXd::Zero( 3, 3 );
transformationMatrixSphericalToCylindrical( 0, 0 ) = cosineOfElevationAngle;
transformationMatrixSphericalToCylindrical( 0, 2 ) = -sineOfElevationAngle;
transformationMatrixSphericalToCylindrical( 1, 1 ) = 1.0;
transformationMatrixSphericalToCylindrical( 2, 0 ) = sineOfElevationAngle;
transformationMatrixSphericalToCylindrical( 2, 2 ) = cosineOfElevationAngle;
// Set up transformation matrix for cylindrical to Cartesian conversion.
Eigen::MatrixXd transformationMatrixCylindricalToCartesian = Eigen::MatrixXd::Zero( 3, 3 );
transformationMatrixCylindricalToCartesian( 0, 0 ) = cosineOfAzimuthAngle;
transformationMatrixCylindricalToCartesian( 0, 1 ) = -sineOfAzimuthAngle;
transformationMatrixCylindricalToCartesian( 1, 0 ) = sineOfAzimuthAngle;
transformationMatrixCylindricalToCartesian( 1, 1 ) = cosineOfAzimuthAngle;
transformationMatrixCylindricalToCartesian( 2, 2 ) = 1.0;
// Compute transformation matrix for spherical to Cartesian conversion.
const Eigen::MatrixXd transformationMatrixSphericalToCartesian
= transformationMatrixCylindricalToCartesian
* transformationMatrixSphericalToCylindrical;
// Perform transformation of position coordinates.
convertedCartesianState( 0 ) = radius * cosineOfAzimuthAngle * cosineOfElevationAngle;
convertedCartesianState( 1 ) = radius * sineOfAzimuthAngle * cosineOfElevationAngle;
convertedCartesianState( 2 ) = radius * sineOfElevationAngle;
// Perform transformation of velocity vector.
convertedCartesianState.segment( 3, 3 ) =
transformationMatrixSphericalToCartesian * sphericalState.segment( 3, 3 );
// Return Cartesian state vector.
return convertedCartesianState;
}
void inertialParametersVectorToUndirectedTreeLoop(const Eigen::VectorXd & parameters_vector,
UndirectedTree & undirected_tree)
{
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
undirected_tree.getLink(i,false)->setInertia(deVectorize(parameters_vector.segment(i*10,10)));
}
}
/**
* Performs eigenvector decomposition of an affinity matrix
*
* @param data the affinity matrix
* @param numDims the number of dimensions to consider when clustering
*/
SpectralClustering::SpectralClustering(Eigen::MatrixXd& data, int numDims):
mNumDims(numDims),
mNumClusters(0)
{
Eigen::MatrixXd Deg = Eigen::MatrixXd::Zero(data.rows(),data.cols());
// calc normalised laplacian
for ( int i=0; i < data.cols(); i++) {
Deg(i,i)=1/(sqrt((data.row(i).sum())) );
}
Eigen::MatrixXd Lapla = Deg * data * Deg;
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> s(Lapla, true);
Eigen::VectorXd val = s.eigenvalues();
Eigen::MatrixXd vec = s.eigenvectors();
//sort eigenvalues/vectors
int n = data.cols();
for (int i = 0; i < n - 1; ++i) {
int k;
val.segment(i, n - i).maxCoeff(&k);
if (k > 0) {
std::swap(val[i], val[k + i]);
vec.col(i).swap(vec.col(k + i));
}
}
//choose the number of eigenvectors to consider
if (mNumDims < vec.cols()) {
mEigenVectors = vec.block(0,0,vec.rows(),mNumDims);
} else {
mEigenVectors = vec;
}
}
//! Function to evaluate a set of double and vector-returning functions and concatenate the results.
Eigen::VectorXd evaluateListOfFunctions(
const std::vector< boost::function< double( ) > >& doubleFunctionList,
const std::vector< std::pair< boost::function< Eigen::VectorXd( ) >, int > > vectorFunctionList,
const int totalSize)
{
Eigen::VectorXd variableList = Eigen::VectorXd::Zero( totalSize );
int currentIndex = 0;
for( unsigned int i = 0; i < doubleFunctionList.size( ); i++ )
{
variableList( i ) = doubleFunctionList.at( i )( );
currentIndex++;
}
for( unsigned int i = 0; i < vectorFunctionList.size( ); i++ )
{
variableList.segment( currentIndex, vectorFunctionList.at( i ).second ) =
vectorFunctionList.at( i ).first( );
currentIndex += vectorFunctionList.at( i ).second;
}
// Check consistency with input
if( currentIndex != totalSize )
{
std::string errorMessage = "Error when evaluating lists of functions, sizes are inconsistent: " +
boost::lexical_cast< std::string >( currentIndex ) + " and " +
boost::lexical_cast< std::string >( totalSize );
throw std::runtime_error( errorMessage );
}
return variableList;
}
void inertialParametersVectorLoop(const UndirectedTree & undirected_tree,
Eigen::VectorXd & parameters_vector)
{
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
parameters_vector.segment(i*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
}
}
bool operator() (
Eigen::VectorXd& newx,
const Eigen::VectorXd& x,
const Eigen::VectorXd& g,
double step_size) {
newx.noalias() = x - step_size * g;
for (ssize_t n = 0; n < newx.size(); n += 3)
newx.segment (n,3).normalize();
return newx != x;
}
// ONLY USE WHEN VECTORS CONTAINS FREE BASE TRANSLATION AND ROTATION
// Since the translation and rotation are switched
// WBI order [T R Q]
// ocra order [R T Q]
/* static */ bool ocraWbiConversions::wbiToOcraBodyVector(int qdof, const Eigen::VectorXd &v_wbi, Eigen::VectorXd &v_ocra)
{
int dof = qdof + DIM_T + DIM_R;
if(dof != v_wbi.size() || dof != v_ocra.size())
{
std::cout<<"ERROR: Input and output matrices - Is the model free root?" <<std::endl;
return false;
}
Eigen::VectorXd t(DIM_T);
Eigen::VectorXd r(DIM_R);
Eigen::VectorXd q(qdof);
t = v_wbi.segment(0, DIM_T);
r = v_wbi.segment(DIM_T, DIM_R);
q = v_wbi.segment(DIM_T+DIM_R, qdof);
v_ocra << r,
t,
q;
}
void inertialParametersVectorLoopFakeLinks(const UndirectedTree & undirected_tree,
Eigen::VectorXd & parameters_vector,
std::vector< std::string > fake_links_names)
{
int real_index_loop = 0;
for(int i=0; i < (int)undirected_tree.getNrOfLinks(); i++ ) {
if( std::find(fake_links_names.begin(), fake_links_names.end(), undirected_tree.getLink(i)->getName()) == fake_links_names.end() ) {
parameters_vector.segment(real_index_loop*10,10) = Vectorize(undirected_tree.getLink(i)->getInertia());
real_index_loop++;
}
}
}
Eigen::VectorXd World::evalDeriv()
{
// compute contact forces
mCollisionHandle->applyContactForces();
// compute derivatives for integration
Eigen::VectorXd deriv = Eigen::VectorXd::Zero(mIndices.back() * 2);
for (unsigned int i = 0; i < getNumSkeletons(); i++)
{
// skip immobile objects in forward simulation
if (mSkeletons[i]->getImmobileState())
continue;
int start = mIndices[i] * 2;
int size = getSkeleton(i)->getNumDofs();
Eigen::VectorXd qddot = mSkeletons[i]->getInvMassMatrix()
* (-mSkeletons[i]->getCombinedVector()
+ mSkeletons[i]->getExternalForces()
+ mSkeletons[i]->getInternalForces()
+ mCollisionHandle->getConstraintForce(i)
);
// Eigen::VectorXd qddot = mSkeletons[i]->getMassMatrix().ldlt().solve(
// -mSkeletons[i]->getCombinedVector()
// + mSkeletons[i]->getExternalForces()
// + mSkeletons[i]->getInternalForces()
// + mCollisionHandle->getConstraintForce(i)
// );
// set velocities
deriv.segment(start, size) = getSkeleton(i)->getPoseVelocity() + (qddot * mTimeStep);
// set qddot (accelerations)
deriv.segment(start + size, size) = qddot;
}
return deriv;
}
void MultiAgent3dNavigation::getStateEstimate(std::vector<tf::Transform> &stateEstimate) {
Eigen::VectorXd mean = ekf->getMean();
unsigned int nA = motionModel->getNumAgents();
unsigned int nT = motionModel->getNumTargets();
unsigned int aSD = motionModel->getAgentStateDim();
unsigned int tSD = motionModel->getTargetStateDim();
stateEstimate.resize(nA+nT);
assert(addOn3d.size() == nA);
for (unsigned int i=0; i<nA; ++i) {
assert(aSD == 3);
Eigen::Vector3d v = mean.segment(i*aSD, 3);
stateEstimate[i].setOrigin(tf::Vector3(v(0), v(1), addOn3d[i].z));
tf::Quaternion q;
q.setRPY(addOn3d[i].roll, addOn3d[i].pitch, v(2));
stateEstimate[i].setRotation(q);
}
assert(nT == 1);
for (unsigned int i=0; i<nT; ++i) {
assert(tSD >= 2);
Eigen::Vector2d v = mean.segment(nA*aSD+i*tSD, 2);
stateEstimate[nA+i].setOrigin(tf::Vector3(v(0), v(1), 0));
}
}
void World::setState(const Eigen::VectorXd& _newState)
{
for (int i = 0; i < getNumSkeletons(); i++)
{
int start = mIndices[i] * 2;
int size = getSkeleton(i)->getNumDofs();
VectorXd pose = _newState.segment(start, size);
VectorXd qDot = _newState.segment(start + size, size);
getSkeleton(i)->clampRotation(pose, qDot);
if (getSkeleton(i)->getImmobileState())
{
// need to update node transformation for collision
getSkeleton(i)->setPose(pose, true, false);
}
else
{
// need to update first derivatives for collision
getSkeleton(i)->setPose(pose, false, true);
getSkeleton(i)->computeDynamics(mGravity, qDot, true);
}
}
}
void getGoal(Eigen::VectorXd &x) override
{
unsigned int offset = 3 * links_ * chainNum_;
if (links_ == 7)
{
Eigen::VectorXd nstep = offset_ * length_;
Eigen::VectorXd estep =
Eigen::AngleAxisd(boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_ *
length_;
Eigen::VectorXd sstep =
Eigen::AngleAxisd(boost::math::constants::pi<double>(), Eigen::Vector3d::UnitZ()) * offset_ * length_;
Eigen::VectorXd wstep =
Eigen::AngleAxisd(3 * boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_ *
length_;
Eigen::VectorXd joint = offset_ + nstep;
x.segment(3 * 0 + offset, 3) = joint;
x.segment(3 * 1 + offset, 3) = x.segment(3 * 0 + offset, 3) + estep;
x.segment(3 * 2 + offset, 3) = x.segment(3 * 1 + offset, 3) + estep;
x.segment(3 * 3 + offset, 3) = x.segment(3 * 2 + offset, 3) + Eigen::Vector3d::UnitZ() * length_;
x.segment(3 * 4 + offset, 3) = x.segment(3 * 3 + offset, 3) + sstep;
x.segment(3 * 5 + offset, 3) = x.segment(3 * 4 + offset, 3) + sstep;
x.segment(3 * 6 + offset, 3) = x.segment(3 * 5 + offset, 3) + wstep;
}
else
{
Eigen::VectorXd step = offset_ * length_;
Eigen::VectorXd joint = offset_ + step;
unsigned int i = 0;
for (; i < links_ / 2; ++i, joint += step)
x.segment(3 * i + offset, 3) = joint;
joint += Eigen::Vector3d::UnitZ() * length_ - step;
for (; i < links_; ++i, joint -= step)
x.segment(3 * i + offset, 3) = joint;
}
}
void getStart(Eigen::VectorXd &x) override
{
const double angle = boost::math::constants::pi<double>() / 16;
const unsigned int offset = 3 * links_ * chainNum_;
const Eigen::VectorXd axis =
Eigen::AngleAxisd(boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_;
const Eigen::VectorXd step = Eigen::Vector3d::UnitZ() * length_;
Eigen::VectorXd joint = offset_ + Eigen::AngleAxisd(angle, axis) * step;
unsigned int i = 0;
for (; i < links_; ++i)
{
x.segment(3 * i + offset, 3) = joint;
if (i < links_ - 2)
joint += step;
else
joint += Eigen::AngleAxisd(-angle, axis) * step;
}
}
void jacobian(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out) const override
{
const unsigned int offset = 3 * links_ * chainNum_;
out.setZero();
Eigen::VectorXd plus(3 * (links_ + 1));
plus.head(3 * links_) = x.segment(offset, 3 * links_);
plus.tail(3) = Eigen::VectorXd::Zero(3);
Eigen::VectorXd minus(3 * (links_ + 1));
minus.head(3) = offset_;
minus.tail(3 * links_) = x.segment(offset, 3 * links_);
const Eigen::VectorXd diagonal = plus - minus;
for (unsigned int i = 0; i < links_; i++)
out.row(i).segment(3 * i + offset, 3) = diagonal.segment(3 * i, 3).normalized();
out.block(1, offset, links_ - 1, 3 * links_ - 3) -= out.block(1, offset + 3, links_ - 1, 3 * links_ - 3);
}
// Computes the difference vector of all design variables between the linearization point at marginalization and the current guess, on the tangent space (i.e. log(x_bar - x))
Eigen::VectorXd MarginalizationPriorErrorTerm::getDifferenceSinceMarginalization()
{
Eigen::VectorXd diff = Eigen::VectorXd(_dimensionDesignVariables);
diff.setZero();
int index = 0;
std::vector<Eigen::MatrixXd>::iterator it_marg = _designVariableValuesAtMarginalization.begin();
std::vector<aslam::backend::DesignVariable*>::iterator it_current = _designVariables.begin();
for(;it_current != _designVariables.end(); ++it_current, ++it_marg)
{
// retrieve current value (xbar) and value at marginalization(xHat)
Eigen::MatrixXd xHat = *it_marg;
//get minimal difference in tangent space
Eigen::VectorXd diffVector;
(*it_current)->minimalDifference(xHat, diffVector);
//int base = index;
int dim = diffVector.rows();
diff.segment(index, dim) = diffVector;
index += dim;
}
return diff;
}
void ExperimentalTrajectory::initializeTrajectory()
{
originalWaypoints = waypoints;
kernelLengthParameter = pointToPointDurationVector.mean();
// Add a waypoint to the end.
int extraWaypoints = 1;
int nStartWp = extraWaypoints;
int nEndWp = extraWaypoints;
int nWpNew = nWaypoints + nStartWp + nEndWp;
Eigen::MatrixXd waypointsTmp = Eigen::MatrixXd::Zero(nDoF, nWpNew);
waypointsTmp.leftCols(nStartWp) = waypoints.leftCols(1).replicate(1,nStartWp);
waypointsTmp.middleCols(nStartWp, nWaypoints) = waypoints;
waypointsTmp.rightCols(nEndWp) = waypoints.rightCols(1).replicate(1,nEndWp);
waypoints.resize(nDoF, nWaypoints);
waypoints = waypointsTmp;
// std::cout << pointToPointDurationVector << std::endl;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWpNew-1);
double extraWpDt = 0.01 * kernelLengthParameter;
// double extraWpDt = 0.01 * pointToPointDurationVector.sum();
// std::cout << "extraWpDt: " << extraWpDt << std::endl;
durationVectorTmp.head(nStartWp) = Eigen::VectorXd::Constant(nStartWp, extraWpDt);
durationVectorTmp.segment(nStartWp, nWaypoints-1) = pointToPointDurationVector;
durationVectorTmp.tail(nEndWp) = Eigen::VectorXd::Constant(nEndWp, extraWpDt);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
nWaypoints = nWpNew;
std::cout << pointToPointDurationVector << std::endl;
calculateVarianceParameters();
}
int main(int argc, char **argv) {
Random::randomize();
std::string paramfile = "multi.ini";
int test = 1;
char c;
while ((c = getopt(argc, argv, "p:h")) != EOF) {
switch (c) {
case 'p':
paramfile = optarg;
break;
case 'h':
default:
std::cerr << "Usage: " << argv[0] << " [options]\n";
std::cerr << "\nOptions:\n";
std::cerr << "-p <file>: use the given parameter file\n";
std::cerr << "-h: this help\n";
return 1;
}
}
TParam p;
p.loadTree(paramfile);
MultiAgentMotionModel *motionModel;
std::string typestr = p("multi_rotor_control/type").toString();
if (typestr == "point2d") {
motionModel = new Point2dMotionModel();
} else if (typestr == "rotor2d") {
motionModel = new Rotor2dMotionModel();
} else {
std::cerr << "Error: Unknown type '" << typestr << "' - exiting\n";
exit(-1);
}
motionModel->init(p);
TargetTrackingController ttc;
ttc.init(p);
std::vector<const SensorModel*> sensorModels = ttc.getTopology().getSensorModels();
if (test == 1) {
// online test
TargetTrajectory tt;
tt.init(p);
EKF ekf(motionModel);
ekf.init(p);
std::ofstream topo_out("topo.out");
std::ofstream multi_out("multi.out");
unsigned int nA = motionModel->getNumAgents();
unsigned int nT = motionModel->getNumTargets();
unsigned int aSD = motionModel->getAgentStateDim();
unsigned int cSD = motionModel->getAgentControlDim();
unsigned int tSD = motionModel->getTargetStateDim();
// test output
Eigen::VectorXd state = p("estimation/initialState").toVectorXd();
for (unsigned int i=0; !tt.atEnd(); ++i) {
Eigen::VectorXd mean = ekf.getMean();
Eigen::MatrixXd cov = ekf.getCovariance();
// Eigen::VectorXd control(nA*cSD);
// Eigen::VectorXd targetPosition = state.segment(aSD*nA, cSD);
// for (unsigned int i=0; i<nA; ++i) {
// control.segment(cSD*i, cSD) = (targetPosition - state.segment(aSD*i, cSD))/p("estimation/motionDt").toDouble();
// }
Eigen::VectorXd control = ttc.getControlTopo(&ekf, motionModel, sensorModels);
multi_out << aSD << " " << cSD << " " << tSD << " 0 0 " << nA << " " << nT << " 0 0 0"
<< " " << state.transpose()
<< " " << control.transpose()
<< " " << mean.transpose();
multi_out << " " << Eigen::Map<Eigen::MatrixXd>(cov.data(), 1, cov.cols()*cov.cols());
// for (int i=0; i<N+1; ++i) {
// multi_out << " " << cov(2*agentStateDim, 2*agentStateDim) << " " << cov(2*agentStateDim, 2*agentStateDim+1) << " " << cov(2*agentStateDim+1, 2*agentStateDim) << " " << cov(2*agentStateDim+1, 2*agentStateDim+1);
// }
multi_out << "\n";
ttc.getTopology().write(topo_out, state);
// simulation
state = motionModel->move(state, control, motionModel->sampleNoise(state, control));
if (Random::uniform() < p("multi_rotor_control/kidnap/target").toDouble()) {
// kidnap target
state.segment(aSD*nA, 2) = tt.randomJump();
ekf.getCovariance().block(aSD*nA, aSD*nA, 4, 4) = Eigen::MatrixXd::Identity(4, 4)*4.0;
} else {
state.segment(aSD*nA, 2) = tt.step();
}
if (Random::uniform() < p("multi_rotor_control/kidnap/agent").toDouble()) {
// kidnap agent
int agent = rand()%nA;
state.segment(aSD*agent, 2) = Eigen::Vector2d(Random::uniform(-1, 1), Random::uniform(-0.5, 0.5));
ekf.getCovariance().block(aSD*agent, aSD*agent, 2, 2) = Eigen::MatrixXd::Identity(2, 2)*4.0;
}
// state estimation
ekf.predict(control);
for (std::vector<const SensorModel*>::const_iterator it = sensorModels.begin();
it != sensorModels.end(); ++it) {
if ((*it)->measurementAvailable(state)) {
Eigen::VectorXd noiseSample = (*it)->sampleNoise(state, (*it)->sense(state));
Eigen::VectorXd measurementSample = (*it)->sense(state, noiseSample);
ekf.correct(measurementSample, *(*it));
}
}
}
}
return 0;
}
int main() {
// Logging:
// --------
const int nbFile=7;
std::vector<std::string> name_vec(nbFile);
name_vec[0]="CoM_X";
name_vec[1]="CoM_Y";
name_vec[2]="CoM_Yaw";
name_vec[3]="CoP_X";
name_vec[4]="CoP_Y";
name_vec[5]="BAS_X";
name_vec[6]="BAS_Y";
std::vector<std::ofstream*> data_vec(nbFile);
std::vector<std::ifstream*> ref_vec(nbFile);
for (int i = 0; i < nbFile; ++i) {
data_vec[i] = new std::ofstream((name_vec[i]+".data").c_str());
ref_vec[i] = new std::ifstream((name_vec[i]+".ref").c_str());
}
// Create and initialize generator:
// -------------------------------
WalkgenAbstract * walk = createWalkgen(CURRENT_QPSOLVERTYPE);
MPCData mpcData;
mpcData.weighting.basePositionInt[0]=0;
mpcData.weighting.basePosition[0]=10;
walk->init(mpcData);
EnvData envData;
Eigen::VectorXd obstacleRadius(3);
Eigen::VectorXd obstaclePositionX(3);
Eigen::VectorXd obstaclePositionY(3);
obstacleRadius(0) = 1;
obstaclePositionX(0) = 2;
obstaclePositionY(0) = -0.2;
obstacleRadius(1) = 0.5;
obstaclePositionX(1) = 4;
obstaclePositionY(1) = 0.1;
obstacleRadius(2) = 0.5;
obstaclePositionX(2) = 5;
obstaclePositionY(2) = -0.3;
envData.obstacleRadius = obstacleRadius;
envData.obstaclePositionX = obstaclePositionX;
envData.obstaclePositionY = obstaclePositionY;
envData.nbObstacle = 3;
// Run:
// ----
BodyState state = walk->bodyState(COM);
state.x(1) += 0;
state.x(2) += 0;
walk->bodyState(COM, state);
double velocity = 0.5;
walk->velReferenceInGlobalFrame(velocity, 0, 0);
walk->posReferenceInGlobalFrame(0, 0, 0);
walk->online(0.0);
double t = 0;
walk->online(t);
MPCSolution result;
Eigen::VectorXd position(10);
Eigen::VectorXd zero(10);
Eigen::VectorXd basePos(20);
basePos.fill(0);
zero.fill(0);
for (t += 0.001; t < 45; t += 0.001){
for(int i=0; i<10; ++i)
{
position(i)=0.16*(i+1)*velocity+result.state_vec[1].baseTrajX_(0);
}
walk->posReferenceInGlobalFrame(position, zero, zero);
result = walk->online(t);
Eigen::VectorXd basePos;
walk->QPBasePosition(basePos);
envData.obstacleLinearizationPointX = basePos.segment(0, 10);
envData.obstacleLinearizationPointY = basePos.segment(10, 10);
walk->envData(envData);
dumpTrajectory(result, data_vec);
}
// Reopen the files:
// -----------------
std::vector<std::ifstream*> check_vec(nbFile);
for(int i = 0;i < nbFile; ++i){
check_vec[i] = new std::ifstream((name_vec[i]+".data").c_str());
}
bool success = ((fabs(result.state_vec[2].baseTrajX_(0)-velocity)<1e-4)
&&(fabs(result.state_vec[2].baseTrajY_(0)-0)<1e-4)
&&(fabs(result.state_vec[2].CoMTrajYaw_(0)-0)<1e-4));
for(unsigned i = 0; i < check_vec.size();++i){
// if the reference file exists, compare with the previous version.
if (*ref_vec[i]){
if (!checkFiles(*check_vec[i],*ref_vec[i])){
//success = false;
}
}
// otherwise, create it
else{
copyFile((name_vec[i]+".data"), (name_vec[i]+".ref"));
}
check_vec[i]->close();
ref_vec[i]->close();
}
makeScilabFile("data", t);
makeScilabFile("ref", t);
for (int i=0; i < nbFile; ++i) {
delete check_vec[i];
delete ref_vec[i];
delete data_vec[i];
}
delete walk;
return (success)?0:1;
}
Eigen::Ref<const Eigen::VectorXd> getTip(const Eigen::VectorXd &x, unsigned int id) const
{
return x.segment(3 * links_ * ((id % chains_) + 1) - 3, 3);
}
Eigen::Ref<const Eigen::VectorXd> getLink(const Eigen::VectorXd &x, const unsigned int idx) const
{
const unsigned int offset = 3 * links_ * chainNum_;
return x.segment(offset + 3 * idx, 3);
}
void MagCal::calcMagComp()
{
/*
* Inspired by
* http://davidegironi.blogspot.it/2013/01/magnetometer-calibration-helper-01-for.html#.UriTqkMjulM
*
* Ellipsoid fit from:
* http://www.mathworks.com/matlabcentral/fileexchange/24693-ellipsoid-fit
*
* To use Eigen to convert matlab code, have a look at Eigen/AsciiQuickReference.txt
*/
if (mMagSamples.size() < 9) {
QMessageBox::warning(this, "Magnetometer compensation",
"Too few points.");
return;
}
int samples = mMagSamples.size();
Eigen::VectorXd ex(samples);
Eigen::VectorXd ey(samples);
Eigen::VectorXd ez(samples);
for (int i = 0;i < samples;i++) {
ex(i) = mMagSamples.at(i).at(0);
ey(i) = mMagSamples.at(i).at(1);
ez(i) = mMagSamples.at(i).at(2);
}
Eigen::MatrixXd eD(samples, 9);
for (int i = 0;i < samples;i++) {
eD(i, 0) = ex(i) * ex(i);
eD(i, 1) = ey(i) * ey(i);
eD(i, 2) = ez(i) * ez(i);
eD(i, 3) = 2.0 * ex(i) * ey(i);
eD(i, 4) = 2.0 * ex(i) * ez(i);
eD(i, 5) = 2.0 * ey(i) * ez(i);
eD(i, 6) = 2.0 * ex(i);
eD(i, 7) = 2.0 * ey(i);
eD(i, 8) = 2.0 * ez(i);
}
Eigen::MatrixXd etmp1 = eD.transpose() * eD;
Eigen::MatrixXd etmp2 = eD.transpose() * Eigen::MatrixXd::Ones(samples, 1);
Eigen::VectorXd eV = etmp1.lu().solve(etmp2);
Eigen::MatrixXd eA(4, 4);
eA(0,0)=eV(0); eA(0,1)=eV(3); eA(0,2)=eV(4); eA(0,3)=eV(6);
eA(1,0)=eV(3); eA(1,1)=eV(1); eA(1,2)=eV(5); eA(1,3)=eV(7);
eA(2,0)=eV(4); eA(2,1)=eV(5); eA(2,2)=eV(2); eA(2,3)=eV(8);
eA(3,0)=eV(6); eA(3,1)=eV(7); eA(3,2)=eV(8); eA(3,3)=-1.0;
Eigen::MatrixXd eCenter = -eA.topLeftCorner(3, 3).lu().solve(eV.segment(6, 3));
Eigen::MatrixXd eT = Eigen::MatrixXd::Identity(4, 4);
eT(3, 0) = eCenter(0);
eT(3, 1) = eCenter(1);
eT(3, 2) = eCenter(2);
Eigen::MatrixXd eR = eT * eA * eT.transpose();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eEv(eR.topLeftCorner(3, 3) * (-1.0 / eR(3, 3)));
Eigen::MatrixXd eVecs = eEv.eigenvectors();
Eigen::MatrixXd eVals = eEv.eigenvalues();
Eigen::MatrixXd eRadii(3, 1);
eRadii(0) = sqrt(1.0 / eVals(0));
eRadii(1) = sqrt(1.0 / eVals(1));
eRadii(2) = sqrt(1.0 / eVals(2));
Eigen::MatrixXd eScale = eRadii.asDiagonal().inverse() * eRadii.minCoeff();
Eigen::MatrixXd eComp = eVecs * eScale * eVecs.transpose();
mMagComp.resize(9);
mMagComp[0] = eComp(0, 0);
mMagComp[1] = eComp(0, 1);
mMagComp[2] = eComp(0, 2);
mMagComp[3] = eComp(1, 0);
mMagComp[4] = eComp(1, 1);
mMagComp[5] = eComp(1, 2);
mMagComp[6] = eComp(2, 0);
mMagComp[7] = eComp(2, 1);
mMagComp[8] = eComp(2, 2);
mMagCompCenter.resize(3);
mMagCompCenter[0] = eCenter(0, 0);
mMagCompCenter[1] = eCenter(1, 0);
mMagCompCenter[2] = eCenter(2, 0);
QVector<double> magX, magY, magZ;
for (int i = 0;i < mMagSamples.size();i++) {
double mx = mMagSamples.at(i).at(0);
double my = mMagSamples.at(i).at(1);
double mz = mMagSamples.at(i).at(2);
mx -= mMagCompCenter.at(0);
my -= mMagCompCenter.at(1);
mz -= mMagCompCenter.at(2);
magX.append(mx * mMagComp.at(0) + my * mMagComp.at(1) + mz * mMagComp.at(2));
magY.append(mx * mMagComp.at(3) + my * mMagComp.at(4) + mz * mMagComp.at(5));
magZ.append(mx * mMagComp.at(6) + my * mMagComp.at(7) + mz * mMagComp.at(8));
}
ui->magSampXyPlot->graph(1)->setData(magX, magY);
ui->magSampXzPlot->graph(1)->setData(magX, magZ);
ui->magSampYzPlot->graph(1)->setData(magY, magZ);
updateMagPlots();
}
