int main()
{
using namespace KDL;
//Definition of kinematic chain
KDL::Tree youBotTree;
//note that it seems youbot_arm.urdf and youbot.urdf use different values.
//parser's assumption is that it return joints which are not of type None.
if (!kdl_parser::treeFromFile("/home/azamat/programming/ros-diamondback/youbot-ros-pkg/youbot_common/youbot_description/robots/youbot.urdf", youBotTree))
{
std::cout << "Failed to construct kdl tree" << std::endl;
return false;
}
// else
// {
// std::cout << "succeeded " << youBotTree.getNrOfJoints()<< std::endl;
// std::cout << "succeeded " << youBotTree.getNrOfSegments()<< std::endl;
// }
KDL::Chain chain;
youBotTree.getChain("arm_link_0","arm_link_5",chain);
unsigned int numberOfJoints = chain.getNrOfJoints();
unsigned int numberOfLinks = chain.getNrOfSegments();
// std::cout << "number of chain links " << numberOfLinks << std::endl;
// std::cout << "number of chain joints " << numberOfJoints << std::endl;
//~Definition of kinematic chain
//Definition of constraints and external disturbances
//--------------------------------------------------------------------------------------//
//Constraint force matrix at the end-effector
//What is the convention for the spatial force matrix; is it the same as in thesis?
//NOTE AZAMAT: Constraint are also defined in local reference frame?!
unsigned int numberOfConstraints = 2;
Twist constraintForce;
Twists constraintForces;
for (unsigned int i = 0; i < numberOfJoints; i++)
{
SetToZero(constraintForce);
constraintForces.push_back(constraintForce);
}
constraintForces[0].vel[0] = 0;
constraintForces[1].vel[1] = 0;
constraintForces[2].vel[2] = 0;
#ifdef X_CONSTRAINT_SET
constraintForces[0].vel[0] = 1;
#endif //~X_CONSTRAINT_SET
#ifdef Y_CONSTRAINT_SET
constraintForces[1].vel[1] = 1;
#endif //~Y_CONSTRAINT_SET
#ifdef Z_CONSTRAINT_SET
constraintForces[2].vel[2] = 1;
#endif //~Z_CONSTRAINT_SET
//Acceleration energy and constraint matrix at the end-effector
Jacobian alpha(numberOfConstraints);
JntArray betha(numberOfConstraints); //set to zero
JntArray bethaControl(numberOfConstraints);
for (unsigned int i = 0; i < numberOfConstraints; i++)
{
alpha.setColumn(i, constraintForces[i]);
betha(i) = 0.0;
bethaControl(i) = 0.0;
}
//arm root acceleration
Vector linearAcc(0.0, 0.0, 9.8); //gravitational acceleration along Z
Vector angularAcc(0.0, 0.0, 0.0);
Twist twist0(linearAcc, angularAcc);
//external forces on the arm
Wrench externalNetForce;
Wrenches externalNetForces;
for (unsigned int i = 0; i < numberOfLinks; i++)
{
externalNetForces.push_back(externalNetForce);
}
//~Definition of constraints and external disturbances
//Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
ChainIdSolver_Vereshchagin constraintSolver(chain, twist0, numberOfConstraints);
//These arrays of joint values contain actual and desired values
//actual is generated by a solver and integrator
//desired is given by an interpolator
//error is the difference between desired-actual
//0-actual, 1-desired, 2-error, 3-sum of error
unsigned int k = 4;
JntArray jointPoses[k];
JntArray jointRates[k];
JntArray jointAccelerations[k];
JntArray jointTorques[k];
JntArray jointControlTorques[k];
for (unsigned int i = 0; i < k; i++)
{
JntArray jointValues(numberOfJoints);
jointPoses[i] = jointValues;
jointRates[i] = jointValues;
jointAccelerations[i] = jointValues;
jointTorques[i] = jointValues;
jointControlTorques[i] = jointValues;
}
//cartesian space/link values
//0-actual, 1-desire, 2-error, 3-errorsum
Frames cartX[k];
Twists cartXDot[k];
Twists cartXDotDot[k];
Twist accLink;
KDL::Frame frameTemp;
for (unsigned int i = 0; i < k; i++) //i is number of variables (actual, desired, error)
{
for (unsigned int j = 0; j < numberOfLinks; j++) //j is number of links
{
cartX[i].push_back(frameTemp);
cartXDot[i].push_back(accLink);
cartXDotDot[i].push_back(accLink);
}
}
// Initial arm position configuration/constraint, negative in clockwise
JntArray jointInitialPose(numberOfJoints);
jointInitialPose(0) = -M_PI/4.0;
jointInitialPose(1) = M_PI / 6.0;
jointInitialPose(2) = M_PI / 12.0;
jointInitialPose(3) = -M_PI / 3.0;
jointInitialPose(4) = M_PI / 18.0;
//corresponds to x=0.031827 y = -0.007827 z = 0.472972
JntArray jointFinalPose(numberOfJoints);
jointFinalPose(0) = M_PI / 2.0;
jointFinalPose(1) = M_PI / 2.5;
jointFinalPose(2) = 0.0;
jointFinalPose(3) = 0.0;
jointFinalPose(4) = 0.0;
// corresponds to 0.024000 -0.367062 0.242885
for (unsigned int i = 0; i < numberOfJoints; i++)
{
//actual initial
jointPoses[0](i) = jointInitialPose(i);
//desired initial
jointPoses[1](i) = jointInitialPose(i);
}
//test
#ifdef FKPOSE_TEST
ChainFkSolverPos_recursive fksolver(chain);
Frame tempEE;
fksolver.JntToCart(jointPoses[0], tempEE, 5);
std::cout << tempEE << std::endl;
fksolver.JntToCart(jointFinalPose, tempEE, 5);
std::cout << tempEE << std::endl;
#endif //~FKPOSE_TEST
//~Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
//Definition of process main loop
//-------------------------------------------------------------------------------------//
double taskTimeConstant = 2; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
double simulationTime = 2.5 * taskTimeConstant;
double timeDelta = 0.001;
double timeToSettle = 0.5;
int status;
double alfa(0.0), beta(0.0), gamma(0.0);
//Controller gain configurations
double ksi[3] = {1.0, 1.0, 1.0}; //damping factor
double Kp[3];
Kp[0] = 240.1475/(timeToSettle*timeToSettle);//0.1475
Kp[1] = 140.0/(timeToSettle*timeToSettle);
Kp[2] = 240.0/(timeToSettle*timeToSettle);
double Kv[3];
Kv[0] = 55.2245*ksi[0]/timeToSettle;//2.2245
Kv[1] = 30.6*ksi[1]/timeToSettle;
Kv[2] = 30.6*ksi[1]/timeToSettle;
double Ki[3] = {0.0, 0.0, 0.0};
double K = 0.005;
//For joint space control without constraints using computed torque
// double ksi[2] = {1.0, 1.0}; //damping factor
double Kpq[numberOfJoints];
Kpq[0] = 2.5/(timeToSettle*timeToSettle);
Kpq[1] = 2.2/(timeToSettle*timeToSettle);
Kpq[2] = 20.1/(timeToSettle*timeToSettle);
Kpq[3] = 40.1/(timeToSettle*timeToSettle);
Kpq[4] = 40.1/(timeToSettle*timeToSettle);
double Kvq[numberOfJoints];
Kvq[0] = .5*ksi[0]/timeToSettle;
Kvq[1] = 1.0*ksi[1]/timeToSettle;
Kvq[2] = 3.0*ksi[1]/timeToSettle;
Kvq[3] = 3.0*ksi[1]/timeToSettle;
Kvq[4] = 3.0*ksi[1]/timeToSettle;
//Interpolator parameters:
// Initial x=0.031827 y = -0.007827 z = 0.472972
// Final 0.024000 -0.367062 0.242885
//cartesian space
double b0_x = 0.031827; //should come from initial joint configuration
double b1_x = 0.0;
double b2_x = ((0.031827 - b0_x)*3.0 / (simulationTime * simulationTime));
double b3_x = -((0.031827 - b0_x)*2.0 / (simulationTime * simulationTime * simulationTime));
double b0_y = -0.007827; //should come from initial joint configuration
double b1_y = 0.0;
double b2_y = ((-0.007827 - b0_y)*3.0 / (simulationTime * simulationTime));
double b3_y = -((-0.007827 - b0_y)*2.0 / (simulationTime * simulationTime * simulationTime));
double b0_z = 0.472972; //should come from initial joint configuration
double b1_z = 0.0;
double b2_z = ((0.242885 - b0_z)*3.0 / (simulationTime * simulationTime));
double b3_z = -((0.242885 - b0_z)*2.0 / (simulationTime * simulationTime * simulationTime));
//joint space
double a0_q0 = jointInitialPose(0);
double a1_q0 = 0.0;
double a2_q0 = ((jointFinalPose(0) - jointInitialPose(0))*3.0 / (simulationTime * simulationTime));
double a3_q0 = -((jointFinalPose(0) - jointInitialPose(0))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q1 = jointInitialPose(1);
double a1_q1 = 0.0;
double a2_q1 = ((jointFinalPose(1) - jointInitialPose(1))*3.0 / (simulationTime * simulationTime));
double a3_q1 = -((jointFinalPose(1) - jointInitialPose(1))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q2 = jointInitialPose(2);
double a1_q2 = 0.0;
double a2_q2 = ((jointFinalPose(2) - jointInitialPose(2))*3.0 / (simulationTime * simulationTime));
double a3_q2 = -((jointFinalPose(2) - jointInitialPose(2))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q3 = jointInitialPose(3);
double a1_q3 = 0.0;
double a2_q3 = ((jointFinalPose(3) - jointInitialPose(3))*3.0 / (simulationTime * simulationTime));
double a3_q3 = -((jointFinalPose(3) - jointInitialPose(3))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q4 = jointInitialPose(4);
double a1_q4 = 0.0;
double a2_q4 = ((jointFinalPose(4) - jointInitialPose(4))*3.0 / (simulationTime * simulationTime));
double a3_q4 = -((jointFinalPose(4) - jointInitialPose(4))*2.0 / (simulationTime * simulationTime * simulationTime));
double feedforwardJointTorque0 = 0.0;
double feedforwardJointTorque1 = 0.0;
double feedforwardJointTorque2 = 0.0;
double feedforwardJointTorque3 = 0.0;
double feedforwardJointTorque4 = 0.0;
//---------------------------------------------------------------------------------------//
//Main simulation loop
for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
{
//Interpolation (Desired) q = a0+a1t+a2t^2+a3t^3
cartX[1][4].p[0] = b0_x + b1_x * t + b2_x * t * t + b3_x * t * t*t;
cartX[1][4].p[1] = b0_y + b1_y * t + b2_y * t * t + b3_y * t * t*t;
cartX[1][4].p[2] = b0_z + b1_z * t + b2_z * t * t + b3_z * t * t*t;
cartXDot[1][4].vel[0] = b1_x + 2 * b2_x * t + 3 * b3_x * t*t;
cartXDot[1][4].vel[1] = b1_y + 2 * b2_y * t + 3 * b3_y * t*t;
cartXDot[1][4].vel[2] = b1_z + 2 * b2_z * t + 3 * b3_z * t*t;
cartXDotDot[1][4].vel[0] = 2 * b2_x + 6 * b3_x*t;
cartXDotDot[1][4].vel[1] = 2 * b2_y + 6 * b3_y*t;
cartXDotDot[1][4].vel[2] = 2 * b2_z + 6 * b3_z*t;
#ifdef DESIRED_CARTESIAN_VALUES
printf("%f %f %f %f %f %f %f\n", t, cartX[1][4].p[0], cartX[1][4].p[1], cartX[1][4].p[2], cartXDot[1][4].vel[0], cartXDot[1][4].vel[1], cartXDot[1][4].vel[2]);
#endif //~DESIRED_CARTESIAN_VALUES
jointPoses[1](0) = a0_q0 + a1_q0 * t + a2_q0 * t * t + a3_q0 * t * t*t;
jointPoses[1](1) = a0_q1 + a1_q1 * t + a2_q1 * t * t + a3_q1 * t * t*t;
jointPoses[1](2) = a0_q2 + a1_q2 * t + a2_q2 * t * t + a3_q2 * t * t*t;
jointPoses[1](3) = a0_q3 + a1_q3 * t + a2_q3 * t * t + a3_q3 * t * t*t;
jointPoses[1](4) = a0_q4 + a1_q4 * t + a2_q4 * t * t + a3_q4 * t * t*t;
jointRates[1](0) = a1_q0 + 2 * a2_q0 * t + 3 * a3_q0 * t*t;
jointRates[1](1) = a1_q1 + 2 * a2_q1 * t + 3 * a3_q1 * t*t;
jointRates[1](2) = a1_q2 + 2 * a2_q2 * t + 3 * a3_q2 * t*t;
jointRates[1](3) = a1_q3 + 2 * a2_q3 * t + 3 * a3_q3 * t*t;
jointRates[1](4) = a1_q4 + 2 * a2_q4 * t + 3 * a3_q4 * t*t;
jointAccelerations[1](0) = 2 * a2_q0 + 6 * a3_q0*t;
jointAccelerations[1](1) = 2 * a2_q1 + 6 * a3_q1*t;
jointAccelerations[1](2) = 2 * a2_q2 + 6 * a3_q2*t;
jointAccelerations[1](3) = 2 * a2_q3 + 6 * a3_q3*t;
jointAccelerations[1](4) = 2 * a2_q4 + 6 * a3_q4*t;
#ifdef DESIRED_JOINT_VALUES
printf("%f %f %f %f %f %f %f %f %f %f %f\n", t,jointPoses[1](0), jointPoses[1](1),jointPoses[1](2), jointPoses[1](3), jointPoses[1](4),
jointRates[1](0), jointRates[1](1), jointRates[1](2), jointRates[1](3), jointRates[1](4));
#endif //~DESIRED_JOINT_VALUES
status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, bethaControl, externalNetForces, jointTorques[0], jointControlTorques[0]);
constraintSolver.getLinkCartesianPose(cartX[0]);
constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
constraintSolver.getLinkCartesianAcceleration(cartXDotDot[0]);
cartX[0][4].M.GetEulerZYX(alfa,beta,gamma);
#ifdef ACTUAL_CARTESIAN_VALUES
printf("%f %f %f %f %f %f %f\n", t, cartX[0][4].p[0], cartX[0][4].p[1], cartX[0][4].p[2], alfa, beta, gamma);
#endif //~ACTUAL_CARTESIAN_VALUES
//Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
for (unsigned int i = 0; i < numberOfJoints; i++)
{
jointRates[0](i) = jointRates[0](i) + jointAccelerations[0](i) * timeDelta; //Euler Forward
jointPoses[0](i) = jointPoses[0](i) + (jointRates[0](i) - jointAccelerations[0](i) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
}
#ifdef ACTUAL_JOINT_VALUES
printf("%f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
t,jointPoses[0](0), jointPoses[0](1),jointPoses[0](2), jointPoses[0](3), jointPoses[0](4), jointRates[0](0), jointRates[0](1), jointRates[0](2), jointRates[0](3), jointRates[0](4),
jointAccelerations[0](0), jointAccelerations[0](1), jointAccelerations[0](2), jointAccelerations[0](3), jointAccelerations[0](4), jointTorques[0](0), jointTorques[0](1), jointTorques[0](2), jointTorques[0](3), jointTorques[0](4));
#endif //~ACTUAL_JOINT_VALUES
//Error
for (unsigned int i = 0; i < numberOfJoints; i++)
{
jointPoses[2](i) = jointPoses[1](i) - jointPoses[0](i);
jointRates[2](i) = jointRates[1](i) - jointRates[0](i);
jointAccelerations[2](i) = jointAccelerations[1](i) - jointAccelerations[0](i);
// jointPoses[3](i) += timeDelta * jointPoses[2](i);
}
#ifdef JOINT_ERROR_VALUES
printf("%f %f %f %f %f %f\n", t,jointPoses[2](0), jointPoses[2](1),jointPoses[2](2), jointPoses[2](3), jointPoses[2](4));
#endif //~JOINT_ERROR_VALUES
cartX[2][4].p[0] = cartX[1][4].p[0] - cartX[0][4].p[0];
cartX[2][4].p[1] = cartX[1][4].p[1] - cartX[0][4].p[1];
cartX[2][4].p[2] = cartX[1][4].p[2] - cartX[0][4].p[2];
cartXDot[2][4].vel[0] = cartXDot[1][4].vel[0] - cartXDot[0][4].vel[0];
cartXDot[2][4].vel[1] = cartXDot[1][4].vel[1] - cartXDot[0][4].vel[1];
cartXDot[2][4].vel[2] = cartXDot[1][4].vel[2] - cartXDot[0][4].vel[2];
cartXDotDot[2][4].vel[0] = cartXDotDot[1][4].vel[0] - cartXDotDot[0][4].vel[0];
cartXDotDot[2][4].vel[1] = cartXDotDot[1][4].vel[1] - cartXDotDot[0][4].vel[1];
cartXDotDot[2][4].vel[2] = cartXDotDot[1][4].vel[2] - cartXDotDot[0][4].vel[2];
cartX[3][4].p[0] += timeDelta * cartX[2][4].p[0]; //for integral term;
cartX[3][4].p[1] += timeDelta * cartX[2][4].p[1];
cartX[3][4].p[2] += timeDelta * cartX[2][4].p[2];
#ifdef CARTESIAN_ERROR_VALUES
printf("%f %f %f %f %f %f %f\n", t, cartX[2][4].p[0], cartX[2][4].p[1], cartX[2][4].p[2], cartXDot[2][4].vel[0], cartXDot[2][4].vel[1], cartXDot[2][4].vel[2]);
#endif //~CARTESIAN_ERROR_VALUES
//Regulator or controller
//acceleration energy control
betha(0) = alpha(0, 0)*(K * cartXDotDot[2][numberOfLinks-1].vel[0] + (Kv[0]) * cartXDot[2][numberOfLinks-1].vel[0] + (Kp[0]) * cartX[2][numberOfLinks-1].p[0]+ Ki[1] * cartX[3][numberOfLinks-1].p[0]);
betha(1) = alpha(1, 1)*(K * cartXDotDot[2][numberOfLinks-1].vel[1] + (Kv[1]) * cartXDot[2][numberOfLinks-1].vel[1] + (Kp[1]) * cartX[2][numberOfLinks-1].p[1] + Ki[1] * cartX[3][numberOfLinks-1].p[1]);
// priority posture control
// jointControlTorques[0](0) = jointAccelerations[1](0) + (Kpq[0])*jointPoses[2](0) + (Kvq[0])*jointRates[2](0);
// jointControlTorques[0](1) = jointAccelerations[1](1) + (Kpq[1])*jointPoses[2](1) + (Kvq[1])*jointRates[2](1);//computed joint torque control
// jointControlTorques[0](2) = jointAccelerations[1](2) + (Kpq[2])*jointPoses[2](2) + (Kvq[2])*jointRates[2](2);
// jointControlTorques[0](3) = jointAccelerations[1](3) + (Kpq[3])*jointPoses[2](3) + (Kvq[3])*jointRates[2](3);
// jointControlTorques[0](4) = jointAccelerations[1](4) + (Kpq[3])*jointPoses[2](4) + (Kvq[4])*jointRates[2](4);
//priority constraint control
// feedforwardJointTorque0 = jointAccelerations[1](0) + (Kpq[0])*jointPoses[2](0) + (Kvq[0])*jointRates[2](0);
feedforwardJointTorque1 = jointAccelerations[1](1) + (Kpq[1])*jointPoses[2](1) + (Kvq[1])*jointRates[2](1);
feedforwardJointTorque2 = jointAccelerations[1](2) + (Kpq[2])*jointPoses[2](2) + (Kvq[2])*jointRates[2](2);
feedforwardJointTorque3 = jointAccelerations[1](3) + (Kpq[3])*jointPoses[2](3) + (Kvq[3])*jointRates[2](3);
feedforwardJointTorque4 = jointAccelerations[1](4) + (Kpq[4])*jointPoses[2](4) + (Kvq[4])*jointRates[2](4);
// jointTorques[0](0) = jointTorques[0](0) + feedforwardJointTorque0;
jointTorques[0](1) = jointTorques[0](1) + feedforwardJointTorque1;
jointTorques[0](2) = jointTorques[0](2) + feedforwardJointTorque2;
jointTorques[0](3) = jointTorques[0](3) + feedforwardJointTorque3;
jointTorques[0](4) = jointTorques[0](4) + feedforwardJointTorque4;
//For cartesian space control one needs to calculate from the obtained joint space value, new cartesian space poses.
//Then based on the difference of the signal (desired-actual) we define a regulation function (controller)
// this difference should be compensated either by joint torques.
// externalNetForces[numberOfLinks-1].force[0] = ((Kv[0]) * cartXDot[2][numberOfLinks-1].vel[0] + (Kp[0]) * cartX[2][numberOfLinks-1].p[0] );
// externalNetForces[numberOfLinks-1].force[1] = ((Kv[1]) * cartXDot[2][numberOfLinks-1].vel[1] + (Kp[1]) * cartX[2][numberOfLinks-1].p[1] );
externalNetForces[numberOfLinks-1].force[2] = ((Kv[2]) * cartXDot[2][numberOfLinks-1].vel[2] + (Kp[2]) * cartX[2][numberOfLinks-1].p[2] );
}
//~Definition of process main loop
//-------------------------------------------------------------------------------------//
return 0;
}
void SolverTest::VereshchaginTest()
{
Vector constrainXLinear(1.0, 0.0, 0.0);
Vector constrainXAngular(0.0, 0.0, 0.0);
Vector constrainYLinear(0.0, 0.0, 0.0);
Vector constrainYAngular(0.0, 0.0, 0.0);
// Vector constrainZLinear(0.0, 0.0, 0.0);
//Vector constrainZAngular(0.0, 0.0, 0.0);
Twist constraintForcesX(constrainXLinear, constrainXAngular);
Twist constraintForcesY(constrainYLinear, constrainYAngular);
//Twist constraintForcesZ(constrainZLinear, constrainZAngular);
Jacobian alpha(1);
//alpha.setColumn(0, constraintForcesX);
alpha.setColumn(0, constraintForcesX);
//alpha.setColumn(0, constraintForcesZ);
//Acceleration energy at the end-effector
JntArray betha(1); //set to zero
betha(0) = 0.0;
//betha(1) = 0.0;
//betha(2) = 0.0;
//arm root acceleration
Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
Vector angularAcc(0.0, 0.0, 0.0);
Twist twist1(linearAcc, angularAcc);
//external forces on the arm
Vector externalForce1(0.0, 0.0, 0.0);
Vector externalTorque1(0.0, 0.0, 0.0);
Vector externalForce2(0.0, 0.0, 0.0);
Vector externalTorque2(0.0, 0.0, 0.0);
Wrench externalNetForce1(externalForce1, externalTorque1);
Wrench externalNetForce2(externalForce2, externalTorque2);
Wrenches externalNetForce;
externalNetForce.push_back(externalNetForce1);
externalNetForce.push_back(externalNetForce2);
//~Definition of constraints and external disturbances
//-------------------------------------------------------------------------------------//
//Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
int numberOfConstraints = 1;
ChainIdSolver_Vereshchagin constraintSolver(chaindyn, twist1, numberOfConstraints);
//These arrays of joint values contain actual and desired values
//actual is generated by a solver and integrator
//desired is given by an interpolator
//error is the difference between desired-actual
//in this test only the actual values are printed.
const int k = 1;
JntArray jointPoses[k];
JntArray jointRates[k];
JntArray jointAccelerations[k];
JntArray jointTorques[k];
for (int i = 0; i < k; i++)
{
JntArray jointValues(chaindyn.getNrOfJoints());
jointPoses[i] = jointValues;
jointRates[i] = jointValues;
jointAccelerations[i] = jointValues;
jointTorques[i] = jointValues;
}
// Initial arm position configuration/constraint
JntArray jointInitialPose(chaindyn.getNrOfJoints());
jointInitialPose(0) = 0.0; // initial joint0 pose
jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
//j0=0.0, j1=pi/6.0 correspond to x = 0.2, y = -0.7464
//j0=2*pi/3.0, j1=pi/4.0 correspond to x = 0.44992, y = 0.58636
//actual
jointPoses[0](0) = jointInitialPose(0);
jointPoses[0](1) = jointInitialPose(1);
//~Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
//Definition of process main loop
//-------------------------------------------------------------------------------------//
//Time required to complete the task move(frameinitialPose, framefinalPose)
double taskTimeConstant = 0.1;
double simulationTime = 1*taskTimeConstant;
double timeDelta = 0.01;
int status;
const std::string msg = "Assertion failed, check matrix and array sizes";
for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
{
status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]);
CPPUNIT_ASSERT((status == 0));
if (status != 0)
{
std::cout << "Check matrix and array sizes. Something does not match " << std::endl;
exit(1);
}
else
{
//Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
//printf("time, j0_pose, j1_pose, j0_rate, j1_rate, j0_acc, j1_acc, j0_constraintTau, j1_constraintTau \n");
printf("%f %f %f %f %f %f %f %f %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointTorques[0](0), jointTorques[0](1));
}
}
}
int main()
{
using namespace KDL;
//Definition of kinematic chain
//-----------------------------------------------------------------------------------------------//
//Joint (const JointType &type=None, const double &scale=1, const double &offset=0, const double &inertia=0, const double &damping=0, const double &stiffness=0)
Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);
Joint rotJoint2 = Joint(Joint::RotZ, 1, 0, 0.01);
Joint rotJoint3 = Joint(Joint::RotZ, 1, 0, 0.01);
Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
Frame frame3(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
Frame frame4(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
//Segment (const Joint &joint=Joint(Joint::None), const Frame &f_tip=Frame::Identity(), const RigidBodyInertia &I=RigidBodyInertia::Zero())
Segment segment1 = Segment(rotJoint0, frame1);
Segment segment2 = Segment(rotJoint1, frame2);
Segment segment3 = Segment(rotJoint2, frame3);
Segment segment4 = Segment(rotJoint3, frame4);
// RotationalInertia (double Ixx=0, double Iyy=0, double Izz=0, double Ixy=0, double Ixz=0, double Iyz=0)
RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
//RigidBodyInertia (double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
RigidBodyInertia inerSegment1(0.5, Vector(0.0, -0.2, 0.0), rotInerSeg1);
RigidBodyInertia inerSegment2(0.5, Vector(0.0, -0.2, 0.0), rotInerSeg1);
RigidBodyInertia inerSegment3(0.5, Vector(0.0, -0.2, 0.0), rotInerSeg1);
RigidBodyInertia inerSegment4(0.5, Vector(0.0, -0.2, 0.0), rotInerSeg1);
segment1.setInertia(inerSegment1);
segment2.setInertia(inerSegment2);
segment3.setInertia(inerSegment3);
segment4.setInertia(inerSegment4);
Chain chain;
chain.addSegment(segment1);
chain.addSegment(segment2);
chain.addSegment(segment3);
chain.addSegment(segment4);
//~Definition of kinematic chain
//Definition of constraints and external disturbances
//--------------------------------------------------------------------------------------//
//Constraint force matrix at the end-effector
//What is the convention for the spatial force matrix; is it the same as in thesis?
//NOTE AZAMAT: Constraint are also defined in local reference frame?!
Vector constrainXLinear(1.0, 0.0, 0.0);
Vector constrainXAngular(0.0, 0.0, 0.0);
Vector constrainYLinear(0.0, 0.0, 0.0);
Vector constrainYAngular(0.0, 0.0, 0.0);
Vector constrainZLinear(0.0, 0.0, 0.0);
Vector constrainZAngular(0.0, 0.0, 0.0);
Twist constraintForcesX(constrainXLinear, constrainXAngular);
Twist constraintForcesY(constrainYLinear, constrainYAngular);
Twist constraintForcesZ(constrainZLinear, constrainZAngular);
Jacobian alpha(1);
alpha.setColumn(0, constraintForcesX);
// alpha.setColumn(1, constraintForcesY);
//Acceleration energy at the end-effector
JntArray betha(1); //set to zero
betha(0) = 0.0;
// betha(1) = 0.0;
//betha(2) = 0.0;
//arm root acceleration
Vector linearAcc(0.0, 9.8, 0.0); //gravitational acceleration along Y
Vector angularAcc(0.0, 0.0, 0.0);
Twist twist1(linearAcc, angularAcc);
//external forces on the arm
Vector externalForce1(0.0, 0.0, 0.0);
Vector externalTorque1(0.0, 0.0, 0.0);
Vector externalForce2(0.0, 0.0, 0.0);
Vector externalTorque2(0.0, 0.0, 0.0);
Wrench externalNetForce1(externalForce1, externalTorque1);
Wrench externalNetForce2(externalForce2, externalTorque2);
Wrenches externalNetForce;
externalNetForce.push_back(externalNetForce1);
externalNetForce.push_back(externalNetForce2);
externalNetForce.push_back(externalNetForce1);
externalNetForce.push_back(externalNetForce2);
//~Definition of constraints and external disturbances
//Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
int numberOfConstraints = 1;
ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);
//These arrays of joint values contain actual and desired values
//actual is generated by a solver and integrator
//desired is given by an interpolator
//error is the difference between desired-actual
//0-actual, 1-desired, 2-error
int k = 4;
JntArray jointPoses[k];
JntArray jointRates[k];
JntArray jointAccelerations[k];
JntArray jointTorques[k];
JntArray biasqDotDot(chain.getNrOfJoints());
for (int i = 0; i < k; i++)
{
JntArray jointValues(chain.getNrOfJoints());
jointPoses[i] = jointValues;
jointRates[i] = jointValues;
jointAccelerations[i] = jointValues;
jointTorques[i] = jointValues;
}
//cartesian space/link values
//0-actual, 1-desire, 2-error, 3-errorsum
k = 4;
Frames cartX[k];
Twists cartXDot[k];
Twists cartXDotDot[k];
Twist accLink;
for (int i = 0; i < k; i++) //i is number of variables (actual, desired, error)
{
for (int j = 0; j < 4; j++) //j is number of links, in this case 4
{
cartX[i].push_back(frame1);
cartXDot[i].push_back(accLink);
cartXDotDot[i].push_back(accLink);
}
}
// Initial arm position configuration/constraint, negative in clockwise
JntArray jointFinalPose(chain.getNrOfJoints());
jointFinalPose(0) = M_PI/2.0;
jointFinalPose(1) = M_PI/24.0;
jointFinalPose(2) = -M_PI/12.0;
jointFinalPose(3) = M_PI/12.0;
//correspond to x = 0.398289 y = 0.026105
JntArray jointInitialPose(chain.getNrOfJoints());
jointInitialPose(0) = 5*M_PI/6.0;
jointInitialPose(1) = M_PI/12.0;
jointInitialPose(2) = -M_PI/6.0;
jointInitialPose(3) = -M_PI/3.0;
//correspond to x = 0.151764 y= 0.366390
//actual initial
jointPoses[0](0) = jointInitialPose(0);
jointPoses[0](1) = jointInitialPose(1);
jointPoses[0](2) = jointInitialPose(2);
jointPoses[0](3) = jointInitialPose(3);
//desired initial
// jointPoses[1](0) = jointInitialPose(0);
// jointPoses[1](1) = jointInitialPose(1);
//~Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
//Definition of process main loop
//-------------------------------------------------------------------------------------//
//-------------------------------------------------------------------------------------//
double taskTimeConstant = 2.5; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
double simulationTime = 2*taskTimeConstant;
double timeDelta = 0.001;
double timeToSettle = 0.015;
int status;
double ksi[2] = {1.0, 1.0}; //damping factor
double Kp[2];
Kp[0] = 5.49996/(timeToSettle*timeToSettle);
Kp[1] = 1.352/(timeToSettle*timeToSettle);
double Kv[2];
Kv[0] = 18.4997*ksi[0]/timeToSettle;
Kv[1] = 7.5189*ksi[1]/timeToSettle;
double Ki[2] = {0.0, 0.0};
double K = 0.005;
/*
//For cartesian space control with constraints and using external forces
double ksi[2] = {1.0, 1.0}; //damping factor
double Kp[2];
Kp[0] = 6000.0/(taskTimeConstant*taskTimeConstant);
Kp[1] = 11000.0/(taskTimeConstant*taskTimeConstant);
double Kv[2];
Kv[0] = 130*ksi[0]/taskTimeConstant;
Kv[1] = 160*ksi[1]/taskTimeConstant;
double Ki[2] = {-5.01, -5.1};
//For cartesian space control without constraints using external forces
double ksi[2] = {1.0, 1.0}; //damping factor
double Kp[2];
Kp[0] = 7000.0/(taskTimeConstant*taskTimeConstant);
Kp[1] = 12000.0/(taskTimeConstant*taskTimeConstant);
double Kv[2];
Kv[0] = 130*ksi[0]/taskTimeConstant;
Kv[1] = 160*ksi[1]/taskTimeConstant;
double Ki[2] = {-5.01, -5.1};
//For joint space control without constraints using computed torque
double ksi[2] = {1.0, 1.0}; //damping factor
double Kp[2];
Kp[0] = 4000.0/(taskTimeConstant*taskTimeConstant);
Kp[1] = 3200.0/(taskTimeConstant*taskTimeConstant);
double Kv[2];
Kv[0] = 20*ksi[0]/taskTimeConstant;
Kv[1] = 30*ksi[1]/taskTimeConstant;
double Ki[2] = {33.5, 10.5};
double Ka[2] = {0.0, 0.0};
*/
//Interpolator parameters:
double b0_y = 0.366390; //should come from initial joint configuration
double b1_y = 0.0;
double b2_y = ((0.026105 - b0_y)*3.0 / (simulationTime * simulationTime)); //yFinal = 0.026105 x= 0.398289
double b3_y = -((0.026105 - b0_y)*2.0 / (simulationTime * simulationTime * simulationTime));
double b0_x = 0.151764; //should come from initial joint configuration
double b1_x = 0.0;
double b2_x = ((0.151764 - b0_x)*3.0 / (simulationTime * simulationTime)); // Xfinal= xInitial = 0.151764
double b3_x = -((0.151764 - b0_x)*2.0 / (simulationTime * simulationTime * simulationTime));
/*
double a0_q0 = jointInitialPose(0);
double a1_q0 = 0.0;
double a2_q0 = ((jointFinalPose(0) - jointInitialPose(0))*3.0 / (simulationTime * simulationTime));
double a3_q0 = -((jointFinalPose(0) - jointInitialPose(0))*2.0 / (simulationTime * simulationTime * simulationTime));
double a0_q1 = jointInitialPose(1);
double a1_q1 = 0.0;
double a2_q1 = ((jointFinalPose(1) - jointInitialPose(1))*3.0 / (simulationTime * simulationTime));
double a3_q1 = -((jointFinalPose(1) - jointInitialPose(1))*2.0 / (simulationTime * simulationTime * simulationTime));
*/
double feedforwardJointTorque0 = 0.0;
double feedforwardJointTorque1 = 0.0;
for (double t = 0.0; t <= simulationTime; t = t + timeDelta)
{
/*
* double j1 = 0;
double j2 = 0;
double j3 = 0;
double j4 = 0;
Eigen::MatrixXd chainJacobian(6, 2);
chainJacobian.setZero();
chainJacobian(5, 0) = 1;
chainJacobian(5, 1) = 1;
// Jacobian inverse
j1 = -0.4 * sin(jointPoses[0](0)) - 0.4 * sin(jointPoses[0](1));
j2 = -0.4 * sin(jointPoses[0](0) + jointPoses[0](0));
j3 = 0.4 * cos(jointPoses[0](0)) + 0.4 * cos(jointPoses[0](0) + jointPoses[0](1));
j4 = 0.4 * cos(jointPoses[0](0) + jointPoses[0](1));
//printf("t, j1, j2, j3, j4 %f %f %f %f %f\n", t, j1, j2, j3, j4);
chainJacobian(0, 0) = j1;
chainJacobian(0, 1) = j2;
chainJacobian(1, 0) = j3;
chainJacobian(1, 1) = j4;
*/
//Interpolation (Desired) q = a0+a1t+a2t^2+a3t^3
//Do we feed these values? then jointPoses[0] = jointPoses[1]; jointRates[0] = jointRates[1];
// But I don't think so, in control desired trajectory plays role of the reference that the controller should strive for from its actual (previous, current) state.
cartX[1][1].p[0] = b0_x + b1_x * t + b2_x * t * t + b3_x * t * t*t;
cartX[1][1].p[1] = b0_y + b1_y * t + b2_y * t * t + b3_y * t * t*t;
cartXDot[1][1].vel[0] = b1_x + 2 * b2_x * t + 3 * b3_x * t*t;
cartXDot[1][1].vel[1] = b1_y + 2 * b2_y * t + 3 * b3_y * t*t;
cartXDotDot[1][1].vel[0] = 2 * b2_x + 6 * b3_x*t;
cartXDotDot[1][1].vel[1] = 2 * b2_y + 6 * b3_y*t;
// printf("%f %f %f %f %f %f %f\n", t, cartX[1][1].p[0], cartX[1][1].p[1], cartXDot[1][1].vel[0], cartXDot[1][1].vel[1], cartXDotDot[1][1].vel[0], cartXDotDot[1][1].vel[1]);
//printf("Desired Cartesian values: %f %f %f %f %f %f %f\n", t, cartX[1][1].p[0], cartX[1][1].p[1], cartXDot[1][1].vel[0], cartXDot[1][1].vel[1], cartXDotDot[1][1].vel[0], cartXDotDot[1][1].vel[1]);
/*
jointPoses[1](0) = a0_q0 + a1_q0 * t + a2_q0 * t * t + a3_q0 * t * t*t;
jointPoses[1](1) = a0_q1 + a1_q1 * t + a2_q1 * t * t + a3_q1 * t * t*t;
jointRates[1](0) = a1_q0 + 2 * a2_q0 * t + 3 * a3_q0 * t*t;
jointRates[1](1) = a1_q1 + 2 * a2_q1 * t + 3 * a3_q1 * t*t;
jointAccelerations[1](0) = 2 * a2_q0 + 6 * a3_q0*t;
jointAccelerations[1](1) = 2 * a2_q1 + 6 * a3_q1*t;
//printf("Desired joint values: %f %f %f %f %f %f %f\n", t, jointPoses[1](0), jointPoses[1](1), jointRates[1](0), jointRates[1](1), jointAccelerations[1](0), jointAccelerations[1](1));
//printf("%f %f %f %f %f %f %f\n", t, jointPoses[1](0), jointPoses[1](1), jointRates[1](0), jointRates[1](1), jointAccelerations[1](0), jointAccelerations[1](1));
*/
status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0],alpha, betha, externalNetForce, jointTorques[0]);
constraintSolver.getLinkCartesianPose(cartX[0]);
constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
constraintSolver.getLinkCartesianAcceleration(cartXDotDot[0]);
printf("%f %f %f %f %f %f %f\n", t, cartX[0][1].p.x(), cartX[0][1].p.y(), cartXDot[0][1].vel[0], cartXDot[0][1].vel[1], cartXDotDot[0][1].vel[0], cartXDotDot[0][1].vel[1]);
// printf("Actual cartesian values: %f %f %f %f %f %f %f\n", t, cartX[0][1].p.x(), cartX[0][1].p.y(), cartXDot[0][1].vel[0], cartXDot[0][1].vel[1], cartXDotDot[0][1].vel[0], cartXDotDot[0][1].vel[1]);
//Integration(robot joint values for rates and poses; actual) at the given "instanteneous" interval for joint position and velocity.
jointRates[0](0) = jointRates[0](0) + jointAccelerations[0](0) * timeDelta; //Euler Forward
jointPoses[0](0) = jointPoses[0](0) + (jointRates[0](0) - jointAccelerations[0](0) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
jointRates[0](1) = jointRates[0](1) + jointAccelerations[0](1) * timeDelta; //Euler Forward
jointPoses[0](1) = jointPoses[0](1) + (jointRates[0](1) - jointAccelerations[0](1) * timeDelta / 2.0) * timeDelta;
jointRates[0](2) = jointRates[0](2) + jointAccelerations[0](2) * timeDelta; //Euler Forward
jointPoses[0](2) = jointPoses[0](2) + (jointRates[0](2) - jointAccelerations[0](2) * timeDelta / 2.0) * timeDelta; //Trapezoidal rule
jointRates[0](3) = jointRates[0](3) + jointAccelerations[0](3) * timeDelta; //Euler Forward
jointPoses[0](3) = jointPoses[0](3) + (jointRates[0](3) - jointAccelerations[0](3) * timeDelta / 2.0) * timeDelta;
// printf("%f %f %f %f %f %f %f %f %f\n", t, jointPoses[0](0), jointPoses[0](1), jointPoses[0](2), jointPoses[0](3), jointAccelerations[0](0), jointAccelerations[0](1), jointAccelerations[0](2), jointAccelerations[0](3));
//printf("Actual joint values: %f %f %f %f %f %f %f %f %f\n", t, jointPoses[0](0), jointPoses[0](1), jointRates[0](0), jointRates[0](1), jointAccelerations[0](0), jointAccelerations[0](1), jointTorques[0](0), jointTorques[0](1));
//Error
/*
jointPoses[2](0) = jointPoses[1](0) - jointPoses[0](0);
jointPoses[2](1) = jointPoses[1](1) - jointPoses[0](1);
jointRates[2](0) = jointRates[1](0) - jointRates[0](0);
jointRates[2](1) = jointRates[1](1) - jointRates[0](1);
jointAccelerations[2](0) = jointAccelerations[1](0) - jointAccelerations[0](0);
jointAccelerations[2](1) = jointAccelerations[1](1) - jointAccelerations[0](1);
jointPoses[3](0) += timeDelta*jointPoses[2](0);
jointPoses[3](1) += timeDelta*jointPoses[2](1);
//printf("%f %f %f %f %f %f %f\n", t, jointPoses[2](0), jointPoses[2](1), jointRates[2](0), jointRates[2](1), jointAccelerations[2](0), jointAccelerations[2](1));
*/
cartX[2][1].p[0] = cartX[1][1].p[0] - cartX[0][1].p[0];
cartX[2][1].p[1] = cartX[1][1].p[1] - cartX[0][1].p[1];
cartXDot[2][1].vel[0] = cartXDot[1][1].vel[0] - cartXDot[0][1].vel[0];
cartXDot[2][1].vel[1] = cartXDot[1][1].vel[1] - cartXDot[0][1].vel[1];
cartXDotDot[2][1].vel[0] = cartXDotDot[1][1].vel[0] - cartXDotDot[0][1].vel[0];
cartXDotDot[2][1].vel[1] = cartXDotDot[1][1].vel[1] - cartXDotDot[0][1].vel[1];
cartX[3][1].p[0] += timeDelta * cartX[2][1].p[0]; //for integral term;
cartX[3][1].p[1] += timeDelta * cartX[2][1].p[1];
//printf("%f %f %f %f %f %f %f\n", t, cartX[2][1].p[0], cartX[2][1].p[1], cartXDot[2][1].vel[0], cartXDot[2][1].vel[1], cartXDotDot[2][1].vel[1], cartXDotDot[2][1].vel[1]);
//Regulator or controller
//acceleration energy control
// betha(0) = alpha(0, 0)*(K * cartXDotDot[2][1].vel[0] + (Kv[0]) * cartXDot[2][1].vel[0] + (Kp[0]) * cartX[2][1].p[0]+ Ki[1] * cartX[3][1].p[0]); // 0.5xgain
// betha(1) = alpha(1, 1)*(K * cartXDotDot[2][1].vel[1] + (Kv[0]) * cartXDot[2][1].vel[1] + (Kp[1]) * cartX[2][1].p[1] + Ki[1] * cartX[3][1].p[0]);
/*
//computed joint torque control
feedforwardJointTorque0 = jointAccelerations[1](0) + (Kp[0])*jointPoses[2](0) + (Kv[0])*jointRates[2](0) + (Ki[0])*jointPoses[3](0);
feedforwardJointTorque1 = jointAccelerations[1](1) + (Kp[1])*jointPoses[2](1) + (Kv[1])*jointRates[2](1)+ (Ki[1])*jointPoses[3](1);
jointTorques[0](0) = jointTorques[0](0) + feedforwardJointTorque0;
jointTorques[0](1) = jointTorques[0](1) + feedforwardJointTorque1;
//For cartesian space control one needs to calculate from the obtained joint space value, new cartesian space poses.
//Then based on the difference of the signal (desired-actual) we define a regulation function (controller)
// this difference should be compensated either by joint torques.
externalNetForce[1].force[0] = cartXDotDot[1][1].vel[0] + ((Kv[0]) * cartXDot[2][1].vel[0] + (Kp[0]) * cartX[2][1].p[0] + (Ki[0]) * cartX[3][1].p[0]);
externalNetForce[1].force[1] = cartXDotDot[1][1].vel[1] + ((Kv[1]) * cartXDot[2][1].vel[1] + (Kp[1]) * cartX[2][1].p[1] + (Ki[1]) * cartX[3][1].p[1]);
*/
externalNetForce[1].force[0] = ((Kv[0]) * cartXDot[2][1].vel[0] + (Kp[0]) * cartX[2][1].p[0] );
externalNetForce[1].force[1] = ((Kv[1]) * cartXDot[2][1].vel[1] + (Kp[1]) * cartX[2][1].p[1] ); //controlling y seems to help a little
}
//~Definition of process main loop
//-------------------------------------------------------------------------------------//
return 0;
}