bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
{
// clear chain
chain = Chain();
// walk down from chain_root and chain_tip to the root of the tree
vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){
parents_chain_root.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){
parents_chain_tip.push_back(s->first);
if (s->first == root_name) break;
}
if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false;
// remove common part of segment lists
SegmentMap::key_type last_segment = root_name;
while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
parents_chain_root.back() == parents_chain_tip.back()){
last_segment = parents_chain_root.back();
parents_chain_root.pop_back();
parents_chain_tip.pop_back();
}
parents_chain_root.push_back(last_segment);
// add the segments from the root to the common frame
for (unsigned int s=0; s<parents_chain_root.size()-1; s++){
Segment seg = getSegment(parents_chain_root[s])->second.segment;
Frame f_tip = seg.pose(0.0).Inverse();
Joint jnt = seg.getJoint();
if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::RotAxis);
else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::TransAxis);
chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(),
jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia()));
}
// add the segments from the common frame to the tip frame
for (int s=parents_chain_tip.size()-1; s>-1; s--){
chain.addSegment(getSegment(parents_chain_tip[s])->second.segment);
}
return true;
}
Chain ModelGetter::GetModel()
{
const int MAX_LEN = 1024 ;
char cur_line[1024] ;
Chain chain ;
while (!infile_.eof())
{
double model[6] ;
infile_.getline(cur_line, MAX_LEN) ;
int num_read = sscanf(cur_line, "%lf %lf %lf %lf %lf %lf", &model[0], &model[1], &model[2], &model[3], &model[4], &model[5]) ;
if (num_read < 6)
break ;
// Now add the data to the chain
Joint J(Joint::RotZ) ;
Vector rot_axis(model[3], model[4], model[5]) ; // KDL doesn't need vector to be normalized. It even behaves nicely with vector=[0 0 0]
double rot_ang = rot_axis.Norm() ;
Rotation R(Rotation::Rot(rot_axis, rot_ang)) ;
Vector trans(model[0], model[1], model[2]) ;
chain.addSegment(Segment(J, Frame(R, trans))) ;
}
return chain ;
}
static PyObject *meth_Chain_addSegment(PyObject *sipSelf, PyObject *sipArgs)
{
PyObject *sipParseErr = NULL;
{
const Segment* a0;
Chain *sipCpp;
if (sipParseArgs(&sipParseErr, sipArgs, "BJ9", &sipSelf, sipType_Chain, &sipCpp, sipType_Segment, &a0))
{
sipCpp->addSegment(*a0);
Py_INCREF(Py_None);
return Py_None;
}
}
/* Raise an exception if the arguments couldn't be parsed. */
sipNoMethod(sipParseErr, sipName_Chain, sipName_addSegment, doc_Chain_addSegment);
return NULL;
}
//#include <chainidsolver_constraint_vereshchagin.hpp>
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 rotJoint1 = Joint(Joint::None,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));
//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);
// 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(1.0, Vector(0.0, -0.2, 0.0), rotInerSeg1);
RigidBodyInertia inerSegment2(1.0, Vector(0.0, -0.2, 0.0), rotInerSeg1);
segment1.setInertia(inerSegment1);
segment2.setInertia(inerSegment2);
Chain chain;
chain.addSegment(segment1);
//chain.addSegment(segment2);
//----------------------------------------------------------------------------------------------//
//Definition of constraints and external disturbances
//--------------------------------------------------------------------------------------//
JntArray arrayOfJoints(chain.getNrOfJoints());
//Constraint force matrix at the end-effector
//What is the convention for the spatial force matrix; is it the same as in thesis?
Vector constrainXLinear(0.0, 1.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);
const Twist constraintForcesX(constrainXLinear, constrainXAngular);
const Twist constraintForcesY(constrainYLinear, constrainYAngular);
const Twist constraintForcesZ(constrainZLinear, constrainZAngular);
Jacobian alpha(1);
alpha.setColumn(0, constraintForcesX);
//alpha.setColumn(1,constraintForcesY);
//alpha.setColumn(2,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);
//-------------------------------------------------------------------------------------//
//solvers
ChainFkSolverPos_recursive fksolver(chain);
//ChainFkSolverVel_recursive fksolverVel(chain);
int numberOfConstraints = 1;
ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);
// Initial arm position configuration/constraint
Frame frameInitialPose;
JntArray jointInitialPose(chain.getNrOfJoints());
jointInitialPose(0) = M_PI/6.0; // initial joint0 pose
//jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
//cartesian space/link accelerations
Twist accLink0;
Twist accLink1;
Twists cartAcc;
cartAcc.push_back(accLink0);
//cartAcc.push_back(accLink1);
//arm joint rate configuration and initial values are zero
JntArray qDot(chain.getNrOfJoints());
//arm joint accelerations returned by the solver
JntArray qDotDot(chain.getNrOfJoints());
//arm joint torques returned by the solver
JntArray qTorque(chain.getNrOfJoints());
arrayOfJoints(0) = jointInitialPose(0);
//arrayOfJoints(1) = jointInitialPose(1);
//Calculate joint values for each cartesian position
Frame frameEEPose;
Twist frameEEVel;
double TimeConstant = 1; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
double timeDelta = 0.002;
bool status;
Frame cartX1;
Frame cartX2;
Frames cartX;
cartX.push_back(cartX1);
cartX.push_back(cartX2);
Twists cartXDot;
cartXDot.push_back(accLink0);
cartXDot.push_back(accLink1);
Twists cartXDotDot;
cartXDotDot.push_back(accLink0);
cartXDotDot.push_back(accLink1);
for (double t = 0.0; t <= 5*TimeConstant; t = t + timeDelta)
{
//at time t0, inputs are arrayOfjoints(0,pi/2.0), qdot(0,0)
//qDot(0) = 0.5;
// qDot(1) = 0.5;
//printf("Inside the main loop %f %f\n",qTorque(0), qTorque(1));
//NOTE AZAMAT:what is qTorque? in implementation returned qTorque is constraint torque generated by the virtual constraint forces.
// these torques are required to satify a virtual constraint and basically are real torques applied to joints to achieve
// these constraint. But if additionally to the constraint generated torques we also had internal motor torques, then should not they
//be added together?
//qTorque(1) = qTorque(1)+internalJointTorque1;
// In what frame of reference are the results expressed?
status = constraintSolver.CartToJnt(arrayOfJoints, qDot, qDotDot, alpha, betha, externalNetForce, qTorque);
if (status >= 0)
{
//For 2-dof arm
//printf("%f %f %f %f %f %f %f %f %f %f %f\n",t , arrayOfJoints(0), arrayOfJoints(1), qDot(0), qDot(1), qDotDot(0), qDotDot(1), qTorque(0), qTorque(1), frameEEPose.p.x(), frameEEPose.p.y());
//printf("%f %f %f %f %f %f %f\n", t, cartAcc[0].vel.x(), cartAcc[0].vel.y(), cartAcc[0].rot.z(), cartAcc[1].vel.x(), cartAcc[1].vel.y(), cartAcc[1].rot.z());
//For 1-dof
printf("Joint actual %f %f %f %f %f\n", t, arrayOfJoints(0), qDot(0), qDotDot(0), qTorque(0));
std::cout<<std::endl;
}
constraintSolver.getLinkCartesianPose(cartX);
constraintSolver.getLinkCartesianVelocity(cartXDot);
constraintSolver.getLinkCartesianAcceleration(cartXDotDot);
constraintSolver.getLinkAcceleration(cartAcc);
printf("Cartesian acc local %f %f %f %f\n", t, cartAcc[0].vel.x(), cartAcc[0].vel.y(), cartAcc[0].rot.z());
printf("Cartesian actual %f %f %f %f %f\n", t, cartX[0].p.x(), cartX[0].p.y(), cartXDot[0].vel[0], cartXDot[0].vel[1]);
std::cout<<std::endl;
//printf("External torque %f\n", externalNetForce[0].torque.z());
//Integration at the given "instanteneous" interval for joint position and velocity.
//These will be inputs for the next time instant
//actually here it should be a time interval and not time from the beginning, that is why timeDelta;
arrayOfJoints(0) = arrayOfJoints(0) + (qDot(0) + qDotDot(0) * timeDelta / 2.0) * timeDelta;
qDot(0) = qDot(0) + qDotDot(0) * timeDelta;
//arrayOfJoints(1) = arrayOfJoints(1) + (qDot(1) + qDotDot(1)*timeDelta/2.0)*timeDelta;
//qDot(1) = qDot(1)+qDotDot(1)*timeDelta;
//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 constraint forces or by joint torques.
// The current version of the algorithm assumes that we don't have control over torques, thus change in constraint forces
//should be a valid option.
}
//torque1=[(m1+m2)a1^2+m2a2^2+2m2a1a2cos2]qDotDot1+[m2a2^2+m2a1a2cos2]qDotDot2-m2a1a2(2qDot1qDot2+qDot2^2)sin2+(m1+m2)ga1cos2+m2ga2cos(1+2)
//torque2=[m2a2^2+m2a1a2cos2]qDotDot1+m2a2^2qDotDot2+m2a1a2qDot1^2sin2+m2ga2cos(1+2)
return 0;
}
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);
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.4, 0.0));
Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 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);
// 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.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
RigidBodyInertia inerSegment2(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
segment1.setInertia(inerSegment1);
segment2.setInertia(inerSegment2);
Chain chain;
chain.addSegment(segment1);
chain.addSegment(segment2);
//~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(3);
alpha.setColumn(0, constraintForcesX);
alpha.setColumn(1, constraintForcesY);
alpha.setColumn(2, constraintForcesZ);
//Acceleration energy at the end-effector
JntArray betha(3); //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);
//~Definition of constraints and external disturbances
//-------------------------------------------------------------------------------------//
//Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
int numberOfConstraints = 3;
ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);
//These arrays of joint values contains values for 3
//consequetive steps
int k = 3;
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
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 < k - 1; j++) //j is number of links
{
cartX[i].push_back(frame1);
cartXDot[i].push_back(accLink);
cartXDotDot[i].push_back(accLink);
}
}
//at time=0.0 positions, rates and accelerations
JntArray jointInitialPose(chain.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=pi/6.0, j1=pi/6.0 correspond to x = 0.5464, y = -0.5464
//~Definition of solver and initial configuration
//-------------------------------------------------------------------------------------//
//First iteration to kick start integration process, because we need 0,1 iterations for the integration process
//We use Euler method for the iteration 0
double timeDelta = 0.001; //sampling interval/step size
double TimeConstant = 60; //Time required to complete the task
double Kp = 150.0;
double Kv = 10.0;
//Interpolator parameters:
double b0_y = -0.7464102; //should come from initial joint configuration
double b1_y = 0.0;
double b2_y = 0.0; //((0.5 + 0.7464)*3.0 / TimeConstant * TimeConstant);
double b3_y = 0.0; //-((0.5 + 0.7464)*2.0 / TimeConstant * TimeConstant * TimeConstant);
double b0_x = 0.2; //should come from initial joint configuration
double b1_x = 0.0;
double b2_x = 0.0; //((0.5 + 0.7464)*3.0 / TimeConstant * TimeConstant);
double b3_x = 0.0; //-((0.5 + 0.7464)*2.0 / TimeConstant * TimeConstant * TimeConstant);
//Desired
cartX[1][1].p[0] = b0_x;
cartX[1][1].p[1] = b0_y;
cartXDot[1][1].vel[0] = 0.0;
cartXDot[1][1].vel[1] = 0.0;
cartXDotDot[1][1].vel[0] = 0.0;
cartXDotDot[1][1].vel[1] = 0.0;
//step 0
jointPoses[0](0) = jointInitialPose(0);
jointPoses[0](1) = jointInitialPose(1);
constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], cartXDotDot[0], alpha, betha, externalNetForce, jointTorques[0]);
//Actual
constraintSolver.getLinkCartesianPose(cartX[0]);
constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
//Error
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];
printf("Iter 0: %f %f %f %f %f %f\n", 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
betha(0) = alpha(0, 0)*((Kp / 10000.0) * cartXDotDot[2][1].vel[0] + (Kv / 200.0) * cartXDot[2][1].vel[0] + (2 * Kp) * cartX[2][1].p[0]);
betha(1) = alpha(1, 1)*((Kp / 10000.0) * cartXDotDot[2][1].vel[1] + (Kv / 250.0) * cartXDot[2][1].vel[1] + (1.5 * Kp) * cartX[2][1].p[1]);
//step 1
jointRates[1](0) = jointRates[0](0) + timeDelta * jointAccelerations[0](0);
jointRates[1](1) = jointRates[0](1) + timeDelta * jointAccelerations[0](1);
jointPoses[1](0) = jointPoses[0](0) + timeDelta * jointRates[0](0);
jointPoses[1](1) = jointPoses[0](1) + timeDelta * jointRates[0](1);
constraintSolver.CartToJnt(jointPoses[1], jointRates[1], jointAccelerations[1], cartXDotDot[0], alpha, betha, externalNetForce, jointTorques[0]);
//Actual
constraintSolver.getLinkCartesianPose(cartX[0]);
constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
//Error
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];
printf("Iter 0: %f %f %f %f %f %f\n", 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
betha(0) = alpha(0, 0)*((Kp / 10000.0) * cartXDotDot[2][1].vel[0] + (Kv / 200.0) * cartXDot[2][1].vel[0] + (2 * Kp) * cartX[2][1].p[0]);
betha(1) = alpha(1, 1)*((Kp / 10000.0) * cartXDotDot[2][1].vel[1] + (Kv / 250.0) * cartXDot[2][1].vel[1] + (1.5 * Kp) * cartX[2][1].p[1]);
//Definition of process main loop
//-------------------------------------------------------------------------------------//
for (double t = 2 * timeDelta; t <= TimeConstant; t = t + timeDelta)
{
//Interpolation q = a0+a1t+a2t^2+a3t^3
//Desired
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;
// AB 2 order: predictor
jointRates[2](0) = jointRates[1](0) + timeDelta * (1.5 * jointAccelerations[1](0) - 0.5 * jointAccelerations[0](0));
jointRates[2](1) = jointRates[1](1) + timeDelta * (1.5 * jointAccelerations[1](1) - 0.5 * jointAccelerations[0](1));
jointPoses[2](0) = jointPoses[1](0) + timeDelta * (1.5 * jointRates[1](0) - 0.5 * jointRates[0](0));
jointPoses[2](1) = jointPoses[1](1) + timeDelta * (1.5 * jointRates[1](1) - 0.5 * jointRates[0](1));
jointTorques[1] = jointTorques[0]; //correction is done on pose,vel and acc, so we need old torques with corrected pose, vel to get corrected acc
// printf("Prediction: %f %f %f %f %f %f %f %f %f\n", t, jointPoses[2](0), jointPoses[2](1), jointRates[2](0), jointRates[2](1), jointAccelerations[1](0), jointAccelerations[1](1), jointTorques[0](0), jointTorques[0](1));
//Function evaluation
constraintSolver.CartToJnt(jointPoses[2], jointRates[2], jointAccelerations[2], cartXDotDot[0], alpha, betha, externalNetForce, jointTorques[0]);
// printf("Evaluation 1: %f %f %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), jointTorques[0](0), jointTorques[0](1));
//AM 2 order: corrector
jointPoses[2](0) = jointPoses[1](0) + timeDelta * ((5 / 12.0) * jointRates[2](0) + (2 / 3.0) * jointRates[1](0) - (1 / 12.0) * jointRates[0](0));
jointPoses[2](1) = jointPoses[1](1) + timeDelta * ((5 / 12.0) * jointRates[2](1) + (2 / 3.0) * jointRates[1](1) - (1 / 12.0) * jointRates[0](1));
jointPoses[0] = jointPoses[1];
jointPoses[1] = jointPoses[2];
jointRates[2](0) = jointRates[1](0) + timeDelta * ((5 / 12.0) * jointAccelerations[2](0) + (2 / 3.0) * jointAccelerations[1](0) - (1 / 12.0) * jointAccelerations[0](0));
jointRates[2](1) = jointRates[1](1) + timeDelta * ((5 / 12.0) * jointAccelerations[2](1) + (2 / 3.0) * jointAccelerations[1](1) - (1 / 12.0) * jointAccelerations[0](1));
jointRates[0] = jointRates[1]; //memorize n+1
jointRates[1] = jointRates[2]; //memorize n+2
// printf("Correction : %f %f %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), jointTorques[0](0), jointTorques[0](1));
//Function evaluation give final corrected acc n+2
constraintSolver.CartToJnt(jointPoses[2], jointRates[2], jointAccelerations[2], cartXDotDot[0], alpha, betha, externalNetForce, jointTorques[1]);
//printf("%f %f %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), jointTorques[1](0), jointTorques[1](1));
jointAccelerations[0] = jointAccelerations[1]; //memorize n+1
jointAccelerations[1] = jointAccelerations[2]; //memoze n+2
jointTorques[0] = jointTorques[1];
constraintSolver.initial_upwards_sweep(jointPoses[1], jointRates[1], jointAccelerations[1], externalNetForce);
//Actual
constraintSolver.getLinkCartesianPose(cartX[0]);
constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
//Error
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];
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
betha(0) = alpha(0, 0)*((Kp / 40000.0) * cartXDotDot[2][1].vel[0] + (Kv / 4000.0) * cartXDot[2][1].vel[0] + (Kp) * cartX[2][1].p[0]);
betha(1) = alpha(1, 1)*((Kp / 20000.0) * cartXDotDot[2][1].vel[1] + (Kv / 500.0) * cartXDot[2][1].vel[1] + (0.75 * Kp) * cartX[2][1].p[1]);
}
return 0;
}
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);
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.4, 0.0));
Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.4, 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);
// 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.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
RigidBodyInertia inerSegment2(0.3, Vector(0.0, -0.4, 0.0), rotInerSeg1);
segment1.setInertia(inerSegment1);
segment2.setInertia(inerSegment2);
Chain chain;
chain.addSegment(segment1);
chain.addSegment(segment2);
//~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(0.0, 0.0, 0.0);
Vector constrainXAngular(0.0, 0.0, 0.0);
Vector constrainYLinear(0.0, 1.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, 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, 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(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
int k = 3;
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
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 < k-1; j++) //j is number of links
{
cartX[i].push_back(frame1);
cartXDot[i].push_back(accLink);
cartXDotDot[i].push_back(accLink);
}
}
// Initial arm position configuration/constraint
JntArray jointInitialPose(chain.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);
//desired
jointPoses[1](0) = jointInitialPose(0);
jointPoses[1](1) = jointInitialPose(1);
//~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;
bool status;
double ksi = 1; //damping factor
double Kp = 0.02/(taskTimeConstant*taskTimeConstant);
double Kv = 1*ksi/taskTimeConstant;
double Ki = 1.0;
double Ka = 0.0;
//Interpolator parameters:
double b0_y = -0.7464102; //should come from initial joint configuration
double b1_y = 0.0;
double b2_y = 0.0;//((0.5 + 0.7464)*3.0 / TimeConstant * TimeConstant);
double b3_y = 0.0;//-((0.5 + 0.7464)*2.0 / TimeConstant * TimeConstant * TimeConstant);
double b0_x = 0.2; //should come from initial joint configuration
double b1_x = 0.0;
double b2_x = 0.0;//((0.5 + 0.7464)*3.0 / TimeConstant * TimeConstant);
double b3_x = 0.0;//-((0.5 + 0.7464)*2.0 / TimeConstant * TimeConstant * TimeConstant);
/*
double a0_q0 = jointPoses[1](0);
double a1_q0 = 0.0;
double a2_q0 = ((qFinalPose(0) - jointPoses[1](0))*3.0 / TimeConstant * TimeConstant);
double a3_q0 = -((qFinalPose(0) - jointPoses[1](0))*2.0 / TimeConstant * TimeConstant * TimeConstant);
double a0_q1 = jointPoses[1](1);
double a1_q1 = 0.0;
double a2_q1 = ((qFinalPose(1) - jointPoses[1](1))*3.0 / TimeConstant * TimeConstant);
double a3_q1 = -((qFinalPose(1) - jointPoses[1](1))*2.0 / TimeConstant * TimeConstant * TimeConstant);
*/
for (double t = 0.0; t <=simulationTime; t = t + timeDelta)
{
//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("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]);
status = constraintSolver.CartToJnt(jointPoses[0], jointRates[0], jointAccelerations[0], alpha, betha, externalNetForce, jointTorques[0]);
/*
constraintSolver.initial_upwards_sweep(jointPoses[0], jointRates[0], jointAccelerations[0], externalNetForce);
constraintSolver.downwards_sweep(alpha, jointTorques[0]);
constraintSolver.constraint_calculation(betha);
constraintSolver.final_upwards_sweep(jointAccelerations[0], cartXDotDot[0], jointTorques[0]);
*/
//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("Joint %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));
constraintSolver.initial_upwards_sweep(jointPoses[0], jointRates[0], jointAccelerations[0], externalNetForce);
constraintSolver.getLinkCartesianPose(cartX[0]);
constraintSolver.getLinkCartesianVelocity(cartXDot[0]);
constraintSolver.getLinkCartesianAcceleration(cartXDotDot[0]);
//printf("Cartesian actual %f %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], cartXDotDot[0][1].rot[2]);
//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);
printf("Errors: %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));
*/
//e = d - a
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];
cartXDotDot[2][1].rot[2] = cartXDotDot[1][1].rot[2] - cartXDotDot[0][1].rot[2];
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("Cartesian error %f %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], cartXDotDot[2][1].rot[2]);
//Regulator or controller
//externalNetForce[1].force[0] = ( (Kv)*cartXDot[2][1].vel[0] + (Kp)*cartX[2][1].p[0] + Ki*cartX[3][1].p[0]);
betha(0) = ( (Kv)*cartXDot[2][1].vel[1] - (Ka)*cartXDot[2][1].rot[2] + (Kp)*cartX[2][1].p[1]);// + Ki*cartX[3][1].p[0]);
//betha(1) = betha(1) +( (Kv)*cartXDot[2][1].vel[1] + (Kp)*cartX[2][1].p[1]);// + Ki*cartX[3][1].p[0]);
//printf("betha %f %f\n", betha(0), betha(1));
}
//~Definition of process main loop
//-------------------------------------------------------------------------------------//
return 0;
}
int main ( int argc, char **argv )
{
ros::init ( argc, argv, "traj_gen" );
ros::NodeHandle n;
ros::Publisher traj_pub = n.advertise<trajectory_msgs::JointTrajectory> ( "/traj_cmd",1 );
//boost::thread t2 = boost::thread::thread ( boost::bind ( &pubTrajectory ) );
ros::ServiceClient client = n.serviceClient<bosch_arm_srvs::GetJointAngles> ( "get_joint_angles" );
bosch_arm_srvs::GetJointAngles srv;
client.call ( srv );
//read destination and duration
KDL::Vector des;
for ( int i=0;i<3;i++ )
des[i]=boost::lexical_cast<double> ( argv[i+1] );
double t=boost::lexical_cast<double> ( argv[4] );
//TODO convert destination to joint angles
Segment seg0=Segment ( Joint ( Joint::None ),
Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ),Vector ( 0,0,0.27 ) ) );
Segment seg1=Segment ( Joint ( Joint::RotZ ),
Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ) ) );
Segment seg2=Segment ( Joint ( Joint::RotZ ),
Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ) ) );
Segment seg3=Segment ( Joint ( Joint::None ),
Frame ( Rotation::RPY ( M_PI/2,-M_PI/2,0 ),Vector ( 0,0,0.50 ) ) );
Segment seg4=Segment ( Joint ( Joint::RotZ ),
Frame ( Vector ( 0.48,0,0 ) ) );
Chain chain;
chain.addSegment ( seg0 );
chain.addSegment ( seg1 );
chain.addSegment ( seg2 );
chain.addSegment ( seg3 );
chain.addSegment ( seg4 );
ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive ( chain );
//linear interpolation in joint space
trajectory_msgs::JointTrajectory traj;
traj.header.stamp=ros::Time::now();
traj.header.frame_id="";
traj.header.seq=0;
traj.points.resize ( npts+1 );
traj.joint_names.resize ( 4 );
traj.joint_names[0]="joint1";
traj.joint_names[1]="joint2";
traj.joint_names[2]="joint3";
traj.joint_names[3]="joint4";
traj.points[i].positions.resize ( 4 );
int npts=ceil ( t*200 );
KDL::JntArray jointpositions = JntArray (3);
KDL::Frame cartpos;
double q3;
bool kinematics_status;
for ( int i=0;i<=npts;i++ )
{
//get the current joint position
client.call ( srv );
//compute forward kinematics.
jointpositions (0) =srv.response.joint_angles[0];
jointpositions (1) =srv.response.joint_angles[1];
q3 =srv.response.joint_angles[2];
jointpositions (2) =srv.response.joint_angles[3];
kinematics_status = fksolver.JntToCart ( jointpositions,cartpos );
if ( kinematics_status>=0 )
{
std::cout << cartpos.p <<std::endl;
}
else
{
printf ( "%s \n","Error: could not calculate forward kinematics :(" );
}
double steps_to_go=npts-i;
Vector step_length_cart= (des-cartpos.p)/steps_to_go;
for ( int j=0;j<4;j++ )
traj.points[i].positions[j]=srv.response.joint_angles[j]+i*dq[j];
traj.points[i].time_from_start=ros::Duration ( i*t/npts );
}
//The first message is always lost
for ( int i=0;i<2;i++ )
{
traj_pub.publish ( traj );
sleep ( 1 );
}
ros::spin();
return 0;
}
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;
}