VectorXd MyWindow::evalDeriv() {
for (unsigned int i = 0; i < mSkels.size(); i++) {
if (mSkels[i]->getImmobileState()) {
mSkels[i]->setPose(mDofs[i], true, false);
} else {
mSkels[i]->setPose(mDofs[i], false, true);
mSkels[i]->computeDynamics(mGravity, mDofVels[i], true);
}
}
mConstraintHandle->computeConstraintForces();
mController->setConstrForces(mConstraintHandle->getTotalConstraintForce(1));
VectorXd deriv = VectorXd::Zero(mIndices.back() * 2);
for (unsigned int i = 0; i < mSkels.size(); i++) {
if (mSkels[i]->getImmobileState())
continue;
int start = mIndices[i] * 2;
int size = mDofs[i].size();
VectorXd qddot = mSkels[i]->getInvMassMatrix() * (-mSkels[i]->getCombinedVector() + mSkels[i]->getExternalForces() + mConstraintHandle->getTotalConstraintForce(i) + mSkels[i]->getInternalForces());
mSkels[i]->clampRotation(mDofs[i], mDofVels[i]);
deriv.segment(start, size) = mDofVels[i] + (qddot * mTimeStep); // semi-implicit
deriv.segment(start + size, size) = qddot;
}
return deriv;
}
VectorXd MyWindow::evalDeriv() {
// compute dynamic equations
for (unsigned int i = 0; i < mSkels.size(); i++) {
if (mSkels[i]->getImmobileState()) {
// need to update node transformation for collision
mSkels[i]->setPose(mDofs[i], true, false);
} else {
// need to update first derivatives for collision
mSkels[i]->setPose(mDofs[i], false, true);
mSkels[i]->computeDynamics(mGravity, mDofVels[i], true);
}
}
// compute contact forces
mCollisionHandle->applyContactForces();
// compute derivatives for integration
VectorXd deriv = VectorXd::Zero(mIndices.back() * 2);
for (unsigned int i = 0; i < mSkels.size(); i++) {
// skip immobile objects in forward simulation
if (mSkels[i]->getImmobileState())
continue;
int start = mIndices[i] * 2;
int size = mDofs[i].size();
VectorXd qddot = mSkels[i]->getInvMassMatrix() * (-mSkels[i]->getCombinedVector() + mSkels[i]->getExternalForces() + mCollisionHandle->getConstraintForce(i));
mSkels[i]->clampRotation(mDofs[i], mDofVels[i]);
deriv.segment(start, size) = mDofVels[i] + (qddot * mTimeStep); // set velocities
deriv.segment(start + size, size) = qddot; // set qddot (accelerations)
}
return deriv;
}
MatrixXd
PatchFit::parab (Matrix<double, Dynamic, 3> q, VectorXd a)
{
Vector3d k3, rv;
k3 << a(0), a(1), 0.0;
rv << a.segment(2,3);
MatrixXd rr;
rr = p_->rexp(rv);
Vector3d c;
if (ccon_)
c = plane_c+a(5)*plane_n;
else
c = a.segment(5,3);
MatrixXd qc(q.rows(),q.cols());
for (int i=0; i<q.rows(); i++)
qc.row(i) = q.row(i) - c.transpose();
MatrixXd ql(q.rows(),3);
ql = qc*rr;
MatrixXd ql_sq(q.rows(),3);
ql_sq = ql.array().square();
return ((ql_sq * k3) - (2.0 * ql * zh));
//TBD: enmo
}
Trajectory Trajectory::generatePolynomialTrajectoryThroughViapoint(const VectorXd& ts, const VectorXd& y_from, const VectorXd& y_yd_ydd_viapoint, double viapoint_time, const VectorXd& y_to)
{
int n_dims = y_from.size();
assert(n_dims==y_to.size());
assert(3*n_dims==y_yd_ydd_viapoint.size()); // Contains y, yd and ydd, so *3
int n_time_steps = ts.size();
int viapoint_time_step = 0;
while (viapoint_time_step<n_time_steps && ts[viapoint_time_step]<viapoint_time)
viapoint_time_step++;
if (viapoint_time_step>=n_time_steps)
{
cerr << __FILE__ << ":" << __LINE__ << ":";
cerr << "ERROR: the time vector does not contain any time smaller than " << viapoint_time << ". Returning min-jerk trajectory WITHOUT viapoint." << endl;
return Trajectory();
}
VectorXd yd_from = VectorXd::Zero(n_dims);
VectorXd ydd_from = VectorXd::Zero(n_dims);
VectorXd y_viapoint = y_yd_ydd_viapoint.segment(0*n_dims,n_dims);
VectorXd yd_viapoint = y_yd_ydd_viapoint.segment(1*n_dims,n_dims);
VectorXd ydd_viapoint = y_yd_ydd_viapoint.segment(2*n_dims,n_dims);
VectorXd yd_to = VectorXd::Zero(n_dims);
VectorXd ydd_to = VectorXd::Zero(n_dims);
Trajectory traj = Trajectory::generatePolynomialTrajectory(ts.segment(0, viapoint_time_step + 1), y_from, yd_from, ydd_from, y_viapoint, yd_viapoint, ydd_viapoint);
traj.append(Trajectory::generatePolynomialTrajectory(ts.segment(viapoint_time_step, n_time_steps - viapoint_time_step), y_viapoint, yd_viapoint, ydd_viapoint, y_to, yd_to, ydd_to));
return traj;
}
void MyWindow::setState(const VectorXd &newState) {
for (unsigned int i = 0; i < mSkels.size(); i++) {
int start = mIndices[i] * 2;
int size = mDofs[i].size();
mDofs[i] = newState.segment(start, size);
mDofVels[i] = newState.segment(start + size, size);
}
}
VectorXd MotionModel::Qprod(VectorXd Q, VectorXd P)
{
double a, x;
VectorXd QP(4), QP2;
Vector3d u, v;
a = Q(0); x = P(0);
v = Q.segment(1,3);
u = P.segment(1,3);
QP2 = a * u + x * v + v.cross(u); // cross of v and u
QP << a * x - v.transpose() * u, QP2;
return QP; // QP should be a 4x1 vector
}
VectorXd vech(MatrixXd& M){
int n = M.rows();
VectorXd V;
V.setZero(n*(n-1)/2+n);
V.segment(0,n) = M.diagonal();
int k = n;
for(int i=0;i<n-1;i++){
V.segment(k,n-i-1) = M.block(i,i+1,1,n-i-1).transpose();
k += n-i-1;
}
return V;
}
double harmonic_potential(VectorXd x, MatrixXd W) {
const int N = x.size()/3;
double hpot = 0.5 * double(x.transpose() * W * x);
double coul =0;
for (int n = 0; n != N; n++) {
for (int m =0; m!=n; m++) {
// Recall: C[n]*x = x[3n:3(n+1)]
coul += 1/((x.segment(3*n,3) - x.segment(3*m,3)).norm());
}
}
return hpot + coul;
}
void pubOdometry()
{
nav_msgs::Odometry odom;
{
odom.header.stamp = _last_imu_stamp;
odom.header.frame_id = "map";
odom.pose.pose.position.x = x(0);
odom.pose.pose.position.y = x(1);
odom.pose.pose.position.z = x(2);
Quaterniond q;
q = RPYtoR(x(3), x(4), x(5)).block<3,3>(0, 0);
odom.pose.pose.orientation.x = q.x();
odom.pose.pose.orientation.y = q.y();
odom.pose.pose.orientation.z = q.z();
odom.pose.pose.orientation.w = q.w();
}
//ROS_WARN("[update] publication done");
ROS_WARN_STREAM("[final] b_g = " << x.segment(_B_G_BEG, _B_G_LEN).transpose());
ROS_WARN_STREAM("[final] b_a = " << x.segment(_B_A_BEG, _B_A_LEN).transpose() << endl);
///ROS_WARN_STREAM("[final] cov_x = " << endl << cov_x << endl);
odom_pub.publish(odom);
}
void MSCKF::addSlideState()
{
SlideState newState;
int nominalStateLength = (int)fullNominalState.size();
int errorStateLength = (int)fullErrorCovariance.rows();
VectorXd tmpNominal = VectorXd::Zero(nominalStateLength + NOMINAL_POSE_STATE_SIZE);
MatrixXd tmpCovariance = MatrixXd::Zero(errorStateLength + ERROR_POSE_STATE_SIZE, errorStateLength + ERROR_POSE_STATE_SIZE);
MatrixXd Jpi = MatrixXd::Zero(9, errorStateLength);
tmpNominal.head(nominalStateLength) = fullNominalState;
tmpNominal.segment(nominalStateLength, NOMINAL_POSE_STATE_SIZE) = fullNominalState.head(NOMINAL_POSE_STATE_SIZE);
fullNominalState = tmpNominal;
newState.q = fullNominalState.head(4);
newState.p = fullNominalState.segment(4, 3);
newState.v = fullNominalState.segment(7, 3);
slidingWindow.push_back(newState);
Jpi.block<3,3>(0, 0) = Matrix3d::Identity(3, 3);
Jpi.block<3,3>(3, 3) = Matrix3d::Identity(3, 3);
Jpi.block<3,3>(6, 6) = Matrix3d::Identity(3, 3);
tmpCovariance.block(0, 0, errorStateLength, errorStateLength) = fullErrorCovariance;
tmpCovariance.block(errorStateLength, 0, 9, errorStateLength) = Jpi * fullErrorCovariance;
tmpCovariance.block(0, errorStateLength, errorStateLength, 9) = fullErrorCovariance * Jpi.transpose();
tmpCovariance.block<ERROR_POSE_STATE_SIZE, ERROR_POSE_STATE_SIZE>(errorStateLength, errorStateLength) =
Jpi * fullErrorCovariance * Jpi.transpose();
fullErrorCovariance = tmpCovariance;
}
void nilss_solve(const std::vector<MatrixXd>& R,
const std::vector<MatrixXd>& D,
const std::vector<VectorXd>& b,
const std::vector<VectorXd>& c,
std::vector<VectorXd>& a)
{
int n = R.size();
assert(D.size() == n + 1);
assert(b.size() == n);
assert(c.size() == n + 1);
std::unique_ptr<MatrixXd> kkt = assemble_kkt(R, D);
std::unique_ptr<VectorXd> rhs = assemble_rhs(b, c);
typedef SparseMatrix<double> SpMat;
SpMat A(kkt->sparseView());
SparseLU<SparseMatrix<double>> solver;
solver.analyzePattern(A);
solver.factorize(A);
VectorXd sol = solver.solve(*rhs);
//VectorXd sol = kkt->partialPivLu().solve(*rhs);
assert(sol.size() % (2 * n + 1) == 0);
int m = sol.size() / (2 * n + 1);
a.empty();
a.reserve(n + 1);
for (int i = 0; i <= n; ++ i) {
a.push_back(sol.segment(i * m, m));
}
}
void ContactDynamics::applySolution() {
const int c = getNumContacts();
// First compute the external forces
VectorXd f_n = mX.head(c);
VectorXd f_d = mX.segment(c, c * mNumDir);
VectorXd lambda = mX.tail(c);
VectorXd forces = mN * f_n;
forces.noalias() += mB * f_d;
// Next, apply the external forces skeleton by skeleton.
int startRow = 0;
for (int i = 0; i < getNumSkels(); i++) {
if (mSkels[i]->getImmobileState())
continue;
int nDof = mSkels[i]->getNumDofs();
mConstrForces[i] = forces.segment(startRow, nDof);
startRow += nDof;
}
for (int i = 0; i < c; i++) {
Contact& contact = mCollisionChecker->getContact(i);
contact.force.noalias() = getTangentBasisMatrix(contact.point, contact.normal) * f_d.segment(i * mNumDir, mNumDir);
contact.force += contact.normal * f_n[i];
}
}
VectorXd PlanarClearance::Gamma()
{
VectorXd tempVector = VectorXd::Zero(nC);
tempVector.segment(0, 3) = Gammad3(bda->A0, bdb->A0, bda->omega, bdb->omega);//`````````````````````````
tempVector.segment(3, 2) = Gammar2(bda->omega, bdb->omega, bdb->A0);
return tempVector;
}
void Iterative_Control::thread_to_state(const Thread* thread, VectorXd& state)
{
const int num_pieces = thread->num_pieces();
state.resize(_size_each_state);
for (int piece_ind=0; piece_ind < thread->num_pieces(); piece_ind++)
{
state.segment(piece_ind*3, 3) = thread->vertex_at_ind(piece_ind);
if (piece_ind < thread->num_edges()) {
state.segment(piece_ind*3 + 3*num_pieces, 3) = thread->edge_at_ind(piece_ind);
}
}
state(6*num_pieces - 3) = thread->end_angle();
state(6*num_pieces - 2) = ((Thread*)thread)->calculate_energy();
//state(3*num_pieces) = thread->end_angle();
//state(3*num_pieces+1) = ((Thread*)thread)->calculate_energy();
}
Eigen::VectorXd ehanning(int len)
{
VectorXd han(len);
if (len%2 == 0)
{
int half = len/2;
VectorXd h = ecalc_hanning(half,len);
han.segment(0,half) = h;
han.segment(half,len-half) = h.segment(0,half).reverse();
} else {
int half = (len+1)/2;
VectorXd h = ecalc_hanning(half,len);
han.segment(0,half) = h;
han.segment(half,len-half) = h.segment(0,half-1).reverse();
}
return han;
}
VectorXd vec(MatrixXd& M){
VectorXd V;
V.setZero(M.rows()*M.cols());
for(int i=0;i<M.cols();i++){
V.segment(i*M.rows(), M.rows()) = M.col(i);
}
return V;
}
MatrixXd
PatchFit::parab_dfda (VectorXd a)
{
Vector3d k3, rv;
k3 << a(0), a(1), 0.0;
rv = a.segment(2,3);
MatrixXd rr;
rr = p_->rexp(rv);
vector<Matrix3d> drr (3);
drr = p_->drexp(rv);
Vector3d c;
if (ccon_)
c = plane_c + a(5)*plane_n;
else
c = a.segment(5,3);
MatrixXd qc(cloud_vec.rows(),cloud_vec.cols());
for (size_t i = 0; i<cloud_vec.rows(); i++)
qc.row(i) = cloud_vec.row(i)-c.transpose();
MatrixXd ql (qc.rows(),3);
ql = qc*rr;
MatrixXd j_full (cloud_vec.rows(),9);
j_full = general_dfda(ql,rr,dfdql(ql,k3),dqldr(qc,drr));
MatrixXd j (cloud_vec.rows(),8);
for (int i=0; i<2; i++)
j.col(i) = j_full.col(i);
for (int i=3; i<9; i++)
j.col(i-1) = j_full.col(i);
MatrixXd m(8,6);
m = Matrix<double, 8, 6>::Identity();
m.block(5,5,3,1) = plane_n;
if (ccon_)
j = j*m;
return j;
}
LieAlgebra MinEnFilter::MotionPrior(const LieGroup & G) {
if (G.order > 1) {
VectorXd v = VectorXd::Zero(G.order*6);
v.segment(0,6) = G.V.segment(0,6);
return LieAlgebra(v, G.order);
} else {
return LieAlgebra(1);
}
}
/*
void thread_to_state(const Thread* thread, VectorXd& state)
{
const int num_pieces = thread->num_pieces();
state.resize(3*num_pieces+1);
for (int piece_ind=0; piece_ind < thread->num_pieces(); piece_ind++)
{
state.segment(piece_ind*3, 3) = thread->vertex_at_ind(piece_ind);
}
state(3*num_pieces) = thread->end_angle();
}
*/
void weight_state(VectorXd& state)
{
const int num_pieces = (state.rows()-1)/3;
for (int piece_ind=0; piece_ind < num_pieces; piece_ind++)
{
state.segment(piece_ind*3, 3) *= WEIGHT_VERTICES;
}
state(3*num_pieces) *= WEIGHT_ANGLE;
}
VectorXd MyWindow::evalDeriv() {
// The root follows the desired acceleration and the rest of the body follows dynamic equations. The root acceleration will impact the rest of the body correctly. Collision or other external forces will alter the root acceleration
setPose();
VectorXd deriv = VectorXd::Zero(mIndices.back() * 2);
for (unsigned int i = 0; i < mSkels.size(); i++) {
// skip immobile objects in forward simulation
if (mSkels[i]->getImmobileState())
continue;
int start = mIndices[i] * 2;
int size = mDofs[i].size();
Vector3d desiredAccel(0.0, 0.0, 0.0);
if (mSimFrame > 500 && mSimFrame < 800)
desiredAccel[0] = 5.0;
else if (mSimFrame > 800 && mSimFrame < 1100)
desiredAccel[0] = -7.0;
else if (mSimFrame > 1100 && mSimFrame < 1400)
desiredAccel[0] = 6.0;
else if (mSimFrame > 1400 && mSimFrame < 1700)
desiredAccel[0] = -5.0;
else if (mSimFrame > 1700 && mSimFrame < 2000)
desiredAccel[0] = 5.0;
else if (mSimFrame > 2000 && mSimFrame < 2300)
desiredAccel[0] = -5.0;
int nDof = mSkels[i]->getNumDofs();
MatrixXd J(3, nDof);
J.setZero();
J(0, 0) = 1.0;
J(1, 1) = 1.0;
J(2, 2) = 1.0;
MatrixXd A = mSkels[i]->getMassMatrix();
A.block(0, 0, nDof, 3) = J.transpose();
VectorXd b = -mSkels[i]->getCombinedVector() + mSkels[i]->getInternalForces() - mSkels[i]->getMassMatrix().block(0, 0, nDof, 3) * desiredAccel;
VectorXd x = A.inverse() * b;
VectorXd qddot = x;
qddot.head(3) = desiredAccel;
mSkels[i]->clampRotation(mDofs[i], mDofVels[i]);
deriv.segment(start, size) = mDofVels[i] + (qddot * mTimeStep); // set velocities
deriv.segment(start + size, size) = qddot; // set qddot (accelerations)
}
return deriv;
}
MatrixXd individualSupportCOPs(RigidBodyManipulator* r, const std::vector<SupportStateElement,Eigen::aligned_allocator<SupportStateElement>>& active_supports,
const MatrixXd& normals, const MatrixXd& B, const VectorXd& beta)
{
const int n_basis_vectors_per_contact = static_cast<int>(B.cols() / normals.cols());
const int n = static_cast<int>(active_supports.size());
int normals_start = 0;
int beta_start = 0;
MatrixXd individual_cops(3, n);
individual_cops.fill(std::numeric_limits<double>::quiet_NaN());
for (size_t j = 0; j < active_supports.size(); j++) {
auto active_support = active_supports[j];
auto contact_pts = active_support.contact_pts;
int ncj = static_cast<int>(contact_pts.size());
int active_support_length = n_basis_vectors_per_contact * ncj;
auto normalsj = normals.middleCols(normals_start, ncj);
Vector3d normal = normalsj.col(0);
bool normals_identical = (normalsj.colwise().operator-(normal)).squaredNorm() < 1e-15;
if (normals_identical) { // otherwise computing a COP doesn't make sense
const auto& Bj = B.middleCols(beta_start, active_support_length);
const auto& betaj = beta.segment(beta_start, active_support_length);
const auto& contact_positions = r->bodies[active_support.body_idx]->contact_pts;
Vector3d force = Vector3d::Zero();
Vector3d torque = Vector3d::Zero();
for (size_t k = 0; k < contact_pts.size(); k++) {
// for (auto k = contact_pts.begin(); k!= contact_pts.end(); k++) {
const auto& Bblock = Bj.middleCols(k * n_basis_vectors_per_contact, n_basis_vectors_per_contact);
const auto& betablock = betaj.segment(k * n_basis_vectors_per_contact, n_basis_vectors_per_contact);
Vector3d point_force = Bblock * betablock;
force += point_force;
Vector3d contact_pt = contact_pts[k].head(3);
auto torquejk = contact_pt.cross(point_force);
torque += torquejk;
}
Vector3d point_on_contact_plane = contact_positions.col(0);
std::pair<Vector3d, double> cop_and_normal_torque = resolveCenterOfPressure(torque, force, normal, point_on_contact_plane);
Vector4d cop_body;
cop_body << cop_and_normal_torque.first, 1.0;
Vector3d cop_world;
r->forwardKin(active_support.body_idx, cop_body, 0, cop_world);
individual_cops.col(j) = cop_world;
}
normals_start += ncj;
beta_start += active_support_length;
}
return individual_cops;
}
std::unique_ptr<VectorXd> assemble_rhs(
const std::vector<VectorXd>& b, const std::vector<VectorXd>& c)
{
int n = b.size(), m = b[0].size();
assert(n > 0 && c.size() == n + 1);
int kktSize = (2 * n + 1) * m;
VectorXd * pVec = new VectorXd(kktSize);
for (int i = 0; i <= n; ++ i) {
pVec->segment(i * m, m) = c[i];
}
int halfSize = (n + 1) * m;
for (int i = 0; i < n; ++ i) {
pVec->segment(halfSize + i * m, m) = b[i];
}
return std::unique_ptr<VectorXd>(pVec);
}
void MotionModel::dfv_by_dxv(VectorXd x_k_k, double delta_t, string type, MatrixXd *dfv_by_dxvRES)
{
// This function calculates the F_k which is the Jacobian of f_k wrt. the state vector x_k_k-1
// Javier Civera book P72. 4.3.1 or P129. A.10
// Input : x_k_k.head(18), delta_t, type
// Output: F_k
VectorXd omegaOld, qOld, qwt;
omegaOld = x_k_k.segment(15,3); // Angular velocity
qOld = x_k_k.segment(8,4); // Orientation quaternion representation
qwt = V2Q(omegaOld * delta_t); // Convert omegaOld to quaternion representation
(*dfv_by_dxvRES).block(8,8,4,4) = dq3_by_dq2(qwt); // dq_k+1^WC/dq_k^WC
if (type.compare("constant_velocity") == 0)
{
(*dfv_by_dxvRES).block(5,12,3,3) = MatrixXd::Identity(3,3) * delta_t; // Delta_t * Identity
(*dfv_by_dxvRES).block(8,15,4,3) = dq3_by_dq1(qOld) * dqomegadt_by_domega(omegaOld, delta_t); // dq_k+1^WC/dw_k^C chain rule
}
/*
else if (type.compare("constant_orientation") == 0)
{
dfv_by_dxvRES.block(8,15,4,3) = MatrixXd::Zero(4,3);
dfv_by_dxvRES.block(15,15,3,3) = MatrixXd::Zero(3,3);
}
else if (type.compare("constant_position") == 0)
{
dfv_by_dxvRES.block(5,12,3,3) = MatrixXd::Zero(3,3);
dfv_by_dxvRES.block(12,12,3,3) = MatrixXd::Zero(3,3);
}
else if (type.compare("constant_position_and_orientation") == 0)
{
dfv_by_dxvRES.block(8,15,4,3) = MatrixXd::Zero(4,3);
dfv_by_dxvRES.block(5,12,3,3) = MatrixXd::Zero(3,3);
dfv_by_dxvRES.block(15,15,3,3) = MatrixXd::Zero(3,3);
dfv_by_dxvRES.block(12,12,3,3) = MatrixXd::Zero(3,3);
}
*/
}
void Dmp::getParameterVectorAll(VectorXd& values) const
{
values.resize(getParameterVectorAllSize());
int offset = 0;
VectorXd cur_values;
for (int dd=0; dd<dim_orig(); dd++)
{
function_approximators_[dd]->getParameterVectorAll(cur_values);
values.segment(offset,cur_values.size()) = cur_values;
offset += cur_values.size();
}
}
void MSCKF::correctNominalState(VectorXd delta)
{
fullNominalState.segment(0, 4) = quaternion_correct(fullNominalState.segment(0, 4), delta.segment(0, 3));
fullNominalState.segment(4, 3) = fullNominalState.segment(4, 3) + delta.segment(3, 3);
fullNominalState.segment(7, 3) = fullNominalState.segment(7, 3) + delta.segment(6, 3);
fullNominalState.segment(10, 3) = fullNominalState.segment(10, 3) + delta.segment(9, 3);
fullNominalState.segment(13, 3) = fullNominalState.segment(13, 3) + delta.segment(12, 3);
fullNominalState.segment(16, 3) = fullNominalState.segment(16, 3) + delta.segment(15, 3); //p_cb
// cout << __FILE__ << ":" << __LINE__ <<endl;
// printNominalState(true);
// loop to correct sliding states
std::list<SlideState>::iterator itr = slidingWindow.begin();
for (int i=0; i<=current_frame; i++)
{
// TODO: check order of multiplication
fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE, 4) =
quaternion_correct(fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE, 4),
delta.segment(ERROR_STATE_SIZE+3 + i*ERROR_POSE_STATE_SIZE, 3));
itr->q = fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE, 4);
fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE+4, 3) =
fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE+4, 3) +
delta.segment(ERROR_STATE_SIZE+3 + i*ERROR_POSE_STATE_SIZE+3, 3);
itr->p = fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE+4, 3);
fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE+7, 3) =
fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE+7, 3) +
delta.segment(ERROR_STATE_SIZE+3 + i*ERROR_POSE_STATE_SIZE+6, 3);
itr->v = fullNominalState.segment(NOMINAL_STATE_SIZE+3 + i*NOMINAL_POSE_STATE_SIZE+7, 3);
itr++;
}
}
void World::setStateForJacobian(VectorXd& world_state) {
int ind = 0;
for (int i = 0; i < threads.size(); i++) {
VectorXd state;
state = world_state.segment(ind, world_state(ind));
threads[i]->setState(state);
ind += state.size();
}
for (int i = 0; i < cursors.size(); i++) {
if(cursors[i]->isAttached()) {
VectorXd state;
state = world_state.segment(ind, world_state(ind));
cursors[i]->setState(state);
cursors[i]->end_eff->setState(state);
ind += state.size();
}
}
assert(ind == world_state.size());
}
void Dmp::setParameterVectorAll(const VectorXd& values)
{
assert(values.size()==getParameterVectorAllSize());
int offset = 0;
VectorXd cur_values;
for (int dd=0; dd<dim_orig(); dd++)
{
int n_parameters_required = function_approximators_[dd]->getParameterVectorAllSize();
cur_values = values.segment(offset,n_parameters_required);
function_approximators_[dd]->setParameterVectorAll(cur_values);
offset += n_parameters_required;
}
}
void MotionModel::fv(VectorXd x_k_k, double delta_t, string type, double std_a, double std_alpha, VectorXd *Xv_km1_k)
{
// This function updates the state for the camera. See Javier Civera book P39. 3.2.1 or P129 A.9
// It assumes the camera has a constant linear and angular velocity, so the
// linear and angular accelerations are zero.
VectorXd calibration, rW, qWR, vW, wW, Q, QP; // The camera state size is assumed to be 18
calibration = x_k_k.head(5); // Five intrinsic parameters are assumed
rW = x_k_k.segment(5,3); // Camera optical center position
qWR = x_k_k.segment(8,4); // Quaternion defines orientation
vW = x_k_k.segment(12,3); // Linear velocity
wW = x_k_k.segment(15,3); // Angular velocity
if (type.compare("constant_velocity") == 0)
{
Q = V2Q(wW * delta_t); // Converts rotation vector to quaternion
QP = Qprod(qWR, Q); // Cross product of qWR and q((wW+OmegaW)*delta_t), where OmegaW is set to 0
*Xv_km1_k << calibration, rW + vW * delta_t, QP, vW, wW; // State update for the camera
}
// The types below are probably not used
else if (type.compare("constant_orientation") == 0)
{
}
else if (type.compare("constant_position") == 0)
{
}
else if (type.compare("constant_position_and_orientation") == 0)
{
}
else if (type.compare("constant_position_and_orientation_location_noise") == 0)
{
}
}
MatrixXd veci(VectorXd& v,int n,int m)
{
if(v.size() !=n*m){
cout << "Wrong dimensions in reshape: " << v.size() << " (" << n << "," << m << ")" << endl;
}
MatrixXd M;
M.setZero(n,m);
int count = 0;
for(int i=0;i<m;i++){
M.col(i) = v.segment(count,n);
count += n;
}
return M;
}
void UnifiedModel::getParameterVectorAll(VectorXd& values) const
{
values.resize(getParameterVectorAllSize());
int offset = 0;
int n_basis_functions = centers_.size();
int n_dims = getExpectedInputDim();
for (int i_bfs=0; i_bfs<n_basis_functions; i_bfs++)
{
values.segment(offset,n_dims) = centers_[i_bfs]; offset += n_dims;
values.segment(offset,n_dims) = covars_[i_bfs].diagonal(); offset += n_dims;
values.segment(offset,n_dims) = slopes_[i_bfs]; offset += n_dims;
values[offset] = offsets_[i_bfs]; offset += 1;
values[offset] = priors_[i_bfs]; offset += 1;
}
/*
Dead code. But kept in for reference in case slopes_as_angles_ will be implemented
VectorXd cur_slopes;
for (int i_dim=0; i_dim<slopes_.cols(); i_dim++)
{
cur_slopes = slopes_.col(i_dim);
if (slopes_as_angles_)
{
// cur_slopes is a slope, but the values vector expects the angle with the x-axis. Do the
// conversion here.
for (int ii=0; ii<cur_slopes.size(); ii++)
cur_slopes[ii] = atan2(cur_slopes[ii],1.0);
}
values.segment(offset,slopes_.rows()) = cur_slopes;
offset += slopes_.rows();
}
*/
assert(offset == getParameterVectorAllSize());
};
