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