/// Determines the goal configuration of the left arm based on the cinder block pose, calls
/// analytical I.K. to find a suitable goal arm configuration and calls RRT routine to get the
/// appropriate trajectory. Finally, using the Timer routine, displays the trajectory.
void SimTab::OnButton(wxCommandEvent &evt) {
static int i = 0;
i = (i + 1) % 4;
// Set the initial config if restarting
if(i == 3) {
krang->setConfig(planar_ids, Eigen::Vector3d(0.0, 0.0, -M_PI_2));
viewer->DrawGLScene();
return;
}
// Get the normal and the location of the cinder block
Eigen::Matrix4d cinderT = mWorld->getSkeleton("Cinder2")->getNode("root")->getWorldTransform();
Eigen::Vector3d normal = -cinderT.block<3,1>(0,2);
Eigen::Vector3d hole (cinderT(0,3), cinderT(1,3), 0.44);
hole += normal * 0.0945;
cout << "\nhole: " << hole.transpose() << endl;
cout << "normal: " << normal.transpose() << endl;
// Determine where we want Krang to be
Eigen::Vector3d perp (-normal(1), normal(0), 0.0);
Eigen::Vector3d goalPos = hole + normal * 0.3 + perp * 0.6;
cout << "goalPos: " << goalPos.transpose() << endl;
double th = atan2(normal(0), -normal(1)) + 0.1;
Eigen::Vector3d goalConfig (goalPos(0), goalPos(1), th);
// Determine the angle we need to be to go the goal position
double th2 = atan2(-goalPos(0), goalPos(1));
//cout << th2 << " " << sqrt(goalPos(0) * goalPos(0) + goalPos(1) * goalPos(1)) << " " << th << endl;
static const double wheelRadius = 0.264; // r, cm
static const double rotationRadius = 0.350837; // R, cm
double v1 = (th2 * rotationRadius) / wheelRadius;
double v3 = (th * rotationRadius) / wheelRadius;
double dist = sqrt(goalPos(0) * goalPos(0) + goalPos(1) * goalPos(1));
double v2 = dist / wheelRadius;
Eigen::Vector3d params (v1,v2,v3);
cout << "params: " << params.transpose() << endl;
if(i == 2) {
cout << "th: " << th << endl;
krang->setConfig(planar_ids, Eigen::Vector3d(goalPos(0), goalPos(1), th));
viewer->DrawGLScene();
return;
}
if(i == 0) {
cout << "th2: " << th2 << endl;
krang->setConfig(planar_ids, Eigen::Vector3d(0.0, 0.0, th2));
viewer->DrawGLScene();
return;
}
// Draw the robot at the goal position having just arrived
if(i == 1) {
krang->setConfig(planar_ids, Eigen::Vector3d(goalPos(0), goalPos(1), th2));
viewer->DrawGLScene();
}
}
/*
* Given the line parameters [n_x,n_y,a_x,a_y] check if
* [n_x, n_y] dot [data.x-a_x, data.y-a_y] < m_delta
*/
bool RSTEstimator::agree(std::vector<double> ¶meters, std::pair<Eigen::Vector3d,Eigen::Vector3d> &data)
{
Eigen::Matrix3d R;
Eigen::Vector3d T;
Eigen::Vector3d dif;
double E1,E2;
R << parameters[0] , parameters[1] , parameters[2],
parameters[3] , parameters[4] , parameters[5],
parameters[6] , parameters[7] , parameters[8];
T << parameters[9] , parameters[10] , parameters[11];
dif = data.first - R*data.second + T; //X21
E1 = dif.transpose()*dif;
dif = data.second - R.inverse() * (data.first-T); //X12
E2 = dif.transpose()*dif;
//std::cout << "E1= " << E1 << "\nE2= " << E2 << "\nE1+E2= "<< E1+E2 << " " << this->deltaSquared <<"\n";
return ( (E1 + E2) < this->deltaSquared);
}
template <typename PointSource, typename PointTarget> inline void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::OptimizationFunctorWithIndices::fdf (const Vector6d& x, double& f, Vector6d& g)
{
Eigen::Matrix4f transformation_matrix = gicp_->base_transformation_;
gicp_->applyState(transformation_matrix, x);
f = 0;
g.setZero ();
Eigen::Matrix3d R = Eigen::Matrix3d::Zero ();
const int m = static_cast<const int> (gicp_->tmp_idx_src_->size ());
for (int i = 0; i < m; ++i)
{
// The last coordinate, p_src[3] is guaranteed to be set to 1.0 in registration.hpp
Vector4fMapConst p_src = gicp_->tmp_src_->points[(*gicp_->tmp_idx_src_)[i]].getVector4fMap ();
// The last coordinate, p_tgt[3] is guaranteed to be set to 1.0 in registration.hpp
Vector4fMapConst p_tgt = gicp_->tmp_tgt_->points[(*gicp_->tmp_idx_tgt_)[i]].getVector4fMap ();
Eigen::Vector4f pp (transformation_matrix * p_src);
// The last coordiante is still guaranteed to be set to 1.0
Eigen::Vector3d res (pp[0] - p_tgt[0], pp[1] - p_tgt[1], pp[2] - p_tgt[2]);
// temp = M*res
Eigen::Vector3d temp (gicp_->mahalanobis((*gicp_->tmp_idx_src_)[i]) * res);
// Increment total error
f+= double(res.transpose() * temp);
// Increment translation gradient
// g.head<3> ()+= 2*M*res/num_matches (we postpone 2/num_matches after the loop closes)
g.head<3> ()+= temp;
pp = gicp_->base_transformation_ * p_src;
Eigen::Vector3d p_src3 (pp[0], pp[1], pp[2]);
// Increment rotation gradient
R+= p_src3 * temp.transpose();
}
f/= double(m);
g.head<3> ()*= double(2.0/m);
R*= 2.0/m;
gicp_->computeRDerivative(x, R, g);
}
int main(int argc, char* argv[])
{
Eigen::Vector3d geoInit(16,42,1000);
Eigen::Vector3d xyz = labust::tools::geodetic2ecef(geoInit);
std::cout<<"Origin, GEO->ECEF:"<<xyz.transpose()<<std::endl;
Eigen::Vector3d ned = labust::tools::ecef2ned(xyz, geoInit);
std::cout<<"Origin, ECEF->NED:"<<ned.transpose()<<std::endl;
Eigen::Vector3d xyz2 = labust::tools::ned2ecef(ned, geoInit);
std::cout<<"Origin, NED->ECEF:"<<xyz2.transpose()<<std::endl;
Eigen::Vector3d geoInit2 = labust::tools::ecef2geodetic(xyz2);
std::cout<<"Origin, ECEF->GEO:"<<geoInit2.transpose()<<std::endl;
std::cout<<std::endl;
Eigen::Vector3d nedR(1000, 5000, 1000);
std::cout<<"Random NED point: n="<<nedR(0)<<", e="<<nedR(1)<<", d="<<nedR(2)<<std::endl;
Eigen::Vector3d xyz3 = labust::tools::ned2ecef(nedR, geoInit);
std::cout<<"Random, NED->ECEF:"<<xyz3.transpose()<<std::endl;
Eigen::Vector3d geo3 = labust::tools::ecef2geodetic(xyz3);
std::cout<<"Random, ECEF->GEO:"<<geo3.transpose()<<std::endl;
Eigen::Vector3d xyz4 = labust::tools::geodetic2ecef(geo3);
std::cout<<"Random, GEO->ECEF:"<<xyz4.transpose()<<std::endl;
Eigen::Vector3d ned4 = labust::tools::ecef2ned(xyz4, geoInit);
std::cout<<"Random, ECEF->NED:"<<ned4.transpose()<<std::endl;
return 0;
}
void CylindricalNVecConstr::impl_jacobian(jacobian_t& jac, const argument_t& x) const
{
jac.reserve(jac_.dof());
pgdata_->x(x);
sva::PTransformd pos = surfaceFrame_*pgdata_->mbc().bodyPosW[bodyIndex_];
Eigen::Vector3d vec = (targetFrame_.translation() - pos.translation());
double dot = vec.dot(pos.rotation().row(2));
double sign = std::copysign(1., dot);
double vecNDot = sign*2.*vec.dot(pos.rotation().row(2));
const Eigen::MatrixXd& jacVecNMat =
jac_.vectorJacobian(pgdata_->mb(), pgdata_->mbc(),
surfaceFrame_.rotation().row(2).transpose());
jacMat_.row(0).noalias() = vecNDot*vec.transpose()*jacVecNMat.block(3, 0, 3, jac_.dof());
jac_.point(surfaceFrame_.translation());
const Eigen::MatrixXd& jacMat = jac_.jacobian(pgdata_->mb(), pgdata_->mbc());
jac_.point(Eigen::Vector3d::Zero());
jacMat_.row(0).noalias() -= vecNDot*pos.rotation().row(2)*jacMat.block(3, 0, 3, jac_.dof());
jacMat_.row(0).noalias() += (2.*vec.transpose())*jacMat.block(3, 0, 3, jac_.dof());
fullJacobianSparse(pgdata_->mb(), jac_, jacMat_,
jac, {0, pgdata_->qParamsBegin()});
}
/// Get the joint values from the encoders and the imu and compute the center of mass as well
void getState(Vector6d& state, double dt) {
// Read motor encoders, imu and ft and update dart skeleton
hw->updateSensors(dt);
// Calculate the COM of the body
Eigen::Vector3d comRobot = krang->getWorldCOM();
comRobot(2) -= 0.264;
comRobot(0) += 0.00;
if(dbg) cout << "comRobot: " << comRobot.transpose() << endl;
// Get the com of the object
Eigen::Matrix4d T = krang->getNode("lGripper")->getWorldTransform();
Eigen::Vector3d objectCOM = T.topRightCorner<3,1>() + T.block<3,1>(0,2) * 0.20;
if(dbg) cout << "R: " << T.topLeftCorner<3,3>() << endl;
// Combine the two coms
Eigen::Vector3d com = (comRobot * 145.0 + objectCOM * 13.2) / (145 + 13.2);
if(dbg) cout << "com: " << com.transpose() << endl;
// Update the state (note for amc we are reversing the effect of the motion of the upper body)
state(0) = atan2(com(0), com(2));
state(1) = hw->imuSpeed;
state(2) = (hw->amc->pos[0] + hw->amc->pos[1]) / 2.0 + hw->imu;
state(3) = (hw->amc->vel[0] + hw->amc->vel[1]) / 2.0 + hw->imuSpeed;
state(4) = (hw->amc->pos[1] - hw->amc->pos[0]) / 2.0;
state(5) = (hw->amc->vel[1] - hw->amc->vel[0]) / 2.0;
}
void RectangleHandler::initRectangle(const Eigen::VectorXd& Sw, double lambda,
const Eigen::VectorXd& z, Eigen::VectorXd& shapeParams,
Eigen::VectorXd& F) {
//Get the points
Eigen::Vector3d m1(z[0], z[1], 1);
Eigen::Vector3d m2(z[2], z[3], 1);
Eigen::Vector3d m3(z[4], z[5], 1);
Eigen::Vector3d m4(z[6], z[7], 1);
Eigen::Vector3d Ct(Sw[0], Sw[1], Sw[2]);
Eigen::Quaterniond Cq(Sw[3], Sw[4], Sw[5], Sw[6]);
//compute normals
double c2 = (m1.cross(m3).transpose() * m4)[0]
/ (m2.cross(m3).transpose() * m4)[0];
double c3 = (m1.cross(m3).transpose() * m2)[0]
/ (m4.cross(m3).transpose() * m2)[0];
Eigen::Vector3d n2 = c2 * m2 - m1;
Eigen::Vector3d n3 = c3 * m4 - m1;
//Compute rotation matrix columns
Eigen::Vector3d R1 = _K.inverse() * n2;
R1 = R1 / R1.norm();
Eigen::Vector3d R2 = _K.inverse() * n3;
R2 = R2 / R2.norm();
Eigen::Vector3d R3 = R1.cross(R2);
//Compute frame quaternion
Eigen::Matrix3d R;
R << R1, R2, R3;
const Eigen::Quaterniond qOC(R);
Eigen::Quaterniond Fqhat = Cq * qOC;
//Compute frame transaltion
Eigen::Matrix3d omega = _K.transpose().inverse() * _K.inverse();
double ff = sqrt(
(n2.transpose() * omega * n2)[0] / (n3.transpose() * omega * n3)[0]);
Eigen::Vector3d CtOhat = lambda * _K.inverse() * m1;
Eigen::Vector3d Fthat = Ct + Cq.toRotationMatrix() * CtOhat;
//compute shape parameters
Eigen::Vector3d X = _K * R1;
Eigen::Vector3d Y = c2 * lambda * m2 - lambda * m1;
double w = ((X.transpose() * X).inverse() * X.transpose() * Y)[0];
double h = w / ff;
//Write the results
shapeParams << w, h;
F << Fthat[0], Fthat[1], Fthat[2], Fqhat.w(), Fqhat.x(), Fqhat.y(), Fqhat.z();
}
void AnchoredRectangleHandler::initRectangle(const Eigen::VectorXd& Fw,
double lambda, const Eigen::VectorXd& z, Eigen::VectorXd& shapeParamshat,
Eigen::VectorXd& FOhphat, Eigen::VectorXd &FOqhat) {
//Get the points
Eigen::Vector3d m1(z[0], z[1], 1);
Eigen::Vector3d m2(z[2], z[3], 1);
Eigen::Vector3d m3(z[4], z[5], 1);
Eigen::Vector3d m4(z[6], z[7], 1);
Eigen::Vector3d Ft(Fw[0], Fw[1], Fw[2]);
Eigen::Quaterniond Fq(Fw[3], Fw[4], Fw[5], Fw[6]);
//compute normals
double c2 = (m1.cross(m3).transpose() * m4)[0]
/ (m2.cross(m3).transpose() * m4)[0];
double c3 = (m1.cross(m3).transpose() * m2)[0]
/ (m4.cross(m3).transpose() * m2)[0];
Eigen::Vector3d n2 = c2 * m2 - m1;
Eigen::Vector3d n3 = c3 * m4 - m1;
//Compute rotation matrix columns
Eigen::Vector3d R1 = _K.inverse() * n2;
R1 = R1 / R1.norm();
Eigen::Vector3d R2 = _K.inverse() * n3;
R2 = R2 / R2.norm();
Eigen::Vector3d R3 = R1.cross(R2);
//Compute rotation from camera to object
Eigen::Matrix3d R;
R << R1, R2, R3;
Eigen::Quaterniond FOq_e(R);
// and initialize the of the object with respect to the anchor frame
FOqhat << FOq_e.w(), FOq_e.x(), FOq_e.y(), FOq_e.z();
// now initialize lower left corner homogeneous point
FOhphat << z[0], z[1], 1.0;
FOhphat = _K.inverse() * FOhphat;
FOhphat(2) = 1.0 / lambda; // 1/d distance of the plane parallel to the image plane on which features are initialized.
//Compute frame transaltion
Eigen::Matrix3d omega = _K.transpose().inverse() * _K.inverse();
double ff = sqrt(
(n2.transpose() * omega * n2)[0] / (n3.transpose() * omega * n3)[0]);
//compute shape parameters
Eigen::Vector3d X = _K * R1;
Eigen::Vector3d Y = c2 * lambda * m2 - lambda * m1;
double w = ((X.transpose() * X).inverse() * X.transpose() * Y)[0];
//Write the results
shapeParamshat << ff, w / lambda;
}
Eigen::Matrix<double, 1, 11> dquatDiffAxisInvar(const Eigen::Vector4d& q1, const Eigen::Vector4d& q2, const Eigen::Vector3d& u)
{
Vector4d r = quatDiff(q1, q2);
Matrix<double, 4, 8> dr = dquatDiff(q1, q2);
Matrix<double, 1, 11> de;
const auto& rvec = r.tail<3>();
de << 4.0 * r(0) * dr.row(0) + 4.0 * u.transpose() * rvec *u.transpose() * dr.block<3, 8>(1, 0), 4.0 * u.transpose() * rvec * rvec.transpose();
return de;
}
void ManipTool::update_nv(Eigen::Vector3d lv,Eigen::Vector3d& n_hat,Eigen::Vector3d& nv_dot_fb){
P_bar = Eigen::Matrix3d::Identity()-n_hat*n_hat.transpose();
nv_dot = -1*Gama_n*P_bar*L_n*n_hat;
nv_dot_fb = nv_dot;
L_n_dot = -beta_n*L_n+(1/(1+pow(lv.norm(),2)))*lv*lv.transpose();
n_hat = n_hat+nv_dot;
n_hat = n_hat/n_hat.norm();
L_n = L_n + L_n_dot;
}
static double multiplyVectorsImplementation(Eigen::Vector3d a,
Eigen::Vector3d b,
gtsam::OptionalJacobian<1,3> Ha,
gtsam::OptionalJacobian<1,3> Hb) {
if(Ha)
*Ha = b.transpose();
if(Hb)
*Hb = a.transpose();
return a.transpose() * b;
}
// NEED TO BE TESTED!!!!!
//
// A is alway an outside particle
//
//
double interactions::getLambdaBond(const Eigen::Vector3d A, Eigen::Vector3d B, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax){
if (abs(B(1)-A(1))<=DBLDBL_MIN) {
//same y different x
if (A(0)<xmin)
return abs(B(0)-xmin)/abs(A(0)-B(0));
else if (A(0)>xmax)
return abs(B(0)-xmax)/abs(A(0)-B(0));
}
if (abs(B(0)-A(0))<=DBLDBL_MIN) {
//same x different y
if (A(1)<ymin)
return abs(B(1)-ymin)/abs(A(1)-B(1));
else if (A(1)>ymax)
return abs(B(1)-ymax)/abs(A(1)-B(1));
}
double k=(B(1)-A(1))/(B(0)-A(0));
double b=B(1)-k*B(0);
double ym1=k*xmin+b;
double xm1=xmin;
double ym2=ymin;
double xm2=(ymin-b)/k;
double ym3=ymax;
double xm3=(ymax-b)/k;
double ym4=k*xmax+b;
double xm4=xmax;
int i,n;
Eigen::MatrixXd M(4,3);
M<<xm1,ym1,0,
xm2,ym2,0,
xm3,ym3,0,
xm4,ym4,0;
Eigen::Vector3d Mm(4);
Mm(0)=((B.transpose()-M.row(0)).array().abs()+(A.transpose()-M.row(0)).array().abs()).sum();
Mm(1)=((B.transpose()-M.row(1)).array().abs()+(A.transpose()-M.row(1)).array().abs()).sum();
Mm(2)=((B.transpose()-M.row(2)).array().abs()+(A.transpose()-M.row(2)).array().abs()).sum();
Mm(3)=((B.transpose()-M.row(3)).array().abs()+(A.transpose()-M.row(3)).array().abs()).sum();
//Find minimal Mm
for (n=i=0; i<4; i++)
n=(Mm(i)<Mm(n))?i:n;
return sqrt((B.transpose()-M.row(0)).array().square().sum())/sqrt((B-A).array().square().sum());
}
bool IncrementalPlaneEstimator::
tryPoint(const Eigen::Vector3f& iPoint, const Eigen::Vector3f& iNormal,
const float iMaxError, const float iMaxAngle) {
const int n = mPoints.size();
if (n < 2) return true;
Eigen::Vector3d p = iPoint.cast<double>();
Eigen::Vector3d sum = mSum+p;
Eigen::Matrix3d sumSquared = mSumSquared + p*p.transpose();
Eigen::Vector4f plane = getPlane(sum, sumSquared, n+1);
if (std::abs(plane.head<3>().cast<float>().dot(iNormal)) <
std::cos(iMaxAngle)) return false;
std::vector<float> prevErrors2 = computeErrors(getCurrentPlane(), mPoints);
float prevTotalError2 =
std::accumulate(prevErrors2.begin(), prevErrors2.end(), 0.0f);
std::vector<float> errors2 = computeErrors(plane, mPoints);
errors2.push_back(computeError(plane, iPoint));
float thresh2 = iMaxError*iMaxError;
int numInliers = 0;
float totalError2 = 0;
for (float e2 : errors2) {
totalError2 += e2;
numInliers += (e2<=thresh2);
}
float deltaError2 = totalError2/(n+1) - prevTotalError2/n;
return deltaError2 < thresh2/n;
//return numInliers==(n+1);
}
void IncrementalPlaneEstimator::
addPoint(const Eigen::Vector3f& iPoint) {
mPoints.push_back(iPoint);
Eigen::Vector3d p = iPoint.cast<double>();
mSum += p;
mSumSquared += p*p.transpose();
}
void CrichtonView<PointT>::onMouse( int event, int x, int y, int flags, void* userdata ) {
if( event != cv::EVENT_LBUTTONDOWN ) {
return;
}
mMutex.lock();
cv::Point3f p; Eigen::Vector3d currentPoint;
p = mPclMap.at<cv::Point3f>(y,x);
currentPoint << (double)p.x, (double)p.y, (double)p.z;
std::cout << "\t * Mouse pressed. Current point: "<< currentPoint.transpose() << std::endl;
double minDist; int minIndex; double dist;
mSelectedCluster = -1;
for( int i = 0; i < mClustersBB.size(); ++i ) {
int cx, cy;
cx = (mClustersBB[i](0) + mClustersBB[i](2) )/2;
cy = (mClustersBB[i](1) + mClustersBB[i](3) )/2;
dist = (x-cx)*(x-cx) + (y-cy)*(y-cy);
if( i == 0 ) { minIndex = 0; minDist = dist; }
else { if( dist < minDist ) { minIndex = i; minDist = dist; } }
}
mSelectedCluster = minIndex;
printf("\t * Selected cluster: %d \n", mSelectedCluster );
mMutex.unlock();
}
void VideoIMUFusion::RunningData::handleIMUVelocity(
const OSVR_TimeValue ×tamp, const Eigen::Vector3d &angVel) {
#ifdef OSVR_FPE
FPExceptionEnabler fpe;
#endif
if (preReport(timestamp)) {
#if 0
static int s = 0;
if (s == 0) {
static const Eigen::IOFormat fmt(3, 0, ", ", ";\n", "", "", "[",
"]");
OSVR_DEV_VERBOSE(
"\nprediction: "
<< state().getAngularVelocity().transpose().format(fmt)
<< "\nMeasurement: " << angVel.transpose().format(fmt)
<< "\nVariance: "
<< state()
.errorCovariance()
.diagonal()
.tail<3>()
.transpose()
.format(fmt));
}
s = (s + 1) % 100;
#endif
m_imuMeasVel.setMeasurement(angVel);
osvr::kalman::correct(state(), processModel(), m_imuMeasVel);
}
}
/**
* @function onMouse
* @brief Tells the point location
*/
static void onMouse( int event, int x, int y, int flags, void* userdata ) {
if( event != cv::EVENT_LBUTTONDOWN ) {
return;
}
mutex.lock();
cv::Point3f p; Eigen::Vector3d currentPoint;
p = gXyzImg.at<cv::Point3f>(y,x);
currentPoint << (double)p.x, (double)p.y, (double)p.z;
std::cout << "\t * Mouse pressed. Current point: "<< currentPoint.transpose() << std::endl;
double minDist; int minIndex; double dist;
gSelectedCluster = -1;
for( int i = 0; i < gClustersBB.size(); ++i ) {
int cx, cy;
cx = (gClustersBB[i](0) + gClustersBB[i](2) )/2;
cy = (gClustersBB[i](1) + gClustersBB[i](3) )/2;
dist = (x-cx)*(x-cx) + (y-cy)*(y-cy);
if( i == 0 ) { minIndex = 0; minDist = dist; }
else { if( dist < minDist ) { minIndex = i; minDist = dist; } }
}
gSelectedCluster = minIndex;
saveData();
mutex.unlock();
}
double IIRNotch::processSample(double input){
bool v = false;
Eigen::Vector3d x_temp ( input , x(0), x(1) );
Eigen::Vector3d y_temp ( 0, y(0), y(1) );
if(v) std::cout << input << "\n";
if(v) std::cout << x_temp.transpose() << " x_temp\n";
if(v) std::cout << y_temp.transpose() << " y_temp\n";
if(v) std::cout << b.transpose() << " b\n";
if(v) std::cout << a.transpose() << " a\n";
if(v){
Eigen::Vector3d bit = x_temp.cross(b);
std::cout << bit.transpose() << " bit\n";
}
double output = (x_temp.dot(b)) - (y_temp.dot(a));
double temp_x = x(0);
x << input , temp_x ;
double temp_y = y(0);
y << output, temp_y ;
if(v) std::cout << x.transpose() << " x\n";
if(v) std::cout << y.transpose() << " y\n\n";
return output;
}
void PartFilter::computeDistance(int partition)
{
std::multimap<int, std::string>::iterator it;
for (int i=0 ; i<mModels.size() ; i++)
{
double distance=0;
it = mOffsetPartToName[i].find(partition);
for (it = mOffsetPartToName[i].equal_range(partition).first ; it != mOffsetPartToName[i].equal_range(partition).second ; ++it)
{
if (mJointNameToPos[(*it).second] != -1)
{
int pos = mJointNameToPos[(*it).second];
double distTemp=0;
// Mahalanobis distance
//cout << (*it).second << "=>" << mPosNames[pos] << endl;
Eigen::Vector3d jtPos = mModels[i]->getJoint((*it).second)->getXYZVect();
Eigen::Vector3d jtObs(mCurrentFrame[pos][1], mCurrentFrame[pos][2], mCurrentFrame[pos][3]);
Eigen::Vector3d diff = jtPos - jtObs;
Eigen::Matrix3d cov;
cov.setIdentity();
distTemp = diff.transpose()*(cov*diff);
distance += distTemp;
}
}
mCurrentDistances[i] = distance;
}
}
/**
* @brief hqp_wrapper::updObstacle Updates obstacles' matrices
* @param levelName Level's ID
* @param Jac Jacobian (6xNc)
* @param n Normalised Vector between end effector and obstacles' position
* @param b scalar b, n*J < b
*/
void hqp_wrapper::updObstacle(std::string levelName, const Eigen::MatrixXd Jac, const Eigen::Vector3d n, const double b ){
int indx = leveNameMap[levelName];
this->B[indx][0] = soth::Bound(b, soth::Bound::BOUND_INF);
this->J[indx] = n.transpose()*Jac.block(0,0,3,this->NC);
// std::cout << "J[indx]:\n" <<J[indx] <<"\n";
}
Eigen::Matrix4d Filter::omegaMatrix(const Eigen::Vector3d vec) {
Eigen::Matrix4d mat;
mat.block<3, 3>(0, 0) = Filter::crossMatrix(-1*vec);
mat.block<1, 3>(3, 0) = -1*(vec.transpose());
mat.block<3, 1>(0, 3) = vec;
mat(3, 3) = 0;
return mat;
}
static void qprod(const Eigen::Vector4d & q, const Eigen::Vector4d & p, Eigen::Vector4d & prod_result) {
double a=q(0);
Eigen::Vector3d v = q.segment(1, 3); //coefficients q
double x=p(0);
Eigen::Vector3d u = p.segment(1, 3); //coefficients p
prod_result << a*x-v.transpose()*u, (a*u+x*v) + v.cross(u);
}
void generateColorModel(const PCRGB::Ptr& pc_in, Eigen::Vector3d& mean, Eigen::Matrix3d& cov)
{
Eigen::MatrixXd X(3, pc_in->size());
for(uint32_t i=0;i<pc_in->size();i++)
extractHSL(pc_in->points[i].rgb, X(0,i), X(1,i), X(2,i));
mean = X.rowwise().sum() / pc_in->size();
cov = X * X.transpose() / pc_in->size() - mean * mean.transpose();
}
/**
* @brief hqp_wrapper::addObstacle
* @param levelName String to be used as ID
* @param Jac Jacobian (6xNc), only the first 3 rows (position) is used currently
* @param n Normalised Vector between end effector and obstacles' position
* @param b scalar b, n*J < b
*/
void hqp_wrapper::addObstacle(std::string levelName, const Eigen::MatrixXd Jac, const Eigen::Vector3d n, double b){
Eigen::MatrixXd Jtmp;
Jtmp = n.transpose()*Jac.block(0,0,3,this->NC);
Eigen::Matrix<double,1,1> B;
B(0) = b;
addStage(levelName,Jtmp,B,soth::Bound::BOUND_INF);
std::cout <<"Adding new element:" << levelName <<", at:" << leveNameMap[levelName]<<"\n";
}
void inPointsCallback(const geometry_msgs::Polygon& pts_array) {
if (pts_array.points.size() != 2) {
ROS_ERROR("wrong number of points in pts_array! should be two");
return;
}
//unpack the points and fill in entry and exit pts
g_O_entry_point(0) = pts_array.points[0].x;
g_O_entry_point(1) = pts_array.points[0].y;
g_O_entry_point(2) = pts_array.points[0].z;
g_O_exit_point(0) = pts_array.points[1].x;
g_O_exit_point(1) = pts_array.points[1].y;
g_O_exit_point(2) = pts_array.points[1].z;
g_got_new_points = true;
cout << "received new entry point = " << g_O_entry_point.transpose() << endl;
cout << "and exit point: " << g_O_exit_point.transpose() << endl;
}
void inPointCallback(const geometry_msgs::Point& pt_msg) {
g_O_entry_point(0) = pt_msg.x;
g_O_entry_point(1) = pt_msg.y;
g_O_entry_point(2) = pt_msg.z;
g_got_new_entry_point = true;
cout << "received new entry point = " << g_O_entry_point.transpose() << endl;
}
bool ArmMotionInterface::unpack_delta_p(void) {
int npts = request_.delta_p.size();
if (npts != 3) {
ROS_WARN("plan_cartesian_delta_p: request delta_p is wrong size");
return false;
}
//copy message data to member var:
for (int i = 0; i < 3; i++) delta_p_(i) = request_.delta_p[i];
cout << "requested delta_p: " << delta_p_.transpose() << endl;
}
//for final project for cs7492 - implicit with conjugate gradient
void mySpring::buildSprJpJv() {
Eigen::Vector3d delPos = (a->getPosition() - b->getPosition());
Eigen::Matrix3d dpdpT = delPos * delPos.transpose();
double currLen = (delPos.norm());
if (currLen != 0) currLen = 1.0 / currLen; //handles if currLen is 0, Jp will be 0
dpdpT = dpdpT*(currLen*currLen);
Jp = (dpdpT + (Id3x3 - dpdpT)*(1 - (restLen*currLen))) * Ks;
//Jd is constant, since kd won't change
}
void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices)
{
type = "Oriented";
orientedPoints.clear();
// compute mean
Eigen::Vector3d center;
center.setZero();
for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
center += v->position;
}
center /= (double)vertices.size();
// adjust for mean and compute covariance
Eigen::Matrix3d covariance;
covariance.setZero();
for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
Eigen::Vector3d pAdg = v->position - center;
covariance += pAdg * pAdg.transpose();
}
covariance /= (double)vertices.size();
// compute eigenvectors for the covariance matrix
Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
// project min and max points on each principal axis
double min1 = INFINITY, max1 = -INFINITY;
double min2 = INFINITY, max2 = -INFINITY;
double min3 = INFINITY, max3 = -INFINITY;
double d = 0.0;
eigenVectors.transpose();
for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
d = eigenVectors.row(0).dot(v->position);
if (min1 > d) min1 = d;
if (max1 < d) max1 = d;
d = eigenVectors.row(1).dot(v->position);
if (min2 > d) min2 = d;
if (max2 < d) max2 = d;
d = eigenVectors.row(2).dot(v->position);
if (min3 > d) min3 = d;
if (max3 < d) max3 = d;
}
// add points to vector
orientedPoints.push_back(eigenVectors.row(0) * min1);
orientedPoints.push_back(eigenVectors.row(0) * max1);
orientedPoints.push_back(eigenVectors.row(1) * min2);
orientedPoints.push_back(eigenVectors.row(1) * max2);
orientedPoints.push_back(eigenVectors.row(2) * min3);
orientedPoints.push_back(eigenVectors.row(2) * max3);
}
Eigen::Vector4f IncrementalPlaneEstimator::
getPlane(const Eigen::Vector3d& iSum,
const Eigen::Matrix3d& iSumSquared,
const double iCount) {
Eigen::Vector3d mean = iSum/iCount;
Eigen::Matrix3d cov = iSumSquared/iCount - mean*mean.transpose();
Eigen::Vector4d plane;
plane.head<3>() = cov.jacobiSvd(Eigen::ComputeFullV).matrixV().col(2);
plane[3] = -plane.head<3>().dot(mean);
return plane.cast<float>();
}