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