// Computes the gradient for conservative advancement // gradient: | (radius * -sin theta * sin phi) * y(1) + ... // (radius * cos theta * sin phi) * y(2) | // | (radius * cos theta * cos phi) * y(1) + ... // (radius * sin theta * cos phi) * y(2) + ... // (radius * -sin phi) * y(3) | void SpherePrimitive::calc_gradient(const VectorNd& x, void* data, VectorNd& g) { // get the pair std::pair<double, VectorNd>& data_pair = *((std::pair<double, VectorNd>*) data); // get radius and y double r = data_pair.first; VectorNd& y = data_pair.second; // get components of y const double Y1 = y[0]; const double Y2 = y[1]; const double Y3 = y[2]; // get theta and phi double THETA = x[0]; double PHI = x[1]; // compute trig functions double CTHETA = std::cos(THETA); double STHETA = std::sin(THETA); double CPHI = std::cos(PHI); double SPHI = std::sin(PHI); // setup gradient g.resize(2); g[0] = (-STHETA * SPHI) * Y1 + (CTHETA * SPHI) * Y2; g[1] = (CTHETA * CPHI) * Y1 + (STHETA * CPHI) * Y2 - SPHI * Y3; g *= -r; }
/// Determines (and sets) the value of Q from the axes and the inboard link and outboard link transforms void UniversalJoint::determine_q(VectorNd& q) { const unsigned X = 0, Y = 1, Z = 2; // get the outboard link RigidBodyPtr outboard = get_outboard_link(); // verify that the outboard link is set if (!outboard) throw std::runtime_error("determine_q() called on NULL outboard link!"); // set proper size for q this->q.resize(num_dof()); // get the poses of the joint and outboard link shared_ptr<const Pose3d> Fj = get_pose(); shared_ptr<const Pose3d> Fo = outboard->get_pose(); // compute transforms Transform3d wTo = Pose3d::calc_relative_pose(Fo, GLOBAL); Transform3d jTw = Pose3d::calc_relative_pose(GLOBAL, Fj); Transform3d jTo = jTw * wTo; // determine the joint transformation Matrix3d R = jTo.q; // determine q1 and q2 -- they are uniquely determined by examining the rotation matrix // (see get_rotation()) q.resize(num_dof()); q[DOF_1] = std::atan2(R(Z,Y), R(Y,Y)); q[DOF_2] = std::atan2(R(X,Z), R(X,X)); }
/** * Assume the signed distance function is Phi(q(t)), so * Phi(q(t_0 + \Delta t))_{t_0} \approx Phi(q(t_0)) + d/dt Phi(q(t_0)) * dt ==> * d/dt Phi(q(t_0)) \approx (Phi(q(t_0 + \Delta t)) - Phi(q(t_0))/dt */ void SignedDistDot::compute_signed_dist_dot_Jacobians(UnilateralConstraintProblemData& q, MatrixNd& Cdot_iM_CnT, MatrixNd& Cdot_iM_CsT, MatrixNd& Cdot_iM_CtT, MatrixNd& Cdot_iM_LT, VectorNd& Cdot_v) { const double DT = NEAR_ZERO; vector<shared_ptr<DynamicBodyd> > tmp_supers1, tmp_supers2, isect; VectorNd gc, gv; // get all pairs of bodies involved in contact vector<shared_ptr<DynamicBodyd> > ubodies; for (unsigned i=0; i< q.signed_distances.size(); i++) { // get the two single bodies shared_ptr<SingleBodyd> s1 = q.signed_distances[i].a->get_single_body(); shared_ptr<SingleBodyd> s2 = q.signed_distances[i].b->get_single_body(); // get the two super bodies shared_ptr<DynamicBodyd> sb1 = ImpactConstraintHandler::get_super_body(s1); shared_ptr<DynamicBodyd> sb2 = ImpactConstraintHandler::get_super_body(s2); // add the bodies to ubodies ubodies.push_back(sb1); ubodies.push_back(sb2); } // get all unique bodies involved in contact std::sort(ubodies.begin(), ubodies.end()); ubodies.erase(std::unique(ubodies.begin(), ubodies.end()), ubodies.end()); // save all configurations for all bodies involved in contact map<shared_ptr<DynamicBodyd>, VectorNd> gc_map; for (unsigned i=0; i< ubodies.size(); i++) ubodies[i]->get_generalized_coordinates_euler(gc_map[ubodies[i]]); // save all velocities for all bodies involved in contact map<shared_ptr<DynamicBodyd>, VectorNd> gv_map; for (unsigned i=0; i< ubodies.size(); i++) ubodies[i]->get_generalized_velocity(DynamicBodyd::eSpatial, gv_map[ubodies[i]]); // resize Cdot(v) Cdot_v.resize(q.signed_distances.size()); // for each pair of bodies for (unsigned k=0; k< q.signed_distances.size(); k++) { // get the two single bodies shared_ptr<SingleBodyd> s1 = q.signed_distances[k].a->get_single_body(); shared_ptr<SingleBodyd> s2 = q.signed_distances[k].b->get_single_body(); // get the signed distance between the two bodies double phi = q.signed_distances[k].dist; // integrates bodies' positions forward shared_ptr<DynamicBodyd> sup1 = ImpactConstraintHandler::get_super_body(s1); shared_ptr<DynamicBodyd> sup2 = ImpactConstraintHandler::get_super_body(s2); tmp_supers1.clear(); tmp_supers1.push_back(sup1); if (sup1 != sup2) tmp_supers1.push_back(sup2); integrate_positions(tmp_supers1, DT); // compute the signed distance function double phi_new = calc_signed_dist(s1, s2); // set the appropriate entry of Cdot(v) Cdot_v[k] = (phi_new - phi)/DT; // restore coordinates and velocities restore_coords_and_velocities(tmp_supers1, gc_map, gv_map); } // resize the Jacobians Cdot_iM_CnT.resize(q.signed_distances.size(), q.N_CONTACTS); Cdot_iM_CsT.resize(q.signed_distances.size(), q.N_CONTACTS); Cdot_iM_CtT.resize(q.signed_distances.size(), q.N_CONTACTS); Cdot_iM_LT.resize(q.signed_distances.size(), q.N_LIMITS); // prepare iterators for contacts ColumnIteratord Cn_iter = Cdot_iM_CnT.column_iterator_begin(); ColumnIteratord Cs_iter = Cdot_iM_CsT.column_iterator_begin(); ColumnIteratord Ct_iter = Cdot_iM_CtT.column_iterator_begin(); ColumnIteratord L_iter = Cdot_iM_LT.column_iterator_begin(); // for each pair of bodies for (unsigned k=0; k< q.signed_distances.size(); k++) { // get the two single bodies shared_ptr<SingleBodyd> s1 = q.signed_distances[k].a->get_single_body(); shared_ptr<SingleBodyd> s2 = q.signed_distances[k].b->get_single_body(); // get the two bodies involved shared_ptr<DynamicBodyd> sup1 = ImpactConstraintHandler::get_super_body(s1); shared_ptr<DynamicBodyd> sup2 = ImpactConstraintHandler::get_super_body(s2); tmp_supers1.clear(); tmp_supers1.push_back(sup1); tmp_supers1.push_back(sup2); // sort the vector so we can do the intersection std::sort(tmp_supers1.begin(), tmp_supers1.end()); // get the signed distance between the two bodies double phi = q.signed_distances[k].dist; // for each contact constraint for (unsigned i=0; i< q.contact_constraints.size(); i++) { // zero the Cn, Cs, and Ct iterators *Cn_iter = 0.0; *Cs_iter = 0.0; *Ct_iter = 0.0; // see whether constraint will have any effect on this pair of bodies isect.clear(); tmp_supers2.clear(); q.contact_constraints[i]->get_super_bodies(std::back_inserter(tmp_supers2)); // sort the vector so we can do the intersection std::sort(tmp_supers2.begin(), tmp_supers2.end()); // do the intersection std::set_intersection(tmp_supers1.begin(), tmp_supers1.end(), tmp_supers2.begin(), tmp_supers2.end(), std::back_inserter(isect)); if (isect.empty()) continue; // apply a test impulse in the normal direction apply_impulse(*q.contact_constraints[i], q.contact_constraints[i]->contact_normal); // integrates bodies' positions forward integrate_positions(isect, DT); // compute the signed distance function double phi_new = calc_signed_dist(s1, s2); // set the appropriate entry of the Jacobian *Cn_iter = (phi_new - phi)/DT - q.Cdot_v[k]; Cn_iter++; // restore coordinates and velocities restore_coords_and_velocities(isect, gc_map, gv_map); // apply a test impulse in the first tangent direction apply_impulse(*q.contact_constraints[i], q.contact_constraints[i]->contact_tan1); // integrates bodies' positions forward integrate_positions(isect, DT); // compute the signed distance function phi_new = calc_signed_dist(s1, s2); // set the appropriate entry of the Jacobian *Cs_iter = (phi_new - phi)/DT - q.Cdot_v[k]; Cs_iter++; // restore coordinates and velocities restore_coords_and_velocities(isect, gc_map, gv_map); // apply a test impulse in the second tangent direction apply_impulse(*q.contact_constraints[i], q.contact_constraints[i]->contact_tan2); // integrates bodies' positions forward integrate_positions(isect, DT); // compute the signed distance function phi_new = calc_signed_dist(s1, s2); // set the appropriate entry of the Jacobian *Ct_iter = (phi_new - phi)/DT - q.Cdot_v[k]; Ct_iter++; // restore coordinates and velocities restore_coords_and_velocities(isect, gc_map, gv_map); } // for each limit constraint for (unsigned i=0; i< q.limit_constraints.size(); i++) { // zero the LT iterator *L_iter = 0.0; // see whether constraint will have any effect on this pair of bodies isect.clear(); tmp_supers2.clear(); q.limit_constraints[i]->get_super_bodies(std::back_inserter(tmp_supers2)); std::set_intersection(tmp_supers1.begin(), tmp_supers1.end(), tmp_supers2.begin(), tmp_supers2.end(), std::back_inserter(isect)); if (isect.empty()) continue; // apply a test impulse in the limit direction apply_impulse(*q.limit_constraints[i]); // integrates bodies' positions forward integrate_positions(isect, DT); // compute the signed distance function double phi_new = calc_signed_dist(s1, s2); // set the appropriate entry of the Jacobian *L_iter = (phi_new - phi)/DT - q.Cdot_v[k]; L_iter++; // restore coordinates and velocities restore_coords_and_velocities(isect, gc_map, gv_map); } } }
/** * \param x the solution is returned here; zeros will be returned at appropriate indices for inactive contacts */ void ImpactConstraintHandler::solve_nqp_work(UnilateralConstraintProblemData& q, VectorNd& x) { const double INF = std::numeric_limits<double>::max(); // setup constants const unsigned N_CONTACTS = q.N_CONTACTS; const unsigned N_LIMITS = q.N_LIMITS; const unsigned N_CONSTRAINT_EQNS_IMP = q.N_CONSTRAINT_EQNS_IMP; const unsigned CN_IDX = 0; const unsigned CS_IDX = N_CONTACTS; const unsigned CT_IDX = CS_IDX + N_CONTACTS; const unsigned CL_IDX = CT_IDX + N_CONTACTS; const unsigned NVARS = N_LIMITS + CL_IDX; // setup the optimization data _ipsolver->epd = &q; _ipsolver->mu_c.resize(N_CONTACTS); _ipsolver->mu_visc.resize(N_CONTACTS); // setup true friction cone for every contact for (unsigned i=0; i< N_CONTACTS; i++) { _ipsolver->mu_c[i] = sqr(q.contact_constraints[i]->contact_mu_coulomb); _ipsolver->mu_visc[i] = (sqr(q.Cs_v[i]) + sqr(q.Ct_v[i])) * sqr(q.contact_constraints[i]->contact_mu_viscous); } // setup matrices MatrixNd& R = _ipsolver->R; MatrixNd& H = _ipsolver->H; VectorNd& c = _ipsolver->c; VectorNd& z = _ipsolver->z; // init z (particular solution) z.set_zero(NVARS); // first, compute the appropriate nullspace if (N_CONSTRAINT_EQNS_IMP > 0) { // compute the homogeneous solution _A = q.Jx_iM_JxT; (_workv = q.Jx_v).negate(); try { _LA.solve_LS_fast1(_A, _workv); } catch (NumericalException e) { _A = q.Jx_iM_JxT; _LA.solve_LS_fast2(_A, _workv); } z.set_sub_vec(q.ALPHA_X_IDX, _workv); // setup blocks of A _A.resize(N_CONSTRAINT_EQNS_IMP, NVARS); SharedMatrixNd b1 = _A.block(0, N_CONSTRAINT_EQNS_IMP, 0, N_CONTACTS); SharedMatrixNd b2 = _A.block(0, N_CONSTRAINT_EQNS_IMP, N_CONTACTS, N_CONTACTS*2); SharedMatrixNd b3 = _A.block(0, N_CONSTRAINT_EQNS_IMP, N_CONTACTS*2, N_CONTACTS*3); SharedMatrixNd b4 = _A.block(0, N_CONSTRAINT_EQNS_IMP, N_CONTACTS*3, N_CONTACTS*3+N_LIMITS); // compute the nullspace MatrixNd::transpose(q.Cn_iM_JxT, b1); MatrixNd::transpose(q.Cs_iM_JxT, b2); MatrixNd::transpose(q.Ct_iM_JxT, b3); MatrixNd::transpose(q.L_iM_JxT, b4); _LA.nullspace(_A, R); } else // clear the nullspace R.resize(0,0); // get number of qp variables const unsigned N_PRIMAL = (R.columns() > 0) ? R.columns() : NVARS; // setup number of nonlinear inequality constraints const unsigned NONLIN_INEQUAL = N_CONTACTS; // init the QP matrix and vector H.resize(N_PRIMAL, N_PRIMAL); c.resize(H.rows()); // setup row (block) 1 -- Cn * iM * [Cn' Cs Ct' L'] unsigned col_start = 0, col_end = N_CONTACTS; unsigned row_start = 0, row_end = N_CONTACTS; SharedMatrixNd Cn_iM_CnT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_CONTACTS; SharedMatrixNd Cn_iM_CsT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_CONTACTS; SharedMatrixNd Cn_iM_CtT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_LIMITS; SharedMatrixNd Cn_iM_LT = H.block(row_start, row_end, col_start, col_end); // setup row (block) 2 -- Cs * iM * [Cn' Cs' Ct' L'] row_start = row_end; row_end += N_CONTACTS; col_start = 0; col_end = N_CONTACTS; SharedMatrixNd Cs_iM_CnT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_CONTACTS; SharedMatrixNd Cs_iM_CsT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_CONTACTS; SharedMatrixNd Cs_iM_CtT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_LIMITS; SharedMatrixNd Cs_iM_LT = H.block(row_start, row_end, col_start, col_end); // setup row (block) 3 -- Ct * iM * [Cn' Cs' Ct' L'] row_start = row_end; row_end += N_CONTACTS; col_start = 0; col_end = N_CONTACTS; SharedMatrixNd Ct_iM_CnT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_CONTACTS; SharedMatrixNd Ct_iM_CsT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_CONTACTS; SharedMatrixNd Ct_iM_CtT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_LIMITS; SharedMatrixNd Ct_iM_LT = H.block(row_start, row_end, col_start, col_end); // setup row (block 4) -- L * iM * [Cn' Cs' Ct' L'] row_start = row_end; row_end += N_LIMITS; col_start = 0; col_end = N_CONTACTS; SharedMatrixNd L_iM_CnT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_CONTACTS; SharedMatrixNd L_iM_CsT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_CONTACTS; SharedMatrixNd L_iM_CtT = H.block(row_start, row_end, col_start, col_end); col_start = col_end; col_end += N_LIMITS; SharedMatrixNd L_iM_LT = H.block(row_start, row_end, col_start, col_end); SharedMatrixNd L_block = H.block(row_start, row_end, 0, col_end); // copy to row block 1 (contact normals) q.Cn_iM_CnT.get_sub_mat(0, N_CONTACTS, 0, N_CONTACTS, Cn_iM_CnT); q.Cn_iM_CsT.get_sub_mat(0, N_CONTACTS, 0, N_CONTACTS, Cn_iM_CsT); q.Cn_iM_CtT.get_sub_mat(0, N_CONTACTS, 0, N_CONTACTS, Cn_iM_CtT); q.Cn_iM_LT.get_sub_mat(0, N_CONTACTS, 0, N_LIMITS, Cn_iM_LT); // copy to row block 2 (first contact tangents) MatrixNd::transpose(Cn_iM_CsT, Cs_iM_CnT); q.Cs_iM_CsT.get_sub_mat(0, N_CONTACTS, 0, N_CONTACTS, Cs_iM_CsT); q.Cs_iM_CtT.get_sub_mat(0, N_CONTACTS, 0, N_CONTACTS, Cs_iM_CtT); q.Cs_iM_LT.get_sub_mat(0, N_CONTACTS, 0, N_LIMITS, Cs_iM_LT); // copy to row block 3 (second contact tangents) MatrixNd::transpose(Cn_iM_CtT, Ct_iM_CnT); MatrixNd::transpose(Cs_iM_CtT, Ct_iM_CsT); q.Ct_iM_CtT.get_sub_mat(0, N_CONTACTS, 0, N_CONTACTS, Ct_iM_CtT); q.Ct_iM_LT.get_sub_mat(0, N_CONTACTS, 0, N_LIMITS, Ct_iM_LT); // copy to row block 6 (limits) MatrixNd::transpose(Cn_iM_LT, L_iM_CnT); MatrixNd::transpose(Cs_iM_LT, L_iM_CsT); MatrixNd::transpose(Ct_iM_LT, L_iM_CtT); q.L_iM_LT.get_sub_mat(0, N_LIMITS, 0, N_LIMITS, L_iM_LT); // get components of c SharedVectorNd Cn_v = c.segment(0, N_CONTACTS); SharedVectorNd Cs_v = c.segment(N_CONTACTS, N_CONTACTS*2); SharedVectorNd Ct_v = c.segment(N_CONTACTS*2, N_CONTACTS*3); SharedVectorNd L_v = c.segment(N_CONTACTS*3, N_CONTACTS*3+N_LIMITS); // setup c q.Cn_v.get_sub_vec(0, N_CONTACTS, Cn_v); q.Cs_v.get_sub_vec(0, N_CONTACTS, Cs_v); q.Ct_v.get_sub_vec(0, N_CONTACTS, Ct_v); L_v = q.L_v; // ****** now setup linear inequality constraints ****** // determine whether to use kappa constraint const unsigned KAPPA = 1; // determine number of linear inequality constraints const unsigned N_INEQUAL = q.N_CONTACTS + N_LIMITS + KAPPA; // get Cn sub blocks SharedConstMatrixNd sub_Cn_Cn = q.Cn_iM_CnT.block(0, q.N_CONTACTS, 0, q.N_CONTACTS); SharedConstMatrixNd sub_Cn_Cs = q.Cn_iM_CsT.block(0, q.N_CONTACTS, 0, q.N_CONTACTS); SharedConstMatrixNd sub_Cn_Ct = q.Cn_iM_CtT.block(0, q.N_CONTACTS, 0, q.N_CONTACTS); SharedConstMatrixNd sub_Cn_L = q.Cn_iM_LT.block(0, q.N_CONTACTS, 0, q.N_LIMITS); // setup Cn block MatrixNd& Cn_block = _ipsolver->Cn_block; Cn_block.resize(q.N_CONTACTS, NVARS); Cn_block.set_sub_mat(0, 0, sub_Cn_Cn); Cn_block.set_sub_mat(0, N_CONTACTS, sub_Cn_Cs); Cn_block.set_sub_mat(0, N_CONTACTS*2, sub_Cn_Ct); Cn_block.set_sub_mat(0, N_CONTACTS*3, sub_Cn_L); // verify that L_block can be reset _ipsolver->L_block.reset(); _ipsolver->Cn_v.reset(); _ipsolver->L_block = L_block; _ipsolver->Cn_v = q.Cn_v.segment(0, N_CONTACTS); // setup optimizations in nullspace (if necessary) if (R.columns() > 0) { R.transpose_mult(H, _RTH); _RTH.mult(R, H); R.transpose_mult(c, _workv); c = _workv; _RTH.mult(z, _workv); c += _workv; } FILE_LOG(LOG_CONSTRAINT) << "ImpactConstraintHandler::solve_nqp_work() entered" << std::endl; FILE_LOG(LOG_CONSTRAINT) << " Cn * inv(M) * Cn': " << std::endl << q.Cn_iM_CnT; FILE_LOG(LOG_CONSTRAINT) << " Cn * inv(M) * Cs': " << std::endl << q.Cn_iM_CsT; FILE_LOG(LOG_CONSTRAINT) << " Cn * inv(M) * Ct': " << std::endl << q.Cn_iM_CtT; FILE_LOG(LOG_CONSTRAINT) << " Cn * inv(M) * L': " << std::endl << q.Cn_iM_LT; FILE_LOG(LOG_CONSTRAINT) << " Cn * inv(M) * Jx': " << std::endl << q.Cn_iM_JxT; FILE_LOG(LOG_CONSTRAINT) << " Cs * inv(M) * Cs': " << std::endl << q.Cs_iM_CsT; FILE_LOG(LOG_CONSTRAINT) << " Cs * inv(M) * Ct': " << std::endl << q.Cs_iM_CtT; FILE_LOG(LOG_CONSTRAINT) << " Cs * inv(M) * L': " << std::endl << q.Cs_iM_LT; FILE_LOG(LOG_CONSTRAINT) << " Cs * inv(M) * Jx': " << std::endl << q.Cs_iM_JxT; FILE_LOG(LOG_CONSTRAINT) << " Ct * inv(M) * Ct': " << std::endl << q.Ct_iM_CtT; FILE_LOG(LOG_CONSTRAINT) << " Ct * inv(M) * L': " << std::endl << q.Ct_iM_LT; FILE_LOG(LOG_CONSTRAINT) << " Ct * inv(M) * Jx': " << std::endl << q.Ct_iM_JxT; FILE_LOG(LOG_CONSTRAINT) << " L * inv(M) * L': " << std::endl << q.L_iM_LT; FILE_LOG(LOG_CONSTRAINT) << " L * inv(M) * Jx': " << std::endl << q.L_iM_JxT; FILE_LOG(LOG_CONSTRAINT) << " Jx * inv(M) * Jx': " << std::endl << q.Jx_iM_JxT; FILE_LOG(LOG_CONSTRAINT) << " Cn * v: " << q.Cn_v << std::endl; FILE_LOG(LOG_CONSTRAINT) << " Cs * v: " << q.Cs_v << std::endl; FILE_LOG(LOG_CONSTRAINT) << " Ct * v: " << q.Ct_v << std::endl; FILE_LOG(LOG_CONSTRAINT) << " L * v: " << q.L_v << std::endl; FILE_LOG(LOG_CONSTRAINT) << " Jx * v: " << q.Jx_v << std::endl; FILE_LOG(LOG_CONSTRAINT) << "H matrix: " << std::endl << H; FILE_LOG(LOG_CONSTRAINT) << "c vector: " << c << std::endl; // setup ipopt options _app.Options()->SetIntegerValue("print_level", 0); _app.Options()->SetNumericValue("constr_viol_tol", 0.0005); // _app.Options()->SetIntegerValue("max_iter", 10000); // _app.Options()->SetStringValue("derivative_test", "second-order"); // set the ipsolver tolerance on the Coulomb friction and kappa constraints _ipsolver->_tol = 0.0; // solve the nonlinear QP using the interior-point algorithm _app.Initialize(); Ipopt::ApplicationReturnStatus status = _app.OptimizeTNLP(_ipsolver); // look for acceptable solve conditions if (!(status == Ipopt::Solve_Succeeded || status == Ipopt::Solved_To_Acceptable_Level)) throw std::runtime_error("Could not solve nonlinearly constrained QP"); // get the final solution out SharedVectorNd cn = _ipsolver->z.segment(0, N_CONTACTS); SharedVectorNd cs = _ipsolver->z.segment(N_CONTACTS, N_CONTACTS*2); SharedVectorNd ct = _ipsolver->z.segment(N_CONTACTS*2, N_CONTACTS*3); SharedVectorNd l = _ipsolver->z.segment(N_CONTACTS*3, N_CONTACTS*3+q.N_LIMITS); // put x in the expected format x.resize(q.N_VARS); x.set_sub_vec(q.CN_IDX, cn); x.set_sub_vec(q.CS_IDX, cs); x.set_sub_vec(q.CT_IDX, ct); x.set_sub_vec(q.L_IDX, l); FILE_LOG(LOG_CONSTRAINT) << "nonlinear QP solution: " << x << std::endl; if (LOGGING(LOG_CONSTRAINT)) { VectorNd workv; SharedVectorNd xsub = _ipsolver->z.segment(0, c.rows()); H.mult(xsub, workv) *= 0.5; workv += c; FILE_LOG(LOG_CONSTRAINT) << "(signed) computed energy dissipation: " << xsub.dot(workv) << std::endl; } FILE_LOG(LOG_CONSTRAINT) << "ImpactConstraintHandler::solve_nqp() exited" << std::endl; }