void stateCallback(const nav_msgs::Odometry::ConstPtr& state) { vel[0] = state->twist.twist.linear.x; vel[1] = state->twist.twist.linear.y; vel[2] = state->twist.twist.linear.z; pos[2] = state->pose.pose.position.z; //q.x() = state->pose.pose.orientation.x; //q.y() = state->pose.pose.orientation.y; //q.z() = state->pose.pose.orientation.z; //q.w() = state->pose.pose.orientation.w; float q_x = state->pose.pose.orientation.x; float q_y = state->pose.pose.orientation.y; float q_z = state->pose.pose.orientation.z; float q_w = state->pose.pose.orientation.w; float yaw = atan2(2*(q_w*q_z+q_x*q_y),1-2*(q_y*q_y+q_z*q_z)); //Eigen::Matrix3d R(q); force(0) = offset_x+k_vxy*(vel_des(0)-vel(0))+m*acc_des(0); if(pd_control==0) force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+m*acc_des(1); else if(pd_control==1) { pos[1]=state->pose.pose.position.y; force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+k_xy*(0-pos[1])+m*acc_des(1); } force(2) = (k_z*(pos_des(2)-pos(2))+k_vz*(vel_des(2)-vel(2))+m*g(2)+m*acc_des(2)); b3 = force.normalized(); b2 = b3.cross(b1w); b1 = b2.cross(b3); R_des<<b1[0],b2[0],b3[0], b1[1],b2[1],b3[1], b1[2],b2[2],b3[2]; Eigen::Quaternionf q_des(R_des); quadrotor_msgs::SO3Command command; command.force.x = force[0]; command.force.y = force[1]; command.force.z = force[2]; command.orientation.x = q_des.x(); command.orientation.y = q_des.y(); command.orientation.z = q_des.z(); command.orientation.w = q_des.w(); command.kR[0] = k_R; command.kR[1] = k_R; command.kR[2] = k_R; command.kOm[0] = k_omg; command.kOm[1] = k_omg; command.kOm[2] = k_omg; command.aux.current_yaw = yaw; command.aux.enable_motors = true; command.aux.use_external_yaw = true; control_pub.publish(command); }
void JointTest::doUpdate(double time, wocra::wOcraModel& state, void** args) { wocra::wOcraFullPostureTaskManager* tmp_tmFull = dynamic_cast<wocra::wOcraFullPostureTaskManager*>(taskManagers["tmFull"]); Eigen::VectorXd taskErrorVector = tmp_tmFull->getTaskError(); // std::cout << taskErrorVector.transpose() << std::endl; taskErr = abs(taskErrorVector(jIndex)); // std::cout << taskErr << std::endl; if (( counter>=400) && jIndex<nDoF){ if ((goToMin == true) && (goToMax==false)){ q_des(jIndex) = jointMin(jIndex); tmp_tmFull->setPosture(q_des); goToMin = false; goToMax = true; std::cout << "\nJoint: " << jointNames[jIndex] << "-> moving to lower limit, " << jointMin(jIndex) << std::endl; } else if ((goToMin == false )&& (goToMax==true)){ q_des(jIndex) = jointMax(jIndex); tmp_tmFull->setPosture(q_des); goToMax = false; std::cout << "\nJoint: " << jointNames[jIndex] << "-> moving to upper limit, " << jointMax(jIndex) << std::endl; } else if ((goToMin == false) && (goToMax==false)){ q_des = q_init; tmp_tmFull->setPosture(q_des); jIndex++; goToMin = true; } counter = 0; } counter ++ ; }
void kinematic_constraints::OrientationConstraint::print(std::ostream &out) const { if (link_model_) { out << "Orientation constraint on link '" << link_model_->getName() << "'" << std::endl; Eigen::Quaterniond q_des(desired_rotation_matrix_); out << "Desired orientation:" << q_des.x() << "," << q_des.y() << "," << q_des.z() << "," << q_des.w() << std::endl; } else out << "No constraint" << std::endl; }
kinematic_constraints::ConstraintEvaluationResult kinematic_constraints::OrientationConstraint::decide(const robot_state::RobotState &state, bool verbose) const { if (!link_model_) return ConstraintEvaluationResult(true, 0.0); const robot_state::LinkState *link_state = state.getLinkState(link_model_->getName()); if (!link_state) { logWarn("No link in state with name '%s'", link_model_->getName().c_str()); return ConstraintEvaluationResult(false, 0.0); } Eigen::Vector3d xyz; if (mobile_frame_) { Eigen::Matrix3d tmp; tf_->transformRotationMatrix(state, desired_rotation_frame_id_, desired_rotation_matrix_, tmp); Eigen::Affine3d diff(tmp.inverse() * link_state->getGlobalLinkTransform().rotation()); xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints } else { Eigen::Affine3d diff(desired_rotation_matrix_inv_ * link_state->getGlobalLinkTransform().rotation()); xyz = diff.rotation().eulerAngles(0, 1, 2); // 0,1,2 corresponds to ZXZ, the convention used in sampling constraints } xyz(0) = std::min(fabs(xyz(0)), boost::math::constants::pi<double>() - fabs(xyz(0))); xyz(1) = std::min(fabs(xyz(1)), boost::math::constants::pi<double>() - fabs(xyz(1))); xyz(2) = std::min(fabs(xyz(2)), boost::math::constants::pi<double>() - fabs(xyz(2))); bool result = xyz(2) < absolute_z_axis_tolerance_+std::numeric_limits<double>::epsilon() && xyz(1) < absolute_y_axis_tolerance_+std::numeric_limits<double>::epsilon() && xyz(0) < absolute_x_axis_tolerance_+std::numeric_limits<double>::epsilon(); if (verbose) { Eigen::Quaterniond q_act(link_state->getGlobalLinkTransform().rotation()); Eigen::Quaterniond q_des(desired_rotation_matrix_); logInform("Orientation constraint %s for link '%s'. Quaternion desired: %f %f %f %f, quaternion actual: %f %f %f %f, error: x=%f, y=%f, z=%f, tolerance: x=%f, y=%f, z=%f", result ? "satisfied" : "violated", link_model_->getName().c_str(), q_des.x(), q_des.y(), q_des.z(), q_des.w(), q_act.x(), q_act.y(), q_act.z(), q_act.w(), xyz(0), xyz(1), xyz(2), absolute_x_axis_tolerance_, absolute_y_axis_tolerance_, absolute_z_axis_tolerance_); } return ConstraintEvaluationResult(result, constraint_weight_ * (xyz(0) + xyz(1) + xyz(2))); }
void stateCallback(const nav_msgs::Odometry::ConstPtr& state) { if(mode==1){ vel_des[0]=rc_vx; vel_des[1]=rc_vy; vel_des[2]=0; pos_des[1]=0; pos_des[2]=rc_z; } else if(mode==2) { double cur_t = ros::Time::now().toSec(); double t = cur_t-init_t; vel_des[2]=0; if(t<t1){ vel_des[0]=ax*t; vel_des[1]=ay*t; acc_des[0]=ax; acc_des[1]=ay; acc_des[2]=0; } else if(t<des_t-t1&&t>=t1){ vel_des[0]=des_vx; vel_des[1]=des_vy; acc_des = Eigen::Vector3f::Zero(); } else if(t>=des_t-t1&&t<des_t){ vel_des[0] = des_vx-ax*(t+t1-des_t); vel_des[1] = des_vy-ay*(t+t1-des_t); acc_des[0] = -ax; acc_des[1] = -ay; acc_des[2] = 0; } else if(t>=des_t){ vel_des = Eigen::Vector3f::Zero(); acc_des = Eigen::Vector3f::Zero(); } vel_des[0] = vel_des[0]+rc_vx/2; vel_des[1] = vel_des[1]+rc_vy/2; } else if(mode==3) { double cur_t = ros::Time::now().toSec(); double t = cur_t-init_t; vel_des[2]=0; if(t<t1){ vel_des[0]=ax*t; vel_des[1]=ay*t; acc_des[0]=ax; acc_des[1]=ay; acc_des[2]=0; } else if(t<des_t-t3&&t>=t1){ vel_des[0]=des_vx; vel_des[1]=des_vy; acc_des = Eigen::Vector3f::Zero(); } else if(t>=des_t-t3&&t<des_t){ vel_des[0] = 0; vel_des[1] = 0; acc_des[0] = -ax3; acc_des[1] = -ay3; acc_des[2] = 0; stop = 1; } else if(t>=des_t){ stop = 0; vel_des = Eigen::Vector3f::Zero(); acc_des = Eigen::Vector3f::Zero(); } vel_des[0] = vel_des[0]+rc_vx/2; vel_des[1] = vel_des[1]+rc_vy/2; } vel(0) = state->twist.twist.linear.x; vel(1) = state->twist.twist.linear.y; vel(2) = state->twist.twist.linear.z; pos(2) = state->pose.pose.position.z; //q.x() = state->pose.pose.orientation.x; //q.y() = state->pose.pose.orientation.y; //q.z() = state->pose.pose.orientation.z; //q.w() = state->pose.pose.orientation.w; float q_x = state->pose.pose.orientation.x; float q_y = state->pose.pose.orientation.y; float q_z = state->pose.pose.orientation.z; float q_w = state->pose.pose.orientation.w; float yaw = atan2(2*(q_w*q_z+q_x*q_y),1-2*(q_y*q_y+q_z*q_z)); if(stop==0){ force(0) = offset_x+k_vxy*(vel_des(0)-vel(0))+m*acc_des(0); if(pd_control==0) force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+m*acc_des(1); else if(pd_control==1) { pos(1)=state->pose.pose.position.y; force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+k_xy*(0-pos(1))+m*acc_des(1); } force(2) = (k_z*(pos_des(2)-pos(2))+k_vz*(vel_des(2)-vel(2))+m*g(2)+m*acc_des(2)); } else if(stop==1) { force(0) = offset_x+m*acc_des(0); force(1) = offset_y+m*acc_des(1); } b3 = force.normalized(); b2 = b3.cross(b1w); b1 = b2.cross(b3); R_des<<b1[0],b2[0],b3[0], b1[1],b2[1],b3[1], b1[2],b2[2],b3[2]; Eigen::Quaternionf q_des(R_des); quadrotor_msgs::SO3Command command; command.force.x = force[0]; command.force.y = force[1]; command.force.z = force[2]; command.orientation.x = q_des.x(); command.orientation.y = q_des.y(); command.orientation.z = q_des.z(); command.orientation.w = q_des.w(); command.kR[0] = k_R; command.kR[1] = k_R; command.kR[2] = k_R; command.kOm[0] = k_omg; command.kOm[1] = k_omg; command.kOm[2] = k_omg; command.aux.current_yaw = yaw; command.aux.enable_motors = true; command.aux.use_external_yaw = true; control_pub.publish(command); opticalflow_msgs::Traj traj; traj.x = pos_des[0]; traj.y = pos_des[1]; traj.z = pos_des[2]; traj.vx = vel_des[0]; traj.vy = vel_des[1]; traj.vz = vel_des[2]; traj.acc_x = acc_des[0]; traj.acc_y = acc_des[1]; traj.acc_z = acc_des[2]; traj.stop = 0; traj.mode = mode; traj.use_sensor = true; traj_pub.publish(traj); }
int setupAndSolveQP(NewQPControllerData *pdata, std::shared_ptr<drake::lcmt_qp_controller_input> qp_input, double t, Map<VectorXd> &q, Map<VectorXd> &qd, const Ref<Matrix<bool, Dynamic, 1>> &b_contact_force, QPControllerOutput *qp_output, std::shared_ptr<QPControllerDebugData> debug) { // The primary solve loop for our controller. This constructs and solves a Quadratic Program and produces the instantaneous desired torques, along with reference positions, velocities, and accelerations. It mirrors the Matlab implementation in atlasControllers.InstantaneousQPController.setupAndSolveQP(), and more documentation can be found there. // Note: argument `debug` MAY be set to NULL, which signals that no debug information is requested. // look up the param set by name AtlasParams *params; std::map<string,AtlasParams>::iterator it; it = pdata->param_sets.find(qp_input->param_set_name); if (it == pdata->param_sets.end()) { mexWarnMsgTxt("Got a param set I don't recognize! Using standing params instead"); it = pdata->param_sets.find("standing"); if (it == pdata->param_sets.end()) { mexErrMsgTxt("Could not fall back to standing parameters either. I have to give up here."); } } // cout << "using params set: " + it->first + ", "; params = &(it->second); // mexPrintf("Kp_accel: %f, ", params->Kp_accel); int nu = pdata->B.cols(); int nq = pdata->r->num_positions; // zmp_data Map<Matrix<double, 4, 4, RowMajor>> A_ls(&qp_input->zmp_data.A[0][0]); Map<Matrix<double, 4, 2, RowMajor>> B_ls(&qp_input->zmp_data.B[0][0]); Map<Matrix<double, 2, 4, RowMajor>> C_ls(&qp_input->zmp_data.C[0][0]); Map<Matrix<double, 2, 2, RowMajor>> D_ls(&qp_input->zmp_data.D[0][0]); Map<Matrix<double, 4, 1>> x0(&qp_input->zmp_data.x0[0][0]); Map<Matrix<double, 2, 1>> y0(&qp_input->zmp_data.y0[0][0]); Map<Matrix<double, 2, 1>> u0(&qp_input->zmp_data.u0[0][0]); Map<Matrix<double, 2, 2, RowMajor>> R_ls(&qp_input->zmp_data.R[0][0]); Map<Matrix<double, 2, 2, RowMajor>> Qy(&qp_input->zmp_data.Qy[0][0]); Map<Matrix<double, 4, 4, RowMajor>> S(&qp_input->zmp_data.S[0][0]); Map<Matrix<double, 4, 1>> s1(&qp_input->zmp_data.s1[0][0]); Map<Matrix<double, 4, 1>> s1dot(&qp_input->zmp_data.s1dot[0][0]); // // whole_body_data if (qp_input->whole_body_data.num_positions != nq) mexErrMsgTxt("number of positions doesn't match num_dof for this robot"); Map<VectorXd> q_des(qp_input->whole_body_data.q_des.data(), nq); Map<VectorXd> condof(qp_input->whole_body_data.constrained_dofs.data(), qp_input->whole_body_data.num_constrained_dofs); PIDOutput pid_out = wholeBodyPID(pdata, t, q, qd, q_des, ¶ms->whole_body); qp_output->q_ref = pid_out.q_ref; // mu // NOTE: we're using the same mu for all supports double mu; if (qp_input->num_support_data == 0) { mu = 1.0; } else { mu = qp_input->support_data[0].mu; for (int i=1; i < qp_input->num_support_data; i++) { if (qp_input->support_data[i].mu != mu) { mexWarnMsgTxt("Currently, we assume that all supports have the same value of mu"); } } } const int dim = 3, // 3D nd = 2*m_surface_tangents; // for friction cone approx, hard coded for now assert(nu+6 == nq); vector<DesiredBodyAcceleration> desired_body_accelerations; desired_body_accelerations.resize(qp_input->num_tracked_bodies); Vector6d body_pose_des, body_v_des, body_vdot_des; Vector6d body_vdot; for (int i=0; i < qp_input->num_tracked_bodies; i++) { int body_id0 = qp_input->body_motion_data[i].body_id - 1; double weight = params->body_motion[body_id0].weight; desired_body_accelerations[i].body_id0 = body_id0; Map<Matrix<double, 6, 4,RowMajor>>coefs_rowmaj(&qp_input->body_motion_data[i].coefs[0][0]); Matrix<double, 6, 4> coefs = coefs_rowmaj; evaluateCubicSplineSegment(t - qp_input->body_motion_data[i].ts[0], coefs, body_pose_des, body_v_des, body_vdot_des); desired_body_accelerations[i].body_vdot = bodyMotionPD(pdata->r, q, qd, body_id0, body_pose_des, body_v_des, body_vdot_des, params->body_motion[body_id0].Kp, params->body_motion[body_id0].Kd); desired_body_accelerations[i].weight = weight; desired_body_accelerations[i].accel_bounds = params->body_motion[body_id0].accel_bounds; // mexPrintf("body: %d, vdot: %f %f %f %f %f %f weight: %f\n", body_id0, // desired_body_accelerations[i].body_vdot(0), // desired_body_accelerations[i].body_vdot(1), // desired_body_accelerations[i].body_vdot(2), // desired_body_accelerations[i].body_vdot(3), // desired_body_accelerations[i].body_vdot(4), // desired_body_accelerations[i].body_vdot(5), // weight); // mexPrintf("tracking body: %d, coefs[:,0]: %f %f %f %f %f %f coefs(", body_id0, } int n_body_accel_eq_constraints = 0; for (int i=0; i < desired_body_accelerations.size(); i++) { if (desired_body_accelerations[i].weight < 0) n_body_accel_eq_constraints++; } MatrixXd R_DQyD_ls = R_ls + D_ls.transpose()*Qy*D_ls; pdata->r->doKinematics(q,false,qd); //--------------------------------------------------------------------- vector<SupportStateElement> available_supports = loadAvailableSupports(qp_input); vector<SupportStateElement> active_supports = getActiveSupports(pdata->r, pdata->map_ptr, q, qd, available_supports, b_contact_force, params->contact_threshold, pdata->default_terrain_height); int num_active_contact_pts=0; for (vector<SupportStateElement>::iterator iter = active_supports.begin(); iter!=active_supports.end(); iter++) { num_active_contact_pts += iter->contact_pts.size(); } pdata->r->HandC(q,qd,(MatrixXd*)nullptr,pdata->H,pdata->C,(MatrixXd*)nullptr,(MatrixXd*)nullptr,(MatrixXd*)nullptr); pdata->H_float = pdata->H.topRows(6); pdata->H_act = pdata->H.bottomRows(nu); pdata->C_float = pdata->C.head(6); pdata->C_act = pdata->C.tail(nu); bool include_angular_momentum = (params->W_kdot.array().maxCoeff() > 1e-10); if (include_angular_momentum) { pdata->r->getCMM(q,qd,pdata->Ag,pdata->Agdot); pdata->Ak = pdata->Ag.topRows(3); pdata->Akdot = pdata->Agdot.topRows(3); } Vector3d xcom; // consider making all J's into row-major pdata->r->getCOM(xcom); pdata->r->getCOMJac(pdata->J); pdata->r->getCOMJacDot(pdata->Jdot); pdata->J_xy = pdata->J.topRows(2); pdata->Jdot_xy = pdata->Jdot.topRows(2); MatrixXd Jcom,Jcomdot; if (x0.size()==6) { Jcom = pdata->J; Jcomdot = pdata->Jdot; } else { Jcom = pdata->J_xy; Jcomdot = pdata->Jdot_xy; } MatrixXd B,JB,Jp,Jpdot,normals; int nc = contactConstraintsBV(pdata->r,num_active_contact_pts,mu,active_supports,pdata->map_ptr,B,JB,Jp,Jpdot,normals,pdata->default_terrain_height); int neps = nc*dim; VectorXd x_bar,xlimp; MatrixXd D_float(6,JB.cols()), D_act(nu,JB.cols()); if (nc>0) { if (x0.size()==6) { // x,y,z com xlimp.resize(6); xlimp.topRows(3) = xcom; xlimp.bottomRows(3) = Jcom*qd; } else { xlimp.resize(4); xlimp.topRows(2) = xcom.topRows(2); xlimp.bottomRows(2) = Jcom*qd; } x_bar = xlimp-x0; D_float = JB.topRows(6); D_act = JB.bottomRows(nu); } int nf = nc*nd; // number of contact force variables int nparams = nq+nf+neps; Vector3d kdot_des; if (include_angular_momentum) { VectorXd k = pdata->Ak*qd; kdot_des = -params->Kp_ang*k; // TODO: parameterize } //---------------------------------------------------------------------- // QP cost function ---------------------------------------------------- // // min: ybar*Qy*ybar + ubar*R*ubar + (2*S*xbar + s1)*(A*x + B*u) + // w_qdd*quad(qddot_ref - qdd) + w_eps*quad(epsilon) + // w_grf*quad(beta) + quad(kdot_des - (A*qdd + Adot*qd)) VectorXd f(nparams); { if (nc > 0) { // NOTE: moved Hqp calcs below, because I compute the inverse directly for FastQP (and sparse Hqp for gurobi) VectorXd tmp = C_ls*xlimp; VectorXd tmp1 = Jcomdot*qd; MatrixXd tmp2 = R_DQyD_ls*Jcom; pdata->fqp = tmp.transpose()*Qy*D_ls*Jcom; // mexPrintf("fqp head: %f %f %f\n", pdata->fqp(0), pdata->fqp(1), pdata->fqp(2)); pdata->fqp += tmp1.transpose()*tmp2; pdata->fqp += (S*x_bar + 0.5*s1).transpose()*B_ls*Jcom; pdata->fqp -= u0.transpose()*tmp2; pdata->fqp -= y0.transpose()*Qy*D_ls*Jcom; pdata->fqp -= (params->whole_body.w_qdd.array()*pid_out.qddot_des.array()).matrix().transpose(); if (include_angular_momentum) { pdata->fqp += qd.transpose()*pdata->Akdot.transpose()*params->W_kdot*pdata->Ak; pdata->fqp -= kdot_des.transpose()*params->W_kdot*pdata->Ak; } f.head(nq) = pdata->fqp.transpose(); } else { f.head(nq) = -pid_out.qddot_des; } } f.tail(nf+neps) = VectorXd::Zero(nf+neps); int neq = 6+neps+6*n_body_accel_eq_constraints+qp_input->whole_body_data.num_constrained_dofs; MatrixXd Aeq = MatrixXd::Zero(neq,nparams); VectorXd beq = VectorXd::Zero(neq); // constrained floating base dynamics // H_float*qdd - J_float'*lambda - Dbar_float*beta = -C_float Aeq.topLeftCorner(6,nq) = pdata->H_float; beq.topRows(6) = -pdata->C_float; if (nc>0) { Aeq.block(0,nq,6,nc*nd) = -D_float; } if (nc > 0) { // relative acceleration constraint Aeq.block(6,0,neps,nq) = Jp; Aeq.block(6,nq,neps,nf) = MatrixXd::Zero(neps,nf); // note: obvious sparsity here Aeq.block(6,nq+nf,neps,neps) = MatrixXd::Identity(neps,neps); // note: obvious sparsity here beq.segment(6,neps) = (-Jpdot -params->Kp_accel*Jp)*qd; } // add in body spatial equality constraints // VectorXd body_vdot; MatrixXd orig = MatrixXd::Zero(4,1); orig(3,0) = 1; int equality_ind = 6+neps; MatrixXd Jb(6,nq); MatrixXd Jbdot(6,nq); for (int i=0; i<desired_body_accelerations.size(); i++) { if (desired_body_accelerations[i].weight < 0) { // negative implies constraint if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) { pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb); pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot); for (int j=0; j<6; j++) { if (!std::isnan(desired_body_accelerations[i].body_vdot(j))) { Aeq.block(equality_ind,0,1,nq) = Jb.row(j); beq[equality_ind++] = -Jbdot.row(j)*qd + desired_body_accelerations[i].body_vdot(j); } } } } } if (qp_input->whole_body_data.num_constrained_dofs>0) { // add joint acceleration constraints for (int i=0; i<qp_input->whole_body_data.num_constrained_dofs; i++) { Aeq(equality_ind,(int)condof[i]-1) = 1; beq[equality_ind++] = pid_out.qddot_des[(int)condof[i]-1]; } } int n_ineq = 2*nu+2*6*desired_body_accelerations.size(); MatrixXd Ain = MatrixXd::Zero(n_ineq,nparams); // note: obvious sparsity here VectorXd bin = VectorXd::Zero(n_ineq); // linear input saturation constraints // u=B_act'*(H_act*qdd + C_act - Jz_act'*z - Dbar_act*beta) // using transpose instead of inverse because B is orthogonal Ain.topLeftCorner(nu,nq) = pdata->B_act.transpose()*pdata->H_act; Ain.block(0,nq,nu,nc*nd) = -pdata->B_act.transpose()*D_act; bin.head(nu) = -pdata->B_act.transpose()*pdata->C_act + pdata->umax; Ain.block(nu,0,nu,nparams) = -1*Ain.block(0,0,nu,nparams); bin.segment(nu,nu) = pdata->B_act.transpose()*pdata->C_act - pdata->umin; int constraint_start_index = 2*nu; for (int i=0; i<desired_body_accelerations.size(); i++) { pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb); pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot); Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = Jb; bin.segment(constraint_start_index,6) = -Jbdot*qd + desired_body_accelerations[i].accel_bounds.max; constraint_start_index += 6; Ain.block(constraint_start_index,0,6,pdata->r->num_positions) = -Jb; bin.segment(constraint_start_index,6) = Jbdot*qd - desired_body_accelerations[i].accel_bounds.min; constraint_start_index += 6; } for (int i=0; i<n_ineq; i++) { // remove inf constraints---needed by gurobi if (std::isinf(double(bin(i)))) { Ain.row(i) = 0*Ain.row(i); bin(i)=0; } } GRBmodel * model = nullptr; int info=-1; // set obj,lb,up VectorXd lb(nparams), ub(nparams); lb.head(nq) = pdata->qdd_lb; ub.head(nq) = pdata->qdd_ub; lb.segment(nq,nf) = VectorXd::Zero(nf); ub.segment(nq,nf) = 1e3*VectorXd::Ones(nf); lb.tail(neps) = -params->slack_limit*VectorXd::Ones(neps); ub.tail(neps) = params->slack_limit*VectorXd::Ones(neps); VectorXd alpha(nparams); MatrixXd Qnfdiag(nf,1), Qneps(neps,1); vector<MatrixXd*> QBlkDiag( nc>0 ? 3 : 1 ); // nq, nf, neps // this one is for gurobi VectorXd w = (params->whole_body.w_qdd.array() + REG).matrix(); #ifdef USE_MATRIX_INVERSION_LEMMA double max_body_accel_weight = -numeric_limits<double>::infinity(); for (int i=0; i < desired_body_accelerations.size(); i++) { max_body_accel_weight = max(max_body_accel_weight, desired_body_accelerations[i].weight); } bool include_body_accel_cost_terms = desired_body_accelerations.size() > 0 && max_body_accel_weight > 1e-10; if (pdata->use_fast_qp > 0 && !include_angular_momentum && !include_body_accel_cost_terms) { // TODO: update to include angular momentum, body accel objectives. // We want Hqp inverse, which I can compute efficiently using the // matrix inversion lemma (see wikipedia): // inv(A + U'CV) = inv(A) - inv(A)*U* inv([ inv(C)+ V*inv(A)*U ]) V inv(A) if (nc>0) { MatrixXd Wi = ((1/(params->whole_body.w_qdd.array() + REG)).matrix()).asDiagonal(); if (R_DQyD_ls.trace()>1e-15) { // R_DQyD_ls is not zero pdata->Hqp = Wi - Wi*Jcom.transpose()*(R_DQyD_ls.inverse() + Jcom*Wi*Jcom.transpose()).inverse()*Jcom*Wi; } } else { pdata->Hqp = MatrixXd::Constant(nq,1,1/(1+REG)); } #ifdef TEST_FAST_QP if (nc>0) { MatrixXd Hqp_test(nq,nq); MatrixXd W = w.asDiagonal(); Hqp_test = (Jcom.transpose()*R_DQyD_ls*Jcom + W).inverse(); if (((Hqp_test-pdata->Hqp).array().abs()).maxCoeff() > 1e-6) { mexErrMsgTxt("Q submatrix inverse from matrix inversion lemma does not match direct Q inverse."); } } #endif Qnfdiag = MatrixXd::Constant(nf,1,1/REG); Qneps = MatrixXd::Constant(neps,1,1/(.001+REG)); QBlkDiag[0] = &pdata->Hqp; if (nc>0) { QBlkDiag[1] = &Qnfdiag; QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps) } MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams); VectorXd bin_lb_ub(n_ineq+2*nparams); Ain_lb_ub << Ain, // note: obvious sparsity here -MatrixXd::Identity(nparams,nparams), MatrixXd::Identity(nparams,nparams); bin_lb_ub << bin, -lb, ub; info = fastQPThatTakesQinv(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha); //if (info<0) mexPrintf("fastQP info = %d. Calling gurobi.\n", info); } else { #endif if (nc>0) { pdata->Hqp = Jcom.transpose()*R_DQyD_ls*Jcom; if (include_angular_momentum) { pdata->Hqp += pdata->Ak.transpose()*params->W_kdot*pdata->Ak; } pdata->Hqp += params->whole_body.w_qdd.asDiagonal(); pdata->Hqp += REG*MatrixXd::Identity(nq,nq); } else { pdata->Hqp = (1+REG)*MatrixXd::Identity(nq,nq); } // add in body spatial acceleration cost terms for (int i=0; i<desired_body_accelerations.size(); i++) { if (desired_body_accelerations[i].weight > 0) { if (!inSupport(active_supports,desired_body_accelerations[i].body_id0)) { pdata->r->forwardJac(desired_body_accelerations[i].body_id0,orig,1,Jb); pdata->r->forwardJacDot(desired_body_accelerations[i].body_id0,orig,1,Jbdot); for (int j=0; j<6; j++) { if (!std::isnan(desired_body_accelerations[i].body_vdot[j])) { pdata->Hqp += desired_body_accelerations[i].weight*(Jb.row(j)).transpose()*Jb.row(j); f.head(nq) += desired_body_accelerations[i].weight*(qd.transpose()*Jbdot.row(j).transpose() - desired_body_accelerations[i].body_vdot[j])*Jb.row(j).transpose(); } } } } } Qnfdiag = MatrixXd::Constant(nf,1,params->w_grf+REG); Qneps = MatrixXd::Constant(neps,1,params->w_slack+REG); QBlkDiag[0] = &pdata->Hqp; if (nc>0) { QBlkDiag[1] = &Qnfdiag; QBlkDiag[2] = &Qneps; // quadratic slack var cost, Q(nparams-neps:end,nparams-neps:end)=eye(neps) } MatrixXd Ain_lb_ub(n_ineq+2*nparams,nparams); VectorXd bin_lb_ub(n_ineq+2*nparams); Ain_lb_ub << Ain, // note: obvious sparsity here -MatrixXd::Identity(nparams,nparams), MatrixXd::Identity(nparams,nparams); bin_lb_ub << bin, -lb, ub; if (pdata->use_fast_qp > 0) { // set up and call fastqp info = fastQP(QBlkDiag, f, Aeq, beq, Ain_lb_ub, bin_lb_ub, pdata->state.active, alpha); //if (info<0) mexPrintf("fastQP info=%d... calling Gurobi.\n", info); } else { // use gurobi active set model = gurobiActiveSetQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.vbasis,pdata->state.vbasis_len,pdata->state.cbasis,pdata->state.cbasis_len,alpha); CGE(GRBgetintattr(model,"NumVars",&(pdata->state.vbasis_len)), pdata->env); CGE(GRBgetintattr(model,"NumConstrs",&(pdata->state.cbasis_len)), pdata->env); info=66; //info = -1; } if (info<0) { model = gurobiQP(pdata->env,QBlkDiag,f,Aeq,beq,Ain,bin,lb,ub,pdata->state.active,alpha); int status; CGE(GRBgetintattr(model, "Status", &status), pdata->env); //if (status!=2) mexPrintf("Gurobi reports non-optimal status = %d\n", status); } #ifdef USE_MATRIX_INVERSION_LEMMA } #endif //---------------------------------------------------------------------- // Solve for inputs ---------------------------------------------------- qp_output->qdd = alpha.head(nq); VectorXd beta = alpha.segment(nq,nc*nd); // use transpose because B_act is orthogonal qp_output->u = pdata->B_act.transpose()*(pdata->H_act*qp_output->qdd + pdata->C_act - D_act*beta); //y = pdata->B_act.jacobiSvd(ComputeThinU|ComputeThinV).solve(pdata->H_act*qdd + pdata->C_act - Jz_act.transpose()*lambda - D_act*beta); bool foot_contact[2]; foot_contact[0] = b_contact_force(pdata->rpc.body_ids.r_foot) == 1; foot_contact[1] = b_contact_force(pdata->rpc.body_ids.l_foot) == 1; qp_output->qd_ref = velocityReference(pdata, t, q, qd, qp_output->qdd, foot_contact, &(params->vref_integrator), &(pdata->rpc)); // Remember t for next time around pdata->state.t_prev = t; // If a debug pointer was passed in, fill it with useful data if (debug) { debug->active_supports.resize(active_supports.size()); for (int i=0; i < active_supports.size(); i++) { debug->active_supports[i] = active_supports[i]; } debug->nc = nc; debug->normals = normals; debug->B = B; debug->alpha = alpha; debug->f = f; debug->Aeq = Aeq; debug->beq = beq; debug->Ain_lb_ub = Ain_lb_ub; debug->bin_lb_ub = bin_lb_ub; debug->Qnfdiag = Qnfdiag; debug->Qneps = Qneps; debug->x_bar = x_bar; debug->S = S; debug->s1 = s1; debug->s1dot = s1dot; debug->s2dot = qp_input->zmp_data.s2dot; debug->A_ls = A_ls; debug->B_ls = B_ls; debug->Jcom = Jcom; debug->Jcomdot = Jcomdot; debug->beta = beta; } // if we used gurobi, clean up if (model) { GRBfreemodel(model); } // GRBfreeenv(env); return info; }
for (int i=0; i < params.lb.size(); i++) { if (!has_joint_override(i) && params.enabled(i)) { int disable_body_1idx = params.disable_when_body_in_support(i); if (disable_body_1idx == 0 || !inSupport(supports, disable_body_1idx - 1)) { double w_lb = 0; double w_ub = 0; if (!isInf(params.lb(i))) { w_lb = logisticSigmoid(params.weight(i), params.k_logistic(i), params.lb(i), robot_state.q(i)); } if (!isInf(params.ub(i))) { w_ub = logisticSigmoid(params.weight(i), params.k_logistic(i), robot_state.q(i), params.ub(i)); } double weight = std::max(w_ub, w_lb); drake::lcmt_joint_pd_override override; override.position_ind = i + 1; override.qi_des = q_des(i); override.qdi_des = 0; override.kp = params.kp(i); override.kd = params.kd(i); override.weight = weight; joint_pd_override.push_back(override); } } } } void applyJointPDOverride(const std::vector<drake::lcmt_joint_pd_override> &joint_pd_override, const DrakeRobotState &robot_state, PIDOutput &pid_out, VectorXd &w_qdd) { for (std::vector<drake::lcmt_joint_pd_override>::const_iterator it = joint_pd_override.begin(); it != joint_pd_override.end(); ++it) { int ind = it->position_ind - 1; double err_q = it->qi_des - robot_state.q(ind); double err_qd = it->qdi_des - robot_state.qd(ind);
void stateCallback(const nav_msgs::Odometry::ConstPtr& state) { opticalflow_msgs::StateMsg state_msg; pos[0] = state->pose.pose.position.x; pos[1] = state->pose.pose.position.y; state_msg.vicon_vx = state->twist.twist.linear.x; state_msg.vicon_vy = state->twist.twist.linear.y; state_msg.sensor_vx = of_vx; state_msg.sensor_vy = of_vy; if(!use_sensor){ vel[0] = state_msg.vicon_vx; vel[1] = state_msg.vicon_vy; vel[2] = state->twist.twist.linear.z; pos[2] = state->pose.pose.position.z; } else if(use_sensor){ vel[0] = state_msg.vicon_vx; vel[1] = state_msg.vicon_vy; vel[2] = of_vz; pos[2] = of_z; } q.x() = state->pose.pose.orientation.x; q.y() = state->pose.pose.orientation.y; q.z() = state->pose.pose.orientation.z; q.w() = state->pose.pose.orientation.w; state_msg.roll = atan2(2*(q.w()*q.x()+q.y()*q.z()),1-2*(q.x()*q.x()+q.y()*q.y())); state_msg.pitch = asin(2*(q.w()*q.y()-q.z()*q.x())); state_msg.yaw = atan2(2*(q.w()*q.z()+q.x()*q.y()),1-2*(q.y()*q.y()+q.z()*q.z())); //Eigen::Matrix3d R(q); if(mode!=4) { force(0) = offset_x+(k_xy*(pos_des(0)-pos(0))+k_vxy*(vel_des(0)-vel(0))+m*acc_des(0)); force(1) = offset_y+(k_xy*(pos_des(1)-pos(1))+k_vxy*(vel_des(1)-vel(1))+m*acc_des(1)); std::cout<<"offset_x = "<<offset_x-force(0)<<", offset_y = "<<offset_y-force(1)<<std::endl; } else if(mode==4) { force(0) = offset_x+k_vxy/3*(vel_des(0)-vel(0))+m*acc_des(0); force(1) = offset_y+k_vxy/3*(vel_des(1)-vel(1))+m*acc_des(1); } if(!use_sensor){ force(2) = (k_z*(pos_des(2)-pos(2))+k_vz*(vel_des(2)-vel(2))+m*g(2)+m*acc_des(2)); } else if(use_sensor){ force(2) = (k_z*(pos_des(2)-pos(2))+k_vz*(vel_des(2)-vel(2))+m*g(2)+m*acc_des(2)); } b3 = force.normalized(); // ROS_INFO("b3:[%f,%f,%f]",b3[0],b3[1],b3[2]); b2 = b3.cross(b1w); b1 = b2.cross(b3); R_des<<b1[0],b2[0],b3[0], b1[1],b2[1],b3[1], b1[2],b2[2],b3[2]; Eigen::Quaterniond q_des(R_des); quadrotor_msgs::SO3Command command; command.force.x = force[0]; command.force.y = force[1]; command.force.z = force[2]; command.orientation.x = q_des.x(); command.orientation.y = q_des.y(); command.orientation.z = q_des.z(); command.orientation.w = q_des.w(); command.kR[0] = k_R; command.kR[1] = k_R; command.kR[2] = k_R; command.kOm[0] = k_omg; command.kOm[1] = k_omg; command.kOm[2] = k_omg; command.aux.current_yaw = state_msg.yaw; command.aux.enable_motors = true; command.aux.use_external_yaw = true; control_pub.publish(command); state_msg.des_roll = atan2(2*(q_des.w()*q_des.x()+q_des.y()*q_des.z()),1-2*(q_des.x()*q_des.x()+q_des.y()*q_des.y())); state_msg.des_pitch = asin(2*(q_des.w()*q_des.y()-q_des.z()*q_des.x())); state_msg.des_yaw = atan2(2*(q_des.w()*q_des.z()+q_des.x()*q_des.y()),1-2*(q_des.y()*q_des.y()+q_des.z()*q_des.z())); state_msg.x = pos(0); state_msg.y = pos(1); state_msg.z = pos(2); state_msg.vx = vel(0); state_msg.vy = vel(1); state_msg.vz = vel(2); state_msg.des_x = pos_des(0); state_msg.des_y = pos_des(1); state_msg.des_z = pos_des(2); state_msg.des_vx = vel_des(0); state_msg.des_vy = vel_des(1); state_msg.des_vz = vel_des(2); state_msg.des_accx = acc_des(0); state_msg.des_accy = acc_des(1); state_msg.des_accz = acc_des(2); state_msg_pub.publish(state_msg); }