int simEmbInterpolateTransformations(const float* position1,const float* quaternion1,const float* position2,const float* quaternion2,float interpolFactor,float* positionOut,float* quaternionOut)
{
if (!hasLaunched())
return(-1);
// V-REP quaternion, internally: w x y z
// V-REP quaternion, at interfaces: x y z w (like ROS)
C7Vector tr1;
tr1.Q(0)=quaternion1[3];
tr1.Q(1)=quaternion1[0];
tr1.Q(2)=quaternion1[1];
tr1.Q(3)=quaternion1[2];
tr1.X(0)=position1[0];
tr1.X(1)=position1[1];
tr1.X(2)=position1[2];
C7Vector tr2;
tr2.Q(0)=quaternion2[3];
tr2.Q(1)=quaternion2[0];
tr2.Q(2)=quaternion2[1];
tr2.Q(3)=quaternion2[2];
tr2.X(0)=position2[0];
tr2.X(1)=position2[1];
tr2.X(2)=position2[2];
C7Vector trOut;
trOut.buildInterpolation(tr1,tr2,interpolFactor);
quaternionOut[0]=trOut.Q(1);
quaternionOut[1]=trOut.Q(2);
quaternionOut[2]=trOut.Q(3);
quaternionOut[3]=trOut.Q(0);
positionOut[0]=trOut.X(0);
positionOut[1]=trOut.X(1);
positionOut[2]=trOut.X(2);
return(1);
}
void C4X4Matrix::buildInterpolation(const C4X4Matrix& fromThis,const C4X4Matrix& toThat,float t)
{ // Builds the interpolation (based on t) from 'fromThis' to 'toThat'
C7Vector out;
out.buildInterpolation(fromThis.getTransformation(),toThat.getTransformation(),t);
(*this)=out;
}
void CikEl::prepareIkEquations(extIkReal interpolFact)
{ // Before calling this function, make sure that joint's temp. param. are initialized!
// Make also sure the tooltip is built on a joint before 'base' and that base
// is parent of 'tooltip'.
// interpolFact is the interpolation factor we use to compute the target pose:
// interpolPose=tooltipPose*(1-interpolFact)+targetPose*interpolFact
// We first take care of dummies linked to path objects in a "sliding" manner (not fixed but assigned to the path):
// Case 1. Target is the free sliding dummy:
CDummy* dummyObj=Ct::ct->objCont->getDummy(getTarget());
CDummy* tipObj=Ct::ct->objCont->getDummy(tooltip);
// We get the jacobian and the rowJointIDs:
rowJointIDs=new std::vector<int>;
rowJointStages=new std::vector<int>;
C4X4Matrix oldMatr;
CMatrix* Ja=CIkRoutine::getJacobian(this,oldMatr,rowJointIDs,rowJointStages);
// oldMatr now contains the cumulative transf. matr. of tooltip relative to base
C4X4Matrix oldMatrInv(oldMatr.getInverse());
int doF=Ja->cols;
int equationNumber=0;
C4X4Matrix dummyCumul;
C4X4Matrix m;
if (dummyObj!=NULL)
{
C3DObject* baseObj=Ct::ct->objCont->getObject(base);
C4X4Matrix baseCumul;
baseCumul.setIdentity();
if (baseObj!=NULL)
baseCumul=baseObj->getCumulativeTransformation(true).getMatrix();
baseObj=Ct::ct->objCont->getObject(alternativeBaseForConstraints);
if (baseObj!=NULL)
baseCumul=baseObj->getCumulativeTransformation(true).getMatrix();
baseCumul.inverse();
dummyCumul=dummyObj->getCumulativeTransformationPart1(true).getMatrix();
dummyCumul=baseCumul*dummyCumul; // target is relative to the base (or the alternative base)!
C7Vector tr;
tr.buildInterpolation(oldMatr.getTransformation(),dummyCumul.getTransformation(),interpolFact);
m=tr;
// We prepare matrix and errorVector and their respective sizes:
if (constraints&sim_ik_x_constraint)
equationNumber++;
if (constraints&sim_ik_y_constraint)
equationNumber++;
if (constraints&sim_ik_z_constraint)
equationNumber++;
if (constraints&sim_ik_alpha_beta_constraint)
equationNumber+=2;
if (constraints&sim_ik_gamma_constraint)
equationNumber++;
}
matrix=new CMatrix(equationNumber,doF);
matrix_correctJacobian=new CMatrix(equationNumber,doF);
errorVector=new CMatrix(equationNumber,1);
if (dummyObj!=NULL)
{
// We set up the position/orientation errorVector and the matrix:
int pos=0;
if (constraints&sim_ik_x_constraint)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(0,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(0,i);
}
(*errorVector)(pos,0)=(m.X(0)-oldMatr.X(0))*positionWeight;
pos++;
}
if (constraints&sim_ik_y_constraint)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(1,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(1,i);
}
(*errorVector)(pos,0)=(m.X(1)-oldMatr.X(1))*positionWeight;
pos++;
}
if (constraints&sim_ik_z_constraint)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(2,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(2,i);
}
(*errorVector)(pos,0)=(m.X(2)-oldMatr.X(2))*positionWeight;
pos++;
}
if ( (constraints&sim_ik_alpha_beta_constraint)&&(constraints&sim_ik_gamma_constraint) )
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(3,i);
(*matrix)(pos+1,i)=(*Ja)(4,i);
(*matrix)(pos+2,i)=(*Ja)(5,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(3,i)*IK_DIVISION_FACTOR;
(*matrix_correctJacobian)(pos+1,i)=(*Ja)(4,i)*IK_DIVISION_FACTOR;
(*matrix_correctJacobian)(pos+2,i)=(*Ja)(5,i)*IK_DIVISION_FACTOR;
}
C4X4Matrix diff(oldMatrInv*m);
C3Vector euler(diff.M.getEulerAngles());
(*errorVector)(pos,0)=euler(0)*orientationWeight/IK_DIVISION_FACTOR;
(*errorVector)(pos+1,0)=euler(1)*orientationWeight/IK_DIVISION_FACTOR;
(*errorVector)(pos+2,0)=euler(2)*orientationWeight/IK_DIVISION_FACTOR;
pos=pos+3;
}
else
{
if (constraints&sim_ik_alpha_beta_constraint)
{
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(3,i);
(*matrix)(pos+1,i)=(*Ja)(4,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(3,i)*IK_DIVISION_FACTOR;
(*matrix_correctJacobian)(pos+1,i)=(*Ja)(4,i)*IK_DIVISION_FACTOR;
}
C4X4Matrix diff(oldMatrInv*m);
C3Vector euler(diff.M.getEulerAngles());
(*errorVector)(pos,0)=euler(0)*orientationWeight/IK_DIVISION_FACTOR;
(*errorVector)(pos+1,0)=euler(1)*orientationWeight/IK_DIVISION_FACTOR;
pos=pos+2;
}
if (constraints&sim_ik_gamma_constraint)
{ // sim_gamma_constraint can't exist without sim_alpha_beta_constraint!
for (int i=0;i<doF;i++)
{
(*matrix)(pos,i)=(*Ja)(5,i);
(*matrix_correctJacobian)(pos,i)=(*Ja)(5,i)*IK_DIVISION_FACTOR;
}
C4X4Matrix diff(oldMatrInv*m);
C3Vector euler(diff.M.getEulerAngles());
(*errorVector)(pos,0)=euler(2)*orientationWeight/IK_DIVISION_FACTOR;
pos++;
}
}
}
delete Ja; // We delete the jacobian!
}
