/// Multiplies a vector of spatial axes by a vector
SVelocityd mult(const vector<SVelocityd>& t, const VectorNd& v)
{
const unsigned SPATIAL_DIM = 6;
if (t.size() != v.size())
throw MissizeException();
// verify that the vector is not empty - we lose frame info!
if (t.empty())
throw std::runtime_error("loss of frame information");
// setup the result
SVelocityd result = SVelocityd::zero();
// verify that all twists are in the same pose
result.pose = t.front().pose;
for (unsigned i=1; i< t.size(); i++)
if (t[i].pose != result.pose)
throw FrameException();
// finally, do the computation
const double* vdata = v.data();
for (unsigned j=0; j< SPATIAL_DIM; j++)
for (unsigned i=0; i< t.size(); i++)
result[j] += t[i][j]*vdata[i];
return result;
}
void CartVelController::updatePosition(const VectorNd& position) {
if (q_.size() != position.size()) {
ROS_ERROR_STREAM("Size of joint position vector (" << position.size() << ") doesn't match chain size (" << kdl_chain_.getNrOfJoints() << ").");
return;
}
q_ = position;
}
/// Gets a list of space-delimited and/or comma-delimited vectors from the underlying string value
void XMLAttrib::get_vector_value(SVector6d& v)
{
// indicate this attribute has been processed
processed = true;
VectorNd w = VectorNd::parse(value);
if (w.size() != v.size())
throw MissizeException();
v = SVector6d(w[0], w[1], w[2], w[3], w[4], w[5]);
}
/// Returns a quaternion value from a roll-pitch-yaw attribute
Quatd XMLAttrib::get_rpy_value()
{
// indicate this attribute has been processed
processed = true;
VectorNd v = VectorNd::parse(value);
if (v.size() != 3)
throw std::runtime_error("Unable to parse roll-pitch-yaw from vector!");
return Quatd::rpy(v[0], v[1], v[2]);
}
void setup(){
boost::shared_ptr<Pacer::Controller> ctrl(ctrl_weak_ptr);
q_goal = ctrl->get_joint_generalized_value(Pacer::Controller::position);
qd_goal = Ravelin::VectorNd::zero(q_goal.rows());
qdd_goal = Ravelin::VectorNd::zero(q_goal.rows());
x_goal = ctrl->get_foot_value(Pacer::Controller::position_goal);
for (std::map<std::string,Origin3d>::iterator it = x_goal.begin(); it != x_goal.end(); it++) {
xd_goal[it->first] = Origin3d(0,0,0);
xdd_goal[it->first] = Origin3d(0,0,0);
}
}
/// Returns an Origin3d value from the attribute
Origin3d XMLAttrib::get_origin_value()
{
// indicate this attribute has been processed
processed = true;
VectorNd v = VectorNd::parse(value);
if (v.size() != 3)
throw std::runtime_error("Unable to parse origin from vector!");
Origin3d o;
o.x() = v[0];
o.y() = v[1];
o.z() = v[2];
return o;
}
/// Returns a quaternion value from the attribute
Quatd XMLAttrib::get_quat_value()
{
// indicate this attribute has been processed
processed = true;
VectorNd v = VectorNd::parse(value);
if (v.size() != 4)
throw std::runtime_error("Unable to parse quaternion from vector!");
Quatd q;
q.w = v[0];
q.x = v[1];
q.y = v[2];
q.z = v[3];
return q;
}
// 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));
}
// simulator callback
void post_step_callback(Simulator* sim)
{
// output the sliding velocity at the contact
std::ofstream out("rke.dat", std::ostream::app);
out << sim->current_time << " " << box->calc_kinetic_energy() << std::endl;
out.close();
// save the generalized coordinates of the box
out.open("telemetry.box", std::ostream::app);
VectorNd q;
box->get_generalized_coordinates_euler(q);
out << sim->current_time;
for (unsigned i=0; i< q.size(); i++)
out << " " << q[i];
out << std::endl;
out.close();
}
/// runs the simulator and updates all transforms
bool step(void* arg)
{
// get the simulator pointer
boost::shared_ptr<Simulator> s = *(boost::shared_ptr<Simulator>*) arg;
// get the generalized coordinates for all bodies in alphabetical order
std::vector<ControlledBodyPtr> bodies = s->get_dynamic_bodies();
std::sort(bodies.begin(), bodies.end(), compbody);
VectorNd q;
outfile << s->current_time;
for (unsigned i=0; i< bodies.size(); i++)
{
shared_ptr<DynamicBodyd> db = dynamic_pointer_cast<DynamicBodyd>(bodies[i]);
db->get_generalized_coordinates_euler(q);
for (unsigned j=0; j< q.size(); j++)
outfile << " " << q[j];
}
outfile << std::endl;
// output the iteration #
if (OUTPUT_ITER_NUM)
std::cout << "iteration: " << ITER << " simulation time: " << s->current_time << std::endl;
if (Log<OutputToFile>::reporting_level > 0)
FILE_LOG(Log<OutputToFile>::reporting_level) << "iteration: " << ITER << " simulation time: " << s->current_time << std::endl;
// step the simulator and update visualization
clock_t pre_sim_t = clock();
s->step(STEP_SIZE);
clock_t post_sim_t = clock();
double total_t = (post_sim_t - pre_sim_t) / (double) CLOCKS_PER_SEC;
TOTAL_TIME += total_t;
// output the iteration / stepping rate
if (OUTPUT_SIM_RATE)
std::cout << "time to compute last iteration: " << total_t << " (" << TOTAL_TIME / ITER << "s/iter, " << TOTAL_TIME / s->current_time << "s/step)" << std::endl;
// update the iteration #
ITER++;
// check that maximum number of iterations or maximum time not exceeded
if (ITER >= MAX_ITER || s->current_time > MAX_TIME)
return false;
return true;
}
bool CartVelController::update(const Vector6d& command, VectorNd& velocities) {
// init
if (!initialized_) {
ROS_ERROR("Controller wasn't initilized before calling 'update'.");
return false;
}
setCommand(command);
if (velocities.size() != kdl_chain_.getNrOfJoints()) {
ROS_ERROR_STREAM("Size of velocities vector (" << velocities.size() << ") doesn't match number of joints (" << kdl_chain_.getNrOfJoints() << ").");
return false;
}
for (unsigned int i = 0; i < kdl_chain_.getNrOfJoints(); i++) {
velocities(i) = 0.0;
}
// forward kinematics
// KDL::Frame frame_tip_pose;
// if(chain_fk_solver_->JntToCart(*q_, frame_tip_pose) < 0) {
// ROS_ERROR("Unable to compute forward kinematics");
// return false;
// }
// transform vel command to root frame // not necessary, command is in root frame
// KDL::Frame frame_tip_pose_inv = frame_tip_pose.Inverse();
// KDL::Twist linear_twist = frame_tip_pose_inv * cmd_linear_twist_;
// KDL::Twist angular_twist = frame_tip_pose_inv.M * cmd_angular_twist_;
// KDL::Twist twist(linear_twist.vel, angular_twist.rot);
// cartesian to joint space
ConversionHelper::eigenToKdl(q_, qtmp_);
ConversionHelper::eigenToKdl(cmd_twist_, twist_tmp_);
if(chain_ik_solver_vel_->CartToJnt(qtmp_, twist_tmp_, joint_vel_tmp_) < 0) {
ROS_ERROR("Unable to compute cartesian to joint velocity");
return false;
}
// assign values to output.
ConversionHelper::kdlToEigen(joint_vel_tmp_, velocities);
return true;
}
/// Constructs a vector-valued attribute with the given name
XMLAttrib::XMLAttrib(const std::string& name, const VectorNd& vector_value)
{
this->processed = false;
this->name = name;
std::ostringstream oss;
// if the vector is empty, return prematurely
if (vector_value.size() == 0)
{
this->value = "";
return;
}
// set the first value of the vector
oss << str(vector_value[0]);
// set subsequent values of the vector
for (unsigned i=1; i< vector_value.size(); i++)
oss << " " << str(vector_value[i]);
// get the string value
this->value = oss.str();
}
/**
* 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;
}
/// Solves the Anitescu-Potra model LCP
void ImpactConstraintHandler::apply_ap_model(UnilateralConstraintProblemData& q)
{
/// Matrices and vectors for solving LCP
Ravelin::MatrixNd _UL, _LL, _MM, _UR, _workM;
Ravelin::VectorNd _qq, _workv;
FILE_LOG(LOG_CONSTRAINT) << "ImpactConstraintHandler::apply_ap_model() entered" << std::endl;
unsigned NC = q.N_CONTACTS;
// Num joint limit constraints
unsigned N_LIMIT = q.N_LIMITS;
// Num friction directions + num normal directions
unsigned N_FRICT = NC*4 + NC;
// Total constraints
unsigned N_CONST = N_FRICT + N_LIMIT;
// Num friction constraints
unsigned NK_DIRS = 0;
for(unsigned i=0,j=0,r=0;i<NC;i++){
if (q.contact_constraints[i]->contact_NK > 4)
NK_DIRS+=(q.contact_constraints[i]->contact_NK+4)/4;
else
NK_DIRS+=1;
}
// setup sizes
_UL.set_zero(N_CONST, N_CONST);
_UR.set_zero(N_CONST, NK_DIRS);
_LL.set_zero(NK_DIRS, N_CONST);
_MM.set_zero(_UL.rows() + _LL.rows(), _UL.columns() + _UR.columns());
MatrixNd Cs_iM_CnT,Ct_iM_CnT,Ct_iM_CsT,L_iM_CnT,L_iM_CsT,L_iM_CtT;
Ravelin::MatrixNd::transpose(q.Cn_iM_LT,L_iM_CnT);
Ravelin::MatrixNd::transpose(q.Cs_iM_LT,L_iM_CsT);
Ravelin::MatrixNd::transpose(q.Ct_iM_LT,L_iM_CtT);
Ravelin::MatrixNd::transpose(q.Cn_iM_CsT,Cs_iM_CnT);
Ravelin::MatrixNd::transpose(q.Cn_iM_CtT,Ct_iM_CnT);
Ravelin::MatrixNd::transpose(q.Cs_iM_CtT,Ct_iM_CsT);
/* n r r r r
n Cn_iM_CnT Cn_iM_CsT -Cn_iM_CsT Cn_iM_CtT -Cn_iM_CtT
r Cs_iM_CnT Cs_iM_CsT -Cs_iM_CsT Cs_iM_CtT -Cs_iM_CtT
r -Cs_iM_CnT -Cs_iM_CsT Cs_iM_CsT -Cs_iM_CtT Cs_iM_CtT
r Ct_iM_CnT Ct_iM_CsT -Ct_iM_CsT Ct_iM_CtT -Ct_iM_CtT
r -Ct_iM_CnT -Ct_iM_CsT Ct_iM_CsT -Ct_iM_CtT Ct_iM_CtT
*/
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) << "Cs*inv(M)*Cn': " << std::endl << Cs_iM_CnT;
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_CsT;
FILE_LOG(LOG_CONSTRAINT) << "Ct*inv(M)*Cn': " << std::endl << Ct_iM_CnT;
FILE_LOG(LOG_CONSTRAINT) << "Ct*inv(M)*Cs': " << std::endl << Ct_iM_CsT;
FILE_LOG(LOG_CONSTRAINT) << "L*inv(M)*L': " << std::endl << q.L_iM_LT;
FILE_LOG(LOG_CONSTRAINT) << "Cn*inv(M)*L': " << std::endl << q.Cn_iM_LT;
FILE_LOG(LOG_CONSTRAINT) << "L*inv(M)*Cn': " << std::endl << L_iM_CnT;
FILE_LOG(LOG_CONSTRAINT) << "Cs*inv(M)*L': " << std::endl << q.Cs_iM_LT;
FILE_LOG(LOG_CONSTRAINT) << "Ct*inv(M)*L': " << std::endl << q.Ct_iM_LT;
FILE_LOG(LOG_CONSTRAINT) << "L*inv(M)*Cs': " << std::endl << L_iM_CsT;
FILE_LOG(LOG_CONSTRAINT) << "L*inv(M)*Ct': " << std::endl << L_iM_CtT;
// Set positive submatrices
/*
n r r r r
n Cn_iM_CnT Cn_iM_CsT Cn_iM_CtT
r Cs_iM_CnT Cs_iM_CsT Cs_iM_CtT
r Cs_iM_CsT Cs_iM_CtT
r Ct_iM_CnT Ct_iM_CsT Ct_iM_CtT
r Ct_iM_CsT Ct_iM_CtT
*/
_UL.set_sub_mat(0,0,q.Cn_iM_CnT);
// setup the LCP matrix
// setup the LCP vector
_qq.set_zero(_MM.rows());
_qq.set_sub_vec(0,q.Cn_v);
_UL.set_sub_mat(NC,NC,q.Cs_iM_CsT);
_UL.set_sub_mat(NC,0,Cs_iM_CnT);
_UL.set_sub_mat(0,NC,q.Cn_iM_CsT);
_UL.set_sub_mat(NC+NC,NC+NC,q.Cs_iM_CsT);
_UL.set_sub_mat(NC+NC*2,0,Ct_iM_CnT);
_UL.set_sub_mat(0,NC+NC*2,q.Cn_iM_CtT);
_UL.set_sub_mat(NC+NC*2,NC,Ct_iM_CsT);
_UL.set_sub_mat(NC+NC*3,NC+NC,Ct_iM_CsT);
_UL.set_sub_mat(NC,NC+NC*2,q.Cs_iM_CtT);
_UL.set_sub_mat(NC+NC,NC+NC*3,q.Cs_iM_CtT);
_UL.set_sub_mat(NC+NC*2,NC+NC*2,q.Ct_iM_CtT);
_UL.set_sub_mat(NC+NC*3,NC+NC*3,q.Ct_iM_CtT);
// Joint Limits
_UL.set_sub_mat(N_FRICT,N_FRICT,q.L_iM_LT);
_UL.set_sub_mat(N_FRICT,0,L_iM_CnT);
_UL.set_sub_mat(0,N_FRICT,q.Cn_iM_LT);
_UL.set_sub_mat(NC,N_FRICT,q.Cs_iM_LT);
_UL.set_sub_mat(NC+NC*2,N_FRICT,q.Ct_iM_LT);
_UL.set_sub_mat(N_FRICT,NC,L_iM_CsT);
_UL.set_sub_mat(N_FRICT,NC+NC*2,L_iM_CtT);
// Set negative submatrices
/* n r r r r
n -Cn_iM_CsT -Cn_iM_CtT
r -Cs_iM_CsT -Cs_iM_CtT
r -Cs_iM_CnT -Cs_iM_CsT -Cs_iM_CtT
r -Ct_iM_CsT -Ct_iM_CtT
r -Ct_iM_CnT -Ct_iM_CsT -Ct_iM_CtT
*/
q.Cn_iM_CsT.negate();
q.Cn_iM_CtT.negate();
Cs_iM_CnT.negate();
q.Cs_iM_CsT.negate();
q.Cs_iM_CtT.negate();
Ct_iM_CnT.negate();
Ct_iM_CsT.negate();
q.Ct_iM_CtT.negate();
q.Cs_iM_LT.negate();
q.Ct_iM_LT.negate();
L_iM_CsT.negate();
L_iM_CtT.negate();
_UL.set_sub_mat(NC+NC,0,Cs_iM_CnT);
_UL.set_sub_mat(0,NC+NC,q.Cn_iM_CsT);
_UL.set_sub_mat(NC,NC+NC,q.Cs_iM_CsT);
_UL.set_sub_mat(NC+NC,NC,q.Cs_iM_CsT);
_UL.set_sub_mat(NC+NC*3,0,Ct_iM_CnT);
_UL.set_sub_mat(0,NC+NC*3,q.Cn_iM_CtT);
_UL.set_sub_mat(NC+NC*3,NC,Ct_iM_CsT);
_UL.set_sub_mat(NC+NC*2,NC+NC,Ct_iM_CsT);
_UL.set_sub_mat(NC+NC,NC+NC*2,q.Cs_iM_CtT);
_UL.set_sub_mat(NC,NC+NC*3,q.Cs_iM_CtT);
_UL.set_sub_mat(NC+NC*2,NC+NC*3,q.Ct_iM_CtT);
_UL.set_sub_mat(NC+NC*3,NC+NC*2,q.Ct_iM_CtT);
// Joint limits
_UL.set_sub_mat(NC+NC,N_FRICT,q.Cs_iM_LT);
_UL.set_sub_mat(NC+NC*3,N_FRICT,q.Ct_iM_LT);
_UL.set_sub_mat(N_FRICT,NC+NC,L_iM_CsT);
_UL.set_sub_mat(N_FRICT,NC+NC*3,L_iM_CtT);
// lower left & upper right block of matrix
for(unsigned i=0,j=0,r=0;i<NC;i++)
{
const UnilateralConstraint* ci = q.contact_constraints[i];
if (ci->contact_NK > 4)
{
int nk4 = ( ci->contact_NK+4)/4;
for(unsigned k=0;k<nk4;k++)
{
FILE_LOG(LOG_CONSTRAINT) << "mu_{"<< k<< ","<< i <<"}: " << ci->contact_mu_coulomb << std::endl;
// muK
_LL(r+k,i) = ci->contact_mu_coulomb;
// Xe
_LL(r+k,NC+j) = -cos((M_PI*k)/(2.0*nk4));
_LL(r+k,NC+NC+j) = -cos((M_PI*k)/(2.0*nk4));
// Xf
_LL(r+k,NC+NC*2+j) = -sin((M_PI*k)/(2.0*nk4));
_LL(r+k,NC+NC*3+j) = -sin((M_PI*k)/(2.0*nk4));
// XeT
_UR(NC+j,r+k) = cos((M_PI*k)/(2.0*nk4));
_UR(NC+NC+j,r+k) = cos((M_PI*k)/(2.0*nk4));
// XfT
_UR(NC+NC*2+j,r+k) = sin((M_PI*k)/(2.0*nk4));
_UR(NC+NC*3+j,r+k) = sin((M_PI*k)/(2.0*nk4));
}
r+=nk4;
j++;
}
else
{
FILE_LOG(LOG_CONSTRAINT) << "mu_{"<< i <<"}: " << ci->contact_mu_coulomb << std::endl;
// muK
_LL(r,i) = ci->contact_mu_coulomb;
// Xe
_LL(r,NC+j) = -1.0;
_LL(r,NC+NC+j) = -1.0;
// Xf
_LL(r,NC+NC*2+j) = -1.0;
_LL(r,NC+NC*3+j) = -1.0;
// XeT
_UR(NC+j,r) = 1.0;
_UR(NC+NC+j,r) = 1.0;
// XfT
_UR(NC+NC*2+j,r) = 1.0;
_UR(NC+NC*3+j,r) = 1.0;
r += 1;
j++;
}
}
// setup the LCP matrix
_MM.set_sub_mat(0, _UL.columns(), _UR);
_MM.set_sub_mat(_UL.rows(), 0, _LL);
// setup the LCP vector
_qq.set_sub_vec(NC,q.Cs_v);
_qq.set_sub_vec(NC+NC*2,q.Ct_v);
q.Cs_v.negate();
q.Ct_v.negate();
_qq.set_sub_vec(NC+NC,q.Cs_v);
_qq.set_sub_vec(NC+NC*3,q.Ct_v);
_qq.set_sub_vec(N_FRICT,q.L_v);
_MM.set_sub_mat(0, 0, _UL);
FILE_LOG(LOG_CONSTRAINT) << " LCP matrix: " << std::endl << _MM;
FILE_LOG(LOG_CONSTRAINT) << " LCP vector: " << _qq << std::endl;
// Fix Negations
q.Cn_iM_CsT.negate();
q.Cn_iM_CtT.negate();
// Cs_iM_CnT.negate();
q.Cs_iM_CsT.negate();
q.Cs_iM_CtT.negate();
// Ct_iM_CnT.negate();
// Ct_iM_CsT.negate();
q.Ct_iM_CtT.negate();
q.Cs_iM_LT.negate();
q.Ct_iM_LT.negate();
//L_iM_CsT.negate();
//L_iM_CtT.negate();
q.Cs_v.negate();
q.Ct_v.negate();
// solve the LCP
VectorNd z;
if (!_lcp.lcp_lemke_regularized(_MM, _qq, z, -20, 1, -2))
throw std::exception();
for(unsigned i=0,j=0;i<NC;i++)
{
q.cn[i] = z[i];
q.cs[i] = z[NC+j] - z[NC+NC+j];
q.ct[i] = z[NC+NC*2+j] - z[NC+NC*3+j];
j++;
}
q.l = z.segment(N_FRICT,N_FRICT+N_LIMIT);
// setup a temporary frame
shared_ptr<Pose3d> P(new Pose3d);
// propagate the impulse data
propagate_impulse_data(q);
if (LOGGING(LOG_CONSTRAINT))
{
// compute LCP 'w' vector
VectorNd w;
_MM.mult(z, w) += _qq;
// output new acceleration
FILE_LOG(LOG_CONSTRAINT) << "new normal v: " << w.segment(0, q.contact_constraints.size()) << std::endl;
}
FILE_LOG(LOG_CONSTRAINT) << "cn " << q.cn << std::endl;
FILE_LOG(LOG_CONSTRAINT) << "cs " << q.cs << std::endl;
FILE_LOG(LOG_CONSTRAINT) << "ct " << q.ct << std::endl;
FILE_LOG(LOG_CONSTRAINT) << "l " << q.l << std::endl;
FILE_LOG(LOG_CONSTRAINT) << " LCP result : " << z << std::endl;
FILE_LOG(LOG_CONSTRAINT) << "ImpactConstraintHandler::apply_ap_model() exited" << std::endl;
}
/**
* \param M the generalized inertia matrix of the humanoid
* \param vminus the generalized velocity of the humanoid at the current time
* \param fext the generalized external force vector of the humanoid at the
* current time
* \param N the generalized normal contact wrench for the humanoid
* \param D the generalized tangent contact wrench for the humanoid
* \param R the generalized constraint wrench for the humanoid
* \param J the generalized constraint wrench for the robot
* \param lolimit the lower (negative) actuator limits imposed on the robot
* \param hilimit the upper (positive) actuator limits imposed on the robot
* \param fr contains, on return, the force that the robot should apply to the human at the point of contact
*/
void controller(const MatrixNd& M, const VectorNd& v, const VectorNd& fext, const MatrixNd& N, const MatrixNd& D, const MatrixNd& R, const MatrixNd& J, const VectorNd& lolimit, const VectorNd& hilimit, VectorNd& fr)
{
const double INF = std::numeric_limits<double>::max();
const double DT = 1e-3; // default value for delta t
// setup variable sizes
const unsigned N_ACT = J.columns(); // number of actuators robot commands
const unsigned N_CONTACTS = N.rows();
const unsigned N_SIZE = N_CONTACTS;
const unsigned D_SIZE = N_CONTACTS*2; // two tangent directions
const unsigned R_SIZE = R.rows(); // 6 x X robot/humanoid constraints
// setup indices for variables
const unsigned R_INDEX = 0;
const unsigned D_INDEX = R_INDEX + R_SIZE;
const unsigned N_INDEX = D_INDEX + D_SIZE;
// setup the number of QP variables
const unsigned NVARS = N_SIZE + R_SIZE + D_SIZE;
// do a Cholesky factorization on M so that we can solve with it quickly
MatrixNd cholM = M;
if (!LinAlgd::factor_chol(cholM))
throw std::runtime_error("Unexpectedly unable to factorize generalized inertia matrix!");
// compute k = v^- + inv(M)*fext*DT
VectorNd k = fext;
k *= DT;
LinAlgd::solve_chol_fast(cholM, k);
k += v;
// compute matrices
MatrixNd workM, RiMRT, RiMDT, RiMNT, DiMRT, DiMDT, DiMNT, NiMRT, NiMDT, NiMNT;
// first compute inv(M)*R'
MatrixNd::transpose(R, workM);
LinAlgd::solve_chol_fast(cholM, workM);
// now compute R*inv(M)*R', D*inv(M)*R' (this is identical to R*inv(M)*D'),
// and N*inv(M)*R' (this is identical to R*inv(M)*N')
R.mult(workM, RiMRT);
D.mult(workM, DiMRT);
N.mult(workM, NiMRT);
MatrixNd::transpose(DiMRT, RiMDT);
MatrixNd::transpose(NiMRT, RiMNT);
// now compute inv(M)*D'
MatrixNd::transpose(D, workM);
LinAlgd::solve_chol_fast(cholM, workM);
// now compute D*inv(M)*D' and N*inv(M)*D' (this is identical to D*inv(M)*N')
D.mult(workM, DiMDT);
N.mult(workM, NiMDT);
MatrixNd::transpose(NiMDT, DiMNT);
// finally, compute N*inv(M)*N'
MatrixNd::transpose(N, workM);
LinAlgd::solve_chol_fast(cholM, workM);
N.mult(workM, NiMNT);
// setup the G matrix
MatrixNd G(NVARS, NVARS);
G.block(R_INDEX, R_INDEX+R_SIZE, R_INDEX, R_INDEX+R_SIZE) = RiMRT;
G.block(R_INDEX, R_INDEX+R_SIZE, D_INDEX, D_INDEX+D_SIZE) = RiMDT;
G.block(R_INDEX, R_INDEX+R_SIZE, N_INDEX, N_INDEX+N_SIZE) = RiMNT;
G.block(D_INDEX, D_INDEX+D_SIZE, R_INDEX, R_INDEX+R_SIZE) = DiMRT;
G.block(D_INDEX, D_INDEX+D_SIZE, D_INDEX, D_INDEX+D_SIZE) = DiMDT;
G.block(D_INDEX, D_INDEX+D_SIZE, N_INDEX, N_INDEX+N_SIZE) = DiMNT;
G.block(N_INDEX, N_INDEX+N_SIZE, R_INDEX, R_INDEX+R_SIZE) = NiMRT;
G.block(N_INDEX, N_INDEX+N_SIZE, D_INDEX, D_INDEX+D_SIZE) = NiMDT;
G.block(N_INDEX, N_INDEX+N_SIZE, N_INDEX, N_INDEX+N_SIZE) = NiMNT;
// setup the c vector
VectorNd workv;
VectorNd c(NVARS);
c.segment(R_INDEX, R_INDEX+R_SIZE) = R.mult(k, workv);
c.segment(D_INDEX, D_INDEX+D_SIZE) = R.mult(k, workv);
c.segment(N_INDEX, N_INDEX+N_SIZE) = R.mult(k, workv);
// setup the inequality constraint matrix (P)
MatrixNd JT;
MatrixNd::transpose(J, JT);
MatrixNd P(N_CONTACTS+N_ACT*2,NVARS);
P.block(0, N_CONTACTS, R_INDEX, R_INDEX+R_SIZE) = RiMRT;
P.block(0, N_CONTACTS, R_INDEX, D_INDEX+D_SIZE) = RiMDT;
P.block(0, N_CONTACTS, R_INDEX, N_INDEX+N_SIZE) = RiMNT;
P.block(N_CONTACTS, P.rows(), 0, NVARS).set_zero();
P.block(N_CONTACTS, N_CONTACTS+J.columns(), R_INDEX, R_INDEX+R_SIZE) = JT;
P.block(N_CONTACTS+J.columns(), P.rows(), R_INDEX, R_INDEX+R_SIZE) = JT;
P.block(N_CONTACTS+J.columns(), P.rows(), R_INDEX, R_INDEX+R_SIZE).negate();
// setup the inequality constraint vector (p)
VectorNd p(N_CONTACTS+N_ACT*2);
p.segment(0, N_CONTACTS) = c.segment(N_INDEX, N_INDEX+N_SIZE);
p.segment(N_CONTACTS, J.columns()) = lolimit;
p.segment(N_CONTACTS+J.columns(), N_CONTACTS+J.columns()*2) = hilimit;
p.segment(N_CONTACTS+J.columns(), N_CONTACTS+J.columns()*2).negate();
// setup the equality constraint matrix (Q)
const MatrixNd& Q = D;
// setup the equality constraint vector (q)
VectorNd q(D.rows());
q.set_zero();
// setup bounds on variables -- only N variables have lower bounds
VectorNd lb(NVARS), ub(NVARS);
lb.set_one() *= -INF;
ub.set_one() *= INF;
lb.segment(N_INDEX, N_INDEX+N_SIZE).set_zero();
// solve the QP using QLCPD
Opt::QPActiveSet qp;
VectorNd z;
bool success = qp.qp_activeset(G, c, lb, ub, P, p, Q, q, z);
// verify that we have success
if (!success)
throw std::runtime_error("Unexpected QP solver failure!");
else
fr = z.segment(R_INDEX, R_INDEX+R_SIZE);
}
int main(int argc, char const *argv[])
{
// Small test case for model wrapper
cout << "Leo Model Test Case" << endl;
// Initialize model from lua file
cout << "Initializing model ..." << endl;
LeoModel leo;
leo.loadModelFromFile (model_path.c_str());
cout << "... successful!" << endl << endl;
// Load contact points
cout << "Loading points ..." << endl;
leo.loadPointsFromFile (model_path.c_str(), true);
cout << "... successful!" << endl << endl;
// Load constraint sets
cout << "Loading constraint sets ..." << endl;
leo.loadConstraintSetsFromFile (model_path.c_str(), true);
cout << "... successful!" << endl;
VectorNd q = VectorNd::Zero(leo.nDof);
VectorNd qdot = VectorNd::Zero(leo.nDof);
VectorNd qddot = VectorNd::Zero(leo.nDof);
VectorNd tau = VectorNd::Zero(leo.nDof);
VectorNd rhs = VectorNd::Zero(2*leo.nDof);
// provide initial values
q <<
0.05,
-0.1,
0.1,
0.0;
// 0.1,
// -0.1,
// 0.05;
// assign them to model
leo.q = q;
leo.qdot = qdot;
leo.qddot = qddot;
leo.activeConstraintSet = "";
cout << "Computing inverse dynamics ... " << endl;
cout << " q: " << leo.q.transpose() << endl;
cout << " qdot: " << leo.qdot.transpose() << endl;
cout << "for desired accelerations:" << endl;
cout << " qddot: " << leo.qddot.transpose() << endl;
leo.updateInverseDynamics();
cout << "needed torques:" << endl;
cout << " taus: " << leo.tau.transpose() << endl;
cout << "... successful!" << endl;
cout << "Checking forward dynamics ... " << endl;
cout << " q: " << leo.q.transpose() << endl;
cout << " qdot: " << leo.qdot.transpose() << endl;
cout << "for given torques:" << endl;
cout << " taus: " << leo.tau.transpose() << endl;
leo.updateForwardDynamics();
cout << "resulting accelerations:" << endl;
cout << " qddot: " << leo.qddot.transpose() << endl;
leo.calcForwardDynamicsRhs(rhs.data());
cout << "resulting accelerations:" << endl;
cout << " rhs: " << rhs.transpose() << endl;
cout << "... successful!" << endl;
cout << endl;
// provide initial values
leo.tau <<
0.1,
0.1,
0.1,
0.1;
// 0.1,
// -0.1,
// 0.05;
cout << "Checking forward dynamics ... " << endl;
cout << " q: " << leo.q.transpose() << endl;
cout << " qdot: " << leo.qdot.transpose() << endl;
cout << "for given torques:" << endl;
cout << " taus: " << leo.tau.transpose() << endl;
leo.updateForwardDynamics();
cout << "resulting accelerations:" << endl;
cout << " qddot: " << leo.qddot.transpose() << endl;
cout << endl;
leo.calcForwardDynamicsRhs(rhs.data());
cout << "resulting accelerations:" << endl;
cout << " rhs: " << rhs.transpose() << endl;
cout << "... successful!" << endl;
cout << endl;
return 0;
// // calculate mass matrix
// MatrixNd H = leo.calcMassMatrix();
// cout << "Computing mass matrix ... " << endl;
// cout << H << endl;
// cout << "... successful!" << endl;
// // calculate constraint Jacobian for given constraint set
// MatrixNd G = leo.calcContactJacobian();
// cout << "Computing contact Jacobian ... " << endl;
// cout << G << endl;
// cout << G.transpose() << endl;
// cout << "... successful!" << endl;
// return 0;
}
// calculates naive observation of the first s components of X, storing the
// result in out. For RoboCup, this will correspond to the x and y of the ball
void RbpfModelRolling::observationModel(const VectorNd& X,
VectorSd& out) const {
out = X.head(2);
}
