/// 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);

//.........这里部分代码省略.........

/**
 * \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;
//.........这里部分代码省略.........

/// 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);
//.........这里部分代码省略.........

/**
 * \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();
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

// 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);
}