Vector6F resolveTorsoAcceleration() { Vector6F desired_torso_acceleration = Vector6F::Zero(); // Desired torso position and orientation (ICS) Vector6F poseError; Vector3F ref_torso_p_ICS; Matrix3F ref_torso_R_ICS; Vector3F ref_torso_R_ICS_c1, ref_torso_R_ICS_c2, ref_torso_R_ICS_c3;// columns of ref_torso_R_ICS Vector3F c1,c2,c3; //ref_torso_p_ICS<< 3.0, 5.08, 0.46; //ref_torso_R_ICS_c1 << 0, 0, 1; //ref_torso_R_ICS_c2 << 1, 0, 0; //ref_torso_R_ICS_c3 << 0, 1, 0; //upright ref_torso_p_ICS = popTorsoPositionSetPoint(); ref_torso_R_ICS = popTorsoOrientationSetPoint(); ref_torso_R_ICS_c1 = ref_torso_R_ICS.block(0,0,3,1); ref_torso_R_ICS_c2 = ref_torso_R_ICS.block(0,1,3,1); ref_torso_R_ICS_c3 = ref_torso_R_ICS.block(0,2,3,1); poseError(3) = ref_torso_p_ICS[0] - G_robot_linkinfo_list[2]->link_val2.p_ICS[0]; poseError(4) = ref_torso_p_ICS[1] - G_robot_linkinfo_list[2]->link_val2.p_ICS[1]; poseError(5) = ref_torso_p_ICS[2] - G_robot_linkinfo_list[2]->link_val2.p_ICS[2]; for (int g = 0; g<3;g++) { c1(g) = G_robot_linkinfo_list[2]->link_val2.R_ICS[g][0]; c2(g) = G_robot_linkinfo_list[2]->link_val2.R_ICS[g][1]; c3(g) = G_robot_linkinfo_list[2]->link_val2.R_ICS[g][2]; } Matrix6F XI2 = G_robot->computeSpatialTransformation(2); poseError.head(3) = XI2.block(0,0,3,3) * (0.5*(cr3(c1)* ref_torso_R_ICS_c1 + cr3(c2)* ref_torso_R_ICS_c2 + cr3(c3)* ref_torso_R_ICS_c3)); // there is a bias... lwp poseError.tail(3) = XI2.block(0,0,3,3)* poseError.tail(3); //in torso coordinate Float kp, kd;// PD gain kd = 15; kp = 100; // desired_torso_acceleration(2) = 0.02*kp * poseError(2) + 0.02*kd * ( - TorsoVel_curr(2)); // soft gain on torso pitch desired_torso_acceleration(3) = 0.8*kp * poseError(3) + 0.8*kd * ( - TorsoVel_curr(3)); desired_torso_acceleration(4) = 0.5*kp * poseError(4) + 0.5*kd * ( - TorsoVel_curr(4)); #ifdef BIPED_DEBUG cout<<"desired spatial torso acceleration is: "<<endl<<desired_torso_acceleration<<endl<<endl; #endif return desired_torso_acceleration; }
void HumanoidController::ComputeActualQdd(VectorXF & qddA) { VectorXF generalizedContactForce = VectorXF::Zero(NJ+6); Matrix6F X; MatrixXF Jac; X.setIdentity(); for (int i=0; i<NS; i++) { int linkIndex = SupportIndices[i]; artic->computeJacobian(linkIndex,X,Jac); dmRigidBody * link = (dmRigidBody *) artic->getLink(linkIndex); for (int j=0; j< link->getNumForces(); j++) { dmForce * f = link->getForce(j); Vector6F fContact; f->computeForce(artic->m_link_list[linkIndex]->link_val2,fContact.data()); generalizedContactForce += Jac.transpose()*fContact; } } //cout << "J' f = " << generalizedContactForce.transpose() << endl; //MatrixXF S = MatrixXF::Zero(NJ,NJ+6); //S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ); //VectorXF qdd3 = artic->H.inverse()*(S.transpose() * tau - artic->CandG + generalizedContactForce); //cout << "qdd3 = " << qdd3.transpose() << endl; VectorXF state = VectorXF::Zero(2*STATE_SIZE); state.segment(0,STATE_SIZE) = q; state.segment(STATE_SIZE,STATE_SIZE) = qdDm; VectorXF stateDot = VectorXF::Zero(2*STATE_SIZE); //Process qdds artic->dynamics(state.data(),stateDot.data()); extractQd(stateDot.segment(STATE_SIZE,STATE_SIZE), qddA); //qdds.segment(0,6) = stateDot.segment(STATE_SIZE,STATE_SIZE); //cout << "w x v " << cr3(qd.segment(0,3))*qd.segment(3,3) << endl; qddA.segment(3,3) -= cr3(qdDm.segment(0,3))*qdDm.segment(3,3); Matrix3F ics_R_fb; copyRtoMat(artic->m_link_list[0]->link_val2.R_ICS,ics_R_fb); qddA.head(3) = ics_R_fb.transpose() * qddA.head(3); qddA.segment(3,3) = ics_R_fb.transpose() * qddA.segment(3,3); //cout << "qdd3 = " << qdd3.transpose() << endl; //cout << "qdds = " << qddA.transpose() << endl; //exit(-1); //cout << "CandG " << endl << CandG << endl; //cout << setprecision(6); //cout << "I_0^C = " << endl << artic->H.block(0,0,6,6) << endl; //exit(-1); }
// ----------- void computeBipedLeftLegDesiredQdd() { Matrix6F X_lf; MatrixXF S(2,6); S<< 0,0,0,1,0,0, 0,0,0,0,1,0; X_lf.block<3,3>(0,0) = Matrix3F::Identity(); X_lf.block<3,3>(0,3) = Matrix3F::Zero(); X_lf.block<3,3>(3,0) = -cr3(plf); X_lf.block<3,3>(3,3) = Matrix3F::Identity(); Vector6F LfVel = X_lf * LkneeVel_curr; Vector6F accBiasL; Matrix6XF JL; Matrix6F X_TL; // spatial transformation from torso to left foot accBiasL = G_robot->computeAccelerationBias(8,X_lf,3); JL = G_robot->calculateJacobian(8,X_lf,3); X_TL = X_lf*G_robot->computeSpatialTransformation(8,3); Vector2F qdd_L; Vector3F omega_lf; omega_lf = LfVel.head(3); Vector6F b_term_L = Vector6F::Zero(); b_term_L.tail(3) = cr3(omega_lf)*LfVel.tail(3); Vector2F rhs2 = S*(- X_TL * desired_torso_acc - accBiasL - b_term_L ); //qdd_L = solveJacobianPseudoInverse(JL, rhs2); qdd_L = (S*JL).inverse() * rhs2; #ifdef BIPED_DEBUG cout<<"JL is: "<<endl<<JL<<endl<<endl; cout<<"rhs2 is: "<<endl<<rhs2<<endl<<endl; cout<<"qdd_L: "<<endl<<qdd_L<<endl<<endl; #endif G_robot_linkinfo_list[7]->link_val2.qdd(0) = qdd_L(0); G_robot_linkinfo_list[8]->link_val2.qdd(0) = qdd_L(1); }
// --------- void computeBipedRightLegDesiredQdd() { Matrix6F X_rf; MatrixXF S(2,6); S<< 0,0,0,1,0,0, 0,0,0,0,1,0; X_rf.block<3,3>(0,0) = Matrix3F::Identity(); X_rf.block<3,3>(0,3) = Matrix3F::Zero(); X_rf.block<3,3>(3,0) = -cr3(prf); X_rf.block<3,3>(3,3) = Matrix3F::Identity(); Vector6F RfVel = X_rf*RkneeVel_curr; Vector6F accBiasR; Matrix6XF JR; Matrix6F X_TR; // spatial transformation from torso to right foot accBiasR = G_robot->computeAccelerationBias(6,X_rf,3); JR = G_robot->calculateJacobian(6,X_rf,3); X_TR = X_rf*G_robot->computeSpatialTransformation(6,3); Vector2F qdd_R; Vector3F omega_rf; omega_rf = RfVel.head(3); Vector6F b_term_R = Vector6F::Zero(); b_term_R.tail(3) = cr3(omega_rf)*RfVel.tail(3); Vector2F rhs1 = S*(- X_TR * desired_torso_acc - accBiasR - b_term_R ); //qdd_R = solveJacobianPseudoInverse(S*JR, rhs1); qdd_R = (S*JR).inverse()*rhs1; #ifdef BIPED_DEBUG cout<<"JR is: "<<endl<<JR<<endl<<endl; cout<<"rhs1 is: "<<endl<<rhs1<<endl<<endl; cout<<"qdd_R: "<<endl<<qdd_R<<endl<<endl; #endif G_robot_linkinfo_list[5]->link_val2.qdd(0) = qdd_R(0); G_robot_linkinfo_list[6]->link_val2.qdd(0) = qdd_R(1); }
//! Only call this function after you run ObtainArticulationData() //! Author: PMW void HumanoidController::ComputeComInfo(Matrix6XF & Cmm, Vector6F & bias, Vector3F & pCom, Float & m) { LinkInfoStruct * fb = artic->m_link_list[0]; // floating base dmLink * fbLink = fb->link; CartesianTensor fb_R_ics; //pCom initally contains the position of the torso in the i.c.s fbLink->getPose(fb_R_ics,pCom.data()); Matrix3F ics_R_fb; ics_R_fb << fb_R_ics[0][0] , fb_R_ics[1][0] , fb_R_ics[2][0], fb_R_ics[0][1] , fb_R_ics[1][1] , fb_R_ics[2][1], fb_R_ics[0][2] , fb_R_ics[1][2] , fb_R_ics[2][2]; Vector3F pFBRelCoM; m = fb->I_C.m; pFBRelCoM(0) = -fb->I_C.h[0]/m; // fb frame and com frame have the same orientation pFBRelCoM(1) = -fb->I_C.h[1]/m; pFBRelCoM(2) = -fb->I_C.h[2]/m; //Matrix6F IC0; //CrbToMat(fb->I_C, IC0); //cout << "FB IC0 in CoM Func " << endl << IC0 << endl; // pCom = pBase + pComRelBase pCom -= ics_R_fb*pFBRelCoM; int N = artic->H.cols(); Cmm.resize(6,N); Matrix6F XT = Matrix6F::Zero(); XT.block(0,0,3,3) = ics_R_fb; XT.block(0,3,3,3) = ics_R_fb*cr3(pFBRelCoM); XT.block(3,3,3,3) = ics_R_fb; // XT is force transformation from FB to COM Cmm = XT * artic->H.topRows(6); //expressed in ICS coordinate IC0 = XT*artic->H.block(0,0,6,6)*XT.transpose(); IBarC0 = IC0.block(0,0,3,3); // These quantities (f,a) for the floating base are the result of // inverse dynamics with qdd=0. As a result, fb->link_val2.f // contains the first 6 rows of CandG! bias = XT * (fb->link_val2.f-artic->H.block(0,0,6,6)*fb->link_val2.a); // bias = \dot{A}_G \dot{q} }
int main(int argc, char *argv[]) { Pooma::initialize(argc, argv); Pooma::Tester tester(argc, argv); Interval<1> n1(1,5); Interval<1> n2(4,8); Interval<1> n3(10,20); Interval<2> a(n1,n2); Interval<3> b(n1,n2,n3); Range<1> r1(1,5); Range<1> r2(4,8,2); Range<1> r3(5,9,2); Range<1> r4(10,20,5); Range<2> ra(r1,r2); Range<2> rb(r1,r3); Range<3> rc(r1,r2,r3); tester.out() << "1: touches(" << a[0] << "," << a[1] << ") ? "; tester.out() << touches(a[0], a[1]) << std::endl; tester.check( touches(a[0], a[1]) ); tester.out() << "0: touches(" << a[0] << "," << b[2] << ") ? "; tester.out() << touches(a[0], b[2]) << std::endl; tester.check( touches(a[0], b[2])==0); tester.out() << "1: touches(" << a[0] << "," << ra[0] << ") ? "; tester.out() << touches(a[0], ra[0]) << std::endl; tester.check(touches(a[0], ra[0])); tester.out() << "1: touches(" << ra[0] << "," << ra[1] << ") ? "; tester.out() << touches(ra[0], ra[1]) << std::endl; tester.check( touches(ra[0], ra[1])); tester.out() << "0: touches(" << r2 << "," << r3 << ") ? "; tester.out() << touches(r2, r3) << std::endl; tester.check( touches(r2, r3)==0); tester.out() << "0: touches(" << ra << "," << rb << ") ? "; tester.out() << touches(ra, rb) << std::endl; tester.check( touches(ra, rb) ==0); tester.out() << "1: touches(" << rc << "," << rc << ") ? "; tester.out() << touches(rc, rc) << std::endl; tester.check( touches(rc, rc) ); tester.out() << "------------------------------------" << std::endl; tester.check(" touches ", true); Interval<1> c1(1,10); Interval<1> c2(3,8); Interval<1> c3(5,15); Interval<2> ca(c1, c1); Interval<2> cb(c1, c2); Range<1> cr1(2,20,2); Range<1> cr2(4,16,4); Range<1> cr3(3,15,2); Range<1> cr4(5,15,5); tester.out() << "1: contains(" << c1 << "," << c2 << ") ? "; tester.out() << contains(c1,c2) << std::endl; tester.check(contains(c1,c2)); tester.out() << "0: contains(" << c2 << "," << c1 << ") ? "; tester.out() << contains(c2,c1) << std::endl; tester.check(contains(c2,c1)==0); tester.out() << "0: contains(" << c1 << "," << c3 << ") ? "; tester.out() << contains(c1,c3) << std::endl; tester.check(contains(c1,c3)==0); tester.out() << "1: contains(" << ca << "," << cb << ") ? "; tester.out() << contains(ca,cb) << std::endl; tester.check(contains(ca,cb)); tester.out() << "0: contains(" << cb << "," << ca << ") ? "; tester.out() << contains(cb,ca) << std::endl; tester.check(contains(cb,ca)==0); tester.out() << "1: contains(" << cr1 << "," << cr2 << ") ? "; tester.out() << contains(cr1,cr2) << std::endl; tester.check( contains(cr1,cr2)); tester.out() << "0: contains(" << cr1 << "," << cr3 << ") ? "; tester.out() << contains(cr1,cr3) << std::endl; tester.check(contains(cr1,cr3)==0); tester.out() << "1: contains(" << c3 << "," << cr4 << ") ? "; tester.out() << contains(c3,cr4) << std::endl; tester.check(contains(c3,cr4)); tester.out() << "0: contains(" << cr4 << "," << c3 << ") ? "; tester.out() << contains(cr4,c3) << std::endl; tester.check(contains(cr4,c3)==0); tester.out() << "------------------------------------" << std::endl; Interval<2> s1, s2; Range<2> sr1, sr2; split(cb, s1, s2); tester.out() << "split(" << cb << ") = " << s1 << " and " << s2 << std::endl; tester.check(s1==Interval<2>(Interval<1>(1,5),Interval<1>(3,5))); tester.check(s2==Interval<2>(Interval<1>(6,10),Interval<1>(6,8))); split(rb, sr1, sr2); tester.out() << "split(" << rb << ") = " << sr1 << " and " << sr2 << std::endl; tester.check(sr1==Range<2>(Range<1>(1,2),Range<1>(5,5,2))); tester.check(sr2==Range<2>(Range<1>(3,5),Range<1>(7,9,2))); tester.out() << "------------------------------------" << std::endl; tester.out() << "intersect(" << cb << "," << ca << ") = "; tester.out() << intersect(cb,ca) << std::endl; tester.check(intersect(cb,ca)==Interval<2>(Interval<1>(1,10), Interval<1>(3,8))); tester.out() << "intersect(" << rb << "," << ra << ") = "; tester.out() << intersect(rb,ra) << std::endl; Range<1> i1(1,16,3); Range<1> i2(17,3,-2); tester.out() << "intersect(" << i1 << "," << i2 << ") = "; tester.out() << intersect(i1,i2) << std::endl; tester.check( intersect(i1,i2) == Range<1>(7,14,6)); tester.out() << "intersect(" << i2 << "," << i1 << ") = "; tester.out() << intersect(i2,i1) << std::endl; tester.check( intersect(i2,i1) == Range<1>(13,7,-6)); tester.out() << "------------------------------------" << std::endl; Interval<1> eq1(1,5); Range<1> eq2 = -2 * eq1 + 3; Range<1> eq3(-8,8,4); Range<1> eq4 = 3 * eq1; Range<1> eq5 = 2 * eq1; Range<1> eq6 = 6 * eq1 + 1; tester.out() << "For " << eq1 << " --> " << eq4 << ", then " << eq3 << " --> "; tester.out() << equivSubset(eq1,eq4,eq3) << std::endl; tester.out() << "For " << eq4 << " --> " << eq6 << ", then " << eq3 << " --> "; tester.out() << equivSubset(eq4,eq6,eq3) << std::endl; tester.out() << "For " << eq1 << " --> " << eq2 << ", then " << eq3 << " --> "; tester.out() << equivSubset(eq1,eq2,eq3) << std::endl; tester.out() << "------------------------------------" << std::endl; NewDomain3<Interval<1>, Interval<1>, int>::SliceType_t ba; // tester.out() << "Created initial slice domain ba = " << ba << std::endl; ba = NewDomain3<Interval<1>, Interval<1>, int>::combineSlice(ba,eq1,eq1,7); tester.out() << "After taking slice, ba = " << ba << std::endl; int retval = tester.results("Domain Calc"); Pooma::finalize(); return retval; }
int comms_reply(unsigned int port, void *buf, unsigned int size) { //return no such port when port doesn't exist if (bt(comms_bitmap,port)) { update_port_error(cr3(),-ERR_IPC_NO_SUCH_PORT); return -ERR_IPC_NO_SUCH_PORT; } //return invalid port when port exceeds max ports if (port >= COMMS_MAX_PORTS) { update_error(port,-ERR_IPC_INVALID_PORT); return -ERR_IPC_INVALID_PORT; } //return invalid msg size if buf is less than 1 or exceeds max msg size if ((size < 1) | (size > COMMS_MAX_MSG)) { update_error(port,-ERR_IPC_INVALID_MSG_SIZE); return -ERR_IPC_INVALID_MSG_SIZE; } //return invalid buffer when a buffer equals NULL if (buf == NULL) { update_error(port,-ERR_IPC_INVALID_BUFFER); return -ERR_IPC_INVALID_BUFFER; } //double check if the message is still there if (msg_port[port]->queue != NULL) { //save message into temp msg* temp = msg_port[port]->queue; //check if the sizes are correct if (temp->rep_size != size) { return -ERR_IPC_REPLY_MISMATCH; } //move to next message msg_port[port]->queue = msg_port[port]->queue->next; //if there is no messages make sure last knows this if (msg_port[port] == NULL) { msg_port[port]->last = NULL; } //switch to client pd i386_set_page_directory(temp->pd); //copy buf into reply address copy_4((unsigned int)buf+temp->mult,temp->rep_buf,size/4); //switch back to server pd i386_set_page_directory(msg_port[port]->pcb->pd); //remove the server page table from the message ((page_directory*)(temp->pd))->table[temp->offset] = 0; //free the memory the message used mem_free(temp); //unblock the client unblock_process(port); }else { //will this happen? } //return OK return OK; }
int comms_send(cpu_state* state) { unsigned int port = state->ebx; void *req_buf = (void*)state->ecx; unsigned int req_size = state->edx; void *rep_buf = (void*)state->esi; unsigned int rep_size = state->edi; //return no such port when if the port doesn't exist if (bt(comms_bitmap,port)) { state->eax = -ERR_IPC_NO_SUCH_PORT; return 0; } //return invalid port if port exceeds max ports if (port >= COMMS_MAX_PORTS) { state->eax = -ERR_IPC_INVALID_PORT; return 0; } //return invalid msg size if req_size or rep_size is less than 1 or exceeds max msg size if ((req_size < 1) | (req_size > COMMS_MAX_MSG) | (rep_size < 1) | (rep_size >= COMMS_MAX_MSG)) { state->eax = -ERR_IPC_INVALID_MSG_SIZE; return 0; } //return invalid buffer when a buffer equals NULL if ((req_buf == NULL) | (rep_buf == NULL)) { state->eax = -ERR_IPC_INVALID_BUFFER; return 0; } //allocate memory for the message msg *temp = (struct msg*)mem_alloc(sizeof(struct msg)); //is this the first message? if (msg_port[port]->queue == NULL) { //set as first node msg_port[port]->queue = temp; }else { //let the last message point to it msg_port[port]->last->next = temp; } //the msg is also the last msg msg_port[port]->last = temp; //save it's page directory temp->pd = (void*)cr3(); //save the adress of the request and it's size temp->req_buf = req_buf; temp->req_size = req_size; //save the adress we need to reply to and it's size temp->rep_buf = rep_buf; temp->rep_size = rep_size; //there isn't a next temp->next = NULL; //get the server page directory page_directory* pd = (page_directory*)(msg_port[port]->pcb->pd); //get the page table entry where the server is located unsigned int pt_entry = pd->table[msg_port[port]->index]; //get the client page directory pd = (page_directory*)(temp->pd); //get a open slot in the page directory temp->offset = 0; while ((temp->offset < PTES) && (pd->table[temp->offset] != 0)) { temp->offset++; } //add the server page table into the client's page directory pd->table[temp->offset] = pt_entry; //save the multiplier to work with the server adresses in the client pd temp->mult = (temp->offset - msg_port[port]->index) * 0x400000; //unblock the server if it's blocked and send the message dadelik if (unblock_server(port)) { //flush tlb mem_switch_to_kernel_directory(); i386_set_page_directory(temp->pd); void *buf = (void*)msg_port[port]->pcb->state->ecx; //copy the message request into buf copy_4(temp->req_buf,(unsigned int)buf+temp->mult,req_size/4); } //block the client state->eax = OK; block_process(port,state); //return reschedule return 1; }
//HumanoidController::HumanoidController(dmArticulation * robot) : TaskSpaceControllerA(robot) HumanoidController::HumanoidController(dmArticulation * robot) : TaskSpaceControllerConic(robot) { // Create Linearized Friction Cone Basis // in support frame (z-up) FrictionBasis = MatrixXF::Zero(6,NF); for (int j=0; j<NF; j++) { double angle = (j * 2 * M_PI) / NF; FrictionBasis(3,j) = MU*cos(angle); FrictionBasis(4,j) = MU*sin(angle); FrictionBasis(5,j) = 1; } // Initialize Support Jacobians SupportJacobians.resize(NS); for (int i=0; i<NS; i++) { SupportJacobians[i] = MatrixXF::Zero(6,1); } // Indicate support bodies SupportIndices.resize(NS); // number of support SupportIndices[0] = 11; // support body 0 SupportIndices[1] = 20; // support body 1 contactState.resize(NS); slidingState.resize(NS); // Construct transform from ankle frame to support frame Matrix3F RSup; // Sup - support // RSup << 0,0,1,0,1,0,-1,0,0; RSup << 0,0,1, 0,-1,0, 1,0,0; XformVector Xforms(NS); for (int k=0; k<NS; k++) { Xforms[k].resize(6,6); dmRigidBody * link = (dmRigidBody*) artic->getLink(SupportIndices[k]); dmContactModel * dmContactLattice = (dmContactModel *) link->getForce(0); Vector3F pRel; dmContactLattice->getContactPoint(0,pRel.data()); pRel(1) = 0; pRel(2) = 0; Xforms[k].block(0,0,3,3) = RSup; Xforms[k].block(0,3,3,3) = Matrix3F::Zero(); Xforms[k].block(3,0,3,3) = -RSup*cr3(pRel); Xforms[k].block(3,3,3,3) = RSup; } SupportXforms = Xforms; // Construct force transform from each contact point to support origin PointForceXforms.resize(NS); for (int i=0; i<NS; i++) { PointForceXforms[i].resize(NJ); //cout << "Getting forces for link " << SupportIndices[i] << endl; dmRigidBody * linki = (dmRigidBody*) artic->m_link_list[SupportIndices[i]]->link; dmContactModel * dmContactLattice = (dmContactModel *) linki->getForce(0); //cout << "Assigning Temporary Matricies" << endl; Matrix3F RSup = SupportXforms[i].block(0,0,3,3); Matrix3F tmpMat = SupportXforms[i].block(3,0,3,3)*RSup.transpose(); Vector3F piRelSup; crossExtract(tmpMat,piRelSup); //cout << "pirelsup " << endl << piRelSup << endl; for (int j=0; j<NP; j++) { Vector3F pRel, tmp; //Tmp is now the contact point location relative to the body coordinate dmContactLattice->getContactPoint(j,tmp.data()); // Point of contact (relative to support origin) in support coordinates pRel = RSup*tmp + piRelSup; PointForceXforms[i][j].setIdentity(6,6); PointForceXforms[i][j].block(0,3,3,3) = cr3(pRel); } } // Initialize variables q.resize(STATE_SIZE); qdDm.resize(STATE_SIZE); // joint rate DM qd.resize(RATE_SIZE); // joint rate grfInfo.localContacts = 0; pFoot.resize(NS); vFoot.resize(NS); aFoot.resize(NS); RFoot.resize(NS); // 3x3 pDesFoot.resize(NS); vDesFoot.resize(NS); RDesFoot.resize(NS); aDesFoot.resize(NS); kpFoot.resize(NS); kdFoot.resize(NS); for (int i=0; i<NS; i++) { pFoot[i].resize(3); vFoot[i].resize(6); aFoot[i].resize(6); pDesFoot[i].resize(3); vDesFoot[i].resize(6); aDesFoot[i].resize(6); aDesFoot[i].setZero(); vDesFoot[i].setZero(); pDesFoot[i].setZero(); RDesFoot[i].setZero(); // 3x3 kpFoot[i] = 0; kdFoot[i] = 0; } aComDes.resize(3); aComDes.setZero(); vComDes.resize(3); vComDes.setZero(); pComDes.resize(3); pComDes.setZero(); kComDes.resize(3); // angular momentum around com kComDes.setZero(); int numLinks = artic->getNumLinks(); RDesJoint.resize(numLinks); posDesJoint.resize(numLinks); rateDesJoint.resize(numLinks); accDesJoint.resize(numLinks); kpJoint.resize(numLinks); kdJoint.resize(numLinks); artic->getTrueNumDOFs(); // fill bodyi->index_ext, bodyi->dof for (int i=0; i<numLinks; i++) { LinkInfoStruct * bodyi = artic->m_link_list[i]; int dof = bodyi->dof; if (dof != 0) { if (dof == 6) { posDesJoint[i].setZero(3); // linear position of FB } else if (dof ==3) { // Do nothing, since only uses RDes } else if (dof == 1) { posDesJoint[i].setZero(1); } rateDesJoint[i].setZero(dof); accDesJoint[i].setZero(dof); } } }
HumanoidController::HumanoidController(dmArticulation * robot) : TaskSpaceControllerA(robot) { q.resize(STATE_SIZE); qdDm.resize(STATE_SIZE); qd.resize(RATE_SIZE); SupportIndices.resize(NS); SupportIndices[0] = 5; SupportIndices[1] = 9; contactState.resize(NS); slidingState.resize(NS); Matrix3F RSup; RSup << 0,0,1,0,1,0,-1,0,0; XformVector Xforms(NS); for (int k=0; k<NS; k++) { Xforms[k].resize(6,6); dmRigidBody * link = (dmRigidBody*) artic->getLink(SupportIndices[k]); dmContactModel * dmContactLattice = (dmContactModel *) link->getForce(0); Vector3F pRel; dmContactLattice->getContactPoint(0,pRel.data()); pRel(1) = 0; pRel(2) = 0; Xforms[k].block(0,0,3,3) = RSup; Xforms[k].block(0,3,3,3) = Matrix3F::Zero(); Xforms[k].block(3,0,3,3) = -RSup*cr3(pRel); Xforms[k].block(3,3,3,3) = RSup; } SupportXforms = Xforms; // Construct Point Force Transform PointForceXforms.resize(NS); for (int i=0; i<NS; i++) { PointForceXforms[i].resize(NJ); //cout << "Getting forces for link " << SupportIndices[i] << endl; dmRigidBody * linki = (dmRigidBody*) artic->m_link_list[SupportIndices[i]]->link; dmContactModel * dmContactLattice = (dmContactModel *) linki->getForce(0); //cout << "Assigning Temporary Matricies" << endl; Matrix3F RSup = SupportXforms[i].block(0,0,3,3); Matrix3F tmpMat = SupportXforms[i].block(3,0,3,3)*RSup.transpose(); Vector3F piRelSup; crossExtract(tmpMat,piRelSup); //cout << "pirelsup " << endl << piRelSup << endl; for (int j=0; j<NP; j++) { Vector3F pRel, tmp; //Tmp is now the contact point location relative to the body coordinate dmContactLattice->getContactPoint(j,tmp.data()); // Point of contact (relative to support origin) in support coordinates pRel = RSup*tmp + piRelSup; PointForceXforms[i][j].setIdentity(6,6); PointForceXforms[i][j].block(0,3,3,3) = cr3(pRel); } } grfInfo.localContacts = 0; pFoot.resize(NS); pDesFoot.resize(NS); vFoot.resize(NS); vDesFoot.resize(NS); RFoot.resize(NS); RDesFoot.resize(NS); aFoot.resize(NS); aDesFoot.resize(NS); kpFoot.resize(NS); kdFoot.resize(NS); for (int i=0; i<NS; i++) { pFoot[i].resize(3); pDesFoot[i].resize(3); vFoot[i].resize(6); vDesFoot[i].resize(6); aFoot[i].resize(6); aDesFoot[i].resize(6); aDesFoot[i].setZero(); vDesFoot[i].setZero(); pDesFoot[i].setZero(); RDesFoot[i].setZero(); kpFoot[i] = 0; kdFoot[i] = 0; } //ComTrajectory.setSize(3); //aDesFoot.resize(NS); //pDesFoot.resize(NS); //vDesFoot.resize(NS); //RDesFoot.resize(NS); aComDes.resize(3); aComDes.setZero(); vComDes.resize(3); vComDes.setZero(); pComDes.resize(3); pComDes.setZero(); kComDes.resize(3); kComDes.setZero(); }
void HumanoidController::HumanoidControl(ControlInfo & ci) { int taskRow = 0; Float discountFactor = 1; dmTimespec tv1, tv2, tv3, tv4; //Update Graphics Variables { ComPos[0] = pCom(0); ComPos[1] = pCom(1); ComPos[2] = pCom(2); ComDes[0] = pComDes(0); ComDes[1] = pComDes(1); ComDes[2] = pComDes(2); } // Perform Optimization { dmGetSysTime(&tv2); UpdateConstraintMatrix(); int maxPriorityLevels = OptimizationSchedule.maxCoeff(); const int numTasks = OptimizationSchedule.size(); if (maxPriorityLevels > 0) { for (int level=1; level<=maxPriorityLevels; level++) { taskConstrActive.setZero(numTasks); taskOptimActive.setZero(numTasks); bool runOpt = false; for (int i=0; i<numTasks;i ++) { if (OptimizationSchedule(i) == level) { taskOptimActive(i) = 1; runOpt = true; } else if ((OptimizationSchedule(i) < level) && (OptimizationSchedule(i) > -1)) { taskConstrActive(i) = 1; } } if (runOpt) { UpdateObjective(); UpdateHPTConstraintBounds(); dmGetSysTime(&tv3); //cout << "Optimizing level " << level << endl; Optimize(); for (int i=0; i<numTasks;i ++) { if (OptimizationSchedule(i) == level) { TaskBias(i) += TaskError(i); //cout << "Optimization Level " << level << " task error " << i << " = " << TaskError(i) << endl; } } } } } else { UpdateObjective(); UpdateHPTConstraintBounds(); dmGetSysTime(&tv3); Optimize(); } //exit(-1); // Extract Results hDotOpt = CentMomMat*qdd + cmBias; // Form Joint Input and simulate VectorXF jointInput = VectorXF::Zero(STATE_SIZE); // Extact Desired ZMP info zmpWrenchOpt.setZero(); Vector6F icsForce, localForce; Float * nICS = icsForce.data(), * nLoc = localForce.data(); Float * fICS = nICS+3, * fLoc = nLoc+3; for (int k1 = 0; k1 < NS; k1++) { LinkInfoStruct * listruct = artic->m_link_list[SupportIndices[k1]]; CartesianVector tmp; localForce = SupportXforms[k1].transpose()*fs.segment(6*k1,6); // Apply Spatial Force Transform Efficiently // Rotate Quantities APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,nLoc,nICS); APPLY_CARTESIAN_TENSOR(listruct->link_val2.R_ICS,fLoc,fICS); // Add the r cross f CROSS_PRODUCT(listruct->link_val2.p_ICS,fICS,tmp); ADD_CARTESIAN_VECTOR(nICS,tmp); zmpWrenchOpt+=icsForce; } transformToZMP(zmpWrenchOpt,zmpPosOpt); int k = 7; // Skip over floating base (i=1 initially) for (int i=1; i<artic->m_link_list.size(); i++) { LinkInfoStruct * linki = artic->m_link_list[i]; if (linki->dof) { //cout << "Link " << i << " dof = " << linki->dof << endl; //cout << "qd = " << qdDm.segment(k,linki->dof).transpose() << endl; jointInput.segment(k,linki->dof) = tau.segment(linki->index_ext-6,linki->dof); k+=linki->link->getNumDOFs(); } } //cout << "Tau = " << tau.transpose() << endl; //cout << "Joint input = " << jointInput.transpose() << endl; //exit(-1); //jointInput.segment(7,NJ) = tau; artic->setJointInput(jointInput.data()); ComputeActualQdd(qddA); dmGetSysTime(&tv4); } //Populate Control Info Struct { ci.calcTime = timeDiff(tv1,tv2); ci.setupTime = timeDiff(tv2,tv3); ci.optimTime = timeDiff(tv3,tv4); ci.totalTime = timeDiff(tv1,tv4); ci.iter = iter; } #ifdef CONTROL_DEBUG // Debug Code { cout << "q " << q.transpose() << endl; cout << "qd " << qdDm.transpose() << endl; cout << "qd2 " << qd.transpose() << endl; cout << "Task Bias " << TaskBias << endl; //cout << "H = " << endl << artic->H << endl; cout << "CandG = " << endl << artic->CandG.transpose() << endl; if (simThread->sim_time > 0) { cout << setprecision(5); MSKboundkeye key; double bl,bu; for (int i=0; i<numCon; i++) { MSK_getbound(task, MSK_ACC_VAR, i, &key, &bl, &bu); cout << "i = " << i; switch (key) { case MSK_BK_FR: cout << " Free " << endl; break; case MSK_BK_LO: cout << " Lower Bound " << endl; break; case MSK_BK_UP: cout << " Upper Bound " << endl; break; case MSK_BK_FX: cout << " Fixed " << endl; break; case MSK_BK_RA: cout << " Ranged " << endl; break; default: cout << " Not sure(" << key << ")" << endl; break; } cout << bl << " to " << bu << endl; } cout << "x(57) = " << xx(57) << endl; cout << "tau = " << tau.transpose() << endl; cout << "qdd = " << qdd.transpose() << endl; cout << "fs = " << fs.transpose() << endl; //cout << "lambda = " << lambda.transpose() << endl; VectorXF a = TaskJacobian * qdd; //cout << "a" << endl; VectorXF e = TaskJacobian * qdd - TaskBias; cout << "e = " << e.transpose() << endl; MatrixXF H = artic->H; VectorXF CandG = artic->CandG; MatrixXF S = MatrixXF::Zero(NJ,NJ+6); S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ); VectorXF generalizedContactForce = VectorXF::Zero(NJ+6); for (int i=0; i<NS; i++) { generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6); } VectorXF qdd2 = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG); //cout << "qdd2 = " << qdd2.transpose() << endl << endl << endl; //cout << "CandG " << CandG.transpose() << endl; //cout << "Gen Contact Force " << generalizedContactForce.transpose() << endl; cout << "hdot " << (CentMomMat*qdd + cmBias).transpose() << endl; cout << "cmBias " << cmBias.transpose() << endl; cout << "qd " << qd.transpose() << endl; //VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG); //FullPivHouseholderQR<MatrixXF> fact(H); cout <<"fNet \t" << (fs.segment(3,3) + fs.segment(9,3)).transpose() << endl; Vector3F g; g << 0,0,-9.81; cout <<"hdot - mg\t" << (CentMomMat*qdd + cmBias).segment(3,3).transpose() - totalMass * g.transpose()<< endl; exit(-1); } // Old Debug Code { VectorXF generalizedContactForce = VectorXF::Zero(NJ+6); Matrix6F X; MatrixXF Jac; X.setIdentity(); for (int i=0; i<NS; i++) { int linkIndex = SupportIndices[i]; artic->computeJacobian(linkIndex,X,Jac); dmRigidBody * link = (dmRigidBody *) artic->getLink(linkIndex); for (int j=0; j< link->getNumForces(); j++) { dmForce * f = link->getForce(j); Vector6F fContact; f->computeForce(artic->m_link_list[linkIndex]->link_val2,fContact.data()); generalizedContactForce += Jac.transpose()*fContact; } } cout << "J' f = " << generalizedContactForce.transpose() << endl; VectorXF qdd3 = H.inverse()*(S.transpose() * tau - CandG + generalizedContactForce); cout << "qdd3 = " << qdd3.transpose() << endl; VectorXF state = VectorXF::Zero(2*(NJ+7)); state.segment(0,NJ+7) = q; state.segment(NJ+7,NJ+7) = qdDm; VectorXF stateDot = VectorXF::Zero(2*(NJ+7)); //Process qdds artic->dynamics(state.data(),stateDot.data()); // VectorXF qdds = VectorXF::Zero(NJ+6); qdds.segment(0,6) = stateDot.segment(NJ+7,6); //cout << "w x v " << cr3(qd.segment(0,3))*qd.segment(3,3) << endl; qdds.segment(3,3) -= cr3(qdDm.segment(0,3))*qdDm.segment(3,3); qdds.segment(6,NJ) = stateDot.segment(NJ+7*2,NJ); Matrix3F ics_R_fb; copyRtoMat(artic->m_link_list[0]->link_val2.R_ICS,ics_R_fb); qdds.head(3) = ics_R_fb.transpose() * qdds.head(3); qdds.segment(3,3) = ics_R_fb.transpose() * qdds.segment(3,3); cout << "qdds = " << qdds.transpose() << endl; //cout << "CandG " << endl << CandG << endl; //cout << setprecision(6); //cout << "I_0^C = " << endl << artic->H.block(0,0,6,6) << endl; //exit(-1); } } #endif /*{ MatrixXF H = artic->H; VectorXF CandG = artic->CandG; MatrixXF S = MatrixXF::Zero(NJ,NJ+6); S.block(0,6,NJ,NJ) = MatrixXF::Identity(NJ,NJ); VectorXF generalizedContactForce = VectorXF::Zero(NJ+6); for (int i=0; i<NS; i++) { generalizedContactForce += SupportJacobians[i].transpose()*fs.segment(6*i,6); } qdd = H.inverse()*(S.transpose() * tau + generalizedContactForce- CandG); cout << setprecision(8); cout << "fs " << endl << fs << endl; cout << "qdd " << endl << qdd << endl; } exit(-1);*/ static int numTimes = 0; numTimes++; //Float dummy; //cin >> dummy; //if (numTimes == 2) { // exit(-1); //} //exit(-1); }