//! 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;
}
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;
}
}
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;
}
/**
* Returns the expected return of the given portfolio structure.
*
* @param portfolio the portfolio structure
*
* @return the expected portfolio return
*/
double EfficientPortfolioWithRisklessAsset::portfolioReturn(const Eigen::VectorXd& portfolio)
{
Eigen::VectorXd riskyAssets = portfolio.head(dim - 1);
double riskReturn = riskyAssets.dot(mean);
double risklessReturn = portfolio(dim - 1) * _risklessRate;
return riskyAssets.dot(mean) + portfolio(dim - 1) * _risklessRate;
}
// 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();
}
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()));
}
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;
}
}
}
//! 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;
}
void ODELCPSolver::transferSolFromODEFormulation(const Eigen::VectorXd& _x,
Eigen::VectorXd* _xOut,
int _numDir,
int _numContacts) {
int numOtherConstrs = _x.size() - _numContacts * 3;
*_xOut = Eigen::VectorXd::Zero(_numContacts * (2 + _numDir)
+ numOtherConstrs);
_xOut->head(_numContacts) = _x.head(_numContacts);
int offset = _numDir / 4;
for (int i = 0; i < _numContacts; ++i) {
(*_xOut)[_numContacts + i * _numDir + 0] = _x[_numContacts + i * 2 + 0];
(*_xOut)[_numContacts + i * _numDir + offset] = _x[_numContacts + i * 2
+ 1];
}
for (int i = 0; i < numOtherConstrs; i++)
(*_xOut)[_numContacts * (2 + _numDir) + i] = _x[_numContacts * 3 + i];
}
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 IRLTest::generateData()
{
int num_active_features;
int num_data;
int num_samples_per_data;
double cost_noise_stddev;
num_features_ = 10;
num_active_features = 2;
num_data = 10;
num_samples_per_data = 10;
cost_noise_stddev = 0.1;
// generate random weights
real_weights_ = Eigen::VectorXd::Zero(num_features_);
real_weights_.head(num_active_features).setRandom();
data_.resize(num_data);
for (int i=0; i<num_data; ++i)
{
IRLData* d = new IRLData(num_features_);
Eigen::MatrixXd features = Eigen::MatrixXd::Zero(num_samples_per_data, num_features_);
Eigen::VectorXd target = Eigen::VectorXd::Zero(num_samples_per_data);
features.setRandom();
// compute w^t * phi
Eigen::VectorXd costs = features*real_weights_;
// add random noise to costs
costs += cost_noise_stddev * Eigen::VectorXd::Random(num_samples_per_data);
// set min cost target to 1
int min_index;
double min_cost = costs.minCoeff(&min_index);
target(min_index) = 1.0;
d->addSamples(features, target);
data_[i].reset(d);
}
real_weights_exist_ = 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();
}
//==============================================================================
MyWindow::MyWindow(bioloidgp::robot::HumanoidController* _controller)
: SimWindow(),
mController(_controller),
mController2(NULL),
mTime(0)
{
mDisplayTimeout = 10;
DecoConfig::GetSingleton()->GetDouble("Server", "timeout", mDisplayTimeout);
// 0.166622 0.548365 0.118241 0.810896
//Eigen::Quaterniond q(0.15743952233047084, 0.53507160411429699, 0.10749289301287825, 0.82301687360383402);
// mTrackBall.setQuaternion(q);
//mTrans = Eigen::Vector3d(-32.242, 212.85, 21.7107);
mTrans = Eigen::Vector3d(0, -212.85, 0);
mFrameCount = 0;
string fileName;
mOffsetTransform = Eigen::Isometry3d::Identity();
DecoConfig::GetSingleton()->GetString("Player", "FileName", fileName);
readMovieFile(fileName);
mController->robot()->setPositions(mMovie[0].mPose);
int isRobot2 = 0;
DecoConfig::GetSingleton()->GetInt("Player", "IsRobot2", isRobot2);
if (isRobot2)
{
DecoConfig::GetSingleton()->GetString("Player", "FileName2", fileName);
readMovieFile2(fileName);
Eigen::VectorXd rootPos = mMovie[0].mPose.head(6);
Eigen::VectorXd rootPos2 = mMovie2[0].mPose.head(6);
Eigen::Matrix3d rootR = dart::math::expMapRot(rootPos.head(3));
Eigen::Matrix3d rootR2 = dart::math::expMapRot(rootPos2.head(3));
mOffsetTransform.linear() = rootR * rootR2.inverse();
}
glutTimerFunc(mDisplayTimeout, refreshTimer, 0);
}
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 run_rightarm(){
//only call for this function, the ->jnt_position_act is updated
if((com_okc_right->data_available == true)&&(com_okc_right->controller_update == false)){
// kuka_right_arm->update_robot_stiffness();
kuka_right_arm->get_joint_position_act();
kuka_right_arm->update_robot_state();
right_rs->updated(kuka_right_arm);
if(toolposition_record == true)
ToolOriginposition<<right_rs->robot_position["eef"](0)<<","<<right_rs->robot_position["eef"](1)<<","<<right_rs->robot_position["eef"](2)<<std::endl;
//using all kinds of controllers to update the reference
for(unsigned int i = 0; i < right_ac_vec.size();i++){
if(right_task_vec[i]->mt == JOINTS)
right_ac_vec[i]->update_robot_reference(kuka_right_arm,right_task_vec[i],(right_rs->robot_orien["eef"] * local_hinged_axis_vec).normalized(),ft.head(3),right_rs);
if(right_task_vec[i]->mt == FORCE){
mutex_force.lock();
right_ac_vec[i]->update_robot_reference(kuka_right_arm,right_task_vec[i],ft,right_rs);
mutex_force.unlock();
}
}
//update with act_vec
right_ac_vec[0]->llv.setZero();
right_ac_vec[0]->lov.setZero();
//use CBF to compute the desired joint angle rate
kuka_right_arm->update_cbf_controller();
kuka_right_arm->set_joint_command(right_rmt);
com_okc_right->controller_update = true;
com_okc_right->data_available = false;
}
}
void record_training_cb(boost::shared_ptr<std::string> data){
Eigen::Vector3d local_f = InitOrien.transpose() * ft.head(3);
Eigen::Vector3d local_handle_dir = InitOrien.transpose() * right_rs->robot_orien["eef"].col(1);
Tpose<< local_f(0)<<","<< local_f(1)<<","<< local_f(2)<<","<<local_handle_dir(0)<<","<<local_handle_dir(1)<<","<<local_handle_dir(2)<<std::endl;
std::cout<<"recording training data"<<std::endl;
}
//#define IGL_LINPROG_VERBOSE
IGL_INLINE bool igl::linprog(
const Eigen::VectorXd & c,
const Eigen::MatrixXd & _A,
const Eigen::VectorXd & b,
const int k,
Eigen::VectorXd & x)
{
// This is a very literal translation of
// http://www.mathworks.com/matlabcentral/fileexchange/2166-introduction-to-linear-algebra/content/strang/linprog.m
using namespace Eigen;
using namespace std;
bool success = true;
// number of constraints
const int m = _A.rows();
// number of original variables
const int n = _A.cols();
// number of iterations
int it = 0;
// maximum number of iterations
//const int MAXIT = 10*m;
const int MAXIT = 100*m;
// residual tolerance
const double tol = 1e-10;
const auto & sign = [](const Eigen::VectorXd & B) -> Eigen::VectorXd
{
Eigen::VectorXd Bsign(B.size());
for(int i = 0;i<B.size();i++)
{
Bsign(i) = B(i)>0?1:(B(i)<0?-1:0);
}
return Bsign;
};
// initial (inverse) basis matrix
VectorXd Dv = sign(sign(b).array()+0.5);
Dv.head(k).setConstant(1.);
MatrixXd D = Dv.asDiagonal();
// Incorporate slack variables
MatrixXd A(_A.rows(),_A.cols()+D.cols());
A<<_A,D;
// Initial basis
VectorXi B = igl::colon<int>(n,n+m-1);
// non-basis, may turn out that vector<> would be better here
VectorXi N = igl::colon<int>(0,n-1);
int j;
double bmin = b.minCoeff(&j);
int phase;
VectorXd xb;
VectorXd s;
VectorXi J;
if(k>0 && bmin<0)
{
phase = 1;
xb = VectorXd::Ones(m);
// super cost
s.resize(n+m+1);
s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k+1);
N.resize(n+1);
N<<igl::colon<int>(0,n-1),B(j);
J.resize(B.size()-1);
// [0 1 2 3 4]
// ^
// [0 1]
// [3 4]
J.head(j) = B.head(j);
J.tail(B.size()-j-1) = B.tail(B.size()-j-1);
B(j) = n+m;
MatrixXd AJ;
igl::slice(A,J,2,AJ);
const VectorXd a = b - AJ.rowwise().sum();
{
MatrixXd old_A = A;
A.resize(A.rows(),A.cols()+a.cols());
A<<old_A,a;
}
D.col(j) = -a/a(j);
D(j,j) = 1./a(j);
}else if(k==m)
{
phase = 2;
xb = b;
s.resize(c.size()+m);
// cost function
s<<c,VectorXd::Zero(m);
}else //k = 0 or bmin >=0
{
phase = 1;
xb = b.array().abs();
s.resize(n+m);
// super cost
s<<VectorXd::Zero(n+k),VectorXd::Ones(m-k);
}
while(phase<3)
{
double df = -1;
int t = std::numeric_limits<int>::max();
// Lagrange mutipliers fro Ax=b
VectorXd yb = D.transpose() * igl::slice(s,B);
while(true)
{
if(MAXIT>0 && it>=MAXIT)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! maximum iterations without convergence."<<endl;
#endif
success = false;
break;
}
// no freedom for minimization
if(N.size() == 0)
{
break;
}
// reduced costs
VectorXd sN = igl::slice(s,N);
MatrixXd AN = igl::slice(A,N,2);
VectorXd r = sN - AN.transpose() * yb;
int q;
// determine new basic variable
double rmin = r.minCoeff(&q);
// optimal! infinity norm
if(rmin>=-tol*(sN.array().abs().maxCoeff()+1))
{
break;
}
// increment iteration count
it++;
// apply Bland's rule to avoid cycling
if(df>=0)
{
if(MAXIT == -1)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! degenerate vertex"<<endl;
#endif
success = false;
}
igl::find((r.array()<0).eval(),J);
double Nq = igl::slice(N,J).minCoeff();
// again seems like q is assumed to be a scalar though matlab code
// could produce a vector for multiple matches
(N.array()==Nq).cast<int>().maxCoeff(&q);
}
VectorXd d = D*A.col(N(q));
VectorXi I;
igl::find((d.array()>tol).eval(),I);
if(I.size() == 0)
{
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning! solution is unbounded"<<endl;
#endif
// This seems dubious:
it=-it;
success = false;
break;
}
VectorXd xbd = igl::slice(xb,I).array()/igl::slice(d,I).array();
// new use of r
int p;
{
double r;
r = xbd.minCoeff(&p);
p = I(p);
// apply Bland's rule to avoid cycling
if(df>=0)
{
igl::find((xbd.array()==r).eval(),J);
double Bp = igl::slice(B,igl::slice(I,J)).minCoeff();
// idiotic way of finding index in B of Bp
// code down the line seems to assume p is a scalar though the matlab
// code could find a vector of matches)
(B.array()==Bp).cast<int>().maxCoeff(&p);
}
// update x
xb -= r*d;
xb(p) = r;
// change in f
df = r*rmin;
}
// row vector
RowVectorXd v = D.row(p)/d(p);
yb += v.transpose() * (s(N(q)) - d.transpose()*igl::slice(s,B));
d(p)-=1;
// update inverse basis matrix
D = D - d*v;
t = B(p);
B(p) = N(q);
if(t>(n+k-1))
{
// remove qth entry from N
VectorXi old_N = N;
N.resize(N.size()-1);
N.head(q) = old_N.head(q);
N.head(q) = old_N.head(q);
N.tail(old_N.size()-q-1) = old_N.tail(old_N.size()-q-1);
}else
{
N(q) = t;
}
}
// iterative refinement
xb = (xb+D*(b-igl::slice(A,B,2)*xb)).eval();
// must be due to rounding
VectorXi I;
igl::find((xb.array()<0).eval(),I);
if(I.size()>0)
{
// so correct
VectorXd Z = VectorXd::Zero(I.size(),1);
igl::slice_into(Z,I,xb);
}
// B, xb,n,m,res=A(:,B)*xb-b
if(phase == 2 || it<0)
{
break;
}
if(xb.transpose()*igl::slice(s,B) > tol)
{
it = -it;
#ifdef IGL_LINPROG_VERBOSE
cerr<<"linprog: warning, no feasible solution"<<endl;
#endif
success = false;
break;
}
// re-initialize for Phase 2
phase = phase+1;
s*=1e6*c.array().abs().maxCoeff();
s.head(n) = c;
}
x.resize(std::max(B.maxCoeff()+1,n));
igl::slice_into(xb,B,x);
x = x.head(n).eval();
return success;
}
/**
* Returns the variance of the given portfolio structure.
*
* @param portfolio the portfolio structure
*
* @return the portfolio variance
*/
double EfficientPortfolioWithRisklessAsset::portfolioVariance(const Eigen::VectorXd& portfolio)
{
Eigen::VectorXd riskyAssets = portfolio.head(dim - 1);
return riskyAssets.dot(variance * riskyAssets);
}
void NuTo::StructureBase::ConstraintLinearEquationNodeToElementCreate(int rNode, int rElementGroup, NuTo::Node::eDof,
const double rTolerance,
Eigen::Vector3d rNodeCoordOffset)
{
const int dim = GetDimension();
Eigen::VectorXd queryNodeCoords = NodeGetNodePtr(rNode)->Get(Node::eDof::COORDINATES);
queryNodeCoords = queryNodeCoords + rNodeCoordOffset.head(dim);
std::vector<int> elementGroupIds = GroupGetMemberIds(rElementGroup);
ElementBase* elementPtr = nullptr;
Eigen::VectorXd elementNaturalNodeCoords;
bool nodeInElement = false;
for (auto const& eleId : elementGroupIds)
{
elementPtr = ElementGetElementPtr(eleId);
// Coordinate interpolation must be linear so the shape function derivatives are constant!
assert(elementPtr->GetInterpolationType().Get(Node::eDof::COORDINATES).GetTypeOrder() ==
Interpolation::eTypeOrder::EQUIDISTANT1);
const Eigen::MatrixXd& derivativeShapeFunctionsGeometryNatural =
elementPtr->GetInterpolationType()
.Get(Node::eDof::COORDINATES)
.DerivativeShapeFunctionsNatural(
Eigen::VectorXd::Zero(dim)); // just as _some_ point, as said, constant
// real coordinates of every node in rElement
Eigen::VectorXd elementNodeCoords = elementPtr->ExtractNodeValues(NuTo::Node::eDof::COORDINATES);
switch (mDimension)
{
case 2:
{
Eigen::Matrix2d invJacobian =
dynamic_cast<ContinuumElement<2>*>(elementPtr)
->CalculateJacobian(derivativeShapeFunctionsGeometryNatural, elementNodeCoords)
.inverse();
elementNaturalNodeCoords = invJacobian * (queryNodeCoords - elementNodeCoords.head(2));
}
break;
case 3:
{
Eigen::Matrix3d invJacobian =
dynamic_cast<ContinuumElement<3>*>(elementPtr)
->CalculateJacobian(derivativeShapeFunctionsGeometryNatural, elementNodeCoords)
.inverse();
elementNaturalNodeCoords = invJacobian * (queryNodeCoords - elementNodeCoords.head(3));
}
break;
default:
throw NuTo::Exception(std::string(__PRETTY_FUNCTION__) + ": \t Only implemented for 2D and 3D");
}
if ((elementNaturalNodeCoords.array() > -rTolerance).all() and
elementNaturalNodeCoords.sum() <= 1. + rTolerance)
{
nodeInElement = true;
break;
}
}
if (not nodeInElement)
{
GetLogger() << "Natural node coordinates: \n" << elementNaturalNodeCoords << "\n";
throw Exception(__PRETTY_FUNCTION__, "Node is not inside any element.");
}
auto shapeFunctions =
elementPtr->GetInterpolationType().Get(Node::eDof::DISPLACEMENTS).ShapeFunctions(elementNaturalNodeCoords);
std::vector<Constraint::Equation> equations(dim); // default construction of Equation with rhs = Constant = 0
for (int iDim = 0; iDim < dim; ++iDim)
{
equations[iDim].AddTerm(Constraint::Term(*NodeGetNodePtr(rNode), iDim, 1.));
}
for (int iNode = 0; iNode < shapeFunctions.rows(); ++iNode)
{
int localNodeId = elementPtr->GetInterpolationType().Get(Node::eDof::DISPLACEMENTS).GetNodeIndex(iNode);
auto globalNode = elementPtr->GetNode(localNodeId, Node::eDof::DISPLACEMENTS);
// std::cout << "globalNodeId \t" << globalNodeId << std::endl;
double coefficient = -shapeFunctions(iNode, 0);
for (int iDim = 0; iDim < dim; ++iDim)
equations[iDim].AddTerm(Constraint::Term(*globalNode, iDim, coefficient));
}
Constraints().Add(Node::eDof::DISPLACEMENTS, equations);
}
void ForceServoController::get_desired_lv(Robot *robot, Task *t, Eigen::VectorXd ft, RobotState* rs){
ForceServoTask tst(t->curtaskname.forcet);
tst = *(ForceServoTask*)t;
Eigen::Vector3d v_ratio;
v_ratio.setZero();
//get the desired contact force
double desiredf;
tst.get_desired_cf_kuka(desiredf);
bool rot_by_force;
rot_by_force = true;
if(rot_by_force == true)
{
if ((ft(0) < 1e-05)&&(ft(1) < 1e-05)){
v_ratio.setZero();
// std::cout<<"no control before no contact force."<<std::endl;
}
else{
//desired rotation axis is not changed. so the online estimated global direction of rotation axis is:
//v_g_online = T_l2g_online * T_g2l_init * v_g
Eigen::Vector3d rot_z = rs->robot_orien["eef"] * m_init_tm.transpose() * tst.init_contact_frame.col(2);
Eigen::Matrix3d contact_frame;
contact_frame.setZero();
contact_frame = rs->robot_orien["eef"] * m_init_tm.transpose() * tst.init_contact_frame;
std::cout<<"contact frame "<<contact_frame<<std::endl;
Eigen::Vector3d ft_local,ft_global_nofriction;
ft_local.setZero();
ft_global_nofriction.setZero();
std::cout<<"ft head3 "<<ft(0)<<","<<ft(1)<<","<<ft(2)<<","<<std::endl;
ft_local = contact_frame.transpose() * ft.head(3);
std::cout<<"ft_local before "<<ft_local(0)<<","<<ft_local(1)<<","<<ft_local(2)<<","<<std::endl;
ft_local(1) = 0;
ft_local(2) = 0;
std::cout<<"ft_local after "<<ft_local(0)<<","<<ft_local(1)<<","<<ft_local(2)<<","<<std::endl;
ft_global_nofriction =contact_frame * ft_local;
std::cout<<"ft_global_nofriction "<<ft_global_nofriction(0)<<","<<ft_global_nofriction(1)<<","<<ft_global_nofriction(2)<<","<<std::endl;
Eigen::Vector3d norm_ft = ft_global_nofriction.normalized();
Eigen::Vector3d rot_axis = norm_ft.cross(rot_z);
double angle = M_PI/2.0 - acos(norm_ft.dot(rot_z));
vel_rec2<<angle<<","<<norm_ft(0)<<","<<norm_ft(1)<<","<<norm_ft(2)<<","<<rot_z(0)<<","<<rot_z(1)<<","<<rot_z(2)<<std::endl;
std::cout<<"angle is"<<angle<<std::endl;
v_ratio = 1 * rs->robot_orien["eef"].transpose() * rot_axis * angle;
// std::cout<<"v_ratio is "<<v_ratio(0)<<","<<v_ratio(1)<<","<<v_ratio(2)<<std::endl;
}
}
lov_f = v_ratio;
if(tst.mft == GLOBAL){
if(ft.head(3).norm() != 0)
{
tst.desired_surf_nv = 0.2 * old_ft_dir + 0.8 * ft.head(3).normalized();
}
else
{
tst.desired_surf_nv = tst.desired_init_surf_nv;
}
old_ft_dir = tst.desired_surf_nv;
Eigen::VectorXd delta_g;
delta_g.setZero(6);
delta_g.head(3) = (-1) * (desiredf * tst.desired_surf_nv - ft.head(3));
delta_g(3) = 0;
delta_g(4) = 0;
delta_g(5) = 0;
//vel from global to robot eef
//std::cout<<"global vel from force "<<delta_g(0)<<","<<delta_g(1)<<","<<delta_g(2)<<std::endl;
deltais.head(3) = rs->robot_orien["eef"].transpose() * delta_g.head(3);
deltais(3) = 0;
deltais(4) = 0;
deltais(5) = 0;
}
else{
deltais(0) = 1;
deltais(1) = 1;
deltais(2) = desiredf - ft(2);
//!this two value can be updated by other feedback in future
deltais(3) = 0;
deltais(4) = 0;
deltais(5) = 0;
}
//std::cout<<"current force "<<ft(0)<<","<<ft(1)<<","<<ft(2)<<","<<std::endl;
// std::cout<<"desiredis "<<deltais(0)<<","<<deltais(1)<<","<<deltais(2)<<","<<std::endl;
// std::cout<<"current task name "<<tst.curtaskname.forcet<<std::endl;
// std::cout<<"kop: "<<std::endl;
// std::cout<<Kop[tst.curtaskname.forcet]<<std::endl;
// std::cout<<"kpp: "<<std::endl;
// std::cout<<Kpp[tst.curtaskname.forcet]<<std::endl;
// std::cout<<"tjkm: "<<std::endl;
// std::cout<<tjkm[tst.curtaskname.forcet]<<std::endl;
// std::cout<<"sm: "<<std::endl;
// std::cout<<sm[tst.curtaskname.forcet]<<std::endl;
deltape = Kpp[tst.curtaskname.forcet] * sm[tst.curtaskname.forcet] * deltais + \
Kpi[tst.curtaskname.forcet] * sm[tst.curtaskname.forcet] * deltais_int + \
Kpd[tst.curtaskname.forcet] * sm[tst.curtaskname.forcet] * (deltais - deltais_old);
llv_f = deltape.head(3);
// std::cout<<"local vel from force "<<llv_f(0)<<","<<llv_f(1)<<","<<llv_f(2)<<std::endl;
//llv_f.setZero();
//lov_f = Kop[tst.curtaskname.forcet] * v_ratio;
//~ std::cout<<lov_f(0)<<","<<lov_f(1)<<","<<lov_f(2)<<std::endl;
//~ vel_rec2<<est_v_g(0)<<","<<est_v_g(1)<<","<<est_v_g(2)<<","\
//~ <<filtered_lv[0]<<","<<filtered_lv[1]<<","<<filtered_lv[2]<<","\
//~ <<v_ratio(0)<<","<<v_ratio(1)<<","<<v_ratio(2)<<","\
//~ <<llv_f(0)<<","<<llv_f(1)<<","<<llv_f(2)<<","\
//~ <<lov_f(0)<<","<<lov_f(1)<<","<<lov_f(2)<<std::endl;
//limit_vel(get_llv_limit(),llv_f,lov_f);
deltais_old = deltais;
}
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;
}
//==============================================================================
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();
}
