void Animation::setIK(IKPartType part,bool flag)
{
if(ikOn[part]==flag) return;
ikOn[part]=flag;
if(flag)
{
switch(part)
{
case IK_LHAND: ikTree.setGoal(frame,"lHand"); break;
case IK_RHAND: ikTree.setGoal(frame,"rHand"); break;
case IK_LFOOT: ikTree.setGoal(frame,"lFoot"); break;
case IK_RFOOT: ikTree.setGoal(frame,"rFoot"); break;
default: break;
}
}
else
{
switch(part)
{
case IK_LHAND:
applyIK("lHand");
applyIK("lForeArm");
applyIK("lShldr");
applyIK("lCollar");
if(!ikOn[IK_RHAND])
{
applyIK("chest");
applyIK("abdomen");
}
break;
case IK_RHAND:
applyIK("rHand");
applyIK("rForeArm");
applyIK("rShldr");
applyIK("rCollar");
if (!ikOn[IK_LHAND])
{
applyIK("chest");
applyIK("abdomen");
}
break;
case IK_LFOOT:
applyIK("lThigh");
applyIK("lShin");
applyIK("lFoot");
break;
case IK_RFOOT:
applyIK("rThigh");
applyIK("rShin");
applyIK("rFoot");
break;
default:
break;
}
}
}
void controller::legController(int leg_id, int phase) {
cout << endl << "Leg Controller = " << leg_id << endl;
if(swingStart[leg_id] == phase) {
cout << "Starting swing phase"<< endl;
computePlacementLocation(leg_id, lfHeight[0]);
setFootLocation(leg_id, phase);
}
else if(isInSwing(leg_id)) {
cout << "Swing phase"<< endl;
swingFlag[leg_id] = true;
//Get target position
setFootLocation(leg_id, phase);
vector<float> targetPosition = getTargetPosition(leg_id);
//Get link lengths
vector<float> lengths(2);
for(int i = 0; i < 2; i++) {
lengths[i] = body_bag->getFootLinkLength(leg_id, i);
}
//Set axis perpendicular to the kinematic chain plane
Eigen::MatrixXf axis = Eigen::MatrixXf::Random(4, 1);
axis(0, 0) = 0;
axis(1, 0) = 0;
if(leg_id % 2 == 0) {
axis(2, 0) = 1;
}
else {
axis(2, 0) = -1;
}
axis(3, 0) = 1;
//Get transformation matrices
vector<Eigen::MatrixXf> transformationMatrices(2);
Eigen::MatrixXf alignment = Eigen::MatrixXf::Identity(4, 4);
alignment(1, 1) = 0;
alignment(1, 2) = 1;
alignment(2, 1) = -1;
alignment(2, 2) = 0;
Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4);
const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr(i, j) = rotation_matrix_ode[i*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf mr2 = Eigen::MatrixXf::Identity(4, 4);
const dReal *rotation_matrix_ode2 = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr2(i, j) = rotation_matrix_ode2[i*4+j];
}
}
mr2(3, 0) = 0;
mr2(3, 1) = 0;
mr2(3, 2) = 0;
mr2(3, 3) = 1;
transformationMatrices[0] = mr*alignment.inverse();
transformationMatrices[1] = mr2*alignment.inverse();
//Get translation matrix
const dReal *center_location = dBodyGetPosition(body_bag->getFootLinkBody(leg_id,0)); //get location of the center of the link
Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4); //translate to the start of the link
mt(1, 3) = -body_bag->getFootLinkLength(leg_id, 0)/2;
mr = Eigen::MatrixXf::Identity(4, 4); //get orientation
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 0));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf origin = Eigen::MatrixXf::Random(4, 1);
origin(0, 0) = 0;
origin(1, 0) = 0;
origin(2, 0) = 0;
origin(3, 0) = 1;
Eigen::MatrixXf addition = alignment.inverse()*mr.inverse()*mt*origin; //part to add to the center location
Eigen::MatrixXf translationMatrix = Eigen::MatrixXf::Identity(4, 4);
translationMatrix(0, 3) = center_location[0] + addition(0, 0);
translationMatrix(1, 3) = center_location[1] + addition(1, 0);
translationMatrix(2, 3) = center_location[2] + addition(2, 0);
vector<float> angles = body_bag->getAngles(0);
Eigen::MatrixXf jointAngleChange = applyIK(lengths, transformationMatrices, angles, targetPosition, translationMatrix, axis);
//Use PD controllers to get torque
//link 1
float torque = foot_link_gain_kp[leg_id][0]*jointAngleChange(0,0) + foot_link_gain_kd[leg_id][0]*(jointAngleChange(0,0) - foot_link_pd_error[leg_id][0]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,0), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
for(int i = 0; i < 3; i++) {
swing_torque[leg_id][i] = axis(i, 0)*torque;
}
foot_link_pd_error[leg_id][0] = jointAngleChange(0,0);
//link 2
torque = foot_link_gain_kp[leg_id][1]*jointAngleChange(1,0) + foot_link_gain_kd[leg_id][1]*(jointAngleChange(1,0) - foot_link_pd_error[leg_id][1]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,1), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][1] = jointAngleChange(1,0);
//link 3
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
float swing_phase = computeSwingPhase(leg_id, phase);
float error = ((30*M_PI)/180)*(1 - swing_phase) - ((60*M_PI)/180)*swing_phase - acos(mr(0, 0));
torque = foot_link_gain_kp[leg_id][2]*error + foot_link_gain_kd[leg_id][2]*(error - foot_link_pd_error[leg_id][2]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,2), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][2] = error;
//link 4
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
swing_phase = computeSwingPhase(leg_id, phase);
error = ((90*M_PI)/180)*(1 - swing_phase) - ((30*M_PI)/180)*swing_phase - acos(mr(0, 0));
torque = foot_link_gain_kp[leg_id][3]*error + foot_link_gain_kd[leg_id][3]*(error - foot_link_pd_error[leg_id][3]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,3), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][3] = error;
//Apply gravity compensation
float force = 9.810*body_bag->getDensity()*(body_bag->getFootLinkLength(leg_id, 0) + body_bag->getFootLinkLength(leg_id, 1) + body_bag->getFootLinkLength(leg_id, 2) + body_bag->getFootLinkLength(leg_id, 3));
dBodyAddForce(body_bag->getFootLinkBody(leg_id, 2), 0.0, 9.810*body_bag->getDensity()*body_bag->getFootLinkLength(leg_id, 2), 0.0);
if(leg_id < 2) {
dBodyAddForce(body_bag->getBackLink1Body(), 0.0, force, 0.0);
}
else {
dBodyAddForce(body_bag->getBackLink6Body(), 0.0, force, 0.0);
}
}
else {
cout << "Stance phase"<< endl;
swingFlag[leg_id] = false;
/*for(int i = 0; i < 3; i++){
swing_torque[leg_id][i] = 0;
}*/
stanceLegTreatment(leg_id);
}
}