//! Convert Cartesian to cylindrical state.
Eigen::VectorXd convertCartesianToCylindrical( const Eigen::VectorXd& cartesianState )
{
// Create cylindrical state vector, initialized with zero entries.
Eigen::VectorXd cylindricalState = Eigen::VectorXd::Zero( 6 );
// Compute and set cylindrical coordinates.
cylindricalState.head( 3 ) = convertCartesianToCylindrical(
Eigen::Vector3d( cartesianState.head( 3 ) ) );
// Compute and set cylindrical velocities.
/* If radius = 0, then Vr = sqrt(xdot^2+ydot^2) and Vtheta = 0,
else Vr = (x*xdot+y*ydot)/radius and Vtheta = (x*ydot-y*xdot)/radius.
*/
if ( cylindricalState( 0 ) <= std::numeric_limits< double >::epsilon( ) )
{
cylindricalState.tail( 3 ) <<
std::sqrt( pow( cartesianState( 3 ), 2 ) + pow( cartesianState( 4 ), 2 ) ), // Vr
0.0, // Vtheta
cartesianState( 5 ); // Vz
}
else
{
cylindricalState.tail( 3 ) <<
( cartesianState( 0 ) * cartesianState( 3 )
+ cartesianState( 1 ) * cartesianState( 4 ) ) / cylindricalState( 0 ), // Vr
( cartesianState( 0 ) * cartesianState( 4 )
- cartesianState( 1 ) * cartesianState( 3 ) ) / cylindricalState( 0 ), // Vtheta
cartesianState( 5 ); // Vz
}
return cylindricalState;
}
bool RectangleHandler::getFeaturePoseInWorldFrame(long int id,
Eigen::VectorXd& c) const {
const string &sensor = getFeatureSensor(id);
ParameterWrapper_Ptr f_par = _filter->getParameterByName(sensor + "_F"); // anchor frame
if (!f_par) {
return false;
}
const Eigen::VectorXd &fw = f_par->getEstimate();
Eigen::VectorXd dim(2);
getFeatureDimensions(id, dim);
Eigen::Quaterniond R_WO(fw(3),fw(4),fw(5),fw(6));
Eigen::Vector3d t_WO(fw(0),fw(1),fw(2));
Eigen::Vector3d t_OC(dim(0)/2.0, dim(1)/2.0, 0.0);
c.head(3) = t_WO+R_WO.toRotationMatrix()*t_OC;
c.tail(4) = fw.tail(4);
return true;
}
void ExperimentalTrajectory::removeWaypoint(int index)
{
std::cout << "Removing waypoint index: " << index << "..." << std::endl;
if ( (index>=0) && (index<nWaypoints) )
{
Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints-1);
newWaypointsMat.leftCols(index) = waypoints.leftCols(index);
newWaypointsMat.rightCols(nWaypoints-1-index) = waypoints.rightCols(nWaypoints-1-index);
waypoints.resize(nDoF, nWaypoints-1);
waypoints = newWaypointsMat;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints-2);
int durVecIndex = index;
durationVectorTmp.head(durVecIndex) = pointToPointDurationVector.head(durVecIndex);
durationVectorTmp.tail(nWaypoints -1 -durVecIndex) = pointToPointDurationVector.tail(nWaypoints -1 -durVecIndex);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
reinitialize();
}
else{
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of index bounds. Should have fall between 0 and " << nWaypoints-1 << "." << std::endl;
}
}
void rootBounds( double &lb, double &ub )
{
int deg = coef.rows()-1;
Eigen::VectorXd mycoef = coef.tail(deg).array().abs()/fabs(coef(0));
mycoef(0) += 1.;
ub = mycoef.maxCoeff();
lb = -ub;
}
void set_h_params(const Eigen::VectorXd& p)
{
_h_params = p.head(_tr.rows() * _tr.cols());
for (int c = 0; c < _tr.cols(); c++)
for (int r = 0; r < _tr.rows(); r++)
_tr(r, c) = p[r * _tr.cols() + c];
if (_mean_function.h_params_size() > 0)
_mean_function.set_h_params(p.tail(_mean_function.h_params_size()));
}
FunctionARD(size_t dim_out = 1)
: _mean_function(dim_out), _tr(dim_out, dim_out + 1)
{
Eigen::VectorXd h = Eigen::VectorXd::Zero(dim_out * (dim_out + 1) + _mean_function.h_params_size());
for (size_t i = 0; i < dim_out; i++)
h[i * (dim_out + 2)] = 1;
if (_mean_function.h_params_size() > 0)
h.tail(_mean_function.h_params_size()) = _mean_function.h_params();
this->set_h_params(h);
}
void realRoots(std::vector<double> &roots) const
{
if ( coef[0] == 0 )
{
int deg = coef.rows()-1;
Internal::RootFinder<Eigen::Dynamic>::compute(coef.tail(deg),roots);
} else {
Internal::RootFinder<Eigen::Dynamic>::compute(coef,roots);
}
}
void ExperimentalTrajectory::addNewWaypoint(Eigen::VectorXd newWaypoint, double waypointTime)
{
std::cout << "Adding waypoint at: " << waypointTime << " seconds..." << std::endl;
if ( (newWaypoint.rows() == nDoF) && (waypointTime>=0.0) && (waypointTime<=kernelCenters.maxCoeff()) )
{
//Find out where the new T falls...
for (int i=0; i<kernelCenters.size(); i++)
{
std::cout << "i = " << i << std::endl;
if (kernelCenters(i)>waypointTime) {
youngestWaypointIndex=i;
std::cout << "youngestWaypointIndex" << youngestWaypointIndex << std::endl;
i = kernelCenters.size();
}
}
Eigen::MatrixXd newWaypointsMat = Eigen::MatrixXd::Zero(nDoF, nWaypoints+1);
newWaypointsMat.leftCols(youngestWaypointIndex) = waypoints.leftCols(youngestWaypointIndex);
newWaypointsMat.col(youngestWaypointIndex) = newWaypoint;
newWaypointsMat.rightCols(nWaypoints - youngestWaypointIndex) = waypoints.rightCols(nWaypoints - youngestWaypointIndex);
waypoints.resize(nDoF, nWaypoints+1);
waypoints = newWaypointsMat;
Eigen::VectorXd durationVectorTmp = Eigen::VectorXd::Zero(nWaypoints);
std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;
durationVectorTmp.head(youngestWaypointIndex-1) = pointToPointDurationVector.head(youngestWaypointIndex-1);
durationVectorTmp.row(youngestWaypointIndex-1) << waypointTime - kernelCenters(youngestWaypointIndex-1);
durationVectorTmp.tail(nWaypoints - youngestWaypointIndex) = pointToPointDurationVector.tail(nWaypoints - youngestWaypointIndex);
pointToPointDurationVector.resize(durationVectorTmp.size());
pointToPointDurationVector = durationVectorTmp;
std::cout << "\npointToPointDurationVector\n " << pointToPointDurationVector << std::endl;
reinitialize();
}
else{
if (newWaypoint.rows() != nDoF){
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint is not the right size. Should have dim = " <<nDoF << "x1." << std::endl;
}
if ((waypointTime<=0.0) || (waypointTime>=kernelCenters.maxCoeff())){
std::cout << "[ERROR] (ExperimentalTrajectory::addNewWaypoint): New waypoint time is out of time bounds. Should have fall between 0.0 and " << kernelCenters.maxCoeff() << " seconds." << std::endl;
}
}
}
void realRootsSturm(const double lb, const double ub, std::vector<double> &roots) const
{
if ( coef[0] == 0 )
{
int deg = coef.rows()-1;
Internal::SturmRootFinder<Eigen::Dynamic> sturm( coef.tail(deg) );
sturm.realRoots( lb, ub, roots );
} else {
Internal::SturmRootFinder<Eigen::Dynamic> sturm( coef );
sturm.realRoots( lb, ub, roots );
}
}
//! Convert cylindrical to Cartesian state.
Eigen::VectorXd convertCylindricalToCartesian( const Eigen::VectorXd& cylindricalState )
{
// Create Cartesian state vector, initialized with zero entries.
Eigen::VectorXd cartesianState = Eigen::VectorXd::Zero( 6 );
// Get azimuth angle, theta.
double azimuthAngle = cylindricalState( 1 );
// Compute and set Cartesian coordinates.
cartesianState.head( 3 ) = convertCylindricalToCartesian(
Eigen::Vector3d( cylindricalState.head( 3 ) ) );
// If r = 0 AND Vtheta > 0, then give warning and assume Vtheta=0.
if ( std::fabs(cylindricalState( 0 )) <= std::numeric_limits< double >::epsilon( )
&& std::fabs(cylindricalState( 4 )) > std::numeric_limits< double >::epsilon( ) )
{
std::cerr << "Warning: cylindrical velocity Vtheta (r*thetadot) does not equal zero while "
<< "the radius (r) is zero! Vtheta is taken equal to zero!\n";
// Compute and set Cartesian velocities.
cartesianState.tail( 3 )
<< cylindricalState( 3 ) * std::cos( azimuthAngle ), // xdot
cylindricalState( 3 ) * std::sin( azimuthAngle ), // ydot
cylindricalState( 5 ); // zdot
}
else
{
// Compute and set Cartesian velocities.
cartesianState.tail( 3 )
<< cylindricalState( 3 ) * std::cos( azimuthAngle )
- cylindricalState( 4 ) * std::sin( azimuthAngle ), // xdot
cylindricalState( 3 ) * std::sin( azimuthAngle )
+ cylindricalState( 4 ) * std::cos( azimuthAngle ), // ydot
cylindricalState( 5 ); // zdot
}
return cartesianState;
}
// receive FT Sensor data
void
recvFT(const geometry_msgs::WrenchStampedConstPtr& msg){
mutex_force.lock();
// std::cout <<"Fx"<<"\t"<<"Fy"<<"\t"<<"Fz"<<"\t"<<"Tx"<<"\t"<<"Ty"<<"\t"<<"Tz"<<std::endl;
// if (counter >50){
// std::cout << msg->wrench.force.x<<","<<msg->wrench.force.y<<","<<msg->wrench.force.z<<","<<msg->wrench.torque.x<<","<<msg->wrench.torque.y<<","<<msg->wrench.torque.z<<std::endl;
// counter = 0;
// }
// else{
// counter ++;
// }
Eigen::Vector3d grav_tmp;
grav_tmp.setZero();
grav_tmp(2) = Grav_Schunk_Acc+tool_grav;
ft_gama->raw_ft_f(0) = msg->wrench.force.x;
ft_gama->raw_ft_f(1) = msg->wrench.force.y;
ft_gama->raw_ft_f(2) = msg->wrench.force.z-Grav_Schunk_Acc;
ft_gama->raw_ft_t(0) = msg->wrench.torque.x;
ft_gama->raw_ft_t(1) = msg->wrench.torque.y;
ft_gama->raw_ft_t(2) = msg->wrench.torque.z;
//get rid of tool/pokingstick gravity--calib
ft_gama->calib_ft_f = right_rs->robot_orien["eef"] * ft_gama->raw_ft_f + grav_tmp;
ft_gama->calib_ft_t = ft_gama->raw_ft_t;
Eigen::Vector3d f_offset;
f_offset.setZero();
f_offset(0) = -0.5;
f_offset(1) = -0.5;
f_offset(2) = -0.6;
//use smooth filter to get rid of noise.
ft.head(3) = ft_gama->filtered_gama_f = gama_f_filter->push(ft_gama->calib_ft_f) - f_offset;
//get rid of noise of no contacting
if(ft_gama->filtered_gama_f.norm() <1.5)
ft.head(3).setZero();
ft.tail(3) = ft_gama->filtered_gama_t = gama_t_filter->push(ft_gama->calib_ft_t);
counter_t ++;
if(counter_t%50==0){
counter_t = 0;
// std::cout<<"estimated contact force: "<<ft_gama->filtered_gama_f(0)<<","<<ft_gama->filtered_gama_f(1)<<","
// <<ft_gama->filtered_gama_f(2)<<","<<ft_gama->filtered_gama_t(0)<<","<<ft_gama->filtered_gama_t(1)<<","<<ft_gama->filtered_gama_t(2)<<std::endl;
}
mutex_force.unlock();
}
bool AnchoredRectangleHandler::getFeaturePoseInWorldFrame(long int id,
Eigen::VectorXd& c) const {
const string &sensor = getFeatureSensor(id);
ParameterWrapper_Ptr f_par = _filter->getParameterByName(sensor + "_F"); // anchor frame
ParameterWrapper_Ptr fohp_par = _filter->getParameterByName(sensor + "_FOhp"); // anchor frame
ParameterWrapper_Ptr foq_par = _filter->getParameterByName(sensor + "_FOq"); // anchor frame
if (!f_par || !fohp_par || !foq_par) {
return false;
}
const Eigen::VectorXd &fw = f_par->getEstimate();
const Eigen::VectorXd &FOhp = fohp_par->getEstimate();
const Eigen::VectorXd &FOq = foq_par->getEstimate();
Eigen::Quaterniond Fq_e(fw(3), fw(4), fw(5), fw(6)); // anchor frame orientation wrt world
// compute the position of the lower left corner of the object starting from anchor frame and homogeneous point
Eigen::Vector3d FOhp_e;
FOhp_e << FOhp(0), FOhp(1), 1.0; // construct a proper homogeneous point from the first two components of the parameter
Eigen::Vector3d Ow;
Ow = fw.head(3) + 1 / FOhp(2) * (Fq_e.toRotationMatrix() * FOhp_e);
// orientation of the object
Eigen::Quaterniond FOq_e(FOq(0), FOq(1), FOq(2), FOq(3));
Eigen::Quaterniond R_WO = Fq_e * FOq_e;
Eigen::VectorXd dim(2);
getFeatureDimensions(id, dim);
Eigen::Vector3d t_OC(dim(0) / 2.0, dim(1) / 2.0, 0.0);
c.head(3) = Ow + R_WO.toRotationMatrix() * t_OC;
c.tail(4) << R_WO.w(), R_WO.x(), R_WO.y(), R_WO.z();
return true;
}
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();
}
Eigen::MatrixXd KalmanFilter::Propagate(Eigen::VectorXd x_hat_plus, Eigen::MatrixXd P_plus, double v_m, double w_m, Eigen::MatrixXd Q, double dt)
{
int n = x_hat_plus.size();
double ori = x_hat_plus(2);
Eigen::VectorXd x_hat_min(n);
Eigen::MatrixXd P_min(n,n);
Eigen::MatrixXd Set(n,n+1); //Set of state and covariance
Eigen::MatrixXd Phi_R(3,3);
Eigen::MatrixXd G(3,2);
Eigen::MatrixXd tempTrans;
//Propagate state (Robot and Landmark, no change in Landmark)
x_hat_min.head(3) << v_m*cos(ori),
v_m*sin(ori),
w_m;
x_hat_min.head(3) = x_hat_plus.head(3) + dt*x_hat_min.head(3);
x_hat_min.tail(n-3) = x_hat_plus.tail(n-3);
// Set up Phi_R and G
Phi_R << 1, 0, -dt*v_m*sin(ori),
0, 1, dt*v_m*cos(ori),
0, 0, 1;
G << -dt*cos(ori), 0,
-dt*sin(ori), 0,
0, -dt;
// Propagate Covariance
// P_RR
P_min.block(0,0,3,3) = Phi_R * P_plus.block(0,0,3,3) * Phi_R.transpose() + G * Q * G.transpose();
// P_RL
P_min.block(0,3,3,n-3) = Phi_R * P_plus.block(0,3,3,n-3);
// P_LR
tempTrans = P_min.block(0,3,3,n-3).transpose();
P_min.block(3,0,n-3,3) = tempTrans;
// P_LL are not affected by propagation.
P_min.block(3,3,n-3,n-3) = P_plus.block(3,3,n-3, n-3);
// Make sure the matrix is symmetric, may skip this option
tempTrans = 0.5 * (P_min + P_min.transpose());
P_min = tempTrans;
// Put State vector and Covariance into one matrix and return it.
// We can parse these out in the main function.
Set.block(0,0,n,1) = x_hat_min;
Set.block(0,1,n,n) = P_min;
return Set;
}
static void
_solve_linear_system(TriMesh& src, const TriMesh& dst,
const std::vector<std::vector<int>>& tri_neighbours,
const std::vector<std::pair<int, int> >& corres,
double weights[3]
) {
int rows = 0; int cols = src.vert_num + src.poly_num - corres.size();
assert (tri_neighbours.size() == src.poly_num);
std::vector<std::pair<int, int>> soft_corres;
_setup_kd_correspondence(src, dst, soft_corres);
#if 0
std::ofstream ofs("E:/data/ScapeOriginal/build/data_ming/dt_pairs.txt");
ofs << soft_corres.size() << endl;
for (size_t i=0; i<soft_corres.size(); ++i)
ofs << soft_corres[i].first << "\t" << soft_corres[i].second << std::endl;
printf("check soft pair\n");
getchar();
#endif
for (int i=0; i<src.poly_num; ++i)
rows += 3*tri_neighbours[i].size(); //smooth part
rows += src.poly_num*3; //identity part
static std::vector<bool> vertex_flag; //indicate whether the vertex is hard constrainted by corres
if (vertex_flag.empty()) {
vertex_flag.resize(src.vert_num, false);
for (size_t i=0; i<corres.size(); ++i)
vertex_flag[corres[i].first] = true;
}
for (int i=0; i<soft_corres.size(); ++i) { //soft constraints part
if (vertex_flag[soft_corres[i].first]) continue;
++rows;
}
//vertex_real_col stores two information : unknow vertex's col(offset by
//poly_num) in X and know vertexs' corresponding index in dst mesh.
//there is no need to compute this in each iteration
static std::vector<int> vertex_real_col;
if (vertex_real_col.empty()) {
vertex_real_col.resize(src.vert_num, -1);
std::map<int, int> corres_map;
for (int i=0; i<corres.size(); ++i) corres_map.insert(std::make_pair(corres[i].first, corres[i].second));
int real_col = 0;
for (int i=0; i<src.vert_num; ++i) {
if (vertex_flag[i]) {
vertex_real_col[i] = corres_map[i];
} else {
vertex_real_col[i] = real_col;
real_col++;
}
}
assert (real_col == src.vert_num - corres.size()); //make sure indexes in corres are different from each other
}
SparseMatrix<double> A(rows, cols);
A.reserve(Eigen::VectorXi::Constant(cols, 200));
std::vector<VectorXd> Y(3, VectorXd(rows));
Y[0].setZero();Y[1].setZero();Y[2].setZero();
//precompute Q_hat^-1 in Q_hat_inverse [n v2-v1 v3-v1]^-1
src.updateNorm();
assert (!src.face_norm.empty());
std::vector<Matrix3d> Q_hat_inverse(src.poly_num);
Eigen::Matrix3d _inverse;
for (int i=0; i<src.poly_num; ++i) {
unsigned int* index_i = src.polyIndex[i].vert_index;
Vector3d v[3];
v[0] = src.face_norm[i];
v[1] = src.vertex_coord[index_i[1]] - src.vertex_coord[index_i[0]];//v2-v1
v[2] = src.vertex_coord[index_i[2]] - src.vertex_coord[index_i[0]];//v3-v1
for (int k=0; k<3; ++k)
for (int j=0; j<3; ++j)
Q_hat_inverse[i](k, j) = v[j][k];
_inverse = Q_hat_inverse[i].inverse();
Q_hat_inverse[i] = _inverse;
}
int energy_size[3] = {0, 0, 0}; //each energy part's starting index
//start establishing the large linear sparse system
double weight_smooth = weights[0];
int row = 0;
for (int i=0; i<src.poly_num; ++i) {
Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
unsigned int* index_i = src.polyIndex[i].vert_index;
for (size_t _j=0; _j<tri_neighbours[i].size(); ++_j) {
int j = tri_neighbours[i][_j]; //triangle index
Eigen::Matrix3d& Q_j_hat = Q_hat_inverse[j];
unsigned int* index_j = src.polyIndex[j].vert_index;
for (int k=0; k<3; ++k)
for (int dim = 0; dim < 3; ++dim)
Y[dim](row+k) = 0.0;
for (int k=0; k<3; ++k) {
A.coeffRef(row+k, i) = weight_smooth*Q_i_hat(0, k); //n
A.coeffRef(row+k, j) = -weight_smooth*Q_j_hat(0, k); //n
}
for (int k=0; k<3; ++k)
for (int p=0; p<3; ++p)
if (!vertex_flag[index_i[p]])
A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[p]]) = 0.0;
if (vertex_flag[index_i[0]]) {
for (int k=0; k<3; ++k) {
for (int dim = 0; dim < 3; ++dim)
Y[dim](row + k) += weight_smooth*(Q_i_hat(1, k)+Q_i_hat(2, k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
}
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[0]]) +=
-weight_smooth*(Q_i_hat(1, k)+Q_i_hat(2, k));
}
if (vertex_flag[index_j[0]]) {
for (int k=0; k<3; ++k) {
for (int dim = 0; dim < 3; ++dim)
Y[dim](row + k) += -weight_smooth*(Q_j_hat(1, k)+Q_j_hat(2, k))*dst.vertex_coord[vertex_real_col[index_j[0]]][dim];
}
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[0]]) +=
weight_smooth*(Q_j_hat(1, k)+Q_j_hat(2, k));
}
for (int p=1; p<3; ++p) {//v2 or v3
if (vertex_flag[index_i[p]]) {
for (int k=0; k<3; ++k)
for (int dim=0; dim<3; ++dim)
Y[dim](row + k) += -weight_smooth*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) += weight_smooth*Q_i_hat(p, k);
}
}
for (int p=1; p<3; ++p) {
if (vertex_flag[index_j[p]]) {
for (int k=0; k<3; ++k)
for (int dim=0; dim < 3; ++dim)
Y[dim](row + k) += weight_smooth*Q_j_hat(p, k)*dst.vertex_coord[vertex_real_col[index_j[p]]][dim];
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_j[p]]) += -weight_smooth*Q_j_hat(p, k);
}
}
row += 3;
}
}
energy_size[0] = row;
double weight_identity = weights[1];
for (int i=0; i<src.poly_num; ++i) {
Eigen::Matrix3d& Q_i_hat = Q_hat_inverse[i];
unsigned int* index_i = src.polyIndex[i].vert_index;
Y[0](row) = weight_identity; Y[0](row+1) = 0.0; Y[0](row+2) = 0.0;
Y[1](row) = 0.0; Y[1](row+1) = weight_identity; Y[1](row+2) = 0.0;
Y[2](row) = 0.0; Y[2](row+1) = 0.0; Y[2](row+2) = weight_identity;
for (int k=0; k<3; ++k)
A.coeffRef(row+k, i) = weight_identity*Q_i_hat(0, k); //n
if (vertex_flag[index_i[0]]) {
for (int k=0; k<3; ++k)
for (int dim = 0; dim < 3; ++dim)
Y[dim](row+k) += weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k))*dst.vertex_coord[vertex_real_col[index_i[0]]][dim];
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num+vertex_real_col[index_i[0]]) = -weight_identity*(Q_i_hat(1, k)+Q_i_hat(2,k));
}
for (int p=1; p<3; ++p) {
if (vertex_flag[index_i[p]]) {
for (int k=0; k<3; ++k)
for (int dim=0; dim<3; ++dim)
Y[dim](row + k) += -weight_identity*Q_i_hat(p, k)*dst.vertex_coord[vertex_real_col[index_i[p]]][dim];
} else {
for (int k=0; k<3; ++k)
A.coeffRef(row+k, src.poly_num + vertex_real_col[index_i[p]]) = weight_identity*Q_i_hat(p, k);
}
}
row += 3;
}
energy_size[1] = row;
double weight_soft_constraint = weights[2];
for (int i=0; i<soft_corres.size(); ++i) {
if (vertex_flag[soft_corres[i].first]) continue;
A.coeffRef(row, src.poly_num + vertex_real_col[soft_corres[i].first]) = weight_soft_constraint;
for (int dim=0; dim<3; ++dim)
Y[dim](row) += weight_soft_constraint*dst.vertex_coord[soft_corres[i].second][dim];
++row;
}
energy_size[2] = row;
assert (row == rows);
//start solving the least-square problem
fprintf(stdout, "finished filling matrix\n");
Eigen::SparseMatrix<double> At = A.transpose();
Eigen::SparseMatrix<double> AtA = At*A;
Eigen::SimplicialCholesky<SparseMatrix<double>> solver;
solver.compute(AtA);
if (solver.info() != Eigen::Success) {
fprintf(stdout, "unable to defactorize AtA\n");
exit(-1);
}
VectorXd X[3];
for (int i=0; i<3; ++i) {
VectorXd AtY = At*Y[i];
X[i] = solver.solve(AtY);
Eigen::VectorXd Energy = A*X[i] - Y[i];
Eigen::VectorXd smoothEnergy = Energy.head(energy_size[0]);
Eigen::VectorXd identityEnergy = Energy.segment(energy_size[0], energy_size[1]-energy_size[0]);
Eigen::VectorXd softRegularEnergy = Energy.tail(energy_size[2]-energy_size[1]);
fprintf(stdout, "\t%lf = %lf + %lf + %lf\n",
Energy.dot(Energy), smoothEnergy.dot(smoothEnergy),
identityEnergy.dot(identityEnergy), softRegularEnergy.dot(softRegularEnergy));
}
//fill data back to src
for (int i=0; i<src.poly_num; ++i)
for (int d=0; d<3; ++d)
src.face_norm[i][d] = X[d](i);
for (size_t i=0; i<corres.size(); ++i) src.vertex_coord[corres[i].first] = dst.vertex_coord[corres[i].second];
int p = 0;
for (int i=0; i<src.vert_num; ++i) {
if (vertex_flag[i]) continue;
for (int d=0; d<3; ++d)
src.vertex_coord[i][d] = X[d](src.poly_num+p);
++p;
}
return;
}
//==============================================================================
void MyWindow::timeStepping()
{
processMocapData();
bool isFirst6DofsValid = false;// fromMarkersTo6Dofs();
Eigen::VectorXd motor_qhat = mController->motion()->targetPose(mTime);
Eigen::VectorXd fullMotorAngle = Eigen::VectorXd::Zero(NUM_MOTORS + 2);
if (!mSimulating)
{
motor_qhat = mController->motion()->getInitialPose();
}
// Wait for an event
Eigen::VectorXd motorAngle;
motorAngle = Eigen::VectorXd::Zero(NUM_MOTORS);
//if (mTime < 3)
// motor_qhat = mController->useAnkelStrategy(motor_qhat, mTime);
//Eigen::VectorXd mocapPose = mController->mocap()->GetPose(mTime - 1.0);
//Eigen::VectorXd motor_qhat = mController->motormap()->toMotorMapVectorSameDim(mocapPose);
Eigen::VectorXd motor_qhat_noGriper = Eigen::VectorXd::Zero(NUM_MOTORS);
motor_qhat_noGriper.head(6) = motor_qhat.head(6);
motor_qhat_noGriper.tail(10) = motor_qhat.tail(10);
unsigned char commands[256];
for (int i = 0; i < NUM_MOTORS; ++i)
{
int command = static_cast<int>(motor_qhat_noGriper[i]);
unsigned char highByte, lowByte;
SeparateWord(command, &highByte, &lowByte);
commands[2 * i] = lowByte;
commands[2 * i + 1] = highByte;
//commands[3 * i + 2] = ' ';
}
commands[2 * 16] = '\t';
commands[2 * 16 + 1] = '\n';
mSerial->Write(commands, 2 * 16 + 2);
DWORD dwBytesRead = 0;
char szBuffer[101];
bool bUpdated = false;
do
{
// Read data from the COM-port
LONG lLastError = mSerial->Read(szBuffer, sizeof(szBuffer) - 1, &dwBytesRead);
if (lLastError != ERROR_SUCCESS)
LOG(FATAL) << mSerial->GetLastError() << " Unable to read from COM-port.";
if (dwBytesRead > 0)
{
mTmpBuffer.insert(mTmpBuffer.size(), szBuffer, dwBytesRead);
if (mTmpBuffer.size() >= NUM_BYTES_PER_MOTOR * NUM_MOTORS + 1)
{
bUpdated = ProcessBuffer(mTmpBuffer, motorAngle);
if (bUpdated)
{
fullMotorAngle.head(6) = motorAngle.head(6);
fullMotorAngle.tail(10) = motorAngle.tail(10);
mController->setMotorMapPose(fullMotorAngle);
}
//for (int i = 0; i < 3; ++i)
//{
// std::cout << motorAngle[i] << " "; //<< motor_qhat_noGriper[14];
//}
//std::cout << std::endl;
}
}
else
{
std::cout << "Noting received." << endl;
}
} while (dwBytesRead == sizeof(szBuffer) - 1);
if (isFirst6DofsValid)
mController->setFreeDofs(mFirst6DofsFromMocap);
if (mSimulating)
{
Eigen::VectorXd poseToRecord = mController->robot()->getPositions();
MocapFrame frame;
frame.mTime = mTime;
frame.mMarkerPos = mMarkerPos;
frame.mMarkerOccluded = mMarkerOccluded;
frame.mMotorAngle = poseToRecord;
mRecordedFrames.push_back(frame);
}
//mController->keepFeetLevel();
if (mTimer.isStarted())
{
double elaspedTime = mTimer.getElapsedTime();
//LOG(INFO) << elaspedTime;
LOG(INFO) << mTime << " " << motor_qhat[10] << " " << fullMotorAngle[10] << endl;
mTime += elaspedTime;// mController->robot()->getTimeStep();
}
mTimer.start();
}
